atlasrrt.h File Reference

Detailed Description

Defines a RRT where the sampling is done using an atlas.

Many functions are very similar to those for Trrt but adapted to work in combination with an atlas.

See Also
atlasrrt.c, Tatlasrrt

Definition in file atlasrrt.h.

Data Structures

struct  TAtlasRRTStatistics
 Statistics on the AtlasRRT constrution. More...
 
struct  TSampleInfo
 Information for each sample in an AtlasRRT. More...
 
struct  TChartInfo
 Information for each chart in an AtlasRRT. More...
 
struct  Tatlasrrt
 RRT with an atlas for sampling. More...
 

Macros

#define ATLASRRT_VERBOSE   0
 Vebosity of the AtlasRRT operations. More...
 
#define GET_ATLASRRT_STATISTICS   1
 Set this to one to gather statistics of AtlasRRT construction. More...
 
#define ADJUST_SA   0
 Set this to one to adjust the sampling area. More...
 
#define ATLASRRT_GLOBAL_NN   1
 Set to 1 if nearest neighbours are searched without using the chart relations. More...
 
#define GET_ATLASRRT_GLOBAL_CURV_CHECK   0
 Set this to one to check the global curvature tolerences between charts. More...
 

Functions

void InitAtlasRRTStatistics (TAtlasRRTStatistics *arst)
 Init the Atlas RRT statistics. More...
 
void AccumulateAtlasRRTStatistics (TAtlasRRTStatistics *arst1, TAtlasRRTStatistics *arst2)
 Accumulates two sets of Atlas RRT statistics. More...
 
void PrintAtlasRRTStatistics (Tatlasrrt *ar, TAtlasRRTStatistics *arst)
 Prints the summary of atlasRRT statistics. More...
 
void DeleteAtlasRRTStatistics (TAtlasRRTStatistics *arst)
 Destructor. More...
 
void InitAtlasRRT (Tparameters *pr, boolean parallel, double *ps, unsigned int mode, boolean graph, double *pg, TAtlasBase *w, Tatlasrrt *ar)
 Defines a Atlas-RRT from a given point. More...
 
boolean AtlasRRTSample (Tparameters *pr, unsigned int samplingMode, unsigned int it, unsigned int tree, double *goal, double scale, boolean *exploration, unsigned int *c_rand, double *t_rand, double *q_rand, TAtlasRRTStatistics *arst, Tatlasrrt *ar)
 Generates a random sample to expand the AtlasRRT. More...
 
boolean AtlasRRTValidateSample (Tparameters *pr, double *q_rand, unsigned int tree, boolean expand2goal, unsigned int lastNN2Goal, double *goal, double l, double *h, unsigned int *i_near, TAtlasRRTStatistics *arst, Tatlasrrt *ar)
 Validates a sample generate with AtlasRRTSample. More...
 
boolean AtlasRRT (Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, TAtlasRRTStatistics *str, Tatlasrrt *ar)
 Extends a Atlas-RRT until we reach a targed point. More...
 
boolean AtlasTRRT (Tparameters *pr, double *pg, double *time, double *pl, double *pc, unsigned int *ns, double ***path, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, TAtlasRRTStatistics *str, Tatlasrrt *ar)
 Extends a Atlas-TRRT until we reach a targed point. More...
 
boolean AtlasRRTstar (Tparameters *pr, double *pg, unsigned int *it, double *times, double *costs, double *planningTime, double *pl, unsigned int *ns, double ***path, TAtlasRRTStatistics *str, Tatlasrrt *ar)
 Optimal AtlasRRT on manifolds. More...
 
boolean AtlasBiRRTstar (Tparameters *pr, double *pg, unsigned int *it, double *times, double *costs, double *planningTime, double *pl, unsigned int *ns, double ***path, TAtlasRRTStatistics *str, Tatlasrrt *ar)
 Bidirectional version of AtlasRRTstar. More...
 
