void GOST34112012Update_sse41(GOST34112012Context* ctx, const unsigned char* data, std::size_t len)
{
std::size_t chunksize;
const union uint512_u* d = reinterpret_cast<const union uint512_u*>(data);
while (len > 63 && ctx->bufsize == 0) {
stage2(ctx, d);
data += 64;
len -= 64;
}
while (len) {
chunksize = 64 - ctx->bufsize;
if (chunksize > len) {
chunksize = len;
}
std::memcpy(&ctx->buffer.BYTE[ctx->bufsize], data, chunksize);
ctx->bufsize += chunksize;
len -= chunksize;
data += chunksize;
if (ctx->bufsize == 64) {
stage2(ctx, &ctx->buffer);
ctx->bufsize = 0;
}
}
_mm_empty();
}
int main(int argc, char *argv[])
{
if (argc < 2) {
printf("Supply the disk dump as a parameter!\n");
return -1;
}
char* filename = argv[1];
FILE *fp = fopen(filename, "rb");
if (fp == NULL) {
printf("Cannot open file\n");
return -1;
}
if (is_infected(fp)) {
printf("[+] Petya FOUND on the disk!\n");
} else {
printf("[-] Petya not found on the disk!\n");
}
printf("---\n");
if (stage1(fp) == 0) {
printf("[OK] Stage 1 key recovered!\n");
fclose(fp);
return 0;
}
printf("Invalid Stage1 key! Probably the key has been already erased!\n");
printf("Try to recover from Stage2 by third-party decoder!\n");
printf("Paste the data you got below on one of the following sites:\n");
printf("+ https://petya-pay-no-ransom.herokuapp.com/\n");
printf("+ https://petya-pay-no-ransom-mirror1.herokuapp.com/\n");
stage2(fp);
fclose(fp);
return 0;
}
int stage1(int a, int e, int _n) {
A = a;
P = 1;
E = e;
n = _n;
return stage2();
}
void MaintenanceMngr::slotToolCompleted(ProgressItem* tool)
{
// At each stage, relevant tool instance is set to zero to prevent redondant call to this slot
// from ProgressManager. This will disable multiple triggering in this method.
// There is no memory leak. Each tool instance are delete later by ProgressManager.
if (tool == dynamic_cast<ProgressItem*>(d->newItemsFinder))
{
d->newItemsFinder = 0;
stage2();
}
else if (tool == dynamic_cast<ProgressItem*>(d->thumbsGenerator))
{
d->thumbsGenerator = 0;
stage3();
}
else if (tool == dynamic_cast<ProgressItem*>(d->fingerPrintsGenerator))
{
d->fingerPrintsGenerator = 0;
stage4();
}
else if (tool == dynamic_cast<ProgressItem*>(d->duplicatesFinder))
{
d->duplicatesFinder = 0;
stage5();
}
#ifdef HAVE_KFACE
else if (tool == dynamic_cast<ProgressItem*>(d->faceDetector))
{
d->faceDetector = 0;
stage6();
}
#endif /* HAVE_KFACE */
else if (tool == dynamic_cast<ProgressItem*>(d->imageQualitySorter))
{
d->imageQualitySorter = 0;
stage7();
}
else if (tool == dynamic_cast<ProgressItem*>(d->metadataSynchronizer))
{
d->metadataSynchronizer = 0;
done();
}
}
int main( int argc, char* argv[] ) {
if ( !strcmp( argv[ 1 ], "stage1" ) ) {
stage1( argv[ 2 ], atoi( argv[ 3 ] ), atoi( argv[ 4 ] ) );
}
else if( !strcmp( argv[ 1 ], "stage2" ) ) {
stage2( argv[ 2 ], argv[ 3 ] );
}
else if( !strcmp( argv[ 1 ], "stage3" ) ) {
stage3( argv[ 2 ], argv[ 3 ], argv[ 4 ] );
}
else if( !strcmp( argv[ 1 ], "stage4" ) ) {
stage4( argv[ 2 ], argv[ 3 ], argv[ 4 ] );
}
else if( !strcmp( argv[ 1 ], "stage5" ) ) {
stage5( argv[ 2 ], argv + 3, argc - 3 );
}
return 0;
}
bool execute()
{
schifra::utils::timer timer;
timer.start();
bool result = stage1() &&
stage2() &&
stage3() &&
stage4() &&
stage5() &&
stage6() &&
stage7() &&
stage8() &&
stage9() &&
stage10() &&
stage11() &&
stage12();
timer.stop();
double time = timer.time();
print_codec_properties();
std::cout << "Blocks decoded: " << blocks_processed_ <<
"\tDecoding Failures: " << block_failures_ <<
"\tRate: " << ((blocks_processed_ * data_length) * 8.0) / (1048576.0 * time) << "Mbps" << std::endl;
/*
Note: The throughput rate is not only the throughput of reed solomon
encoding and decoding, but also that of the steps needed to add
simulated transmission errors to the reed solomon block such as
the calculation of the positions and additions of errors and
erasures to the reed solomon block, which normally in a true
data transmission medium would not be taken into consideration.
*/
return result;
}
/* Input: p is the initial generator (sigma), if 0 generate it at random.
n is the number to factor
B1 is the stage 1 bound
B2 is the stage 2 bound
k is the number of blocks for stage 2
verbose is the verbosity level
Output: p is the factor found
Return value: non-zero iff a factor is found (1 for stage 1, 2 for stage 2)
*/
int
pp1 (mpz_t f, mpz_t p, mpz_t n, mpz_t go, double *B1done, double B1,
mpz_t B2min_parm, mpz_t B2_parm, double B2scale, unsigned long k,
const int S, int verbose, int repr, int use_ntt, FILE *os, FILE *es,
char *chkfilename, char *TreeFilename, double maxmem,
gmp_randstate_t rng, int (*stop_asap)(void))
{
int youpi = ECM_NO_FACTOR_FOUND;
int po2 = 0; /* Whether we should use power-of-2 poly degree */
long st;
mpres_t a;
mpmod_t modulus;
mpz_t B2min, B2; /* Local B2, B2min to avoid changing caller's values */
unsigned long dF;
root_params_t root_params;
faststage2_param_t faststage2_params;
const int stage2_variant = (S == 1 || S == ECM_DEFAULT_S);
int twopass = 0;
set_verbose (verbose);
ECM_STDOUT = (os == NULL) ? stdout : os;
ECM_STDERR = (es == NULL) ? stdout : es;
/* if n is even, return 2 */
if (mpz_divisible_2exp_p (n, 1))
{
mpz_set_ui (f, 2);
return ECM_FACTOR_FOUND_STEP1;
}
st = cputime ();
if (mpz_cmp_ui (p, 0) == 0)
pm1_random_seed (p, n, rng);
mpz_init_set (B2min, B2min_parm);
mpz_init_set (B2, B2_parm);
/* Set default B2. See ecm.c for comments */
if (ECM_IS_DEFAULT_B2(B2))
{
if (stage2_variant == 0)
mpz_set_d (B2, B2scale * pow (B1 * PP1_COST, DEFAULT_B2_EXPONENT));
else
mpz_set_d (B2, B2scale * pow (B1 * PP1FS2_COST,
PM1FS2_DEFAULT_B2_EXPONENT));
}
/* set B2min */
if (mpz_sgn (B2min) < 0)
mpz_set_d (B2min, B1);
mpmod_init (modulus, n, repr);
if (use_ntt)
po2 = 1;
if (stage2_variant != 0)
{
long P;
const unsigned long lmax = 1UL<<28; /* An upper bound */
unsigned long lmax_NTT, lmax_noNTT;
mpz_init (faststage2_params.m_1);
faststage2_params.l = 0;
/* Find out what the longest transform length is we can do at all.
If no maxmem is given, the non-NTT can theoretically do any length. */
lmax_NTT = 0;
if (use_ntt)
{
unsigned long t, t2 = 0;
/* See what transform length that the NTT can handle (due to limited
primes and limited memory) */
t = mpzspm_max_len (n);
lmax_NTT = MIN (lmax, t);
if (maxmem != 0.)
{
t = pp1fs2_maxlen (double_to_size (maxmem), n, use_ntt, 0);
t = MIN (t, lmax_NTT);
/* Maybe the two pass variant lets us use a longer transform */
t2 = pp1fs2_maxlen (double_to_size (maxmem), n, use_ntt, 1);
t2 = MIN (t2, lmax_NTT);
if (t2 > t)
{
t = t2;
twopass = 1;
}
lmax_NTT = t;
}
outputf (OUTPUT_DEVVERBOSE, "NTT can handle lmax <= %lu\n", lmax_NTT);
}
/* See what transform length that the non-NTT code can handle */
lmax_noNTT = lmax;
if (maxmem != 0.)
{
unsigned long t;
t = pp1fs2_maxlen (double_to_size (maxmem), n, 0, 0);
lmax_noNTT = MIN (lmax_noNTT, t);
outputf (OUTPUT_DEVVERBOSE, "non-NTT can handle lmax <= %lu\n",
lmax_noNTT);
}
P = choose_P (B2min, B2, MAX(lmax_noNTT, lmax_NTT), k,
&faststage2_params, B2min, B2, use_ntt, ECM_PP1);
if (P == ECM_ERROR)
{
outputf (OUTPUT_ERROR,
"Error: cannot choose suitable P value for your stage 2 "
"parameters.\nTry a shorter B2min,B2 interval.\n");
mpz_clear (faststage2_params.m_1);
return ECM_ERROR;
}
/* See if the selected parameters let us use NTT or not */
if (faststage2_params.l > lmax_NTT)
use_ntt = 0;
if (maxmem != 0.)
{
unsigned long MB;
char *s;
if (!use_ntt)
s = "out";
else if (twopass)
s = " two pass";
else
s = " one pass";
MB = pp1fs2_memory_use (faststage2_params.l, n, use_ntt, twopass)
/ 1048576;
outputf (OUTPUT_VERBOSE, "Using lmax = %lu with%s NTT which takes "
"about %luMB of memory\n", faststage2_params.l, s, MB);
}
}
else
{
mpz_init (root_params.i0);
root_params.d2 = 0; /* Enable automatic choice of d2 */
if (bestD (&root_params, &k, &dF, B2min, B2, po2, use_ntt, maxmem,
(TreeFilename != NULL), modulus) == ECM_ERROR)
{
youpi = ECM_ERROR;
goto clear_and_exit;
}
/* Set default degree for Brent-Suyama extension */
root_params.S = S;
if (root_params.S == ECM_DEFAULT_S)
{
if (modulus->repr == ECM_MOD_BASE2 && modulus->Fermat > 0)
{
/* For Fermat numbers, default is 1 (no Brent-Suyama) */
root_params.S = 1;
}
else
{
mpz_t t;
mpz_init (t);
mpz_sub (t, B2, B2min);
root_params.S = choose_S (t);
mpz_clear (t);
}
}
}
/* Print B1, B2, polynomial and x0 */
print_B1_B2_poly (OUTPUT_NORMAL, ECM_PP1, B1, *B1done, B2min_parm, B2min,
B2, (stage2_variant == 0) ? root_params.S : 1, p, 0, NULL);
/* If we do a stage 2, print its parameters */
if (mpz_cmp (B2, B2min) >= 0)
{
if (stage2_variant != 0)
outputf (OUTPUT_VERBOSE, "P = %lu, l = %lu, s_1 = %lu, k = s_2 = %lu, "
"m_1 = %Zd\n", faststage2_params.P, faststage2_params.l,
faststage2_params.s_1,faststage2_params.s_2,
faststage2_params.m_1);
else
outputf (OUTPUT_VERBOSE, "dF=%lu, k=%lu, d=%lu, d2=%lu, i0=%Zd\n",
dF, k, root_params.d1, root_params.d2,
S == 1 ? faststage2_params.m_1 : root_params.i0);
}
mpres_init (a, modulus);
mpres_set_z (a, p, modulus);
/* since pp1_mul_prac takes an ecm_uint, we have to check
that B1 <= ECM_UINT_MAX */
if (B1 > (double) ECM_UINT_MAX)
{
outputf (OUTPUT_ERROR, "Error, maximal step1 bound for P+1 is %lu\n",
ECM_UINT_MAX);
youpi = ECM_ERROR;
goto clear_and_exit;
}
if (B1 > *B1done)
youpi = pp1_stage1 (f, a, modulus, B1, B1done, go, stop_asap,
chkfilename);
outputf (OUTPUT_NORMAL, "Step 1 took %ldms\n", elltime (st, cputime ()));
if (test_verbose (OUTPUT_RESVERBOSE))
{
mpz_t t;
mpz_init (t);
mpres_get_z (t, a, modulus);
outputf (OUTPUT_RESVERBOSE, "x=%Zd\n", t);
mpz_clear (t);
}
mpres_get_z (p, a, modulus);
if (stop_asap != NULL && (*stop_asap) ())
goto clear_and_exit;
if (youpi == ECM_NO_FACTOR_FOUND && mpz_cmp (B2, B2min) >= 0)
{
if (stage2_variant != 0)
{
if (use_ntt)
youpi = pp1fs2_ntt (f, a, modulus, &faststage2_params, twopass);
else
youpi = pp1fs2 (f, a, modulus, &faststage2_params);
}
else
youpi = stage2 (f, &a, modulus, dF, k, &root_params, ECM_PP1,
use_ntt, TreeFilename, stop_asap);
}
if (youpi > 0 && test_verbose (OUTPUT_NORMAL))
pp1_check_factor (p, f); /* tell user if factor was found by P-1 */
clear_and_exit:
mpres_clear (a, modulus);
mpmod_clear (modulus);
if (stage2_variant != 0)
mpz_clear (faststage2_params.m_1);
else
mpz_clear (root_params.i0);
mpz_clear (B2);
mpz_clear (B2min);
return youpi;
}
int main (int argc, char *argv[])
{
double *A,*A2,*L,*U, temp2;
int i,j,k;
int temp=0;
int offset = 0;
double t1,t2;
if( argc > 1 )
N = atoi(argv[1]);
if( argc > 2 )
//Block = atoi(argv[2]);
M = atoi(argv[2]);
A = (double *)malloc (N*N*sizeof(double));
A2 = (double *)malloc (N*N*sizeof(double));
L = (double *)malloc (N*N*sizeof(double));
U = (double *)malloc (N*N*sizeof(double));
if( A==NULL || A2==NULL || L==NULL || U==NULL) {
printf("Can't allocate memory\n");
exit(1);
}
/* INITIALIZATION */
//InitMatrix(A,N);
InitMatrix3(A,N);
for(i=0; i<N*N; i++) {
A2[i] = A[i]; // Copy of A for verification of correctness
L[i] = 0;
U[i] = 0;
}
/* /\* LU DECOMPOSITION *\/ */
/* for (k=0;k<N-1;k++){ */
/* for (i=k+1;i<N;i++){ */
/* A[i*N+k] = A[i*N+k]/A[k*N+k]; */
/* /\* for (i=k+1;i<N;i++) *\/ */
/* for (j=k+1;j<N;j++) */
/* A[i*N+j] = A[i*N+j] - A[i*N+k]*A[k*N+j]; */
/* } */
/* } */
int *sizedim;
int *start;
int R; //Remain
int itr = 0;
sizedim = (int*)malloc(M*sizeof(int));
start = (int*)malloc(M*sizeof(int));
R = N;
t1 = GetTickCount();
#pragma omp parallel
{
//printf("The number of thread: %d\n", omp_get_num_threads());
#pragma omp master
{
while (N-offset>M){
// printf(" Iteration: %d\n", itr++);
for (i=0;i<M;i++){
if (i<R%M){
sizedim[i]=R/M+1;
start[i]=(R/M+1)*i;
}
else{
sizedim[i]=R/M;
start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M);
}
//printf("%i,%i \n",sizedim[i],start[i]);
}
//Print_Matrix(sizedim,1,M);
stage1(A, offset, sizedim, start, N, M);
//Print_Matrix(A,N,N);
stage2(A, offset, sizedim, start, N, M);
//Print_Matrix(A,N,N);
stage3(A, offset, sizedim, start, N, M);
offset+=sizedim[0];
R=R-sizedim[0];
//Print_Matrix(A,N,N);
} //while
} //master
} //omp parallel
ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N);
t2 = GetTickCount();
printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000);
//Print_Matrix(A,N,N);
/* while (N-offset>Block){ */
/* stepLU(A,Block,offset,N); */
/* offset+=Block; */
/* } */
/* ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N); */
//Print_Matrix(A,N,N);
#ifdef CHECK
/* PROOF OF CORRECTNESS */
for (i=0;i<N;i++)
for (j=0;j<N;j++)
if (i>j)
L[i*N+j] = A[i*N+j];
else
U[i*N+j] = A[i*N+j];
for (i=0;i<N;i++)
L[i*N+i] = 1;
//printf("L=\n");
//Print_Matrix(L,N,N);
//printf("U=\n");
//Print_Matrix(U,N,N);
for (i=0;i<N;i++)
for (j=0;j<N;j++){
temp2=0;
for (k=0;k<N;k++)
temp2+=L[i*N+k]*U[k*N+j];
if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1)
{
temp++;
printf("Error at: [%d, %d\n]",i,j);
}
}
printf("Errors = %d \n", temp);
#endif
return 0;
}
int main (int argc, char *argv[])
{
double *A,*A2,*L,*U, temp2;
int i,j,k;
int temp=0;
int offset = 0;
double t1,t2;
if (argc < 3)
{
printf("Usage: ./lu <Matrix size> <number of blocks per dimension>\n");
exit(1);
}
if( argc > 1 )
N = atoi(argv[1]);
if( argc > 2 )
M = atoi(argv[2]);
A = (double *)malloc (N*N*sizeof(double));
A2 = (double *)malloc (N*N*sizeof(double));
L = (double *)malloc (N*N*sizeof(double));
U = (double *)malloc (N*N*sizeof(double));
if( A==NULL || A2==NULL || L==NULL || U==NULL) {
printf("Can't allocate memory\n");
exit(1);
}
/* INITIALIZATION */
InitMatrix3(A,N);
for(i=0; i<N*N; i++) {
A2[i] = A[i]; // Copy of A for verification of correctness
L[i] = 0;
U[i] = 0;
}
int *sizedim;
int *start;
int R; //Remain
sizedim = (int*)malloc(M*sizeof(int));
start = (int*)malloc(M*sizeof(int));
R = N;
t1 = GetTickCount();
#pragma omp parallel
{
#pragma omp master
{
while (N-offset>M){
for (i=0;i<M;i++){
if (i<R%M){
sizedim[i]=R/M+1;
start[i]=(R/M+1)*i;
}
else{
sizedim[i]=R/M;
start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M);
}
}
stage1(A, offset, sizedim, start, N, M);
stage2(A, offset, sizedim, start, N, M);
stage3(A, offset, sizedim, start, N, M);
offset+=sizedim[0];
R=R-sizedim[0];
} //end of while
} //end of master
} //end of parallel region
ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N);
t2 = GetTickCount();
printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000);
#ifdef CHECK
/* PROOF OF CORRECTNESS */
for (i=0;i<N;i++)
for (j=0;j<N;j++)
if (i>j)
L[i*N+j] = A[i*N+j];
else
U[i*N+j] = A[i*N+j];
for (i=0;i<N;i++)
L[i*N+i] = 1;
for (i=0;i<N;i++)
for (j=0;j<N;j++){
temp2=0;
for (k=0;k<N;k++)
temp2+=L[i*N+k]*U[k*N+j];
if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1)
temp++;
}
printf("Errors = %d \n", temp);
#endif
return;
}
bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
{
double timeout = goal.allowed_planning_time;
ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
std::string attached_object_name = goal.attached_object_name;
const robot_model::JointModelGroup *jmg = NULL;
const robot_model::JointModelGroup *eef = NULL;
// if the group specified is actually an end-effector, we use it as such
if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
{
eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
if (eef)
{ // if we correctly found the eef, then we try to find out what the planning group is
const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
if (eef_parent.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
}
}
else
{
// if a group name was specified, try to use it
jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
if (jmg)
{
// we also try to find the corresponding eef
const std::vector<std::string> &eef_names = jmg->getAttachedEndEffectorNames();
if (eef_names.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name << "'");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
// check to see if there is an end effector that has attached objects associaded, so we can complete the place
for (std::size_t i = 0 ; i < eef_names.size() ; ++i)
{
std::vector<const robot_state::AttachedBody*> attached_bodies;
const robot_model::JointModelGroup *eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
if (eg)
{
// see if there are objects attached to links in the eef
planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
// is is often possible that the objects are attached to the same link that the eef itself is attached,
// so we check for attached bodies there as well
const robot_model::LinkModel *attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
if (attached_link_model)
{
std::vector<const robot_state::AttachedBody*> attached_bodies2;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
}
}
// if this end effector has attached objects, we go on
if (!attached_bodies.empty())
{
// if the user specified the name of the attached object to place, we check that indeed
// the group contains this attachd body
if (!attached_object_name.empty())
{
bool found = false;
for (std::size_t j = 0 ; j < attached_bodies.size() ; ++j)
if (attached_bodies[j]->getName() == attached_object_name)
{
found = true;
break;
}
// if the attached body this group has is not the same as the one specified,
// we cannot use this eef
if (!found)
continue;
}
// if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
if (eef)
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '" << goal.group_name <<
"' that are currently holding objects. It is ambiguous which end-effector to use. Please specify it explicitly.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
// set the end effector (this was initialized to NULL above)
eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
}
}
}
}
// if we know the attached object, but not the eef, we can try to identify that
if (!attached_object_name.empty() && !eef)
{
const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
if (attached_body)
{
// get the robot model link this attached body is associated to
const robot_model::LinkModel *link = attached_body->getAttachedLink();
// check to see if there is a unique end effector containing the link
const std::vector<const robot_model::JointModelGroup*> &eefs = planning_scene->getRobotModel()->getEndEffectors();
for (std::size_t i = 0 ; i < eefs.size() ; ++i)
if (eefs[i]->hasLinkModel(link->getName()))
{
if (eef)
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '" << link->getName() <<
"' which is where the body '" << attached_object_name << "' is attached. It is unclear which end-effector to use.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
eef = eefs[i];
}
}
// if the group is also unknown, but we just found out the eef
if (!jmg && eef)
{
const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
if (eef_parent.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
}
}
if (!jmg || !eef)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
// try to infer attached body name if possible
int loop_count = 0;
while (attached_object_name.empty() && loop_count < 2)
{
// in the first try, look for objects attached to the eef, if the eef is known;
// otherwise, look for attached bodies in the planning group itself
std::vector<const robot_state::AttachedBody*> attached_bodies;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
loop_count++;
if (attached_bodies.size() > 1)
{
ROS_ERROR_NAMED("manipulation", "Multiple attached bodies for group '%s' but no explicit attached object to place was specified", goal.group_name.c_str());
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
return false;
}
else
attached_object_name = attached_bodies[0]->getName();
}
const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
if (!attached_body)
{
ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
return false;
}
ros::WallTime start_time = ros::WallTime::now();
// construct common data for possible manipulation plans
ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
plan_data->planning_group_ = jmg;
plan_data->end_effector_group_ = eef;
plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
plan_data->timeout_ = endtime;
plan_data->path_constraints_ = goal.path_constraints;
plan_data->planner_id_ = goal.planner_id;
plan_data->minimize_object_distance_ = false;
plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
moveit_msgs::AttachedCollisionObject &detach_object_msg = plan_data->diff_attached_object_;
// construct the attached object message that will change the world to what it would become after a placement
detach_object_msg.link_name = attached_body->getAttachedLinkName();
detach_object_msg.object.id = attached_object_name;
detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
collision_detection::AllowedCollisionMatrixPtr approach_place_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
// we are allowed to touch certain other objects with the gripper
approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
// we are allowed to touch the target object slightly while retreating the end effector
std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
approach_place_acm->setEntry(attached_object_name, touch_links, true);
if (!goal.support_surface_name.empty())
{
// we are allowed to have contact between the target object and the support surface before the place
approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
// optionally, it may be allowed to touch the support surface with the gripper
if (goal.allow_gripper_support_collision)
approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
}
// configure the manipulation pipeline
pipeline_.reset();
ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
initialize();
pipeline_.start();
// add possible place locations
for (std::size_t i = 0 ; i < goal.place_locations.size() ; ++i)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
const moveit_msgs::PlaceLocation &pl = goal.place_locations[i];
if (goal.place_eef)
p->goal_pose_ = pl.place_pose;
else
// The goals are specified for the attached body
// but we want to transform them into goals for the end-effector instead
if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
{
p->goal_pose_ = pl.place_pose;
ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the end-effector");
}
p->approach_ = pl.pre_place_approach;
p->retreat_ = pl.post_place_retreat;
p->retreat_posture_ = pl.post_place_posture;
p->id_ = i;
if (p->retreat_posture_.joint_names.empty())
p->retreat_posture_ = attached_body->getDetachPosture();
pipeline_.push(p);
}
ROS_INFO_NAMED("manipulation", "Added %d place locations", (int) goal.place_locations.size());
// wait till we're done
waitForPipeline(endtime);
pipeline_.stop();
last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
if (!getSuccessfulManipulationPlans().empty())
error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
else
{
if (last_plan_time_ > timeout)
error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
else
{
error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
if (goal.place_locations.size() > 0)
{
ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode.");
// everything failed. we now start the pipeline again in verbose mode for one grasp
initialize();
pipeline_.setVerbose(true);
pipeline_.start();
pipeline_.reprocessLastFailure();
waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
pipeline_.stop();
pipeline_.setVerbose(false);
}
}
}
ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_);
return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
xmlNode *
do_calculations(pe_working_set_t * data_set, xmlNode * xml_input, crm_time_t * now)
{
GListPtr gIter = NULL;
int rsc_log_level = LOG_INFO;
/* pe_debug_on(); */
CRM_ASSERT(xml_input || is_set(data_set->flags, pe_flag_have_status));
if (is_set(data_set->flags, pe_flag_have_status) == FALSE) {
set_working_set_defaults(data_set);
data_set->input = xml_input;
data_set->now = now;
} else {
crm_trace("Already have status - reusing");
}
if (data_set->now == NULL) {
data_set->now = crm_time_new(NULL);
}
crm_trace("Calculate cluster status");
stage0(data_set);
if(is_not_set(data_set->flags, pe_flag_quick_location)) {
gIter = data_set->resources;
for (; gIter != NULL; gIter = gIter->next) {
resource_t *rsc = (resource_t *) gIter->data;
if (is_set(rsc->flags, pe_rsc_orphan) && rsc->role == RSC_ROLE_STOPPED) {
continue;
}
rsc->fns->print(rsc, NULL, pe_print_log, &rsc_log_level);
}
}
crm_trace("Applying placement constraints");
stage2(data_set);
if(is_set(data_set->flags, pe_flag_quick_location)){
return NULL;
}
crm_trace("Create internal constraints");
stage3(data_set);
crm_trace("Check actions");
stage4(data_set);
crm_trace("Allocate resources");
stage5(data_set);
crm_trace("Processing fencing and shutdown cases");
stage6(data_set);
crm_trace("Applying ordering constraints");
stage7(data_set);
crm_trace("Create transition graph");
stage8(data_set);
crm_trace("=#=#=#=#= Summary =#=#=#=#=");
crm_trace("\t========= Set %d (Un-runnable) =========", -1);
if (get_crm_log_level() >= LOG_TRACE) {
gIter = data_set->actions;
for (; gIter != NULL; gIter = gIter->next) {
action_t *action = (action_t *) gIter->data;
if (is_set(action->flags, pe_action_optional) == FALSE
&& is_set(action->flags, pe_action_runnable) == FALSE
&& is_set(action->flags, pe_action_pseudo) == FALSE) {
log_action(LOG_TRACE, "\t", action, TRUE);
}
}
}
return data_set->graph;
}
int main (int argc, char *argv[])
{
double *A,*A2,*L,*U, temp2;
int i,j,k;
int temp=0;
int offset = 0;
double t1,t2,t3,t4;
int N = 100;
int Block = 1;
int M=1; //number of blocks per dimension
if( argc > 1 )
N = atoi(argv[1]);
if( argc > 2 )
M = atoi(argv[2]);
A = (double *)malloc (N*N*sizeof(double));
A2 = (double *)malloc (N*N*sizeof(double));
L = (double *)malloc (N*N*sizeof(double));
U = (double *)malloc (N*N*sizeof(double));
if( A==NULL || A2==NULL || L==NULL || U==NULL ) {
printf("Can't allocate memory\n");
exit(1);
}
int *sizedim;
int *start;
int R; //Remain
sizedim = (int*)malloc(M*sizeof(int));
start = (int*)malloc(M*sizeof(int));
R = N;
t1 = GetTickCount();
#pragma omp parallel
{
#pragma omp master
{
while (N-offset>M){
for (i=0;i<M;i++){
if (i<R%M){
sizedim[i]=R/M+1;
start[i]=(R/M+1)*i;
} else {
sizedim[i]=R/M;
start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M);
}
}
stage1(A, offset, sizedim, start, N, M);
t3 = GetTickCount();
stage2(A, offset, sizedim, start, N, M);
stage3(A, offset, sizedim, start, N, M);
t4 = GetTickCount();
total += (t4-t3);
offset+=sizedim[0];
R=R-sizedim[0];
}
}
}
ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N);
t2 = GetTickCount();
printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000);
printf("Time for the task region in secs: %f \n", (total)/1000000);
printf("Time for inside tasks in secs: %f \n", (task_total)/1000000);
printf("Time spent in taskwait in secs: %f \n", (wait_total)/1000000);
#ifdef CHECK
for (i=0;i<N;i++)
for (j=0;j<N;j++)
if (i>j)
L[i*N+j] = A[i*N+j];
else
U[i*N+j] = A[i*N+j];
for (i=0;i<N;i++)
L[i*N+i] = 1;
for (i=0;i<N;i++)
for (j=0;j<N;j++){
temp2=0;
for (k=0;k<N;k++)
temp2+=L[i*N+k]*U[k*N+j];
if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1)
temp++;
}
printf("Errors = %d \n", temp);
#endif
return 0;
}
int bootup_main(int argc, char **argv) {
FILE *fp;
int t=0;
char cmd[1024], buf[1024], name[1024];
time_t curtime;
struct tm *loctime;
signal(SIGINT,SIG_IGN);
putenv("PATH=/bin");
putenv("TERM=linux");
umask(0770);
chdir("/");
print_file("/etc/banner");
xsystem("mount -t proc -o ro virtual /proc");
xsystem("mount -t sysfs -o ro virtual /sys");
stage1();
stage2();
// STAGE 3
chdir("/");
read_cmdline();
xsystem("mdev -s");
xsystem("mount -t devpts -o \"rw,gid=0,mode=620\"");
mk_dev("/dev/ppp",108,0);
if(LCD_PROG==1 && file_exists("/tmp/tools/lcd/lcd.bz2")) {
mk_dev("/dev/parport0",99,0);
mk_dev("/dev/lp0",6,0);
xsystem("tar -C / -jxf /tmp/tools/lcd/lcd.bz2");
} else {
LCD_PROG=0;
}
rename("/dev/random","/dev/random-block");
symlink("/dev/urandom","/dev/random");
xmkdir("/strg");
if(LCD_DEV[0]!='\0') {
snprintf(buf,sizeof(buf),"/dev/%s",LCD_DEV);
if(file_exists(buf)) {
save_to_file("/var/sys/lcd_dev",buf);
if(LCD_PROG==1) save_to_file("/var/sys/lcd_proc","%d",LCD_PROG);
symlink(buf,"/dev/lcd");
lcd_msg(LCD_PROG,"SYSTEM LOADING..","-> STORAGE ON");
}
}
memset(buf,0x0,sizeof(buf));
snprintf(buf,sizeof(buf),"mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV);
if(xsystem("mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV)==0) {
if(file_exists("/strg/.mount_strg")) {
unlink("/strg/.mount_strg");
if(xsystem("umount /strg")==0) {
fprintf_stdout("**** MYBOX SYSTEM APPEARS TO HAVE SHUT DOWN UNCLEANLY ****\n");
lcd_msg(LCD_PROG,"SYSTEM LOADING..","-> FIX STORAGE");
xsystem("e2fsck -y %s",STRG_DEV);
t=1;
}
}
if(t==0) xsystem("umount /strg");
} else {
fprintf_stdout("**** MOUNTING STORAGE DISK FAILED! ****\n");
lcd_msg(LCD_PROG,"SYSTEM LOADING..","STORAGE FAILED !");
xtouch("/strg/.nostrg");
xtouch("/var/sys/nolog");
}
if(!file_exists("/strg/.nostrg")) {
if(xsystem("mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV)==0) {
memset(buf,0x0,sizeof(buf));
snprintf(buf,sizeof(buf),"%s:%s\n",STRG_DEV,STRG_FS);
save_to_file("/strg/.mount_strg",buf);
save_to_file("/var/sys/.mount_strg",buf);
save_to_file("/var/sys/.mount_strg","%s:%s\n",BOOT_DEV,BOOT_FS);
}
}
if(xsystem("swapon %s",SWAP_DEV)==0) {
memset(buf,0x0,sizeof(buf));
snprintf(buf,sizeof(buf),"%s:swap\n",SWAP_DEV);
save_to_file("/var/sys/.mount_swap",buf);
}
xsystem("chmod 700 *");
if((fp=fopen("/etc/inittab","w"))!=NULL) {
fprintf(fp,"::sysinit:/bin/initrc\n");
fprintf(fp,"tty1::respawn:/bin/getty -h -n -L tty1 9600 linux\n");
fprintf(fp,"ttyS0::respawn:/bin/getty -h -n -L ttyS0 9600 vt100\n");
fprintf(fp,"null::respawn:/bin/chkprog\n");
fprintf(fp,"::restart:/bin/init\n");
fprintf(fp,"::ctrlaltdel:/bin/bootdown\n");
fprintf(fp,"::ctrlaltdel:/bin/reset\n");
fprintf(fp,"::ctrlaltdel:/bin/reboot\n");
fprintf(fp,"::shutdown:/bin/bootdown\n");
fclose(fp);
}
if((fp=fopen("/etc/profile","a"))!=NULL) {
fprintf(fp,"xexit() {\n");
fprintf(fp," if [ \"$PPID\" = \"1\" ]; then\n");
fprintf(fp," local logname=\"/strg/mybox/logs/auth-$(date \"+%s\").log\"\n","%Y%m%d");
fprintf(fp," local msg=\"[$(date \"+%s\")] TYPE=console USER=console IP=$(basename $(tty)) MSG=Session logout.\"\n","%d/%m/%Y %H:%M:%S");
fprintf(fp," echo \"$msg\" >> $logname\n");
fprintf(fp," [ ! -z \"$ME\" -a -f \"$ME\" ] && rm -f /tmp/console.session/console_*\n");
fprintf(fp," fi\n");
fprintf(fp," exit\n");
fprintf(fp,"}\n");
fprintf(fp,"alias exit='xexit'\n");
fprintf(fp,"export HISTFILE=/.consolehistory\n");
fprintf(fp,"lcdd_msg() {\n");
fprintf(fp," if [ -f \"/bin/lcdd\" -a -c \"/dev/lcd\" ]; then\n");
fprintf(fp," if [ -f \"/var/sys/lcd_proc\" ]; then\n");
fprintf(fp," echo \"$2\" > /var/sys/lcd_msg\n");
fprintf(fp," else\n");
fprintf(fp," /bin/lcdd \"$1\" \"$2\"\n");
fprintf(fp," fi\n");
fprintf(fp," fi\n");
fprintf(fp,"}\n");
fprintf(fp,"if [ -z $DO_SINGLE ]; then\n");
fprintf(fp," if [ -f \"/bin/iosh\" ]; then\n");
fprintf(fp," XTTY=\"SSL\";\n");
fprintf(fp," if [ -f \"/var/sys/init_start\" ]; then\n");
fprintf(fp," trap : 1 2 3 15\n");
fprintf(fp," echo \"System loading in progress..please wait or login back in a minute\"\n");
fprintf(fp," while [ -f \"/var/sys/init_start\" ]; do sleep 1;done\n");
fprintf(fp," trap 1 2 3 15\n");
fprintf(fp," fi\n");
fprintf(fp," if [ \"$PPID\" = \"1\" ]; then\n");
fprintf(fp," export ME=\"/tmp/console.session/console_${PPID}_$(basename $(tty))_$(date \"+%s\")\";\n","%d:%m:%Y_%H:%M:%S");
fprintf(fp," touch $ME\n");
fprintf(fp," XTTY=\"console\";\n");
fprintf(fp," fi\n");
fprintf(fp," /bin/iosh $XTTY\n");
fprintf(fp," if [ $? != 5 ]; then\n");
fprintf(fp," clear;reset\n");
fprintf(fp," exit\n");
fprintf(fp," fi\n");
fprintf(fp," else\n");
fprintf(fp," echo \"** FAILED TO RUN IO SHELL **\"\n");
fprintf(fp," read io\n");
fprintf(fp," exit\n");
fprintf(fp," fi\n");
fprintf(fp,"else \n");
fprintf(fp," echo \"** MAINTENANCE MODE **\"\n");
fprintf(fp," lcdd_msg \"SYSTEM LOADING.." "-> MAINTENANCE\"\n");
fprintf(fp," read io\n");
fprintf(fp,"fi\n");
fclose(fp);
}
unlink("/strg/mybox/debug.log");
curtime=time(NULL);
loctime=localtime(&curtime);
memset(name,0x0,sizeof(name));
strftime(name, sizeof(name),"system-%Y%m%d.log",loctime);
memset(buf,0x0,sizeof(buf));
snprintf(buf,sizeof(buf),"%s/%s",LOGPATH,name);
memset(cmd,0x0,sizeof(cmd));
strftime(cmd, sizeof(cmd), "[%d/%m/%Y %T] TYPE=INFO MSG=****** SYSTEM LOADING ******\n",loctime);
append_to_file(buf,cmd);
if(file_exists("/bin/getkey")) {
if(system("getkey -c 3 -m \"-> Starting Init: %d\" R")==0) {
fprintf_stdout("\r*** BYPASS CONSOLE LOGIN ***\n");
lcd_msg(LCD_PROG,"SYSTEM LOADING..","BYPASS CONSOLE !");
xtouch("/etc/noconsole");
} else {
fprintf_stdout("\r* Starting Init. Done.\n");
}
}
memset(buf,0x0,sizeof(buf));
snprintf(buf,sizeof(buf),"%s\n","/bin/mdev");
save_to_file("/proc/sys/kernel/hotplug",buf);
if(NUM_NET!=0) save_to_file("/var/sys/numnet_veto","%d",NUM_NET);
do_chroot();
signal(SIGINT,SIG_DFL);
lcd_msg(LCD_PROG,"SYSTEM LOADING..","ERROR !");
fprintf_stdout("You are not supposed to be here, something went wrong!\n");
fprintf_stdout("Press Ctrl+Alt+Del or switch off/on for reboot.\n");
while(1);
exit(0);
}
int main()
{
// initialize IRQ (interrupts)
// this must come before everything else
IRQ_INIT();
// Initialize global pointers
GameStateManager gameStateMan;
OamManager oamMan;
AudioManager audioMan;
PlayState playState(&gameStateMan);
TitleScreenState titleState(&gameStateMan);
PauseState pauseState(&gameStateMan);
GameOverState gameOverState(&gameStateMan);
StoreState storeState(&gameStateMan);
StageEndState stageEndState(&gameStateMan);
g_gameStateMan = &gameStateMan;
g_oamMan = &oamMan;
g_playState = &playState;
g_titleState = &titleState;
g_pauseState = &pauseState;
g_gameOverState = &gameOverState;
g_storeState = &storeState;
g_stageEndState = &stageEndState;
g_audioMan = &audioMan;
// create stage events
StageEvent endEvent;
StageEvent event1;
StageEvent event2;
StageEvent event3;
StageEvent event4;
StageEvent event5;
StageEvent event6;
StageEvent firePowerPowerUpEvent;
StageEvent invinciblePowerUpEvent;
StageEvent bombPowerUpEvent;
g_endEvent = &endEvent;
g_event1 = &event1;
g_event2 = &event2;
g_event3 = &event3;
g_event4 = &event4;
g_event5 = &event5;
g_event6 = &event6;
g_firePowerPowerUpEvent = &firePowerPowerUpEvent;
g_invinciblePowerUpEvent = &invinciblePowerUpEvent;
g_bombPowerUpEvent = &bombPowerUpEvent;
initializeEvents();
StageEvent * stage1Events[24];
int stage1Timing[24];
int stage1yOffset[24];
fillEventsStage1(stage1Events, stage1Timing, stage1yOffset);
Stage stage1(&playState, stage1Events, stage1Timing, stage1yOffset, 24);
g_stage1 = &stage1;
StageEvent * stage2Events[20];
int stage2Timing[20];
int stage2yOffset[20];
fillEventsStage2(stage2Events, stage2Timing, stage2yOffset);
Stage stage2(&playState, stage2Events, stage2Timing, stage2yOffset, 20);
g_stage2 = &stage2;
StageEvent * stage3Events[20];
int stage3Timing[20];
int stage3yOffset[20];
fillEventsStage3(stage3Events, stage3Timing, stage3yOffset);
Stage stage3(&playState, stage3Events, stage3Timing, stage3yOffset, 20);
g_stage3 = &stage3;
videoInit();
g_gameStateMan->pushState(g_titleState);
#ifdef DEBUG
// timers used for debug display
REG_TM1D = 0x10000 - 2808; // overflow into timer 2 every 2808 cycles, approx. 1% of a screen refresh
REG_TM2D = 0;
REG_TM2CNT = TM_CASCADE | TM_ENABLE;
REG_TM1CNT = TM_FREQ_1 | TM_ENABLE;
int oldPercent, diffPercent, oldFrac, diffFrac;
char buf[15];
#endif // DEBUG
while(true)
{
// wait until next VBlank
// prefer this over vid_vsync() - it's
// better for power consumption
VBlankIntrWait();
#ifdef DEBUG
// grab current percentage
oldPercent = REG_TM2D;
oldFrac = REG_TM1D;
#endif // DEBUG
// update shadow OAM to real OAM
g_oamMan->update();
// mix the next frame's audio
g_audioMan->sndMix();
// poll keys - do not do this in other places
key_poll();
// update the game state
g_gameStateMan->update();
#ifdef DEBUG
// grab current percentage, and write it out
diffPercent = REG_TM2D - oldPercent;
diffFrac = REG_TM1D - oldFrac;
// round the percent based on the fractional amount
if (diffFrac > 1404)
{
diffPercent++;
}
else if (diffFrac < -1404)
{
diffPercent--;
}
gba_itoa(diffPercent, buf);
// clear out characters from the last write
write(" ", Vec2(0, 0));
write(buf, Vec2(0, 0));
// reset timer 2 to 0
REG_TM2CNT = 0; // disable timer
REG_TM2D = 0; // set new value to 0
REG_TM2CNT = TM_CASCADE | TM_ENABLE; // reenable timer
#endif // DEBUG
}
}
void stage1(void* ba, void* bb)
{
shared_buf(ba, bb, sz);
stage2(ba, bb);
}
/* Input: p is the initial generator (sigma), if 0, generate it at random.
N is the number to factor
B1 is the stage 1 bound
B2 is the stage 2 bound
B1done is the stage 1 limit to which supplied residue has
already been computed
k is the number of blocks for stage 2
verbose is the verbosity level
Output: f is the factor found, p is the residue at end of stage 1
Return value: non-zero iff a factor is found (1 for stage 1, 2 for stage 2)
*/
int
pm1 (mpz_t f, mpz_t p, mpz_t N, mpz_t go, double *B1done, double B1,
mpz_t B2min_parm, mpz_t B2_parm, double B2scale, unsigned long k,
const int S, int verbose, int repr, int use_ntt, FILE *os, FILE *es,
char *chkfilename, char *TreeFilename, double maxmem,
gmp_randstate_t rng, int (*stop_asap)(void))
{
int youpi = ECM_NO_FACTOR_FOUND;
int base2 = 0;
int Nbits, smallbase;
int po2 = 0; /* Whether we should use power-of-2 poly degree */
long st;
mpmod_t modulus;
mpres_t x;
mpz_t B2min, B2; /* Local B2, B2min to avoid changing caller's values */
unsigned long dF;
root_params_t root_params;
faststage2_param_t faststage2_params;
/* If stage2_variant != 0, we use the new fast stage 2 */
const int stage2_variant = (S == 1 || S == ECM_DEFAULT_S);
set_verbose (verbose);
ECM_STDOUT = (os == NULL) ? stdout : os;
ECM_STDERR = (es == NULL) ? stdout : es;
/* if n is even, return 2 */
if (mpz_divisible_2exp_p (N, 1))
{
mpz_set_ui (f, 2);
return ECM_FACTOR_FOUND_STEP1;
}
st = cputime ();
if (mpz_cmp_ui (p, 0) == 0)
pm1_random_seed (p, N, rng);
mpz_init_set (B2min, B2min_parm);
mpz_init_set (B2, B2_parm);
/* Set default B2. See ecm.c for comments */
if (ECM_IS_DEFAULT_B2(B2))
{
if (stage2_variant == 0)
mpz_set_d (B2, B2scale * pow (B1 * PM1_COST, DEFAULT_B2_EXPONENT));
else
mpz_set_d (B2, B2scale * pow (B1 * PM1FS2_COST,
PM1FS2_DEFAULT_B2_EXPONENT));
}
/* set B2min */
if (mpz_sgn (B2min) < 0)
mpz_set_d (B2min, B1);
if (repr != ECM_MOD_DEFAULT && repr != ECM_MOD_NOBASE2)
{
if (repr == ECM_MOD_MODMULN)
mpmod_init_MODMULN (modulus, N);
else if (repr == ECM_MOD_REDC)
mpmod_init_REDC (modulus, N);
else if (abs (repr) > 16)
{
if (mpmod_init_BASE2 (modulus, repr, N) == ECM_ERROR)
return ECM_ERROR;
}
else
mpmod_init_MPZ (modulus, N);
}
else /* automatic choice */
{
/* Find a good arithmetic for this number */
Nbits = mpz_sizeinbase (N, 2);
base2 = (repr == 0) ? isbase2 (N, BASE2_THRESHOLD) : 0;
smallbase = mpz_fits_uint_p (p);
/* TODO: make dependent on Nbits and base2 */
if (base2)
{
mpmod_init_BASE2 (modulus, base2, N);
}
else if (mpz_size (N) <= 2 * POWM_THRESHOLD && smallbase && B1 <= 1e6)
/* Below POWM_THRESHOLD, mpz_powm uses MODMULN reduction, too, but
without special code for small bases which makes our MODMULN
faster. Above POWM_THRESHOLD mpz_powm uses faster mod reduction,
at about 2*POWM_THRESHOLD it catches up with our smallbase-MODMULN
and then is faster until REDC takes over. */
{
outputf (OUTPUT_VERBOSE, "Using MODMULN\n");
mpmod_init_MODMULN (modulus, N);
}
else if (Nbits > 50000 || (Nbits > 3500 && smallbase))
{
outputf (OUTPUT_VERBOSE, "Using REDC\n");
mpmod_init_REDC (modulus, N);
}
else
{
outputf (OUTPUT_VERBOSE, "Using mpz_powm\n");
mpmod_init_MPZ (modulus, N);
}
}
/* Determine parameters (polynomial degree etc.) */
if (stage2_variant != 0)
{
long P_ntt, P_nontt;
const unsigned long lmax = 1UL<<28; /* An upper bound */
unsigned long lmax_NTT, lmax_noNTT;
faststage2_param_t params_ntt, params_nontt, *better_params;
mpz_init (faststage2_params.m_1);
faststage2_params.l = 0;
mpz_init (params_ntt.m_1);
params_ntt.l = 0;
mpz_init (params_nontt.m_1);
params_nontt.l = 0;
/* Find out what the longest transform length is we can do at all.
If no maxmem is given, the non-NTT can theoretically do any length. */
lmax_NTT = 0;
if (use_ntt)
{
unsigned long t;
/* See what transform length the NTT can handle (due to limited
primes and limited memory) */
t = mpzspm_max_len (N);
lmax_NTT = MIN (lmax, t);
if (maxmem != 0.)
{
t = pm1fs2_maxlen (double_to_size (maxmem), N, use_ntt);
lmax_NTT = MIN (lmax_NTT, t);
}
outputf (OUTPUT_DEVVERBOSE, "NTT can handle lmax <= %lu\n", lmax_NTT);
/* FIXME: if both ntt and no-ntt are tried, but finally ntt is
preferred, the last B2 bound computed is that of no-ntt,
which is thus wrong */
P_ntt = choose_P (B2min, B2, lmax_NTT, k, ¶ms_ntt,
B2min, B2, 1, ECM_PM1);
if (P_ntt != ECM_ERROR)
outputf (OUTPUT_DEVVERBOSE,
"Parameters for NTT: P=%lu, l=%lu\n",
params_ntt.P, params_ntt.l);
}
else
P_ntt = 0; /* or GCC complains about uninitialized var */
/* See what transform length the non-NTT code can handle */
lmax_noNTT = lmax;
if (maxmem != 0.)
{
unsigned long t;
t = pm1fs2_maxlen (double_to_size (maxmem), N, 0);
lmax_noNTT = MIN (lmax_noNTT, t);
outputf (OUTPUT_DEVVERBOSE, "non-NTT can handle lmax <= %lu\n",
lmax_noNTT);
}
if (use_ntt != 2)
P_nontt = choose_P (B2min, B2, lmax_noNTT, k, ¶ms_nontt,
B2min, B2, 0, ECM_PM1);
else
P_nontt = ECM_ERROR;
if (P_nontt != ECM_ERROR)
outputf (OUTPUT_DEVVERBOSE,
"Parameters for non-NTT: P=%lu, l=%lu\n",
params_nontt.P, params_nontt.l);
if (((!use_ntt || P_ntt == ECM_ERROR) && P_nontt == ECM_ERROR) ||
(use_ntt == 2 && P_ntt == ECM_ERROR))
{
outputf (OUTPUT_ERROR,
"Error: cannot choose suitable P value for your stage 2 "
"parameters.\nTry a shorter B2min,B2 interval.\n");
mpz_clear (faststage2_params.m_1);
mpz_clear (params_ntt.m_1);
mpz_clear (params_nontt.m_1);
return ECM_ERROR;
}
/* Now decide wether to take NTT or non-NTT.
How to choose the better one is not an easy question.
It will depend on the speed ratio between NTT/non-NTT code,
their difference in memory use and available memory.
For now, we choose the one that uses a longer transform length.
FIXME: Write something not brain-dead here */
if (use_ntt == 0 || P_ntt == ECM_ERROR ||
(use_ntt == 1 && params_nontt.l > params_ntt.l))
{
better_params = ¶ms_nontt;
use_ntt = 0;
}
else
{
better_params = ¶ms_ntt;
use_ntt = 1;
}
faststage2_params.P = better_params->P;
faststage2_params.s_1 = better_params->s_1;
faststage2_params.s_2 = better_params->s_2;
faststage2_params.l = better_params->l;
mpz_set (faststage2_params.m_1, better_params->m_1);
mpz_clear (params_ntt.m_1);
mpz_clear (params_nontt.m_1);
if (maxmem != 0.)
outputf (OUTPUT_VERBOSE, "Using lmax = %lu with%s NTT which takes "
"about %luMB of memory\n", faststage2_params.l,
(use_ntt) ? "" : "out",
pm1fs2_memory_use (faststage2_params.l, N, use_ntt)/1048576);
}
else
{
mpz_init (root_params.i0);
root_params.d2 = 0; /* Enable automatic choice of d2 */
if (use_ntt || (modulus->repr == ECM_MOD_BASE2 && modulus->Fermat > 0))
po2 = 1;
if (bestD (&root_params, &k, &dF, B2min, B2, po2, use_ntt, maxmem,
(TreeFilename != NULL), modulus) == ECM_ERROR)
{
youpi = ECM_ERROR;
goto clear_and_exit;
}
root_params.S = S;
/* Set default degree for Brent-Suyama extension */
if (root_params.S == ECM_DEFAULT_S)
{
if (modulus->repr == ECM_MOD_BASE2 && modulus->Fermat > 0)
{
/* For Fermat numbers, default is 2 (no Brent-Suyama) */
root_params.S = 2;
}
else
{
mpz_t t;
mpz_init (t);
mpz_sub (t, B2, B2min);
if (mpz_cmp_d (t, 3.5e5) < 0) /* B1 < 50000 */
root_params.S = -4; /* Dickson polys give a slightly better chance of success */
else if (mpz_cmp_d (t, 1.1e7) < 0) /* B1 < 500000 */
root_params.S = -6;
else if (mpz_cmp_d (t, 1.25e8) < 0) /* B1 < 3000000 */
root_params.S = 12; /* but for S>6, S-th powers are faster thanks to invtrick */
else if (mpz_cmp_d (t, 7.e9) < 0) /* B1 < 50000000 */
root_params.S = 24;
else if (mpz_cmp_d (t, 1.9e10) < 0) /* B1 < 100000000 */
root_params.S = 48;
else if (mpz_cmp_d (t, 5.e11) < 0) /* B1 < 1000000000 */
root_params.S = 60;
else
root_params.S = 120;
mpz_clear (t);
}
}
/* We need Suyama's power even and at least 2 for P-1 stage 2 to work
correctly */
if (root_params.S & 1)
root_params.S *= 2; /* FIXME: Is this what the user would expect? */
}
/* Print B1, B2, polynomial and x0 */
print_B1_B2_poly (OUTPUT_NORMAL, ECM_PM1, B1, *B1done, B2min_parm, B2min,
B2, (stage2_variant == 0) ? root_params.S : 1, p, 0, NULL);
/* If we do a stage 2, print its parameters */
if (mpz_cmp (B2, B2min) >= 0)
{
if (stage2_variant != 0)
outputf (OUTPUT_VERBOSE, "P = %lu, l = %lu, s_1 = %lu, k = s_2 = %lu, "
"m_1 = %Zd\n", faststage2_params.P, faststage2_params.l,
faststage2_params.s_1,faststage2_params.s_2,
faststage2_params.m_1);
else
outputf (OUTPUT_VERBOSE, "dF=%lu, k=%lu, d=%lu, d2=%lu, i0=%Zd\n",
dF, k, root_params.d1, root_params.d2, root_params.i0);
}
if (test_verbose (OUTPUT_VERBOSE))
{
if (mpz_sgn (B2min_parm) >= 0)
{
outputf (OUTPUT_VERBOSE,
"Can't compute success probabilities for B1 <> B2min\n");
}
else
{
rhoinit (256, 10);
print_prob (B1, B2, dF, k,
(stage2_variant == 0) ? root_params.S : 1, go);
}
}
mpres_init (x, modulus);
mpres_set_z (x, p, modulus);
st = cputime ();
if (B1 > *B1done)
youpi = pm1_stage1 (f, x, modulus, B1, B1done, go, stop_asap, chkfilename);
st = elltime (st, cputime ());
outputf (OUTPUT_NORMAL, "Step 1 took %ldms\n", st);
if (test_verbose (OUTPUT_RESVERBOSE))
{
mpz_t tx;
mpz_init (tx);
mpres_get_z (tx, x, modulus);
outputf (OUTPUT_RESVERBOSE, "x=%Zd\n", tx);
mpz_clear (tx);
}
if (stop_asap != NULL && (*stop_asap) ())
goto clear_and_exit;
if (youpi == ECM_NO_FACTOR_FOUND && mpz_cmp (B2, B2min) >= 0)
{
if (stage2_variant != 0)
{
if (use_ntt)
youpi = pm1fs2_ntt (f, x, modulus, &faststage2_params);
else
youpi = pm1fs2 (f, x, modulus, &faststage2_params);
}
else
youpi = stage2 (f, &x, modulus, dF, k, &root_params, ECM_PM1,
use_ntt, TreeFilename, stop_asap);
}
if (test_verbose (OUTPUT_VERBOSE))
{
if (mpz_sgn (B2min_parm) < 0)
rhoinit (1, 0); /* Free memory of rhotable */
}
clear_and_exit:
mpres_get_z (p, x, modulus);
mpres_clear (x, modulus);
mpmod_clear (modulus);
if (stage2_variant != 0)
mpz_clear (faststage2_params.m_1);
else
mpz_clear (root_params.i0);
mpz_clear (B2);
mpz_clear (B2min);
return youpi;
}
int stage5() {
A = (A*A)%n;
return stage2();
}
bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
{
double timeout = goal.allowed_planning_time;
ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
std::string planning_group = goal.group_name;
std::string end_effector = goal.end_effector;
if (end_effector.empty() && !planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames();
if (!eefs.empty())
{
end_effector = eefs.front();
if (eefs.size() > 1)
ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" << planning_group << "' is ambiguous. Assuming '" << end_effector << "'");
}
}
else
if (!end_effector.empty() && planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
planning_group = jmg->getEndEffectorParentGroup().first;
if (planning_group.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << end_effector << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'");
}
const robot_model::JointModelGroup *eef = end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!eef)
{
ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::string &ik_link = eef->getEndEffectorParentGroup().second;
ros::WallTime start_time = ros::WallTime::now();
// construct common data for possible manipulation plans
ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
plan_data->end_effector_group_ = eef;
plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
plan_data->timeout_ = endtime;
plan_data->path_constraints_ = goal.path_constraints;
plan_data->planner_id_ = goal.planner_id;
plan_data->minimize_object_distance_ = goal.minimize_object_distance;
plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
moveit_msgs::AttachedCollisionObject &attach_object_msg = plan_data->diff_attached_object_;
// construct the attached object message that will change the world to what it would become after a pick
attach_object_msg.link_name = ik_link;
attach_object_msg.object.id = goal.target_name;
attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
attach_object_msg.touch_links = goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
// we are allowed to touch the target object
approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
// we are allowed to touch certain other objects with the gripper
approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
if (!goal.support_surface_name.empty())
{
// we are allowed to have contact between the target object and the support surface before the grasp
approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
// optionally, it may be allowed to touch the support surface with the gripper
if (goal.allow_gripper_support_collision)
approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
}
// configure the manipulation pipeline
pipeline_.reset();
ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
initialize();
pipeline_.start();
// order the grasps by quality
std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
grasp_order[i] = i;
OrderGraspQuality oq(goal.possible_grasps);
std::sort(grasp_order.begin(), grasp_order.end(), oq);
// feed the available grasps to the stages we set up
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
const moveit_msgs::Grasp &g = goal.possible_grasps[grasp_order[i]];
p->approach_ = g.pre_grasp_approach;
p->retreat_ = g.post_grasp_retreat;
p->goal_pose_ = g.grasp_pose;
p->id_ = grasp_order[i];
// if no frame of reference was specified, assume the transform to be in the reference frame of the object
if (p->goal_pose_.header.frame_id.empty())
p->goal_pose_.header.frame_id = goal.target_name;
p->approach_posture_ = g.pre_grasp_posture;
p->retreat_posture_ = g.grasp_posture;
pipeline_.push(p);
}
// wait till we're done
waitForPipeline(endtime);
pipeline_.stop();
last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
if (!getSuccessfulManipulationPlans().empty())
error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
else
{
if (last_plan_time_ > timeout)
error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
else
{
error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
if (goal.possible_grasps.size() > 0)
{
ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
// everything failed. we now start the pipeline again in verbose mode for one grasp
initialize();
pipeline_.setVerbose(true);
pipeline_.start();
pipeline_.reprocessLastFailure();
waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
pipeline_.stop();
pipeline_.setVerbose(false);
}
}
}
ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);
return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
