rrt.h
Go to the documentation of this file.
1 #ifndef RRTH
2 #define RRTH
3 
4 #include "parameters.h"
5 #include "vector.h"
6 #include "heap.h"
7 #include "wcs.h"
8 
9 #if (_KDTREE==1)
10  #include <cuik-kdtree/cuik-kdtree.h>
11 #endif
12 #if (_KDTREE==2)
13  #include <DNN/ANN_C.h>
14 #endif
15 
32 #define RRT_VERBOSE (_DEBUG>3?_DEBUG-3:0)
33 
43 #define WEIGTHED_NN 1
44 
51 #define PENALIZE_BLOCKED_NODES 0
52 
64 #define PENALIZE_PARENT_NODES 1
65 
79 #define WEIGTHED_REACHED_GOAL 2
80 
87 #define GET_RRT_STATISTICS 1
88 
100 #define HEURISTIC_RRT_STAR (0) //(1+2)
101 
112 #define RRTSTAR_UPDATE_COSTS 0
113 
120 #define RRTSTAR_SYMMETRIC_COST 1
121 
125 #define TEMPERATURE_INIT 0.001
126 
144 #define EXPLORATION_RRT 0
145 
153 #define RRT_PLOT_NODES 1
154 
163 #define RRT_NN_TOPOLOGY 1
164 
171 #define INIT_NUM_SAMPLES_RRT 100
172 
178 #define ONE_TREE 0
179 
187 #define TWO_TREES 1
188 
200 #define TWO_TREES_WITH_SWAP 2
201 
207 #define NOTREE 0
208 
214 #define START2GOAL 1
215 
222 #define GOAL2START 2
223 
229 #define BOTHTREES (START2GOAL|GOAL2START)
230 
247 typedef struct {
248  unsigned int n;
251  unsigned int nBranch;
252  unsigned int nNoEmptyBranch;
254  unsigned int nTreeConnection;
255  unsigned int nNoEmptyTreeConnection;
257  unsigned int nStep;
258  double dQrand;
260  /* Reasons to stop a branch extension */
261  unsigned int nQrandReached;
262  unsigned int nConvergenceError;
263  unsigned int nHighCost;
264  unsigned int nOutOfDomain;
265  unsigned int nDistanceIncreases;
266  unsigned int nCollision;
267  unsigned int nTooFar;
268  unsigned int nStalled;
270  unsigned int nSample;
272  /* Information about sampling */
273  unsigned int nRandom;
275  unsigned int nRejections;
277  /* Information aobut collison checking */
278  unsigned int nCollisionChecks;
279 } TRRTStatistics;
280 
281 /*******************************************************************************/
282 
294 typedef struct {
295  unsigned int id;
296  double cost;
297 } TRRTStep;
298 
307 void CopyRRTStep(void *a,void *b);
308 
309 /*******************************************************************************/
310 
329 typedef struct {
330  double *samples;
331  unsigned int parent;
333  double costp;
335  double cost;
345  double blocked;
348  double ddr;
350  double g;
353  unsigned int m;
354  unsigned int nn;
355  unsigned int *n;
356  double *cn;
358  unsigned int tree;
360  double *u;
363  double time;
366  unsigned int ns;
367  double **path;
369  double **actions;
372  double *times;
375  void *userInfo;
376 } TRRTSampleInfo;
377 
387 typedef struct {
388  TAtlasBase *w;
390  unsigned int m;
392  unsigned int k;
393  unsigned int n;
395  double delta;
397  double temperature;
398  unsigned int nFail;
401  unsigned int nCores;
402  boolean parallel;
405  unsigned int *tp;
407  unsigned int ns;
408  unsigned int ms;
410  Tbox *ambient;
412  TRRTSampleInfo **si;
414  unsigned int mode;
416  boolean graph;
420  double dd;
422  unsigned int da;
424  double *weight;
425  #if (_KDTREE==1)
426  TKDtree *treeS2G;
427  TKDtree *treeG2S;
429  #endif
430  #if (_KDTREE==2)
431  TKDTree *treeS2G;
433  TKDTree *treeG2S;
435  unsigned int nodesT1;
437  unsigned int nodesT2;
439  unsigned int maxNodesT1;
441  unsigned int maxNodesT2;
443  unsigned int *t1ToId;
445  unsigned int *t2ToId;
448  #endif
449 
450 } Trrt;
451 
452 /*******************************************************************************/
453 
461 void InitRRTStatistics(TRRTStatistics *rst);
462 
473 void AccumulateRRTStatistics(TRRTStatistics *rst1,TRRTStatistics *rst2);
474 
483 void PrintRRTStatistics(Trrt *rrt,TRRTStatistics *rst);
484 
492 void DeleteRRTStatistics(TRRTStatistics *rst);
493 
494 /*******************************************************************************/
495 
520 void InitRRT(Tparameters *pr,boolean parallel,boolean simp,double *ps,
521  unsigned int mode,boolean graph,double *pg,
522  unsigned int m,unsigned int da,TAtlasBase *w,Trrt *rrt);
523 
533 double GetTRRTTemperature(Trrt *rrt);
534 
546 boolean IsRRTGraph(Trrt *rrt);
547 
575 boolean RRTSample(unsigned int samplingMode,unsigned int tree,
576  double *goal,double *q_rand,
577  TRRTStatistics *rst,Trrt *rrt);
578 
579 
601 boolean RRTValidateSample(Tparameters *pr,double *q_rand,unsigned int tree,boolean expand2goal,
602  double *goal,double l,double *h,unsigned int *i_near,
603  TRRTStatistics *rst,Trrt *rrt);
604 
618 boolean PointInRRT(double epsilon,unsigned int tree,double *q,Trrt *rrt);
619 
634 unsigned int GetRRTNN(unsigned int tree,double *q_rand,Trrt *rrt);
635 
650 void GetRRTNNInBall(unsigned int tree,double *q_rand,double r,
651  unsigned int *nn,unsigned int **n,Trrt *rrt);
652 
671 void GetRRTNNInBranch(unsigned int tree,
672  unsigned int n1,unsigned int n2,
673  unsigned int *n,unsigned int *nn,
674  Trrt *rrt);
675 
732 boolean AddNodeToRRT(Tparameters *pr,unsigned int tree,
733  unsigned int i_near,double *sample,double *goal,
734  unsigned int *lastSample,void *info,double costp,double cost,
735  boolean blocked,
736  double *u,double time,
737  unsigned int ns,double **path,double **actions,double *times,
738  TRRTStatistics *rst,Trrt *rrt);
739 
740 
753 void AddEdgeToRRT(unsigned int i,unsigned int j,double c,Trrt *rrt);
754 
755 
770 boolean TransitionTestRRT(Tparameters *pr,unsigned int parent,
771  double* q_next,double deltaStep,double* cost,
772  double (*costF)(Tparameters*,boolean,double*,void*),
773  void *costData,Trrt *rrt);
774 
791 void RecursiveReWireRRTstar(Tparameters *pr,Theap *q,double *g,Tvector *steps,double *l,
792  Trrt *rrt);
831 boolean RRTstar(Tparameters *pr,double *pg,
832  unsigned int *it,double *times,double *costs,
833  double *planningTime,double *pl,
834  unsigned int *ns,double ***path,
835  TRRTStatistics *grst,Trrt *rrt);
860 boolean BiRRTstar(Tparameters *pr,double *pg,
861  unsigned int *it,double *times,double *costs,
862  double *planningTime,double *pl,
863  unsigned int *ns,double ***path,
864  TRRTStatistics *grst,Trrt *rrt);
865 
892 boolean ccRRT(Tparameters *pr,double *pg,double *time,
893  double *pl,unsigned int *ns,double ***path,
894  TRRTStatistics *grst,Trrt *rrt);
895 
896 
930 boolean ccTRRT(Tparameters *pr,double *pg,
931  double *time,
932  double *pl, double* pc, unsigned int *ns,double ***path,
933  double (*costF)(Tparameters*,boolean,double*,void*),
934  void *costData,
935  TRRTStatistics *grst,Trrt *rrt);
936 
937 
938 
964 boolean cBiRRT(Tparameters *pr,double *pg,
965  double *time,
966  double *pl,unsigned int *ns,double ***path,
967  TRRTStatistics *grst,Trrt *rrt);
968 
984 double RRTPathSteps(unsigned int sID,Tvector *path,Trrt *rrt);
985 
986 
1006 double ChangeBiRRTSteps(unsigned int l1,unsigned int l2,double c12,
1007  Tvector *steps,Trrt *rrt);
1008 
1027 double UpdateBiRRTSteps(Tvector *steps,Trrt *rrt);
1028 
1051 unsigned int ReconstructRRTPath(Tparameters *pr,unsigned int sID,
1052  double *pl,double *pc,
1053  unsigned int *ns,double ***path,double ***action,double **time,
1054  Trrt *rrt);
1055 
1081 boolean PathStart2GoalInRRT(Tparameters *pr,double *pgs,
1082  unsigned int l1,unsigned int l2,
1083  double *pl,double *pc,
1084  unsigned int *ns,double ***path,double ***action,double **time,
1085  Trrt *rrt);
1086 
1096 boolean Bidirectional(Trrt *rrt);
1097 
1107 unsigned int GetRRTMode(Trrt *rrt);
1108 
1121 boolean InDynamicDomain(unsigned int i,double *q,Trrt *rrt);
1122 
1135 void AdjustDynamicDomain(unsigned int i,boolean collision,Trrt *rrt);
1136 
1149 double GetDynamicDomainRadius(unsigned int i,Trrt *rrt);
1150 
1162 boolean DynamicDomainRRT(Trrt *rrt);
1163 
1173 unsigned int GetRRTNumNodes(Trrt *rrt);
1174 
1184 unsigned int GetRRTActionSpaceDimension(Trrt *rrt);
1185 
1196 double *GetRRTNode(unsigned int i,Trrt *rrt);
1197 
1208 unsigned int GetRRTNodeTree(unsigned int i,Trrt *rrt);
1209 
1221 unsigned int GetRRTParent(unsigned int i,Trrt *rrt);
1222 
1234 void SetRRTParent(unsigned int i,unsigned int p,Trrt *rrt);
1235 
1247 double *GetRRTAction(unsigned int i,Trrt *rrt);
1248 
1261 double GetRRTTime(unsigned int i,Trrt *rrt);
1262 
1274 double GetRRTTimeFromRoot(unsigned int i,Trrt *rrt);
1275 
1286 void *GetRRTNodeInfo(unsigned int i,Trrt *rrt);
1287 
1297 void SetRRTNodeInfo(unsigned int i,void *info,Trrt *rrt);
1298 
1309 double GetRRTNodeCost(unsigned int i,Trrt *rrt);
1310 
1321 double GetRRTNodeCostFromParent(unsigned int i,Trrt *rrt);
1322 
1333 void SetRRTNodeCost(unsigned int i,double costp,double cost,Trrt *rrt);
1334 
1346 void SetRRTCostAndParent(unsigned int i,unsigned int p,double costp,double cost,Trrt *rrt);
1347 
1358 double RRTBlockedProb(unsigned int i,Trrt *rrt);
1359 
1369 unsigned int *GetRRTTopology(Trrt *rrt);
1370 
1384 double *GetRRTWeights(Trrt *rrt);
1385 
1401 double CostToRoot(unsigned int sID,Trrt* rrt);
1402 
1412 void UpdateCostToRoot(unsigned int sID,Trrt* rrt);
1413 
1426 void UpdateTree(unsigned int sID,Trrt* rrt);
1427 
1437 void UpdateCostAndTree(unsigned int sID,Trrt* rrt);
1438 
1439 
1448 unsigned int StepsToRoot(unsigned int sID,Trrt* rrt);
1449 
1482 void PlotRRT(char *fname,int argc, char **arg,Tparameters *pr,
1483  unsigned int xID,unsigned int yID,unsigned int zID,
1484  double *p1,double *p2,Trrt *rrt);
1485 
1498 void SaveRRTNodes(Tparameters *pr,char *fname,boolean saveWithDummies,Trrt *rrt);
1499 
1510 void SaveRRTCosts(Tparameters *pr,char *fname,Trrt *rrt);
1511 
1521 unsigned int RRTMemSize(Trrt *rrt);
1522 
1533 void SaveRRT(Tfilename *fname,Trrt *rrt);
1534 
1553 void LoadRRT(Tparameters *pr,Tfilename *fname,TAtlasBase *w,Trrt *rrt);
1554 
1564 void PrintRRTDefines(FILE *f);
1565 
1566 
1575 void DeleteRRT(Trrt *rrt);
1576 
1577 #endif