void PlotAtlasRRT (char *prefix, int argc, char **arg, Tparameters *pr, unsigned int xID, unsigned int yID, unsigned int zID, Tatlasrrt *ar)
 Pots a projection of a Atlas-RRT. More...
 
unsigned int GetAtlasRRTNumNodes (Tatlasrrt *ar)
 Number of nodes in the AtlasRRT. More...
 
unsigned int GetAtlasRRTNumCharts (Tatlasrrt *ar)
 Number of charts in the AtlasRRT. More...
 
boolean RandomPointInAtlasTree (Tparameters *pr, double scale, unsigned int tree, unsigned int *nm, double *t, double *p, Tatlasrrt *ar)
 Selects a random point in the set of charts of the tree. More...
 
unsigned int GetRRTNNInNeighbourChart (unsigned int tree, unsigned int c_rand, double *t_rand, double *q_rand, Tatlasrrt *ar)
 Selects the nearest point in the chart of Id ChartId, or in a neighbour. More...
 
unsigned int GetRRTNNInChart (unsigned int tree, unsigned int chartId, double *q_rand, double t, double *d, Tatlasrrt *ar)
 Selects the nearest neighbour from the random point in the chart of Id ChartId. More...
 
unsigned int AtlasRRTMemSize (Tatlasrrt *ar)
 Memory used by a given atlasRRT. More...
 
void SaveAtlasRRT (Tparameters *pr, char *prefix, Tatlasrrt *ar)
 Stores the Atlas-RRT information on a file. More...
 
void LoadAtlasRRT (Tparameters *pr, char *prefix, TAtlasBase *w, Tatlasrrt *ar)
 Defines an atlasRRT from the information on a file. More...
 
void DeleteAtlasRRT (Tatlasrrt *ar)
 Destructor. More...
 

Macro Definition Documentation

#define ATLASRRT_VERBOSE   0

Vebosity of the atlasRRT operations. If set to 0 only minimalistic information is printed.

Note that some of the operations of the AtlasRRT are actually performed at the level of RRT. Thus to get a complete information you should set RRT_VERBOSE to 1 too. And the same applies for ATLAS_VERBOSE.

Definition at line 30 of file atlasrrt.h.

Referenced by AtlasBiRRTstarCloseIteration(), AtlasRRTstarCloseIteration(), and main().

#define GET_ATLASRRT_STATISTICS   1

Set this to one to gather statistics of AtlasRRT construction process. These statistics are printed at the end of the AtlasRRT construction.

Definition at line 38 of file atlasrrt.h.

Referenced by main().

#define ADJUST_SA   0

If set (>0) the sampling area is adjusted (reduced/increased) taking into account the branch extension (if successful the sampling area is incresased and if a branch extension failed the sampling area is reduced).

Two modes are possible

  • 1 the sampling area is adjusted locally (just for the chart used for sampling).
  • 2 the sampling area is adjusted globally (for all the charts).

The ratio at which the sampling area is adjusted is defined in MOV_AVG_UP and MOV_AVG_DOWN.

See ADJUST_SR for a related adjustment of the sampling area for each chart.

Definition at line 57 of file atlasrrt.h.

Referenced by main().

#define ATLASRRT_GLOBAL_NN   1

If set to 1 we search for nearest neighbours using Euclidean distance and considering all nodes in the AtlasRRT. If 0, we only consider nodes in neighbouring charts.

Considering only samples in neighbouring charts tends to produce parallel branches and branch crossing specially at the early stages of the RRT where the charts are very large, samples are far a away and nearby charts are not still neighbours (some charts are missing in between them). Used only for experimental reasons.

Definition at line 70 of file atlasrrt.h.

Referenced by main().

#define GET_ATLASRRT_GLOBAL_CURV_CHECK   0

Set this to one to check the global curvature tolerences between charts. Otherwise, it is only the curvature along the direction of expansion that is checked.

Definition at line 78 of file atlasrrt.h.

Referenced by main().

