rrt.h

Go to the documentation of this file.

#ifndef RRTH
#define RRTH

#include "parameters.h"
#include "vector.h"
#include "heap.h"
#include "wcs.h"

#if (_KDTREE==1)
  #include <cuik-kdtree/cuik-kdtree.h>
#endif
#if (_KDTREE==2)
  #include <DNN/ANN_C.h>
#endif

#define RRT_VERBOSE 0


#define GET_RRT_STATISTICS 1

#define HEURISTIC_RRT_STAR (0) //(1+2)

#define RRTSTAR_UPDATE_COSTS 0

#define RRTSTAR_SYMMETRIC_COST 1

#define TEMPERATURE_INIT 0.001

#define EXPLORATION_RRT 0

#define RRT_PLOT_NODES 1

#define RRT_NN_TOPOLOGY 1

#define INIT_NUM_SAMPLES_RRT  100

#define ONE_TREE 0

#define TWO_TREES 1

#define TWO_TREES_WITH_SWAP 2

#define NOTREE 0

#define START2GOAL 1

#define GOAL2START 2

#define BOTHTREES (START2GOAL|GOAL2START)

typedef struct {
  unsigned int n;                      
  unsigned int nBranch;                
  unsigned int nNoEmptyBranch;         
  unsigned int nTreeConnection;        
  unsigned int nNoEmptyTreeConnection; 
  unsigned int nStep;                  
  double dQrand;                       
  /* Reasons to stop a branch extension */
  unsigned int nQrandReached;          
  unsigned int nConvergenceError;      
  unsigned int nHighCost;              
  unsigned int nOutOfDomain;           
  unsigned int nDistanceIncreases;     
  unsigned int nCollision;             
  unsigned int nTooFar;                
  unsigned int nStalled;               
  unsigned int nSample;                
  /* Information about sampling */
  unsigned int nRandom;                
  unsigned int nRejections;            
  /* Information aobut collison checking */
  unsigned int nCollisionChecks;       
}  TRRTStatistics;

/*******************************************************************************/

typedef struct {
  unsigned int id; 
  double cost;     
} TRRTStep;

void CopyRRTStep(void *a,void *b);

/*******************************************************************************/

typedef struct {
  double *samples;      
  unsigned int parent;  
  double costp;         
  double cost;          
  double ddr;           
  double g;             
  unsigned int m;       
  unsigned int nn;      
  unsigned int *n;      
  double *cn;           
  unsigned int tree;    
  void *userInfo;       
} TRRTSampleInfo;

typedef struct {
  TAtlasBase *w;                   
  unsigned int m;                  
  unsigned int k;                  
  unsigned int n;                  
  double delta;                    
  double temperature;              
  unsigned int nFail;              
  unsigned int nCores;             
  boolean parallel;                
  unsigned int *tp;                
  unsigned int ns;                 
  unsigned int ms;                 
  Tbox *ambient;                   
  TRRTSampleInfo **si;             
  unsigned int mode;               
  boolean graph;                   
  double dd;                       
  #if (_KDTREE==1) 
    TKDtree *treeS2G;              
    TKDtree *treeG2S;              
  #endif
  #if (_KDTREE==2)
    TKDTree *treeS2G;              
    TKDTree *treeG2S;              
    unsigned int nodesT1;          
    unsigned int nodesT2;          
    unsigned int maxNodesT1;       
    unsigned int maxNodesT2;       
    unsigned int *t1ToId;          
    unsigned int *t2ToId;          
  #endif

} Trrt;

/*******************************************************************************/

void InitRRTStatistics(TRRTStatistics *rst);

void AccumulateRRTStatistics(TRRTStatistics *rst1,TRRTStatistics *rst2); 

void PrintRRTStatistics(Trrt *rrt,TRRTStatistics *rst);

void DeleteRRTStatistics(TRRTStatistics *rst);

/*******************************************************************************/

void InitRRT(Tparameters *pr,boolean parallel,boolean simp,double *ps,
             unsigned int mode,boolean graph,double *pg,
             unsigned int m,TAtlasBase *w,Trrt *rrt);

double GetTRRTTemperature(Trrt *rrt);

boolean IsRRTGraph(Trrt *rrt);

boolean RRTSample(unsigned int samplingMode,unsigned int tree,
                  double *goal,double *q_rand,
                  TRRTStatistics *rst,Trrt *rrt);


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);

