Go to the documentation of this file.
250 if (rst!=NULL) rst-> dQrand+=dqr;
255 if (rst!=NULL) rst-> nStep++;
320 if ((rst1!=NULL)&&(rst2!=NULL))
363 fprintf(stderr, "%% **************************************************\n");
365 fprintf(stderr, "RRT Statistics (averaged over %u repetitions)\n",rst-> n);
367 fprintf(stderr, "RRT Statistics\n\n");
368 fprintf(stderr, " Num. random samples : %.2f\n",
369 ( double)rst-> nRandom/( double)rst-> n);
370 fprintf(stderr, " Num. rejected random samples : %.2f (%.2f)\n",
373 fprintf(stderr, " Num. accepted random samples : %.2f (%.2f)\n",
380 double miDD,maDD,avDD,r;
386 for(i=0;i<rrt-> ns;i++)
400 fprintf(stderr, " No dynamic domain effect.");
402 fprintf(stderr, " Dynamic domain radius : %.2f <-- %.2f --> %.2f (%u/%u)\n",
403 miDD,avDD/( double)n,maDD,n,rrt-> ns);
405 fprintf(stderr, " Average distance to Qrand : %.2f\n",
410 fprintf(stderr, " Num. direct RRT extensions : %.2f\n",
411 ( double)rst-> nBranch/( double)rst-> n);
414 fprintf(stderr, " Num. no-empty direct extensions : %.2f (%.2f)\n",
415 ( double)m/( double)rst-> n,
416 ( double)m/( double)rst-> nBranch);
418 fprintf(stderr, " Num. RRT connections : %.2f\n",
421 fprintf(stderr, " Num. no-empty RRT connections : %.2f (%.2f)\n",
422 ( double)m/( double)rst-> n,
425 fprintf(stderr, " Total branches : %.2f\n",
426 ( double)nExt/( double)rst-> n);
428 fprintf(stderr, " Total no-empty branches : %.2f (%.2f)\n",
429 ( double)m/( double)rst-> n,
430 ( double)m/( double)nExt);
434 fprintf(stderr, " Total branches : %.2f\n",
435 ( double)rst-> nBranch/( double)rst-> n);
437 fprintf(stderr, " Total no-empty branches : %.2f (%.2f)\n",
438 ( double)m/( double)rst-> n,
439 ( double)m/( double)rst-> nBranch);
441 fprintf(stderr, " Reasons to stop the branch/connection\n");
443 fprintf(stderr, " q_rand reached : %.2f (%.2f)\n",
444 ( double)m/( double)rst-> n,
445 ( double)m/( double)nExt);
448 fprintf(stderr, " Convergence errors : %.2f (%.2f)\n",
449 ( double)m/( double)rst-> n,
450 ( double)m/( double)nExt);
453 fprintf(stderr, " High cost configurations : %.2f (%.2f)\n",
454 ( double)m/( double)rst-> n,
455 ( double)m/( double)nExt);
458 fprintf(stderr, " Out of domain : %.2f (%.2f)\n",
459 ( double)m/( double)rst-> n,
460 ( double)m/( double)nExt);
463 fprintf(stderr, " Distance to q_rand increases : %.2f (%.2f)\n",
464 ( double)m/( double)rst-> n,
465 ( double)m/( double)nExt);
468 fprintf(stderr, " Collision : %.2f (%.2f)\n",
469 ( double)m/( double)rst-> n,
470 ( double)m/( double)nExt);
473 fprintf(stderr, " Too far from previous sample : %.2f (%.2f)\n",
474 ( double)m/( double)rst-> n,
475 ( double)m/( double)nExt);
478 fprintf(stderr, " Stalled branch : %.2f (%.2f)\n",
479 ( double)m/( double)rst-> n,
480 ( double)m/( double)nExt);
483 fprintf(stderr, " Num. extension steps : %.2f\n",
484 ( double)rst-> nStep/( double)rst-> n);
485 fprintf(stderr, " Num. steps per branch : %.2f\n",
486 ( double)rst-> nStep/( double)nExt);
488 fprintf(stderr, " Num. collision checks : %.2f\n",
492 fprintf(stderr, " Num. samples in (bi)RRT : %.2f\n",
493 ( double)rst-> nSample/( double)rst-> n);
494 fprintf(stderr, " Samples per non-empty extens. : %.2f\n",
556 unsigned int tree, unsigned int i_near,
557 double *q_rand, double *goal,
558 unsigned int *lastSample,
559 double (*costF)( Tparameters*, boolean , double*, void*),
583 double *pl, double* pc, unsigned int *ns,
584 double ***path, Trrt *rrt);
605 unsigned int *ns, double ***path, Trrt *rrt);
647 double *q_rand, double *goal,
648 unsigned int *i_goal,
676 double gamma, unsigned int nn, unsigned int *n, double **c,
677 double h, Theap *q, unsigned int *t,
701 unsigned int id_new, double gamma,
702 unsigned int nn, unsigned int *n, double *c,
722 double time, double gamma,
723 double *times, double *costs,
742 double time, double gamma,
743 double *times, double *costs,
763 Error( "Missmatch number of variables in SetRRTTopology");
768 while((i<rrt->m)&&(!hasS))
788 if ((i>=rrt-> ns)||(j>=rrt-> ns))
789 Error( "Nodes out of range in AddEdgeToRRT");
793 si=(k==0?rrt-> si[i]:rrt-> si[j]);
799 si-> n[si-> nn]=(k==0?j:i);
807 double (*costF)( Tparameters*, boolean, double*, void*), void *costData, Trrt *rrt)
816 *cost=costF(pr, TRUE,q_next,costData);
817 prevCost=rrt-> si[parent]-> cost;
821 if ((prevCost+epsilon)>(*cost))
825 threshold=exp((prevCost-(*cost))/(rrt-> temperature*deltaStep));
837 if (rrt-> nFail>nFailMax)
848 unsigned int tree, unsigned int i_near,
849 double *q_rand, double *goal,
850 unsigned int *lastSample,
851 double (*costF)( Tparameters*, boolean, double*, void*),
856 boolean validSample,inDomain,inCollision,goalReached,distanceDecrease,tooFar,
857 qRandReached,tooClose;
860 #if (!EXPLORATION_RRT)
863 double *v,*q_near,*q_next,*q_tmp;
877 NEW(v,rrt-> m, double);
878 NEW(q_next,rrt-> m, double);
879 NEW(q_tmp,rrt-> m, double);
880 memcpy(q_next,q_near,rrt-> m* sizeof( double));
890 printf( " Distance q_rand-tree: %f (%.12f)\n",d1,rrt-> delta);
897 distanceDecrease= TRUE;
904 while((!goalReached)&&(validSample)&&(!qRandReached)&&(inDomain)&&(trans)&&
905 (!inCollision)&&(distanceDecrease)&&(!tooFar)&&(!tooClose))
925 tooFar=(dp>2*rrt-> delta);
931 distanceDecrease=(d1<d);
933 if (distanceDecrease)
947 #if (GET_RRT_STATISTICS)
954 lastSample,NULL,dp,cost,rst,rrt);
955 #if (GET_RRT_STATISTICS)
964 #if (!EXPLORATION_RRT)
966 goalReached=(dg<rrt-> delta);
969 if ((!ccRRT)&&(!goalReached))
979 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
982 #if (GET_RRT_STATISTICS)
986 fprintf(stderr, " Configuration in collision.\n");
991 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
994 #if (GET_RRT_STATISTICS)
998 fprintf(stderr, " Configuration cost too high.\n");
1003 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
1006 #if (GET_RRT_STATISTICS)
1010 fprintf(stderr, " Configuration not in domain.\n");
1015 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
1018 #if (GET_RRT_STATISTICS)
1022 fprintf(stderr, " Distance to q_rand increases (or stalled)\n");
1027 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
1030 #if (GET_RRT_STATISTICS)
1034 fprintf(stderr, " Previous configuration is too far (too large advance step)\n");
1039 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
1042 #if (GET_RRT_STATISTICS)
1046 fprintf(stderr, " Previous configuration is too close (small advance step)\n");
1051 #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
1054 #if (GET_RRT_STATISTICS)
1058 fprintf(stderr, " Sample does not converge to manifold.\n");
1072 return(goalReached);
1077 unsigned int *ns, double ***path, Trrt *rrt)
1094 unsigned int *ns, double ***path, Trrt *rrt)
1097 unsigned int nv,nvs,ms,n,i,k,current,next;
1100 boolean *systemVars;
1103 boolean reached,collision;
1139 FALSE,NULL,&reached,&collision,NULL,&lns,&lpath,rrt-> w);
1141 fprintf(stderr, "{%u->%u}[%g]",current,next,c);
1148 (*pc)+=(dc>0?dc:0)+epsilon*c;
1152 Warning( "Path segment could not be reconstructed in Steps2PathinRRT");
1204 unsigned int mode, boolean graph, double *pg,
1207 double epsilon,er,*pWithDummies;
1209 boolean collision= FALSE;
1210 unsigned int samplingMode;
1213 #if (EXPLORATION_RRT)
1215 Error( "An EXPLORATION_RRT must be one directional");
1228 rrt-> nCores=omp_get_max_threads();
1238 fprintf(stderr, "Number of computing cores (rrt) : %u\n",rrt-> nCores);
1261 for(i=0;i<rrt-> ms;i++)
1278 memcpy(rrt-> si[0]-> samples,ps, sizeof( double)*rrt-> m);
1284 memcpy(rrt-> si[1]-> samples,pg, sizeof( double)*rrt-> m);
1308 rrt-> n=rrt-> m-rrt-> k;
1315 Error( "The start point is not in domain");
1324 Error( "The start point is not on the manifold");
1328 Error( "Start point for the RRT is in collision");
1332 rrt-> si[0]-> ddr=0.0;
1340 NEW(rrt-> si[0]-> n,rrt-> si[0]-> m, unsigned int);
1349 rrt-> si[1]-> ddr=0.0;
1357 NEW(rrt-> si[1]-> n,rrt-> si[1]-> m, unsigned int);
1364 Error( "The goal point is not in domain");
1371 Error( "The goal point is not on the manifold");
1375 Error( "Goal point for the RRT is in collision");
1385 InitRectangle(nd,NULL,NULL,&rec);
1392 rrt->treeS2G=InitKDTree(&rec,rrt-> tp,25,dr,1,&(rrt-> si[0]-> samples),&i);
1398 rrt->treeG2S=InitKDTree(&rec,rrt-> tp,25,dr,1,&(rrt-> si[1]-> samples),&i);
1401 AddPoint2KDtree(rrt-> si[1]-> samples,i,&(rrt->treeS2G));
1406 DeleteRectangle(&rec);
1413 rrt->treeS2G=CreateKDTree(rrt-> m);
1415 rrt->treeS2G=CreateKDTreeT(rrt-> m,( int *)rrt-> tp);
1417 AddPoint2KDTree(rrt-> si[0]-> samples,rrt->treeS2G);
1424 rrt->treeG2S=CreateKDTree(rrt-> m);
1426 rrt->treeG2S=CreateKDTreeT(rrt-> m,( int *)rrt-> tp);
1428 AddPoint2KDTree(rrt-> si[1]-> samples,rrt->treeG2S);
1430 rrt->maxNodesT1=rrt-> ms;
1431 rrt->maxNodesT2=rrt-> ms;
1433 NEW(rrt->t1ToId,rrt->maxNodesT1, unsigned int);
1434 NEW(rrt->t2ToId,rrt->maxNodesT2, unsigned int);
1448 AddPoint2KDTree(rrt-> si[1]-> samples,rrt->treeS2G);
1470 boolean RRTSample( unsigned int samplingMode, unsigned int tree,
1471 double *goal, double *q_rand,
1474 boolean expand2Goal;
1476 #if (GET_RRT_STATISTICS)
1484 memcpy(q_rand,goal,rrt-> m* sizeof( double));
1486 fprintf(stderr, " Expanding to goal----------------------------------\n");
1496 SampleInKDtree(q_rand,rrt->treeS2G);
1498 SampleInKDtree(q_rand,rrt->treeG2S);
1506 return(expand2Goal);
1510 double *goal, double l, double *h, unsigned int *i_near,
1513 boolean validSample= TRUE;
1516 if ((expand2goal)||(goal==NULL))
1538 #if (GET_RRT_STATISTICS)
1543 return(validSample);
1556 inear=NearestNeighbour(q_rand,rrt->treeS2G);
1560 inear=NearestNeighbour(q_rand,rrt->treeG2S);
1564 unsigned int inear1,inear2;
1567 inear1=NearestNeighbour(q_rand,rrt->treeS2G);
1568 inear2=NearestNeighbour(q_rand,rrt->treeG2S);
1573 inear=(d1<d2?inear1:inear2);
1579 inear=NearestNeighbour(q_rand,rrt->treeS2G);
1596 QueryKDTree(q_rand,&inear,&d,rrt->treeS2G);
1597 inear=rrt->t1ToId[inear];
1603 QueryKDTree(q_rand,&inear,&d,rrt->treeG2S);
1604 inear=rrt->t2ToId[inear];
1610 signed int inear1,inear2;
1612 QueryKDTree(q_rand,&inear1,&d1,rrt->treeG2S);
1613 QueryKDTree(q_rand,&inear2,&d2,rrt->treeS2G);
1616 inear=rrt->t1ToId[inear1];
1618 inear=rrt->t2ToId[inear2];
1624 QueryKDTree(q_rand,&inear,&d,rrt->treeS2G);
1626 return(( unsigned int)inear);
1631 unsigned int j,inear;
1637 for(j=0;j<rrt-> ns;j++)
1654 unsigned int *nn, unsigned int **n, Trrt *rrt)
1662 NeighboursInBall(q_rand,r,nn,n,rrt->treeS2G);
1666 NeighboursInBall(q_rand,r,nn,n,rrt->treeG2S);
1670 unsigned int nn1,nn2;
1671 unsigned int *n1,*n2;
1673 NeighboursInBall(q_rand,r,&nn1,&n1,rrt->treeS2G);
1674 NeighboursInBall(q_rand,r,&nn2,&n2,rrt->treeG2S);
1677 NEW(*n,*nn, unsigned int);
1688 NeighboursInBall(q_rand,r,nn,n,rrt->treeS2G);
1703 QueryKDTreeR(q_rand,r,( int *)(nn),( int **)(n),&dst,rrt->treeS2G);
1707 (*n)[i]=rrt->t1ToId[(*n)[i]];
1713 QueryKDTreeR(q_rand,r,( int *)(nn),( int **)(n),&dst,rrt->treeG2S);
1717 (*n)[i]=rrt->t2ToId[(*n)[i]];
1722 unsigned int nn1,nn2;
1723 unsigned int *n1,*n2;
1725 QueryKDTreeR(q_rand,r,( int *)(&nn1),( int **)(&n1),&dst,rrt->treeS2G);
1727 QueryKDTreeR(q_rand,r,( int *)(&nn2),( int **)(&n2),&dst,rrt->treeG2S);
1731 NEW(*n,*nn, unsigned int);
1733 (*n)[i]=rrt->t1ToId[n1[i]];
1736 (*n)[nn1+i]=rrt->t2ToId[n2[i]];
1743 QueryKDTreeR(q_rand,r,( int *)(nn),( int **)(n),&dst,rrt->treeS2G);
1756 NEW(*n,m, unsigned int);
1757 for(j=0;j<rrt-> ns;j++)
1775 unsigned int n1, unsigned int n2,
1776 unsigned int *n, unsigned int *nn,
1787 Error( "GetRRTNNInBranch is only defined for bidirectional RRTs");
1790 Error( "The two nodes defining the branch in GetRRTNNInBranch are not in the same tree");
1792 if (tree==rrt-> si[n1]-> tree)
1793 Error( "Parameter missmatch in GetRRTNNInBranch (the branch is in the query tree)");
1796 Error( "Can not query both trees in GetRRTNNInBranch");
1808 cnn=NearestNeighbour(rrt-> si[cn]-> samples,rrt->treeS2G);
1811 cnn=NearestNeighbour(rrt-> si[cn]-> samples,rrt->treeG2S);
1821 QueryKDTree(rrt-> si[cn]-> samples,&cnn,&cd,rrt->treeS2G);
1822 cnn=rrt->t1ToId[cnn];
1827 QueryKDTree(rrt-> si[cn]-> samples,&cnn,&cd,rrt->treeG2S);
1828 cnn=rrt->t2ToId[cnn];
1839 for(j=0;j<rrt-> ns;j++)
1841 if (rrt-> si[j]-> tree==tree)
1862 } while((!done)||(cn== NO_UINT));
1865 Error( "Wrongly delimited branch in GetRRTNNInBranch");
1869 unsigned int i_near, double *sample,
1870 double *goal, unsigned int *lastSample,
1871 void *info, double costp, double cost,
1876 #if (GET_RRT_STATISTICS)
1882 Error( "Samples must be assigned to a single tree in AddNodeToRRT");
1884 if (rrt-> ns==rrt-> ms)
1888 si=rrt-> si[rrt-> ns];
1891 memcpy(si-> samples,sample,rrt-> m* sizeof( double));
1911 AddPoint2KDtree(si-> samples,rrt-> ns,&(rrt->treeS2G));
1913 AddPoint2KDtree(si-> samples,rrt-> ns,&(rrt->treeG2S));
1916 AddPoint2KDtree(si-> samples,rrt-> ns,&(rrt->treeS2G));
1924 if (rrt->nodesT1==rrt->maxNodesT1)
1925 MEM_DUP(rrt->t1ToId,rrt->maxNodesT1, unsigned int);
1926 rrt->t1ToId[rrt->nodesT1]=rrt-> ns;
1929 AddPoint2KDTree(si-> samples,rrt->treeS2G);
1934 if (rrt->nodesT2==rrt->maxNodesT2)
1935 MEM_DUP(rrt->t2ToId,rrt->maxNodesT2, unsigned int);
1936 rrt->t2ToId[rrt->nodesT2]=rrt-> ns;
1939 AddPoint2KDTree(si-> samples,rrt->treeG2S);
1943 AddPoint2KDTree(si-> samples,rrt->treeS2G);
1946 *lastSample=rrt-> ns;
1954 NEW(si-> n,si-> m, unsigned int);
1955 NEW(si-> cn,si-> m, double);
1960 fprintf(stderr, " New sample %u dp:%f\n",rrt-> ns,
1971 double *q_rand, double *goal,
1972 unsigned int *i_goal,
1976 unsigned int i_next;
1977 boolean goalFound,collision,reached;
1980 NEW(q_next,rrt-> m, double);
1984 TRUE,NULL,&reached,&collision,q_next,NULL,NULL,rrt-> w);
1991 tree=rrt-> si[i_near]-> tree;
1995 goalFound= AddNodeToRRT(pr,tree,i_near,q_next,goal,&i_next,NULL,
1996 d,rrt-> si[i_near]-> cost+d,rst,rrt);
1998 if ((goal!=NULL)&&(i_goal!=NULL)&&(goalFound)&&(*i_goal== NO_UINT))
2011 rrt-> si[i_next]-> cost+d,rst,rrt);
2013 fprintf(stderr, " {%u->%u}[%g] **\n",*i_goal,rrt-> si[*i_goal]-> parent,
2024 double gamma, unsigned int nn, unsigned int *n, double **c,
2025 double h, Theap *q, unsigned int *t,
2029 double ic,cn,c_new,epsilon,*q_new;
2040 fprintf(stderr, " Wire %u [nn:%u g:%g]\n",id_new,nn,gamma);
2042 #if (RRTSTAR_SYMMETRIC_COST)
2046 sNew=rrt-> si[id_new];
2051 NEW(reached,nn, boolean);
2052 NEW(cost,nn, double);
2054 #pragma omp parallel for private(i) schedule(dynamic) if (rrt->parallel)
2075 TRUE,NULL,&(reached[i]),&collision,NULL,NULL,NULL,rrt-> w);
2084 #if (GET_RRT_STATISTICS)
2087 (*t)|=rrt-> si[n[i]]-> tree;
2096 #if (GET_RRT_STATISTICS)
2100 #if (!RRTSTAR_UPDATE_COSTS)
2109 c_new=rrt-> si[n[i]]-> cost+cn;
2110 if (c_new<(sNew-> cost-epsilon))
2113 fprintf(stderr, " {%u->%u}[%g] ->",id_new,sNew-> parent,sNew-> costp);
2119 fprintf(stderr, "{%u->%u}[%g]\n",id_new,sNew-> parent,sNew-> costp);
2126 #if (RRTSTAR_SYMMETRIC_COST)
2137 Error( "RRT must be in graph mode to use a heap");
2138 c_new=rrt-> si[id_new]-> cost;
2148 unsigned int id_new,i;
2149 double c_new,epsilon,ct;
2154 Error( "RecursiveReWireRRTstar can only be used if RRT in graph mode");
2159 fprintf(stderr, " Rewire\n");
2165 Error( "Reading an inconsistent node from the priority queue in RecursiveReWireRRTstar");
2167 sNew=rrt-> si[id_new];
2170 for(i=0;i<sNew-> nn;i++)
2172 sNear=rrt-> si[sNew-> n[i]];
2180 c_new=sNew-> cost+sNew-> cn[i];
2181 if (c_new<(sNear-> cost-epsilon))
2184 fprintf(stderr, " {%u->%u}[%g %g] ->",sNew-> n[i],sNear-> parent,sNear-> costp,sNear-> cost);
2195 fprintf(stderr, "{%u->%u}[%g %g]\n",sNew-> n[i],sNear-> parent,sNear-> costp,sNear-> cost);
2213 unsigned int id_new, double gamma,
2214 unsigned int nn, unsigned int *n, double *c,
2218 unsigned int i,parent;
2220 double cn,c_new,epsilon;
2222 #if (!RRTSTAR_SYMMETRIC_COST)
2228 Error( "ReWireRRTstar must not be used if the RRT is in graph mode");
2233 fprintf(stderr, " Rewire\n");
2235 sNew=rrt-> si[id_new];
2236 #if (!RRTSTAR_SYMMETRIC_COST)
2243 sNear=rrt-> si[n[i]];
2247 if ((n[i]==id_new)||(n[i]==parent))
2251 #if (RRTSTAR_SYMMETRIC_COST)
2256 rrt-> m,rrt-> n,q_new,sNear-> samples,gamma,
2257 TRUE,NULL,&reached,&collision,NULL,NULL,NULL,rrt-> w);
2267 c_new=sNew-> cost+cn;
2268 if (c_new<sNear->cost-epsilon)
2271 fprintf(stderr, " {%u->%u}[%g] ->",n[i],sNear-> parent,sNear-> costp);
2277 fprintf(stderr, "{%u->%u}[%g]\n",n[i],sNear-> parent,sNear-> costp);
2295 double time, double gamma,
2296 double *times, double *costs,
2303 fprintf(stderr, "Iteration: %u (s:%u t:%g g:%g c:%g [%g])\n",it,rrt-> ns,time,gamma,
2307 fprintf(stderr, "Iteration: %u (s:%u t:%g)\n",it,rrt-> ns,time);
2319 costs[it]=rrt-> si[id_goal]-> cost;
2324 double time, double gamma,
2325 double *times, double *costs,
2332 fprintf(stderr, "Iteration: %u (s:%u t:%g g:%g c:%g)\n",it,rrt-> ns,time,gamma,l);
2335 fprintf(stderr, "Iteration: %u (s:%u t:%g)\n",it,rrt-> ns,time);
2352 unsigned int *it, double *times, double *costs,
2353 double *planningTime, double *pl, unsigned int *ns, double ***path,
2357 Error( "RRTstar is not designed for exploration RRTs");
2360 return( BiRRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,grst,rrt));
2363 double *pWithDummies,*pgs,*q_rand;
2364 unsigned int samplingMode;
2365 unsigned int i_near;
2367 boolean expand2Goal;
2378 double gamma,ct_gamma;
2379 unsigned int i,id_new,id_goal;
2381 boolean done,optimal,validSample;
2383 unsigned int maxIterations;
2387 unsigned int numSamples;
2391 Error( "RRTstart must be used to a just initilized RRT");
2393 #if (GET_RRT_STATISTICS)
2400 for(i=0;i<rrt-> ns;i++)
2418 Error( "The targed point for the RRTstar is not in domain");
2425 Error( "The target point for the RRTstar is not on the manifold");
2429 Error( "The target point for the RRTstar is in collision");
2434 NEW(q_rand,rrt-> m, double);
2438 for(i=0;i<maxIterations;i++)
2449 optimal=(gamma>0.0);
2460 while ((!done)&&(time<maxTime)&&(*it<maxIterations))
2462 #if (HEURISTIC_RRT_STAR&(1))
2473 l=rrt-> si[id_goal]-> cost;
2484 &h,&i_near,rst,rrt);
2486 } while((!validSample)&&(time<maxTime));
2512 numSamples=(rrt-> ns<3?3:rrt-> ns);
2513 gamma=ct_gamma*pow(log(numSamples)/numSamples,1/( double)rrt-> k);
2520 WireRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(rrt-> graph?&q:NULL),&vt,rst,rrt);
2539 fprintf(stderr, " Blocked [%u]\n",i_near);
2555 #if (GET_RRT_STATISTICS)
2569 fprintf(stderr, "Final number of samples: %u\n",rrt-> ns);
2584 unsigned int *it, double *times, double *costs,
2585 double *planningTime, double *pl, unsigned int *ns, double ***path,
2589 Error( "RRTstar is not designed for exploration RRTs");
2594 Error( "RRTstar operates on two-directional RRTs");
2596 return( RRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,grst,rrt));
2601 unsigned int i_near;
2610 double gamma,ct_gamma;
2611 unsigned int i,id_new;
2613 boolean done,optimal,validSample;
2617 unsigned int maxIterations,samplingMode;
2621 unsigned int numSamples;
2623 boolean connected,collision;
2627 Error( "RRTstart must be used to a just initilized RRT");
2629 #if (GET_RRT_STATISTICS)
2636 for(i=0;i<rrt-> ns;i++)
2651 NEW(q_rand,rrt-> m, double);
2655 for(i=0;i<maxIterations;i++)
2666 optimal=(gamma>0.0);
2678 while ((!done)&&(time<maxTime)&&(*it<maxIterations))
2691 &h,&i_near,rst,rrt);
2693 } while((!validSample)&&(time<maxTime));
2720 numSamples=(rrt-> ns<3?3:rrt-> ns);
2721 gamma=ct_gamma*pow(log(numSamples)/numSamples,1/( double)rrt-> k);
2728 WireRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(rrt-> graph?&q:NULL),&vt,rst,rrt);
2760 rrt-> m,rrt-> n,q_new,rrt-> si[i_near]-> samples,l-l1,
2761 TRUE,NULL,&connected,&collision,NULL,NULL,NULL,rrt-> w);
2776 fprintf(stderr, " Blocked [%u]\n",i_near);
2794 #if (GET_RRT_STATISTICS)
2808 fprintf(stderr, "Final number of samples: %u\n",rrt-> ns);
2823 double *pl, double* pc, unsigned int *ns, double ***path,
2824 double (*costF)( Tparameters*, boolean, double*, void*),
2828 double *pWithDummies,*pgs,*q_rand;
2829 unsigned int it,i_near;
2831 boolean pathFound,done,expand2Goal;
2832 unsigned int lastSample;
2837 unsigned int maxNodes;
2838 unsigned int samplingMode;
2839 boolean validSample;
2843 Error( "ccTRRT should only be initialized with the start config");
2846 Error( "ccTRRT operates on one-directional RRTs");
2848 #if (GET_RRT_STATISTICS)
2855 for(i=0;i<rrt-> ns;i++)
2871 Error( "The targed point for the ccRRT is not in domain");
2878 Error( "The target point for the ccRRT is not on the manifold");
2882 Warning( "The target point for the ccRRT is in collision");
2890 NEW(q_rand,rrt-> m, double);
2898 while ((!done)&&(*time<maxTime)&&(rrt-> ns<maxNodes))
2902 fprintf(stderr, "Iteration: %u (s:%u t:%g)\n",it,rrt-> ns,*time);
2915 &h,&i_near,rst,rrt);
2918 } while((!validSample)&&(*time<maxTime));
2925 #if (GET_RRT_STATISTICS)
2930 costF,costData,rst,rrt);
2931 #if (GET_RRT_STATISTICS)
2932 if (lastSample!=i_near)
2941 fprintf(stderr, "Final number of samples: %u\n",rrt-> ns);
2950 #if (GET_RRT_STATISTICS)
2967 double *pl, unsigned int *ns, double ***path,
2970 double *pWithDummies,*pgs,*q_rand;
2971 unsigned int it,i_near;
2973 boolean pathFound,done,expand2Goal;
2974 unsigned int lastSample;
2979 unsigned int maxNodes;
2980 unsigned int samplingMode;
2981 boolean validSample;
2985 Error( "ccRRT operates on one-directional RRTs");
2987 #if (GET_RRT_STATISTICS)
2994 for(i=0;i<rrt-> ns;i++)
3010 Error( "The targed point for the ccRRT is not in domain");
3017 Error( "The target point for the ccRRT is not on the manifold");
3021 Warning( "The target point for the ccRRT is in collision");
3026 NEW(q_rand,rrt-> m, double);
3034 while ((!done)&&(*time<maxTime)&&(rrt-> ns<maxNodes))
3038 fprintf(stderr, "Iteration: %u (s:%u t:%g)\n",it,rrt-> ns,*time);
3051 &h,&i_near,rst,rrt);
3054 } while((!validSample)&&(*time<maxTime));
3060 #if (GET_RRT_STATISTICS)
3066 #if (GET_RRT_STATISTICS)
3067 if (lastSample!=i_near)
3076 fprintf(stderr, "Final number of samples: %u\n",rrt-> ns);
3083 #if (GET_RRT_STATISTICS)
3100 double *pl, unsigned int *ns, double ***path,
3103 double *pWithDummies,*pgs,*q_rand;
3104 unsigned int it,i_near;
3109 unsigned int lastSample1;
3110 unsigned int samplingMode;
3113 unsigned int lastSample2;
3115 boolean validSample;
3118 #if (EXPLORATION_RRT)
3119 Error( "cBiRRT can not be used in exploration mode.");
3123 Error( "cBiRRT operates on bidirectional RRTs");
3125 #if (GET_RRT_STATISTICS)
3132 for(i=0;i<rrt-> ns;i++)
3147 Error( "The targed point is not in domain");
3154 Error( "The target point for the RRT is not on the manifold");
3158 Warning( "The target point for the RRT is in collision");
3163 NEW(q_rand,rrt-> m, double);
3176 while ((!pathFound)&&(*time<maxTime))
3180 fprintf(stderr, "Iteration: %u (s:%u t:%g)\n",it,rrt-> ns,*time);
3188 RRTSample(samplingMode,t1,NULL,q_rand,rst,rrt);
3192 &h,&i_near,rst,rrt);
3195 } while((!validSample)&&(*time<maxTime));
3201 #if (GET_RRT_STATISTICS)
3207 #if (GET_RRT_STATISTICS)
3208 if (lastSample1!=i_near)
3214 memcpy(q_rand,rrt-> si[lastSample1]-> samples,rrt-> m* sizeof( double));
3222 #if (GET_RRT_STATISTICS)
3227 #if (GET_RRT_STATISTICS)
3228 if (lastSample2!=i_near)
3250 fprintf(stderr, "Final number of samples: %u\n",rrt-> ns);
3256 Error( "Inconsitancy in cBiRRT pathFound");
3261 #if (GET_RRT_STATISTICS)
3353 return(lp1+c12+lp2);
3359 unsigned int i,n,current,next,id1,id2;
3383 Error( "Undefined node in path in BiRRT");
3396 Error( "A global path in a BiRRT without transition between trees?");
3402 unsigned int l1, unsigned int l2,
3403 double *pl, double* pc, unsigned int *ns, double ***path,
3408 #if (EXPLORATION_RRT)
3418 pathFound=(d12<2.0*rrt-> delta);
3422 unsigned int ns1,ns2;
3447 pathFound=(d<2.0*rrt-> delta);
3453 AddNodeToRRT(pr, START2GOAL,nn,pgs,pgs,&g,NULL,d,0,NULL,rrt);
3475 if ((rrt-> dd>0)&&(rrt-> si[i]-> ddr>0.0))
3485 if (rrt-> si[i]-> ddr==0.0)
3516 return(rrt-> si[i]-> ddr);
3548 return(rrt-> si[i]-> tree);
3562 if ((i>=rrt-> ns)||(p>=rrt-> ns))
3563 Error( "Wrong node/parent in SetRRTParent");
3566 Error( "Can not change the parent of a root node");
3572 Error( "Nodes can only swap trees in TWO_TREES_WITH_SWAP RRTs");
3590 Error( "RRT node out of range in SetRRTNodeInfo");
3596 return(rrt-> si[i]-> cost);
3617 Error( "RRT node out of range in SetRRTNodeCost");
3630 Error( "A RRT node moved from one tree to another in SetRRTCostAndParent");
3635 Error( "RRT node out of range in SetRRTCostAndParent");
3668 Error( "Nodes can only change trees in TWO_TREES_WITH_SWAP mode");
3678 Error( "Undefined root node in UpdateTree");
3682 Error( "Wrong root in UpdateTree");
3735 unsigned int xID, unsigned int yID, unsigned int zID, Trrt *rrt)
3742 double x[2],y[2],z[2];
3748 for(i=0;i<rrt-> ns;i++)
3757 NEW(px,rrt-> ns, double);
3758 NEW(py,rrt-> ns, double);
3759 NEW(pz,rrt-> ns, double);
3761 for(i=0;i<rrt-> ns;i++)
3769 if ((cx<0)&&(px[i]<cx))
3771 if ((cx>0)&&(px[i]>cx))
3773 if ((cy<0)&&(py[i]<cy))
3775 if ((cy>0)&&(py[i]>cy))
3777 if ((cz<0)&&(pz[i]<cz))
3779 if ((cz>0)&&(pz[i]>cz))
3814 for(i=2;i<rrt-> ns;i++)
3834 #if (RRT_PLOT_NODES)
3839 for(i=0;i<rrt-> ns;i++)
3886 unsigned int i,j,nv,nvs;
3887 boolean *systemVars;
3892 if (saveWithDummies)
3899 Error( "Could not open the file to store the boxes in SaveRRTNodes");
3909 for(i=0;i<rrt-> ns;i++)
3915 if (saveWithDummies)
3916 fprintf(fo, "%u { %u ",i,nv);
3918 fprintf(fo, "%u { %u ",i,nvs);
3921 if ((saveWithDummies)||(systemVars[j]))
3922 fprintf(fo, "[%.12g,%.12g] ",o[j],o[j]);
3940 unsigned int ima,imi;
3948 Error( "Could not open the file to store the costs in SaveRRTCosts");
3952 for(i=0;i<rrt-> ns;i++)
3955 fprintf(fo, "%f\n",c);
3967 fprintf(stderr, " Extreme costs : %f (%u) -- %f (%u)\n",mi,imi,ma,ima);
3978 b= sizeof(double)*rrt-> m*rrt-> ns;
3983 for(i=0;i<rrt-> ns;i++)
3984 b+=rrt-> si[i]-> nn*( sizeof( double)+ sizeof( unsigned int));
3998 Error( "Could not open file to store rrt");
4000 fprintf(f, "%u\n",rrt-> graph);
4001 fprintf(f, "%u\n",rrt-> mode);
4002 fprintf(f, "%f\n",rrt-> dd);
4004 fprintf(f, "%f\n",KDtreeSamplingExpansion(rrt->treeS2G));
4009 fprintf(f, "%u\n",rrt-> m);
4010 fprintf(f, "%u\n",rrt-> k);
4011 fprintf(f, "%u\n",rrt-> n);
4013 fprintf(f, "%f\n",rrt-> delta);
4016 fprintf(f, "%u\n",rrt-> nFail);
4018 fprintf(f, "%u\n",rrt-> ms);
4019 fprintf(f, "%u\n",rrt-> ns);
4021 for(i=0;i<rrt-> ns;i++)
4023 for(j=0;j<rrt-> m;j++)
4024 fprintf(f, "%.12f ",rrt-> si[i]-> samples[j]);
4026 fprintf(f, "%u %u %f %f %f\n",rrt-> si[i]-> parent,rrt-> si[i]-> tree,
4031 fprintf(f, "%f %u %u",rrt-> si[i]-> g,rrt-> si[i]-> m,rrt-> si[i]-> nn);
4032 for(j=0;j<rrt-> si[i]-> nn;j++)
4033 fprintf(f, " %u %f",rrt-> si[i]-> n[j],rrt-> si[i]-> cn[j]);
4049 Error( "Could not open file to read the RRT");
4053 fscanf(f, "%u",&(rrt-> graph));
4054 fscanf(f, "%u",&(rrt-> mode));
4055 fscanf(f, "%lf",&(rrt-> dd));
4056 fscanf(f, "%lf",&dr);
4058 fscanf(f, "%u",&(rrt-> m));
4059 fscanf(f, "%u",&(rrt-> k));
4060 fscanf(f, "%u",&(rrt-> n));
4062 fscanf(f, "%lf",&(rrt-> delta));
4065 fscanf(f, "%u",&(rrt-> nFail));
4067 fscanf(f, "%u",&(rrt-> ms));
4068 fscanf(f, "%u",&(rrt-> ns));
4071 for(i=0;i<rrt-> ms;i++)
4086 InitRectangle(nd,NULL,NULL,&rec);
4093 rrt->treeS2G=InitKDTree(&rec,rrt-> tp,25,dr,0,NULL,NULL);
4095 rrt->treeG2S=InitKDTree(&rec,rrt-> tp,25,dr,0,NULL,NULL);
4097 DeleteRectangle(&rec);
4103 rrt->treeS2G=CreateKDTree(rrt-> m);
4105 rrt->treeS2G=CreateKDTreeT(rrt-> m,( int *)rrt-> tp);
4110 rrt->treeG2S=CreateKDTree(rrt-> m);
4112 rrt->treeG2S=CreateKDTreeT(rrt-> m,( int *)rrt-> tp);
4114 rrt->maxNodesT1=rrt-> ms;
4115 rrt->maxNodesT2=rrt-> ms;
4117 NEW(rrt->t1ToId,rrt->maxNodesT1, unsigned int);
4118 NEW(rrt->t2ToId,rrt->maxNodesT2, unsigned int);
4125 for(i=0;i<rrt-> ns;i++)
4130 for(j=0;j<rrt-> m;j++)
4131 fscanf(f, "%lf",&(rrt-> si[i]-> samples[j]));
4133 fscanf(f, "%u",&(rrt-> si[i]-> parent));
4134 fscanf(f, "%u",&(rrt-> si[i]-> tree));
4135 fscanf(f, "%lf",&(rrt-> si[i]-> costp));
4136 fscanf(f, "%lf",&(rrt-> si[i]-> cost));
4137 fscanf(f, "%lf",&(rrt-> si[i]-> ddr));
4145 AddPoint2KDtree(rrt-> si[i]-> samples,i,&(rrt->treeS2G));
4147 AddPoint2KDtree(rrt-> si[i]-> samples,i,&(rrt->treeG2S));
4150 AddPoint2KDtree(rrt-> si[i]-> samples,i,&(rrt->treeS2G));
4158 rrt->t1ToId[rrt->nodesT1]=i;
4160 AddPoint2KDTree(rrt-> si[i]-> samples,rrt->treeS2G);
4164 rrt->t2ToId[rrt->nodesT2]=i;
4166 AddPoint2KDTree(rrt-> si[i]-> samples,rrt->treeG2S);
4170 AddPoint2KDTree(rrt-> si[i]-> samples,rrt->treeS2G);
4175 fscanf(f, "%lf",&(rrt-> si[i]-> g));
4176 fscanf(f, "%u",&(rrt-> si[i]-> m));
4177 fscanf(f, "%u",&(rrt-> si[i]-> nn));
4178 NEW(rrt-> si[i]-> n,rrt-> si[i]-> m, unsigned int);
4180 for(j=0;j<rrt-> si[i]-> nn;j++)
4182 fscanf(f, "%u",&(rrt-> si[i]-> n[j]));
4183 fscanf(f, "%lf",&(rrt-> si[i]-> cn[j]));
4205 for(i=0;i<rrt-> ns;i++)
4210 free(rrt-> si[i]-> n);
4211 free(rrt-> si[i]-> cn);
4228 DeleteKDtree(rrt->treeS2G);
4230 DeleteKDtree(rrt->treeG2S);
4233 DeleteKDTree(rrt->treeS2G);
4236 DeleteKDTree(rrt->treeG2S);
double GetRRTNodeCost(unsigned int i, Trrt *rrt) Returns the cost associated with a node.
boolean cBiRRT(Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, TRRTStatistics *grst, Trrt *rrt) Extends a RRT until we reach a targed point.
void PlotVect3d(unsigned int n, double *x, double *y, double *z, Tplot3d *p) Adds a polyline to the current object.
CBLAS_INLINE void ScaleVector(double f, unsigned int s, double *v) Scales a vector.
Statistics on the RRT construction.
void DeleteVector(void *vector) Destructor.
void AddEdgeToRRT(unsigned int i, unsigned int j, double c, Trrt *rrt) Adds a neighbouring relation to an RRT.
unsigned int nCollisionChecks
Tinterval * GetBoxInterval(unsigned int n, Tbox *b) Returns a pointer to one of the intervals defining the box.
unsigned int StepsToRoot(unsigned int sID, Trrt *rrt) Computes the number of steps from a node to the root.
boolean RRTSample(unsigned int samplingMode, unsigned int tree, double *goal, double *q_rand, TRRTStatistics *rst, Trrt *rrt) Generates a random sample to expand the RRT.
void DeleteRRT(Trrt *rrt) Destructor.
unsigned int VectorSize(Tvector *vector) Gets the number of elements in a vector.
void PrintRRTStatistics(Trrt *rrt, TRRTStatistics *rst) Prints the summary of RRT statistics.
void SaveRRTNodes(Tparameters *pr, char *fname, boolean saveWithDummies, Trrt *rrt) Stores the nodes of the RRT.
#define CS_WD_ERROR_IN_SIMP_EQUATIONS(pr, p, wcs) Computes the error in the simplified system for a given point.
#define CT_EPSILON Numerical tolerance.
unsigned int GetRRTParent(unsigned int i, Trrt *rrt) Returns identifier of the parent of one of the nodes of the RRT.
#define CS_WD_GENERATE_SIMPLIFIED_POINT(pr, p, r, wcs) Generates a simplified point from an original one.
boolean BiRRTstar(Tparameters *pr, double *pg, unsigned int *it, double *times, double *costs, double *planningTime, double *pl, unsigned int *ns, double ***path, TRRTStatistics *grst, Trrt *rrt) Optimal RRT on manifolds.
double DistanceTopology(unsigned int s, unsigned int *tp, double *v1, double *v2) Computes the distance of two points.
boolean ccRRT(Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, TRRTStatistics *grst, Trrt *rrt) Extends a RRT until we reach a targed point.
#define NEW(_var, _n, _type) Allocates memory space.
void GetRRTNNInBall(unsigned int tree, double *q_rand, double r, unsigned int *nn, unsigned int **n, Trrt *rrt) Locates the all the sample closer than a given distance.
Data structure to hold the information about the name of a file.
void ArrayPi2Pi(unsigned int n, unsigned int *t, double *a) Applies PI2PI to an array.
double RRTPathLength(Tparameters *pr, unsigned int sID, Trrt *rrt) Path length from a sample to the root.
void ConcatVectors(Tvector *vector1, Tvector *vector) Concatenates two vectors.
void ReWireRRTstar(Tparameters *pr, unsigned int id_new, double gamma, unsigned int nn, unsigned int *n, double *c, Tvector *steps, double *l, Trrt *rrt) Rewires an RRT star.
void SetRRTNodeInfo(unsigned int i, void *info, Trrt *rrt) Changes the user info associated with a node.
unsigned int nConvergenceError
boolean TransitionTestRRT(Tparameters *pr, unsigned int parent, double *q_next, double deltaStep, double *cost, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, Trrt *rrt) transition Test for Transition RRT
void WireRRTstar(Tparameters *pr, unsigned int id_new, unsigned int i_near, double gamma, unsigned int nn, unsigned int *n, double **c, double h, Theap *q, unsigned int *t, TRRTStatistics *rst, Trrt *rrt) Wires a node to the RRT star.
void * GetVectorElement(unsigned int i, Tvector *vector) Returns a pointer to a vector element.
unsigned int Steps2PathinRRT(Tparameters *pr, Tvector *steps, double *pl, double *pc, unsigned int *ns, double ***path, Trrt *rrt) Defines a path from a vector of steps.
unsigned int GetRRTNodeTree(unsigned int i, Trrt *rrt) Returns the tree for a given node.
#define CT_NFAIL_MAX Number of successive trnasition test failues requiered before increasing the temperature.
#define RRT_VERBOSE Vebosity of the RRT operations.
void InitStatistics(unsigned int np, double v, Tstatistics *t) Constructor.
CBLAS_INLINE double Norm(unsigned int s, double *v) Computes the norm of a vector.
void SetRRTNodeCost(unsigned int i, double costp, double cost, Trrt *rrt) Changes the cost associated with a node.
#define CT_MAX_PLANNING_TIME Maximum time for path planning.
void DifferenceVectorTopology(unsigned int s, unsigned int *tp, double *v1, double *v2, double *v) Substracts two vectors.
void NewRRTHighCost(TRRTStatistics *rst) A high cost configuration is reached.
void NewRRTQrandReached(TRRTStatistics *rst) Random sample reached.
boolean AddNodeToRRT(Tparameters *pr, unsigned int tree, unsigned int i_near, double *sample, double *goal, unsigned int *lastSample, void *info, double costp, double cost, TRRTStatistics *rst, Trrt *rrt) Adds a point to a RRT.
CBLAS_INLINE void AccumulateVector(unsigned int s, double *v1, double *v2) Adds a vector to another vectors.
void NewRRTRandomSample(TRRTStatistics *rst) New random sample.
#define CT_SAMPLING Sampling mode.
double UpdateBiRRTSteps(Tvector *steps, Trrt *rrt) Updates the current optimal path.
Definition of the Tfilename type and the associated functions.
void LoadRRT(Tparameters *pr, Tfilename *fname, TAtlasBase *w, Trrt *rrt) Defines a RRT from the information on a file.
#define START2GOAL One of the possible trees.
#define ONE_TREE One of the modes for the RRT.
#define TEMPERATURE_INIT Initial temperature used in Transition based rrt searches.
void InitPlot3d(char *name, boolean axes, int argc, char **arg, Tplot3d *p) Constructor.
#define COST_EXT File extension for arrays of costs.
void NewRRTOutOfDomain(TRRTStatistics *rst) The branch leaves the domain.
#define CT_GAMMA Contant part of the search radius for nearest neightours in RRT*.
void NewRRTStep(TRRTStatistics *rst) New step in branch extension.
void Error(const char *s) General error function.
void NewRRTTreeConnection(TRRTStatistics *rst) New attempt to connect the two trees.
#define INIT_NUM_SAMPLES_RRT Initial room for samples of an rrt.
void NewRRTBranch(TRRTStatistics *rst) New branch creation.
boolean ccTRRT(Tparameters *pr, double *pg, double *time, double *pl, double *pc, unsigned int *ns, double ***path, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, TRRTStatistics *grst, Trrt *rrt) Extends a T-RRT until we reach a targed point.
void RandomPointInBox(boolean *used, double *c, Tbox *b) Returns the a random point along the selected dimensions.
#define CT_MAX_PLANNING_ITERATIONS Maximum iterations for path planning.
unsigned int GetBoxNIntervals(Tbox *b) Box dimensionality.
double randomDouble() Returns a random double in the [0,1] interval.
void NewRRTTooFar(TRRTStatistics *rst) New large step.
void AddElement2Heap(unsigned int id, void *e, Theap *heap) Adds an element to the heap.
void DeleteDoublePair(void *a) Destructor for pairs of doubles.
Definition of a rrt on a manifold.
void ReverseConcatSamples(unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path) Reverses and concats a path.
void DeleteFileName(Tfilename *fn) Destructor.
unsigned int GetRRTNN(unsigned int tree, double *q_rand, Trrt *rrt) Locates the nearest sample in the tree of samples.
#define CT_CUT_X Limit of domain of the X dimension of 3d plots.
boolean RRTValidateSample(Tparameters *pr, double *q_rand, unsigned int tree, boolean expand2goal, double *goal, double l, double *h, unsigned int *i_near, TRRTStatistics *rst, Trrt *rrt) Validates a sample generate with RRTSample.
void RecursiveReWireRRTstar(Tparameters *pr, Theap *q, double *g, Tvector *steps, double *l, Trrt *rrt) A rewire that is propagates as much as necessary over the graph.
void AddSample2Samples(unsigned int nv, double *sample, unsigned int nvs, boolean *systemVars, unsigned int *ms, unsigned int *ns, double ***path) Adds a sample to a set of samples.
boolean LessThanDoublePair(void *a, void *b, void *userData) Comparison operator for paris of doubles.
unsigned int ReconstructRRTPath(Tparameters *pr, unsigned int sID, double *pl, double *pc, unsigned int *ns, double ***path, Trrt *rrt) Collects samples from the tree root to a given sample.
double LowerLimit(Tinterval *i) Gets the lower limit.
void GetRRTNNInBranch(unsigned int tree, unsigned int n1, unsigned int n2, unsigned int *n, unsigned int *nn, Trrt *rrt) Locates the nearest sample in the tree to a branch.
void SetRRTCostAndParent(unsigned int i, unsigned int p, double costp, double cost, Trrt *rrt) Changes the cost and the parent associated with a node.
unsigned int * GetRRTTopology(Trrt *rrt) Returns a pointer to an array with the topology for each variable.
void ResetVector(Tvector *vector) Resets a vector.
void UpdateCostToRoot(unsigned int sID, Trrt *rrt) Updates the cost from a node to the root.
void * GetHeapElement(unsigned int i, Theap *heap) Returns a pointer to a heap element.
double FirstInPair(TDoublePair *p) The first element of a pair.
#define TWO_TREES One of the modes for the RRT.
#define CT_CUT_Z Limit of domain of the Z dimension of 3d plots.
#define CS_WD_SIMP_INEQUALITIES_HOLD(pr, p, wcs) Cheks if all inequalities hold.
boolean InDynamicDomain(unsigned int i, double *q, Trrt *rrt) Checks if a sample is in the dynamic domaiin of a given node.
#define KDTREE_SAMPLING One of the possible sampling modes.
boolean AddBranchToRRT(Tparameters *pr, boolean ccRRT, unsigned int tree, unsigned int i_near, double *q_rand, double *goal, unsigned int *lastSample, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, TRRTStatistics *rst, Trrt *rrt) Extends a RRT as much as possible towards a given sample.
Definitions of constants and macros used in several parts of the cuik library.
unsigned int nNoEmptyBranch
void InitVector(unsigned int ele_size, void(*Copy)(void *, void *), void(*Delete)(void *), unsigned int max_ele, Tvector *vector) Constructor.
#define HEURISTIC_RRT_STAR Whether to use an heuristic in AtlarRRT*.
#define CS_WD_NEWTON_IN_SIMP(pr, p, wcs) Applies a Newton procedure to move a point towads the manifold.
Statistics associated with a solving process.
unsigned int nQrandReached
#define MOV_AVG_UP Weight of new data when computing moving averages.
boolean HeapEmpty(Theap *heap) Checks if a heap is empty.
boolean DynamicDomainRRT(Trrt *rrt) TRUE if the dynamic domain is active.
double RRTPathSteps(unsigned int sID, Tvector *path, Trrt *rrt) Produces a vector with the nodes in a given branch.
void DeleteHeap(Theap *heap) Destructor.
unsigned int nNoEmptyTreeConnection
void Warning(const char *s) General warning function.
#define BOTHTREES Used for samples in both trees.
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn) Constructor.
double GetRRTNodeCostFromParent(unsigned int i, Trrt *rrt) Returns the cost from the parent node, if defined.
double UpperLimit(Tinterval *i) Gets the uppser limit.
void PlotRRT(char *fname, int argc, char **arg, Tparameters *pr, unsigned int xID, unsigned int yID, unsigned int zID, Trrt *rrt) Pots a projection of a RRT.
void InitRRT(Tparameters *pr, boolean parallel, boolean simp, double *ps, unsigned int mode, boolean graph, double *pg, unsigned int m, TAtlasBase *w, Trrt *rrt) Defines a RRT from a given point.
void CopyRRTStep(void *a, void *b) Copy constructor for RRTSteps.
void UpdateTree(unsigned int sID, Trrt *rrt) Updates the tree assignment a node to the root.
boolean PathStart2GoalInRRT(Tparameters *pr, double *pgs, unsigned int l1, unsigned int l2, double *pl, double *pc, unsigned int *ns, double ***path, Trrt *rrt) Determines the path from start to goal using an RRT.
double * GetRRTNode(unsigned int i, Trrt *rrt) Returns one of the nodes of the RRT.
Type defining the equations on which the atlas is defined.
unsigned int RRTMemSize(Trrt *rrt) Memory used by a given RRT.
#define CS_WD_GET_SYSTEM_VARS(sv, wcs) Gets the system variables.
char * GetFileFullName(Tfilename *fn) Gets the file full name (paht+name+extension).
#define SOL_EXT File extension for solution files.
#define EXPLORATION_RRT Select between exploration and goal driven RRT.
unsigned int nOutOfDomain
#define CS_WD_REGENERATE_ORIGINAL_POINT(pr, p, o, wcs) Completes an original point from a simplified one.
void InitHeap(unsigned int ele_size, void(*Copy)(void *, void *), void(*Delete)(void *), boolean(*LessThan)(void *, void *, void *), void *userData, boolean hasIDs, unsigned int max_ele, Theap *heap) Constructor.
double CostToRoot(unsigned int sID, Trrt *rrt) Computes the cost from a node to the root.
unsigned int nDistanceIncreases
double ConnectSamples(Tparameters *pr, unsigned int *tp, boolean *sv, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, TJacobian *sJ, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w) Determines the connection between two points on the manifold.
double ChangeBiRRTSteps(unsigned int l1, unsigned int l2, double c12, Tvector *steps, Trrt *rrt) Changes the optimal path in a bidirectional RRT.
void ReverseSamples(unsigned int ns, double **path) Reverses a set of samples.
void BiRRTstarCloseIteration(unsigned int it, double l, double time, double gamma, double *times, double *costs, Trrt *rrt) Prints information about the BiRRT* iteration.
unsigned int GetRRTNumNodes(Trrt *rrt) Number of nodes in the RRT.
void * GetRRTNodeInfo(unsigned int i, Trrt *rrt) Returns the user info associated with a node.
#define MEM_DUP(_var, _n, _type) Duplicates a previously allocated memory space.
#define NO_UINT Used to denote an identifier that has not been initialized.
#define RRT_NN_TOPOLOGY Set to 1 to use the topology in the search for nearest-neighbours.
CBLAS_INLINE void SumVectorScale(unsigned int s, double *v1, double w, double *v2, double *v) Adds two vectors with a scale.
boolean IsRRTGraph(Trrt *rrt) Identifies RRT with a graph structure.
void NewRRTSampleRejection(TRRTStatistics *rst) New sample rejection.
void NewRRTConvergenceError(TRRTStatistics *rst) Convergence error.
void InitSamples(unsigned int *ms, unsigned int *ns, double ***path) Initializes a set of samples.
void SetRRTParent(unsigned int i, unsigned int p, Trrt *rrt) Changes the parent for a given node.
double SquaredDistanceTopology(unsigned int s, unsigned int *tp, double *v1, double *v2) Computes the squared distance of two points.
unsigned int AddStepToRRTstar(Tparameters *pr, unsigned int i_near, double *q_rand, double *goal, unsigned int *i_goal, TRRTStatistics *rst, Trrt *rrt) Extend strategy for the RRT.
void DeleteBox(void *b) Destructor.
#define CT_DYNAMIC_DOMAIN Dynamic domain.
void DeleteRRTStatistics(TRRTStatistics *rst) Destructor.
#define SOL_WITH_DUMMIES_EXT File extension for solution files with dummies.
void NewDoublePair(double f, double s, TDoublePair *p) Constructor.
unsigned int GetRRTMode(Trrt *rrt) Identifies mode of the RRTs.
void SetRRTTopology(Tparameters *pr, Trrt *rrt) Initializes the topology array in the RRT.
void NewRRTCollisionCheck(TRRTStatistics *rst) New collision check.
unsigned int ExtractMinElement(void *e, Theap *heap) Extracts and removes the minimal element of a heap.
double DistanceTopologyMin(double t, unsigned int s, unsigned int *tp, double *v1, double *v2) Computes the distance of two points, if it is below a given threshold.
#define MEM_EXPAND(_var, _n, _type) Expands a previously allocated memory space.
#define TWO_TREES_WITH_SWAP One of the modes for the RRT.
double GetParameter(unsigned int n, Tparameters *p) Gets the value for a particular parameter.
#define CS_WD_REGENERATE_SOLUTION_POINT(pr, p, r, wcs) Compleates a solution point with the dummy variables.
#define CT_CUT_Y Limit of domain of the Y dimension of 3d plots.
void SaveRRTCosts(Tparameters *pr, char *fname, Trrt *rrt) Stores the cost associated with the nodes of the RRT.
void DeleteColor(Tcolor *c) Destructor.
#define CS_WD_IN_COLLISION(f, pr, s, sPrev, wcs) Checks if a configuration is in collision.
void NewRRTDistanceQrand(double dqr, TRRTStatistics *rst) New distance to q_rand.
void RRTstarCloseIteration(unsigned int it, unsigned int id_goal, double time, double gamma, double *times, double *costs, Trrt *rrt) Prints information about the RRT* iteration.
#define CT_DELTA Size of the steps in the path connecting two configurations.
double GetTRRTTemperature(Trrt *rrt) Temperature of the T-RRT.
#define CT_COEF_TEMP Scale factor of the temperature parameter for Transition-based RRT.
#define CT_N_DOF Dimensionality of the solution space for the mechanism at hand.
Auxiliary functions to deal with sets of samples.
#define GOAL2START One of the possible trees.
void NewRRTCollision(TRRTStatistics *rst) New collision.
Definition of basic randomization functions.
#define CS_WD_INIT_CD(pr, mt, wcs) Initializes the collision detector.
double GetElapsedTime(Tstatistics *t) Elapsed time.
void NewColor(double r, double g, double b, Tcolor *c) Constructor.
void CopyDoublePair(void *a, void *b) Copy constructor for pairs of doubles.
boolean Bidirectional(Trrt *rrt) Identifies bidirectional RRTs.
void AccumulateRRTStatistics(TRRTStatistics *rst1, TRRTStatistics *rst2) Accumulates two sets of RRT statistics.
void DeleteSamples(unsigned int ns, double **path) Deletes the space used by a set of samples.
void AdjustDynamicDomain(unsigned int i, boolean collision, Trrt *rrt) Sets the dynamic domain.
void NewRRTNoEmptyBranch(TRRTStatistics *rst) New no empty branch.
unsigned int nTreeConnection
#define CS_WD_GENERATE_SIMP_INITIAL_BOX(pr, b, wcs) Computes the global box for the simplified system.
boolean RRTstar(Tparameters *pr, double *pg, unsigned int *it, double *times, double *costs, double *planningTime, double *pl, unsigned int *ns, double ***path, TRRTStatistics *grst, Trrt *rrt) Optimal RRT on manifolds.
void InitRRTStatistics(TRRTStatistics *rst) Init the RRT statistics.
void NewRRTSample(TRRTStatistics *rst) New sample.
void NewRRTNoEmptyTreeConnection(TRRTStatistics *rst) New non-void attempt to connect the two trees.
unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p) Start a composed object.
void DeleteStatistics(Tstatistics *t) Destructor.
Information for each sample in a RRT.
#define DIVERGED One of the possible outputs of the Newton iteration.
void NewRRTDistanceIncreases(TRRTStatistics *rst) The distance to q_rand increase.
#define CS_WD_GET_SIMP_TOPOLOGY(pr, tp, wcs) Gets the simplified variable topology.
double GetDynamicDomainRadius(unsigned int i, Trrt *rrt) Returns the current dynamic domain radius for a given sample.
void Close3dObject(Tplot3d *p) Closes a composed object.
void UpdateCostAndTree(unsigned int sID, Trrt *rrt) A combination of UpdateCostToRoot and UpdateTree.
void UpdateHeapElement(unsigned int id, void *e, Theap *heap) Updates the position of an element in the heap.
void ClosePlot3d(boolean quit, double average_x, double average_y, double average_z, Tplot3d *p) Destructor.
#define CT_MAX_NODES_RRT Maximum number of nodes in an RRT.
unsigned int NewVectorElement(void *e, Tvector *vector) Adds an element to the vector.
#define TOPOLOGY_S One of the possible topologies.
boolean PointInBoxTopology(boolean *used, boolean update, unsigned int n, double *v, double tol, unsigned int *tp, Tbox *b) Checks if a point is included in a(sub-) box.
void NewRRTStalled(TRRTStatistics *rst) New stalled branch.
void SaveRRT(Tfilename *fname, Trrt *rrt) Stores the RRT information on a file.
#define CS_WD_ORIGINAL_IN_COLLISION(pr, o, oPrev, wcs) Checks if a configuration is in collision.
#define MOV_AVG_DOWN Weight of new data when computing moving averages.
|
Follow us!