Function Documentation

void DeleteAtlasRRTStatistics ( TAtlasRRTStatistics arst)

Deletes the atlasRRT statistics object.

Parameters
arstThe AtlasRRT statistics to delete.

Definition at line 745 of file atlasrrt.c.

Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and main().

void InitAtlasRRT ( Tparameters pr,
boolean  parallel,
double *  ps,
unsigned int  mode,
boolean  graph,
double *  pg,
TAtlasBase w,
Tatlasrrt ar 
)

Defines a Atlas-RRT with a single sample, the root.

Parameters
prThe set of parameters.
parallelTRUE if tha AtlasRRT will be processed (build, etc) in parallel. Right now only AtlasRRT* executes in parallel. The rest of processes are still not parallelized.
psA point on the manifold from where to start the RRT. The RRT root. This is a sample including only system variables.
modeMode for the RRT: ONE_TREE, TWO_TREES, TWO_TREES_WITH_SWAP.
graphIf TRUE the RRT is actually a graphs since we store the neighbours for each node, not just the parent. This is used to propagate improvements for RRT*.
pgThe goal for the planning. Only used if the AtlasRRT is bidirectional.
wThe base type with the equations (and possibly obstacles).
arThe Atlas-RRT to create.

Definition at line 2855 of file atlasrrt.c.

References AddChart2AtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CT_CE, CT_E, CT_N_DOF, CT_R, Tatlasrrt::e, Error(), EXPLORATION_RRT, FALSE, TSampleInfo::generateChart, GetAtlasNumCharts(), GetBoxNIntervals(), GetParameter(), GetRRTNode(), GetRRTNumNodes(), GetRRTTopology(), GOAL2START, TSampleInfo::id, INIT_NUM_CHARTS, INIT_NUM_SAMPLES_RRT, InitAtlasFromPoint(), InitRRT(), TChartInfo::it, Tatlasrrt::J, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, NO_UINT, Tatlasrrt::ns, ONE_TREE, Tatlasrrt::parallel, TSampleInfo::pc, Tatlasrrt::r, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, TRUE, and Tatlasrrt::w.

Referenced by main().

boolean AtlasRRTSample ( Tparameters pr,
unsigned int  samplingMode,
unsigned int  it,
unsigned int  tree,
double *  goal,
double  scale,
boolean exploration,
unsigned int *  c_rand,
double *  t_rand,
double *  q_rand,
TAtlasRRTStatistics arst,
Tatlasrrt ar 
)

Generates a random sample to expand the AtlasRRT. The sample can be generated with different policies according to the mode (from the ambient space, or near the existing nodes) and the goal (if defined the goal is used as random sample 1 out of 100 times).

Parameters
prThe set of parameters.
samplingModePolicy to generate the random sample. This can be AMBIENT_SAMPLING (to generate it from the ambient space) or KDTREE_SAMPLING (to generate it using the kd-tree). or TANGENT_SAMPLING (to use the charts). The KDTREE_SAMPLING only works if you use the cuik-kdtree library. Otherwise AMBIENT_SAMPLING is used instead
itCurrent iteration.
treeThe tree to use in the sampling. Only used if mode is KDTREE_SAMPLING.
goalGoal node. NULL if we do not have goal (i.e. we are defining an exploration RRT) or if we can not use the goal as a random sample.
scaleThe global scale factor for the sampling areas of the charts.
explorationSet to TRUE if samplingMode is TANGENT_SAMPLING and the random sample is in the outer parts of the chart used for sampling.
c_randThe index of the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
t_randThe parameters on the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
q_randSpace to store the random sample.
arstAtlas statistics. This is only used we are repeating experiments. Otherwise is is NULL.
arAtlasRRT to use.
Returns
TRUE if the goal is used as random sample.

Definition at line 2993 of file atlasrrt.c.