unsigned int GetRRTNN(unsigned int tree,double *q_rand,Trrt *rrt);

void GetRRTNNInBall(unsigned int tree,double *q_rand,double r,
                    unsigned int *nn,unsigned int **n,Trrt *rrt);

void GetRRTNNInBranch(unsigned int tree,
                      unsigned int n1,unsigned int n2,
                      unsigned int *n,unsigned int *nn,
                      Trrt *rrt);

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); 

void AddEdgeToRRT(unsigned int i,unsigned int j,double c,Trrt *rrt);


boolean TransitionTestRRT(Tparameters *pr,unsigned int parent,
                          double* q_next,double deltaStep,double* cost, 
                          double (*costF)(Tparameters*,boolean,double*,void*),
                          void *costData,Trrt *rrt);

void RecursiveReWireRRTstar(Tparameters *pr,Theap *q,double *g,Tvector *steps,double *l,
                            Trrt *rrt);
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);
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);

boolean ccRRT(Tparameters *pr,double *pg,double *time,
              double *pl,unsigned int *ns,double ***path,
              TRRTStatistics *grst,Trrt *rrt);


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);



boolean cBiRRT(Tparameters *pr,double *pg,
               double *time,
               double *pl,unsigned int *ns,double ***path,
               TRRTStatistics *grst,Trrt *rrt);


double RRTPathSteps(unsigned int sID,Tvector *path,Trrt *rrt);


double ChangeBiRRTSteps(unsigned int l1,unsigned int l2,double c12,
                        Tvector *steps,Trrt *rrt);

double UpdateBiRRTSteps(Tvector *steps,Trrt *rrt);

boolean PathStart2GoalInRRT(Tparameters *pr,double *pgs,
                            unsigned int l1,unsigned int l2,
                            double *pl,double *pc,unsigned int *ns,double ***path,
                            Trrt *rrt);

boolean Bidirectional(Trrt *rrt);

unsigned int GetRRTMode(Trrt *rrt);

boolean InDynamicDomain(unsigned int i,double *q,Trrt *rrt);

void AdjustDynamicDomain(unsigned int i,boolean collision,Trrt *rrt);

double GetDynamicDomainRadius(unsigned int i,Trrt *rrt);

boolean DynamicDomainRRT(Trrt *rrt);

unsigned int GetRRTNumNodes(Trrt *rrt);

double *GetRRTNode(unsigned int i,Trrt *rrt);

unsigned int GetRRTNodeTree(unsigned int i,Trrt *rrt);

unsigned int GetRRTParent(unsigned int i,Trrt *rrt);

void SetRRTParent(unsigned int i,unsigned int p,Trrt *rrt);

void *GetRRTNodeInfo(unsigned int i,Trrt *rrt);

void SetRRTNodeInfo(unsigned int i,void *info,Trrt *rrt);

double GetRRTNodeCost(unsigned int i,Trrt *rrt);

double GetRRTNodeCostFromParent(unsigned int i,Trrt *rrt);

void SetRRTNodeCost(unsigned int i,double costp,double cost,Trrt *rrt);


void SetRRTCostAndParent(unsigned int i,unsigned int p,double costp,double cost,Trrt *rrt);


unsigned int *GetRRTTopology(Trrt *rrt);

double CostToRoot(unsigned int sID,Trrt* rrt);

void UpdateCostToRoot(unsigned int sID,Trrt* rrt);

void UpdateTree(unsigned int sID,Trrt* rrt);

void UpdateCostAndTree(unsigned int sID,Trrt* rrt);


unsigned int StepsToRoot(unsigned int sID,Trrt* rrt);

void PlotRRT(char *fname,int argc, char **arg,Tparameters *pr,
             unsigned int xID,unsigned int yID,unsigned int zID,Trrt *rrt);

void SaveRRTNodes(Tparameters *pr,char *fname,boolean saveWithDummies,Trrt *rrt);

void SaveRRTCosts(Tparameters *pr,char *fname,Trrt *rrt);

unsigned int RRTMemSize(Trrt *rrt);

void SaveRRT(Tfilename *fname,Trrt *rrt);

void LoadRRT(Tparameters *pr,Tfilename *fname,TAtlasBase *w,Trrt *rrt);

void DeleteRRT(Trrt *rrt);

#endif