References AMBIENT_SAMPLING, Tatlasrrt::atlas, Tatlasrrt::birrt, CS_WD_SIMP_INEQUALITIES_HOLD, DecreaseChartSamplingRadius(), Error(), FALSE, GetAtlasChart(), IncreaseChartSamplingRadius(), Tatlasrrt::k, KDTREE_SAMPLING, Tatlasrrt::m, NewAtlasRRTRandomSample(), NewAtlasRRTSampleRejection(), NO_UINT, Norm(), Tatlasrrt::r, randomDouble(), RandomPointInAtlasTree(), Tatlasrrt::rrt, RRTSample(), TANGENT_SAMPLING, and Tatlasrrt::w.

Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().

boolean AtlasRRTValidateSample ( Tparameters pr,
double *  q_rand,
unsigned int  tree,
boolean  expand2goal,
unsigned int  lastNN2Goal,
double *  goal,
double  l,
double *  h,
unsigned int *  i_near,
TAtlasRRTStatistics arst,
Tatlasrrt ar 
)

Checks if a given sample is valid to expand the AtlasRRT.

Parameters
prThe set of parameters.
q_randThe random sample.
treeThe tree where to look for nearest neighbours.
expand2goalTRUE if the random sample is the goal.
lastNN2GoalLast nearest-neighbour used when expanding to goal. We can avoid expanding the same sample to the goal if it was not succesful in a previous attempt.
goalThe goal.
lThe shortest path from start to goal so far. Only used for RRT*-like algorithms and it only has effect if it is not INF.
hA lower estimate of the distance between the random sample and the goal. This is only relevant in RRT*-like algorhtms.
i_nearThe nearest neighbour in the RRT.
arstStatistics on the AtlasRRT construction process (only used if GET_RRT_STATISTICS is set).
arAtlasRRT to use.
Returns
TRUE if the random sample is valid.

Definition at line 3081 of file atlasrrt.c.

References TSampleInfo::c, DistanceTopology(), GetRRTNN(), GetRRTNNInNeighbourChart(), HEURISTIC_RRT_STAR, InDynamicDomain(), INF, Tatlasrrt::m, NewAtlasRRTSampleRejection(), NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, Tatlasrrt::tp, and TRUE.

Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().

boolean AtlasRRT ( Tparameters pr,
double *  pg,
double *  time,
double *  pl,
unsigned int *  ns,
double ***  path,
TAtlasRRTStatistics str,
Tatlasrrt ar 
)

Adds as many branches as necessary to the Atlas-RRT until a targed configuration is reached (approached at a small distance).

Parameters
prThe set of parameters.
pgA point on the manifold to reach with the AtlasRRT. This is also a sample including only system variables. It is only used for one-directional RRTs. For bidirectional ones we use the one given in InitAtlasRRT.
timeTime actually used in the planning.
plThe length of the output path, if any. If no path is found. this is undefined.
nsNumber of steps of the output paht (if any).
pathConfigurations defining the output path (points on the manifold).
strStructure to accumulate statistics on the Atlas RRT construction. This is only used we are repeating experiments. Otherwise is is NULL.
arThe Atlas-RRT to expand.

Definition at line 3133 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DETECT_BIFURCATIONS, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DecreaseChartSamplingRadius(), DeleteAtlasRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetAtlasChart(), GetElapsedTime(), GetParameter(), GetRRTNNInBranch(), GOAL2START, IncreaseChartSamplingRadius(), INF, InitAtlasRRTStatistics(), InitStatistics(), Tatlasrrt::k, Tatlasrrt::m, MOV_AVG_DOWN, MOV_AVG_UP, Tatlasrrt::nc, NEW, NewAtlasRRTBranch(), NewAtlasRRTChart(), NewAtlasRRTDistanceQrand(), NewAtlasRRTNoEmptyBranch(), NewAtlasRRTNoEmptyTreeConnection(), NewAtlasRRTSample(), NewAtlasRRTTreeConnection(), NO_UINT, Tatlasrrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, TANGENT_SAMPLING, Tatlasrrt::tp, TRUE, and Tatlasrrt::w.

Referenced by main().

boolean AtlasTRRT ( Tparameters pr,
double *  pg,
double *  time,
double *  pl,
double *  pc,
unsigned int *  ns,
double ***  path,
double(*)(Tparameters *, boolean, double *, void *)  costF,
void *  costData,
TAtlasRRTStatistics str,
Tatlasrrt ar 
)

Adds as many branches as necessary to the Atlas-TRRT until a targed configuration is reached (approached at a small distance).

The difference with respect AtlasRRT is that here we take into account a cost function using the strategy proposed in

L. Jaillet, J. Cortes, T. Simeon, Sampling-based path planning on configuration-space costmaps IEEE Transactions on Robotics, Vol. 26(4), pp. 635 - 646, 2010. http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5477164

Parameters
prThe set of parameters.
pgA point on the manifold to reach with the AtlasTRRT. This is also a sample including only system variables. It is only used for one-directional RRTs. No bidirectional search with AtlasTRRT.
timeTime actually used in the planning.
plThe length of the output path, if any. If no path is found. this is undefined.
pcThe cost of the output path, if any. If no path is found. this is undefined.
nsNumber of steps of the output paht (if any).
pathConfigurations defining the output path (points on the manifold).
costFThe cost function, if any.
costDataThe last parameter of the cost funtion, if any.
strStructure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
arThe Atlas-RRT to expand.

Definition at line 3399 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DETECT_BIFURCATIONS, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteStatistics(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), GetTRRTTemperature(), INF, InitAtlasRRTStatistics(), InitStatistics(), Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), Tatlasrrt::rrt, SetRRTNodeCost(), START2GOAL, Tatlasrrt::tp, TRUE, and Tatlasrrt::w.

Referenced by main().

boolean AtlasRRTstar ( Tparameters pr,
double *  pg,
unsigned int *  it,
double *  times,
double *  costs,
double *  planningTime,
double *  pl,
unsigned int *  ns,
double ***  path,
TAtlasRRTStatistics str,
Tatlasrrt ar 
)

Tries to determine an optimal path to the goal using the RRT* method by S. Karaman and E. Frazzoli "Sampling-based algorithms for optimal motion planning" International Journal of Robotics Research, 2011 but adapted to operate on manifolds. In this case the connections of the connection between points on the manifold is done using the method by Berenson, D., Srinivasa, S., and Kuffner, J. (2011). Task space regions: A framework for pose-constrained manipulation planning. International Journal of Robotics Research. doi 10.1177/0278364910396389.

Note that in this case the RRT construction is not stopped when the goal is reached but continues refining the path for the maximum time allowed for the tree construction.

This is the atlas-based version of RRTstar. The main difference is that the atlas is used to sample new nodes and to determine paths between samples.

If constant GAMMA is 0 this procedure only finds the first path to the goal and then stops (to attempt to improve the path is done).

Parameters
prThe set of parameters.
pgA point on the manifold to reach with the RRT. This is also a sample including only system variables.
it[output] Number of iterations actually executed.
timesOptional table to store the time at each iteration. If not used set to NULl at call.
costsOptional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTimeTime actually used in the planning.
plThe length of the output path, if any. If no path is found. this is undefined.
nsNumber of steps of the output path (if any).
pathConfigurations defining the output path (points on the manifold).
strStructure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
arThe AtlasRRT to expand.
Returns
TRUE if the path exists.

Definition at line 3908 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), AddStepToAtlasRRTstar(), Tatlasrrt::ambient, AtlasBiRRTstar(), AtlasRRTSample(), AtlasRRTstarCloseIteration(), AtlasRRTValidateSample(), Tatlasrrt::birrt, CopyDoublePair(), CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_EPSILON, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteDoublePair(), DeleteHeap(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNNInBall(), GetRRTNodeCost(), HEURISTIC_RRT_STAR, INF, InitAtlasRRTStatistics(), InitHeap(), InitStatistics(), IsRRTGraph(), Tatlasrrt::k, LessThanDoublePair(), Tatlasrrt::m, Tatlasrrt::ms, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, Tatlasrrt::parallel, PointInBoxTopology(), PrintAtlasRRTStatistics(), ReconstructAtlasRRTPath(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, Tatlasrrt::tp, TRUE, UpdateCostAndTree(), Tatlasrrt::w, and WireAtlasRRTstar().

Referenced by AtlasBiRRTstar(), and main().

boolean AtlasBiRRTstar ( Tparameters pr,
double *  pg,
unsigned int *  it,
double *  times,
double *  costs,
double *  planningTime,
double *  pl,
unsigned int *  ns,
double ***  path,
TAtlasRRTStatistics str,
Tatlasrrt ar 
)

The same as AtlasRRTstar but here a bi-directional RRT is built.

Parameters
prThe set of parameters.
pgA point on the manifold to reach with the RRT. This is also a sample including only system variables.
it[output] Number of iterations actually executed.
timesOptional table to store the time at each iteration. If not used set to NULl at call.
costsOptional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTimeTime actually used in the planning.
plThe length of the output path, if any. If no path is found. this is undefined.
nsNumber of steps of the output path (if any).
pathConfigurations defining the output path (points on the manifold).
strStructure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
arThe AtlasRRT to expand.
Returns
TRUE if the path exists.

Definition at line 4148 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddStepToAtlasRRTstar(), AtlasBiRRTstarCloseIteration(), AtlasRRTSample(), AtlasRRTstar(), AtlasRRTValidateSample(), Tatlasrrt::birrt, BOTHTREES, ChangeBiRRTSteps(), CopyDoublePair(), CopyRRTStep(), CT_DELTA, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteDoublePair(), DeleteHeap(), DeleteRRTStep(), DeleteStatistics(), DeleteVector(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GetRRTNNInBall(), GetRRTNodeCost(), GetRRTNodeTree(), GOAL2START, HEURISTIC_RRT_STAR, INF, InitAtlasRRTStatistics(), InitHeap(), InitStatistics(), InitVector(), IsRRTGraph(), Tatlasrrt::k, LessThanDoublePair(), Tatlasrrt::m, Tatlasrrt::ms, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, Tatlasrrt::parallel, PrintAtlasRRTStatistics(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, Steps2PathinAtlasRRT(), Tatlasrrt::tp, TRUE, UpdateBiRRTSteps(), and WireAtlasRRTstar().

Referenced by AtlasRRTstar().

void PlotAtlasRRT ( char *  prefix,
int  argc,
char **  arg,
Tparameters pr,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tatlasrrt ar 
)

Pots a projection of an atlasRRT.

Combines the plot of the Atlas and the RRT.

Parameters
prefixPrefix for the name of the file where to store the plots.
argcNumber arguments given to the program calling this function. This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
argArguments given to the program calling this function. This is used This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
prThe set of parameters.
xIDThe first ambient dimension where to project (in the original system including system vars and dummies).
yIDThe second ambient dimension where to project (in the original system including system vars and dummies).
zIDThe third ambient dimension where to project (in the original system including system vars and dummies).
arThe Atlas-RRT to plot.
See Also
PlotAtlas, PlotRRT.

Plots a 3d projection of an atlasRRT defined on a manifold. That is it plots the atlas, the RRT and some information about the relation between the two structures.

Although the ambien space can have arbitrary dimension we project it on 3 dimensions.

The output plot can be visualized using geomview.

Parameters
prefixPrefix for the name of the files where to store the plots of the RRT the Atlas and the information linking the RRT and the Atlas.
argcNumber arguments given to the program calling this function. This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
argArguments given to the program calling this function. This is used This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
prThe set of parameters.
xIDThe first ambient dimension where to project (in the original system including system vars and dummies).
yIDThe second ambient dimension where to project (in the original system including system vars and dummies).
zIDThe thrid ambient dimension where to project (in the original system including system vars and dummies).
arThe atlasRRT to plot.
See Also
PlotAtlas, PlotRRT.

Pots a projection of a Atlas-RRT.

Plots a 3d projection of an atlasRRT defined on a manifold. That is it plots the atlas, the RRT and some information about the relation between the two structures.

Although the ambien space can have arbitrary dimension we project it on 3 dimensions.

The output plot can be visualized using geomview.

Parameters
prefixPrefix for the name of the files where to store the plots of the RRT the Atlas and the information linking the RRT and the Atlas.
argcNumber arguments given to the program calling this function. This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
argArguments given to the program calling this function. This is used This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
prThe set of parameters.
xIDThe first ambient dimension where to project (in the original system including system vars and dummies).
yIDThe second ambient dimension where to project (in the original system including system vars and dummies).
zIDThe thrid ambient dimension where to project (in the original system including system vars and dummies).
arThe atlasRRT to plot.
See Also
PlotAtlas, PlotRRT.

Definition at line 4470 of file atlasrrt.c.

References Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetAtlasChart(), GetFileFullName(), GetRRTNodeTree(), GetRRTParent(), GOAL2START, InitPlot3d(), Tatlasrrt::k, Local2Global(), Tatlasrrt::m, NEW, NewColor(), Tatlasrrt::ns, PLOT3D_EXT, PLOT_AS_POLYGONS, PlotAtlas(), PlotRRT(), PlotVect3d(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, StartNew3dObject(), TSampleInfo::t, and Tatlasrrt::w.

Referenced by main().

unsigned int GetAtlasRRTNumNodes ( Tatlasrrt ar)

Returns the number of nodes (samples) in the AtlasRRT.

Parameters
arThe AtlasRRT to query.
Returns
The number of samples stored in the AtlasRRT so far.

Definition at line 4400 of file atlasrrt.c.

References Tatlasrrt::ns.

Referenced by main().

unsigned int GetAtlasRRTNumCharts ( Tatlasrrt ar)

Returns the number of charts in the AtlasRRT.

Parameters
arThe AtlasRRT to query.
Returns
The number of charts in the AtlasRRT so far.

Definition at line 4405 of file atlasrrt.c.

References Tatlasrrt::nc.

Referenced by main().

boolean RandomPointInAtlasTree ( Tparameters pr,
double  scale,
unsigned int  tree,
unsigned int *  nm,
double *  t,
double *  p,
Tatlasrrt ar 
)

Selects a random point in the set of charts reached by a given tree.

Parameters
prThe set of parameters.
treeThe tree.
nmThe indice of selected chart.
scaleThe global scale factor for the sampling areas of the charts.
tThe parameters of the random point on the chart.
pThe random point on the manifold.
arThe atlasRRT to query
Returns
TRUE if the random point could be actually determined.

Definition at line 2562 of file atlasrrt.c.

References Tatlasrrt::atlas, Tatlasrrt::birrt, BOTHTREES, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, GetAtlasChart(), GetAtlasNumCharts(), GetChartSamplingRadius(), Tatlasrrt::k, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, randomMax(), RandomPointInAtlas(), RandomPointInChart(), randomWithDistribution(), START2GOAL, and Tatlasrrt::tp.

Referenced by AtlasRRTSample().

unsigned int GetRRTNNInNeighbourChart ( unsigned int  tree,
unsigned int  c_rand,
double *  t_rand,
double *  q_rand,
Tatlasrrt ar 
)

Selects the nearest neighbour from the random point in the chart of Id ChartId, or in one of its neighbour charts.

Parameters
treeThe tree.
c_randChart used to generate q_rand.
t_randParameters of q_rand on chart c_rand.
q_randThe random point.
arAhe atlasRRT to query.
Returns
TRUE if the random point could be actually determined.

Definition at line 4411 of file atlasrrt.c.

References Tatlasrrt::atlas, ChartNeighbourID(), ChartNumNeighbours(), GetAtlasChart(), GetRRTNNInChart(), INF, and NO_UINT.

Referenced by AtlasRRTValidateSample().

unsigned int GetRRTNNInChart ( unsigned int  tree,
unsigned int  chartId,
double *  q_rand,
double  t,
double *  d,
Tatlasrrt ar 
)

Selects the nearest neighbour from the random point in the chart of Id ChartId.

Parameters
treeTthe tree.
chartIdThe Id of the random point chart.
q_randThe random point.
tThe distance threshold. We are searching for neighbours closer than this threshold, if any. Use INF to consider all samples.
dThe output distance to the nearest neighbour.
arThe atlasRRT to query.
Returns
TRUE if the random point could be actually determined.

Definition at line 4443 of file atlasrrt.c.

References Tatlasrrt::birrt, Tatlasrrt::ci, DistanceTopologyMin(), GetRRTNodeTree(), TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, and Tatlasrrt::tp.

Referenced by GetRRTNNInNeighbourChart().

unsigned int AtlasRRTMemSize ( Tatlasrrt ar)

Returns the approximated memory used (in bytes) by a given atlasRRT.

Parameters
arThe atlasRRT.
Returns
The size of the atlasRRT in bytes.

Definition at line 4677 of file atlasrrt.c.

References Tatlasrrt::atlas, AtlasMemSize(), Tatlasrrt::rrt, and RRTMemSize().

Referenced by main().

void SaveAtlasRRT ( Tparameters pr,
char *  prefix,
Tatlasrrt ar 
)

Stores the atlasRRT as a normal atlas and a RRT than can be latter read/plotted with the usual programs.

Note that this does not save all the information stored in the AtlasRRT but only that necessary for plotting.

Parameters
prThe parameter set.
prefixPrefix for the name of the files where to store the RRT, the Atlas and the information linking the RRT and the Atlas.
arThe Atlas-RRT to store.

Definition at line 4687 of file atlasrrt.c.

References Tatlasrrt::atlas, ATLAS_EXT, ATLAS_RRT_EXT, Tatlasrrt::birrt, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CreateFileName(), DeleteFileName(), Tatlasrrt::e, Error(), GetFileFullName(), Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, Tatlasrrt::ns, Tatlasrrt::r, Tatlasrrt::rrt, RRT_EXT, SaveAtlas(), SaveAtlasRRTChartInfo(), SaveAtlasRRTSampleInfo(), SaveRRT(), and Tatlasrrt::si.

Referenced by main().

void LoadAtlasRRT ( Tparameters pr,
char *  prefix,
TAtlasBase w,
Tatlasrrt ar 
)

Construct an atlasRRT from the information previously stored in a file by SaveAtlasRRT.

Parameters
prThe set of parameters.
prefixPrefix for the name of the files where to store the RRT the Atlas and the information linking the RRT and the Atlas.
wThe world with the equations defining the manifold where the atlas is defined. This is not stored in the file and must be provided externally.
arThe atlasRRT to define.

Definition at line 4744 of file atlasrrt.c.

References Tatlasrrt::ambient, Tatlasrrt::atlas, ATLAS_EXT, ATLAS_RRT_EXT, Tatlasrrt::birrt, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CreateFileName(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, DeleteFileName(), Tatlasrrt::e, Error(), FALSE, GetFileFullName(), GetRRTTopology(), Tatlasrrt::J, Tatlasrrt::k, LoadAtlas(), LoadAtlasRRTChartInfo(), LoadAtlasRRTSampleInfo(), LoadRRT(), Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, Tatlasrrt::ns, Tatlasrrt::parallel, Tatlasrrt::r, Tatlasrrt::rrt, RRT_EXT, Tatlasrrt::si, Tatlasrrt::tp, and Tatlasrrt::w.

Referenced by main().

void DeleteAtlasRRT ( Tatlasrrt ar)