atlasrrt.c File Reference

Detailed Description

Implementation of the AtlasRRT functions.

See Also
atlasrrt.h, Tatlasrrt

Definition in file atlasrrt.c.

Macros

#define ADD_ALL   0
 One of the addition modes for AddBranchToAtlasRRT. More...
 
#define ADD_LAST   1
 One of the addition modes for AddBranchToAtlasRRT. More...
 
#define ADD_LAST_NO_REP   2
 One of the addition modes for AddBranchToAtlasRRT. More...
 
#define ADD_NONE   3
 One of the addition modes for AddBranchToAtlasRRT. More...
 

Functions

void NewAtlasRRTBranch (TAtlasRRTStatistics *arst)
 New branch creation. More...
 
void NewAtlasRRTNoEmptyBranch (TAtlasRRTStatistics *arst)
 New no empty branch. More...
 
void NewAtlasRRTTreeConnection (TAtlasRRTStatistics *arst)
 New attempt to connect the two trees. More...
 
void NewAtlasRRTNoEmptyTreeConnection (TAtlasRRTStatistics *arst)
 New non-void attempt to connect the two trees. More...
 
void NewAtlasRRTDistanceQrand (double dqr, TAtlasRRTStatistics *arst)
 New distance to q_rand. More...
 
void NewAtlasRRTStep (TAtlasRRTStatistics *arst)
 New step in branch extension. More...
 
void NewAtlasRRTNoConvergentStep (TAtlasRRTStatistics *arst)
 Problems with convergence. More...
 
void NewAtlasRRTQrandReached (TAtlasRRTStatistics *arst)
 Random sample reached. More...
 
void NewAtlasRRTNotInDomain (TAtlasRRTStatistics *arst)
 New node not in the given domain. More...
 
void NewAtlasRRTCollision (TAtlasRRTStatistics *arst)
 New collision. More...
 
void NewAtlasRRTTooLong (TAtlasRRTStatistics *arst)
 New long branch. More...
 
void NewAtlasRRTTooFar (TAtlasRRTStatistics *arst)
 New large step. More...
 
void NewAtlasRRTOverlap (TAtlasRRTStatistics *arst)
 New overlap. More...
 
void NewAtlasRRTInitChartError (TAtlasRRTStatistics *arst)
 New chart due to error when trying to create a chart. More...
 
void NewAtlasRRTOutOfChart (TAtlasRRTStatistics *arst)
 New chart due to radius. More...
 
void NewAtlasRRTLargeCurvature (TAtlasRRTStatistics *arst)
 New chart due large curvature between two charts. More...
 
void NewAtlasRRTDirLargeCurvature (TAtlasRRTStatistics *arst)
 New chart due to the curvature in the expansion direction. More...
 
void NewAtlasRRTStepReduction (TAtlasRRTStatistics *arst)
 Step reduction. More...
 
void NewAtlasRRTSample (TAtlasRRTStatistics *arst)
 New sample. More...
 
void NewAtlasRRTChart (TAtlasRRTStatistics *arst)
 New Chart. More...
 
void NewAtlasRRTNoConnectToParent (TAtlasRRTStatistics *arst)
 A chart that does not intersect with its parent. More...
 
void NewAtlasRRTBlockBySingularity (TAtlasRRTStatistics *arst)
 Number of times we try to create a char at a singular regions. More...
 
void NewAtlasRRTRandomSample (TAtlasRRTStatistics *arst)
 New random sample. More...
 
void NewAtlasRRTSampleRejection (TAtlasRRTStatistics *arst)
 New sample rejection. More...
 
void NewAtlasRRTCollisionCheck (TAtlasRRTStatistics *arst)
 New collision check. More...
 
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...
 
double AddBranchToAtlasRRT (Tparameters *pr, unsigned int it, unsigned int addMode, boolean revisitCharts, boolean checkCollisions, boolean onManifold, boolean explorationSample, double maxLength, unsigned int i_near, double *origRandSample, double *goalSample, void *st, unsigned int *lastSampleID, boolean *reachedQRand, boolean *reachedGoal, unsigned int *ns, double ***path, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, Tatlasrrt *ar)
 Adds a new branch to the AtlasRRT. More...
 
void NewTemptativeSample (Tparameters *pr, unsigned int it, unsigned int *nChanges, boolean revisitCharts, boolean chartCreated, boolean checkCollisions, boolean onManifold, TSampleInfo *currentSampleInfo, double *currentParam, double *origRandSample, unsigned int *randChart, double *randParam, double *randSample, double currentDelta, double d, double *deltaParam, unsigned int *nextChart, double *nextParam, double *nextSample, void *st, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, double *cost, boolean *blocked, boolean *inCollision, boolean *valid, Tatlasrrt *ar)
 Generates a new sample in between q_near and q_rand. More...
 
double AddSample2AtlasRRT (Tparameters *pr, unsigned int tree, unsigned int *currentID, unsigned int nextChart, double *nextParam, double *nextSample, double dp, double cost, Tatlasrrt *ar)
 Adds a sample to the AtlasRRT. More...
 
boolean AddChart2AtlasRRT (Tparameters *pr, unsigned int tree, unsigned int it, TSampleInfo *currentSampleInfo, boolean *intersectParent, Tatlasrrt *ar)
 Adds a chart to the Atlas RRT. More...
 
void PopulateWithSamples (Tparameters *pr, unsigned int id, Tatlasrrt *ar)
 Assign samples to a newly created chart. More...
 
void AddChart2Tree (unsigned int tree, unsigned int chartId, Tatlasrrt *ar)
 Add a chart to a tree. More...
 
boolean PointTowardRandSample (unsigned int cId, double d, double *t, unsigned int samplingMode, boolean onManifold, double *origRandSample, unsigned int *randChart, double *randParam, double *randSample, double *deltaParam, Tatlasrrt *ar)
 Determines the motion on the current chart to approach q_rand. More...
 
unsigned int ReconstructAtlasRRTPath (Tparameters *pr, unsigned int sID, double *pl, unsigned int *ns, double ***path, Tatlasrrt *ar)
 Collects samples from the tree root to a given sample. More...
 
unsigned int Steps2PathinAtlasRRT (Tparameters *pr, Tvector *steps, double *pl, double *pc, unsigned int *ns, double ***path, Tatlasrrt *ar)
 Defines a path from a vector of steps. More...
 
void SmoothPathInAtlasRRT (Tparameters *pr, unsigned int sID, Tatlasrrt *ar)
 Local optimiziation of the path to a node. More...
 
unsigned int AddStepToAtlasRRTstar (Tparameters *pr, unsigned int it, boolean expand2Goal, unsigned int i_near, double *q_rand, unsigned int *id_goal, double *goal, TAtlasRRTStatistics *arst, Tatlasrrt *ar)
 Adds a node to an AtlasRRT*. More...
 
void WireAtlasRRTstar (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, TAtlasRRTStatistics *arst, Tatlasrrt *ar)
 Wires a node to the AtlasRRT star. More...
 
void ReWireAtlasRRTstar (Tparameters *pr, unsigned int id_new, double gamma, unsigned int nn, unsigned int *n, double *c, Tvector *steps, double *l, TAtlasRRTStatistics *arst, Tatlasrrt *ar)
 Rewires an AtlasRRT star. More...
 
void AtlasRRTstarCloseIteration (unsigned int it, unsigned int id_goal, double time, double gamma, double *times, double *costs, Tatlasrrt *ar)
 Prints information about the AtlasRRT* iteration. More...
 
void AtlasBiRRTstarCloseIteration (unsigned int it, double l, double time, double gamma, double *times, double *costs, Tatlasrrt *ar)
 Prints information about the BiAtlasRRT* iteration. More...
 
void SaveAtlasRRTSampleInfo (FILE *f, TSampleInfo *si, Tatlasrrt *ar)
 Saves the information associated with a sample in an AtlasRRT. More...
 
void SaveAtlasRRTChartInfo (FILE *f, TChartInfo *ci, Tatlasrrt *ar)
 Saves the information associated with a chart in an AtlasRRT. More...
 
void LoadAtlasRRTSampleInfo (FILE *f, TSampleInfo *si, Tatlasrrt *ar)
 Reads the information associated with a sample in an AtlasRRT. More...
 
void LoadAtlasRRTChartInfo (FILE *f, TChartInfo *ci, Tatlasrrt *ar)
 Reads the information associated with a chart in an AtlasRRT. More...
 
void PlotQrand (Tparameters *pr, char *prefix, unsigned int inear, unsigned int c_rand, double *t_rand, double *q_rand, unsigned int xID, unsigned int yID, unsigned int zID, Tatlasrrt *ar)
 Plots a point where q_rang is and a line to its chart. More...
 
void PlotConnection (Tparameters *pr, char *prefix, unsigned int target, unsigned int near, unsigned int end, unsigned int xID, unsigned int yID, unsigned int zID, Tatlasrrt *ar)
 Graphical representation of the attempt to connect two trees. 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...
 
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...
 
unsigned int GetAtlasRRTNumNodes (Tatlasrrt *ar)
 Number of nodes in the AtlasRRT. More...
 
unsigned int GetAtlasRRTNumCharts (Tatlasrrt *ar)
 Number of charts in the AtlasRRT. 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...
 
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. 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 ADD_ALL   0

Adds all nodes generated in the branch extension.

Definition at line 31 of file atlasrrt.c.

Referenced by AddBranchToAtlasRRT(), AtlasRRT(), and AtlasTRRT().

#define ADD_LAST   1

Adds anly the last generated node during the branch extension.

Definition at line 38 of file atlasrrt.c.

Referenced by AddBranchToAtlasRRT(), and AddStepToAtlasRRTstar().

#define ADD_LAST_NO_REP   2

Adds anly the last generated node during the branch extension if it is not already in the tree.

Definition at line 46 of file atlasrrt.c.

Referenced by AddBranchToAtlasRRT(), and AddStepToAtlasRRTstar().

#define ADD_NONE   3

Do not add any node during the branch extension.

Definition at line 53 of file atlasrrt.c.

Referenced by AtlasBiRRTstar(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().

Function Documentation

void NewAtlasRRTBranch ( TAtlasRRTStatistics arst)

To be called everty time we start a new atlasRRT branch.

In the context of RRT* this must be interpreted as the number of possible edges (number of sample pairs whose connections is attempted).

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 367 of file atlasrrt.c.

References TAtlasRRTStatistics::nBranch.

Referenced by AtlasRRT(), and WireAtlasRRTstar().

void NewAtlasRRTNoEmptyBranch ( TAtlasRRTStatistics arst)

To be called every time we managed to add a non-emtpy branch to the AtlasRRT.

In the context of RRT* this must be interpreted as the number of edges (number of sample pairs whose connections is valid).

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 372 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoEmptyBranch.

Referenced by AtlasRRT(), and WireAtlasRRTstar().

void NewAtlasRRTTreeConnection ( TAtlasRRTStatistics arst)

New attempt to connect the two trees. This is only updated in bi-directional Atlas RRTs.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 377 of file atlasrrt.c.

References TAtlasRRTStatistics::nTreeConnection.

Referenced by AtlasRRT().

void NewAtlasRRTNoEmptyTreeConnection ( TAtlasRRTStatistics arst)

New attempt to connect the two trees that resulted in a non empty branch. This is only updated in bi-directional Atlas RRTs.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 382 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoEmptyTreeConnection.

Referenced by AtlasRRT().

void NewAtlasRRTDistanceQrand ( double  dqr,
TAtlasRRTStatistics arst 
)

New distance from q_rand to the tree. These distances are accumulated and then we compute the average using the number of attempts for branch extension.

Parameters
dqrNew tree-q_rand distance.
arstThe AtlasRRT statistics to modify.

Definition at line 387 of file atlasrrt.c.

References TAtlasRRTStatistics::dQrand.

Referenced by AtlasRRT().

void NewAtlasRRTStep ( TAtlasRRTStatistics arst)

To be called everytime we add a new step to a branch.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 392 of file atlasrrt.c.

References TAtlasRRTStatistics::nStep.

Referenced by NewTemptativeSample().

void NewAtlasRRTNoConvergentStep ( TAtlasRRTStatistics arst)

To be called when we create a new chart due to convergence issues in the current chart.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 397 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoConvergent.

Referenced by NewTemptativeSample().

void NewAtlasRRTQrandReached ( TAtlasRRTStatistics arst)

To be called every time the random sample is reached.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 402 of file atlasrrt.c.

References TAtlasRRTStatistics::nQrandReached.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

void NewAtlasRRTNotInDomain ( TAtlasRRTStatistics arst)

To be called everty time a branch is stopped since it leaves the given domain.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 407 of file atlasrrt.c.

References TAtlasRRTStatistics::nNotInDomain.

Referenced by NewTemptativeSample().

void NewAtlasRRTCollision ( TAtlasRRTStatistics arst)

To be called every time a branch extension is stopped due to a collision.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 412 of file atlasrrt.c.

References TAtlasRRTStatistics::nCollision.

Referenced by NewTemptativeSample().

void NewAtlasRRTTooLong ( TAtlasRRTStatistics arst)

To be called every time a branch extension is stopped because the branch is already too long.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 417 of file atlasrrt.c.

References TAtlasRRTStatistics::nTooLong.

Referenced by AddBranchToAtlasRRT().

void NewAtlasRRTTooFar ( TAtlasRRTStatistics arst)

To be called every time a branch extension is stopped because the brach is scaping the ball defined with q_near, q_rand.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 422 of file atlasrrt.c.

References TAtlasRRTStatistics::nTooFar.

Referenced by AddBranchToAtlasRRT().

void NewAtlasRRTOverlap ( TAtlasRRTStatistics arst)

To be called every time a branch extension is stopped to avoid the overlap with another branch.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 427 of file atlasrrt.c.

References TAtlasRRTStatistics::nOverlap.

Referenced by NewTemptativeSample().

void NewAtlasRRTInitChartError ( TAtlasRRTStatistics arst)

To be called every time the attempt to create a chart fails. This triggers the creation of a new chart in the precedent sample (where the chart was successfully created before).

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 437 of file atlasrrt.c.

References TAtlasRRTStatistics::nErrorNewChart.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

void NewAtlasRRTOutOfChart ( TAtlasRRTStatistics arst)

To be called every time we create a new chart since a node reached the border of the current chart (i.e., it leaves the ball defining the maximum chart validity area).

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 432 of file atlasrrt.c.

References TAtlasRRTStatistics::nOutOfChart.

Referenced by NewTemptativeSample().

void NewAtlasRRTLargeCurvature ( TAtlasRRTStatistics arst)

Called when there is a large curvature along a branch, and thus, a new chart has to be created. In this case the new chart has too large curvature error with respect to the current chart.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 442 of file atlasrrt.c.

References TAtlasRRTStatistics::nLargeCurvature.

Referenced by NewTemptativeSample().

void NewAtlasRRTDirLargeCurvature ( TAtlasRRTStatistics arst)

Called every time we create a new chart as it is the a cheap test that do not require the creation of temporary chart

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 447 of file atlasrrt.c.

References TAtlasRRTStatistics::nDirLargeCurvature.

Referenced by NewTemptativeSample().

void NewAtlasRRTStepReduction ( TAtlasRRTStatistics arst)

To be called everytime we have to reduce the default size between two nodes on a atlasRRT branch to be able to generate a valid new node. The default step size is given by the DELTA parameter.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 452 of file atlasrrt.c.

References TAtlasRRTStatistics::nStepReduction.

Referenced by AddBranchToAtlasRRT().

void NewAtlasRRTSample ( TAtlasRRTStatistics arst)

To be called every time we actually add a node (or sample) to the atlasRRT.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 457 of file atlasrrt.c.

References TAtlasRRTStatistics::nSample.

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

void NewAtlasRRTChart ( TAtlasRRTStatistics arst)

To be called every time we actualy add a chart to the atlasRRT.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 462 of file atlasrrt.c.

References TAtlasRRTStatistics::nChart.

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

void NewAtlasRRTNoConnectToParent ( TAtlasRRTStatistics arst)

When creating a new chart it should intersect with its parent chart. If this does not happen we have to record it.

Definition at line 467 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoConnect.

Referenced by AddBranchToAtlasRRT().

void NewAtlasRRTBlockBySingularity ( TAtlasRRTStatistics arst)

Number of times we try to create a char at a singular regions.

Note that singularities are zero volume areas but due to the numerical tolerances they have some thickness and, thus,the probability of having points on this almost singular regions is not null.

A point on a singularity blocks the extension of branches. If the number of times this happens is large, we could fail to solve a problem because we can not cross singular regions. A typical solution is to decrease parameter 'epsilon'.

Parameters
arstThe AtlasRRT statistics to modify.

Definition at line 472 of file atlasrrt.c.

References TAtlasRRTStatistics::nSingular.

Referenced by AddBranchToAtlasRRT().

void NewAtlasRRTRandomSample ( TAtlasRRTStatistics arst)

Counts the number of random samples generated.

Definition at line 477 of file atlasrrt.c.

References TAtlasRRTStatistics::nRandom.

Referenced by AtlasRRTSample().

void NewAtlasRRTSampleRejection ( TAtlasRRTStatistics arst)

Counts the number os random samples rejected.

Definition at line 482 of file atlasrrt.c.

References TAtlasRRTStatistics::nRejections.

Referenced by AtlasRRTSample(), and AtlasRRTValidateSample().

void NewAtlasRRTCollisionCheck ( TAtlasRRTStatistics arst)

New collision check.

Definition at line 487 of file atlasrrt.c.

References TAtlasRRTStatistics::nCollisionChecks.

Referenced by NewTemptativeSample().

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().

double AddBranchToAtlasRRT ( Tparameters pr,
unsigned int  it,
unsigned int  addMode,
boolean  revisitCharts,
boolean  checkCollisions,
boolean  onManifold,
boolean  explorationSample,
double  maxLength,
unsigned int  i_near,
double *  origRandSample,
double *  goalSample,
void *  st,
unsigned int *  lastSampleID,
boolean reachedQRand,
boolean reachedGoal,
unsigned int *  ns,
double ***  path,
double(*)(Tparameters *, boolean, double *, void *)  costF,
void *  costData,
Tatlasrrt ar 
)

Add a new branch to the AtlasRRT. The branch is extened until the given goal is reached or until a problem in the extension occurs (obstacles, convergence problems, singularities, etc.).

Note that this function is also used to check if a given sample can be connected to the tree but without actually adding a branch to the tree. In this case, though the charts necessaries to connect the tree and the given sample are generated if not ye defined (i.e., while we do not add samples we might add charts. This extension-without-addition is basically used for RRT*.

Please, see ConnectSamplesChart for a simpler version of this method.

Parameters
prThe set of parameters.
itCurrent iteration. Used to timestamp new charts and to stop the branch extension if entering previously explored areas.
addModeSelect whether to add all generated nodes along the branck, only the last one or none of them. (ADD_ALL, ADD_LAST, ADD_LAST_NO_REP, ADD_NONE).
revisitChartsIf FALSE, new branches do not enter other charts in the same tree. Typically FALSE if we are just extending one tree towards a random point and TRUE if we are connecting the two trees (or two samples already in the RRT).
checkCollisionsTRUE if we have to check for collisions while extending the tree.
onManifoldTRUE if the goal of the tree is on the manifold. In this case the goal is typically another node in the RRT.
explorationSampleTRUE if the sample is out of the area properly covered by the atlas. These kind of samples potentially create new charts and we avoid them to enter pre-existing charts to avoid branch crossing.
maxLengthMaximum length of the branch. Set to 0 to use the default maximum lenght (2*distance q_near-q_rand). This is used when testing the possible connection between samples withing a given box.
i_nearIndex of the sample in the RRT from where to start the extension
origRandSampleThe targed sample. This must be given in the simplified system. Caution!! this sample is modified inside the function.
goalSampleGlobal goal for the RRT (also given in simplified form). Use NULL if there is no goal (we are defining an EXPLORATION_RRT).
stPointer to a statistics structure only used if GET_ATLASRRT_STATISTICS is set to 1. Otherwise NULL.
lastSampleIDIdentifier given to the last sample added to the RRT. If no sample is added to the tree, it returns i_near (the identifier of the sample from which to extend the tree).
reachedQRand(Output) TRUE if the given randSample is reached. Note many times q_rand is not on the manifold. In this case there is no way to reach q_rand. This only makes sense if q_rand is on the manifold (like when trying to connect two points on the manifold).
reachedGoal(Output) TRUE if the given goalSample is reached.
nsNumber of samples in the branch. Only updated if different from NULL.
pathSamples along the path between the two given points. Only used if different from NULL. This is typically used to recover paths on trees. In this case we do not want to store the nodes in the tree (i.e. addMode is ADD_NONE) but to retrive them.
costFThe cost Function used in case of Transition-based AtlasRRT. NULL for other AtlasRRT exploration.
costDataThe last parameter for the cost function. Only used if the cost function is different from NULL.
arthe AtlasRRT to extend.
Returns
The lenght of the just added branch if the randSample is reached or INF if not.

Definition at line 1300 of file atlasrrt.c.

References AccumulateVector(), ADD_ALL, ADD_LAST, ADD_LAST_NO_REP, AddChart2AtlasRRT(), AddSample2AtlasRRT(), AddSample2Samples(), AdjustDynamicDomain(), Tatlasrrt::atlas, TSampleInfo::c, CT_DELTA, CT_EPSILON, CT_MAX_NODES_RRT, CT_SAMPLING, DifferenceVectorTopology(), Distance(), DistanceTopology(), Error(), FALSE, TSampleInfo::generateChart, GetAtlasChart(), GetParameter(), GetRRTNN(), GetRRTNodeTree(), TSampleInfo::id, InitSamples(), Tatlasrrt::k, TSampleInfo::lsc, Tatlasrrt::m, Manifold2Chart(), NEW, NewAtlasRRTBlockBySingularity(), NewAtlasRRTChart(), NewAtlasRRTInitChartError(), NewAtlasRRTNoConnectToParent(), NewAtlasRRTQrandReached(), NewAtlasRRTSample(), NewAtlasRRTStepReduction(), NewAtlasRRTTooFar(), NewAtlasRRTTooLong(), NewTemptativeSample(), NO_UINT, Tatlasrrt::ns, TSampleInfo::pc, PointTowardRandSample(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, Tatlasrrt::tp, TRUE, and Warning().

Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRT(), AtlasTRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().

void NewTemptativeSample ( Tparameters pr,
unsigned int  it,
unsigned int *  nChanges,
boolean  revisitCharts,
boolean  chartCreated,
boolean  checkCollisions,
boolean  onManifold,
TSampleInfo currentSampleInfo,
double *  currentParam,
double *  origRandSample,
unsigned int *  randChart,
double *  randParam,
double *  randSample,
double  currentDelta,
double  d,
double *  deltaParam,
unsigned int *  nextChart,
double *  nextParam,
double *  nextSample,
void *  st,
double(*)(Tparameters *, boolean, double *, void *)  costF,
void *  costData,
double *  cost,
boolean blocked,
boolean inCollision,
boolean valid,
Tatlasrrt ar 
)

Generates a new sample interpolating in between q_near and q_rand and projecting to the manifold.

This is used to extend a AtlasRRT branch.

This function consideres the possible transition to a neighbouring chart of the new sample. If this transition occurs, the random point is also translated to the new chart.

Parameters
prThe set of parameters.
itCurrent iteration. Used to stop the branch extension if we enter in an existing chart from a just created one.
nChangesNumber of times the branch moved to an existing chart.
revisitChartsIf FALSE, new branches do not enter other charts in the same tree. Typically FALSE if we are just extending one tree towards a random point and TRUE if we are connecting the two trees (or two samples already in the RRT).
chartCreatedTRUE if the branch under extension already created a chart.
checkCollisionsTRUE if next sample has to be collision free.
onManifoldTRUE if the goal sample in on the manifold (typically another node of the tree).
currentSampleInfoInformation relative to the sample from where to generate the next sample.
currentParamParameters of the current sample in the current chart. These can be different from those in currentSampleInfo due to the topology of the ambient space. The ones in currentSampleInfo are 'normalized' (i.e., the smaller possible). The ones in currentParam are not 'normalized'.
randChartChart including the random sample (migh be modified inside).
randParamParameters of the random sample in its chart (migh be modified inside).
randSampleRandom sample in ambient space (migh be modified inside).
origRandSampleThe original random sample. Only used if 'onManifold' is TRUE.
currentDeltaStep size from current sample to the new one.
dDistance to the objective sample.
deltaParamNormal vector giving the direction toward randSample in the current chart.
checkCollisionsTRUE if the next sample has be be collision free.
nextChartIdentifier of the chart including the new sample.
nextParamParameters in the chart of the new sample.
nextSampleNew sample on the manifold.
stStatistics about chart construction.
costFThe cost Function used in case of Tansition-based AtlasRRT.
costDataThe last parameter for the cost funcion.
costthe cost of the new sample if any
blockedIs set to TRUE if a new sample can not be reached for any reason (domain, entering a pre-defined chart, etc).
inCollisionIs set to TRUE if an obstacle of an environment limit is found when determining the new sample.
validIs set to FALSE if the new sample is outside the applicablity area of the current chart. This should induce the generation of a new chart.
arThe atlas RRT.

Definition at line 1702 of file atlasrrt.c.

References Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Tatlasrrt::ce, Chart2Manifold(), Tatlasrrt::ci, CloseCharts(), CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_SAMPLING, DeleteChart(), DetermineChartNeighbour(), Distance(), DistanceTopology(), Tatlasrrt::e, EnlargeChart(), FALSE, GetAtlasChart(), GetParameter(), TSampleInfo::id, InitChart(), InsideChartPolytope(), Tatlasrrt::J, Tatlasrrt::k, Tatlasrrt::m, Manifold2Chart(), NEW, NewAtlasRRTCollision(), NewAtlasRRTCollisionCheck(), NewAtlasRRTDirLargeCurvature(), NewAtlasRRTInitChartError(), NewAtlasRRTLargeCurvature(), NewAtlasRRTNoConvergentStep(), NewAtlasRRTNotInDomain(), NewAtlasRRTOutOfChart(), NewAtlasRRTOverlap(), NewAtlasRRTQrandReached(), NewAtlasRRTStep(), Norm(), PointInBoxTopology(), PointTowardRandSample(), Tatlasrrt::r, Tatlasrrt::rrt, TSampleInfo::s, SumVectorScale(), Tatlasrrt::tp, TransitionTestRRT(), TChartInfo::tree, TRUE, Tatlasrrt::w, and Warning().

Referenced by AddBranchToAtlasRRT().

double AddSample2AtlasRRT ( Tparameters pr,
unsigned int  tree,
unsigned int *  currentID,
unsigned int  nextChart,
double *  nextParam,
double *  nextSample,
double  dp,
double  cost,
Tatlasrrt ar 
)

Adds a sample to the AtlasRRT connected to a given sample already in the tree.

Parameters
prThe set of parameteres.
treeIn which tree (START2GOAL or GOAL2START) to insert the sample. Only considered if the altasRRT is bidirectional.
currentIDIdentifier of the current sample. Parent of the sample to add.
nextChartChart for the new sample.
nextParamParameters in the chart of the sample to add.
nextSampleSample to add.
dpDistance to parent sample.
costCost for this node. This can be different thinks: the cost of the node evaluated with a user-given funcion (in TRRT) or the distance from the root of the tree (in RRT*)
arThe AtlasRRT.
Returns
Distance from the new sample to its parent.

Definition at line 1967 of file atlasrrt.c.

References AddChart2Tree(), AddNodeToRRT(), Tatlasrrt::atlas, TSampleInfo::c, Tatlasrrt::ci, DistanceTopology(), Error(), TSampleInfo::generateChart, GetAtlasChart(), GetRRTNode(), TSampleInfo::id, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, Manifold2Chart(), MEM_DUP, Tatlasrrt::ms, NEW, Tatlasrrt::ns, TSampleInfo::pc, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, and Tatlasrrt::tp.

Referenced by AddBranchToAtlasRRT().

boolean AddChart2AtlasRRT ( Tparameters pr,
unsigned int  tree,
unsigned int  it,
TSampleInfo currentSampleInfo,
boolean intersectParent,
Tatlasrrt ar 
)

Used when a sample triggers the generation of a new chart.

Parameters
prThe set of parameters.
treeThe identifier of the tree to which the sample belongs.
itCurrent iteration. Used to timestamp the charts.
currentSampleInfoInformation relative to the sample from where to generate the next sample.
intersectParent(Output) TRUE if the added chart intersects with its parent. Used for debug.
arThe AtlasRRT.
Returns
TRUE if the chart can actually be generated. If not, it is most likely because the given point is at a (almost) singular region.

Definition at line 2104 of file atlasrrt.c.

References AddChart2Tree(), AddTrustedChart2Atlas(), Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::ci, EnlargeChart(), Error(), FALSE, ForceChartCut(), TSampleInfo::generateChart, GetAtlasChart(), GetAtlasNumCharts(), TSampleInfo::id, INIT_NUM_CHARTS, TChartInfo::it, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Manifold2Chart(), Tatlasrrt::mc, MEM_EXPAND, Tatlasrrt::nc, NEW, NO_UINT, TSampleInfo::pc, PopulateWithSamples(), TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, and TRUE.

Referenced by AddBranchToAtlasRRT(), and InitAtlasRRT().

void PopulateWithSamples ( Tparameters pr,
unsigned int  id,
Tatlasrrt ar 
)

After generating a new chart (with identifier id) some of the samples assigned to their neighbours have to be assigned to the new chart.

Parameters
prThe set of parameters.
idIdentifier of the new chart.
arThe atlasRRT.

Definition at line 2267 of file atlasrrt.c.

References AddChart2Tree(), Tatlasrrt::atlas, TSampleInfo::c, ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::ci, CT_EPSILON, DetermineChartNeighbour(), EnlargeChart(), GetAtlasChart(), GetParameter(), GetRRTNodeTree(), Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Manifold2Chart(), NEW, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, and Tatlasrrt::tp.

Referenced by AddChart2AtlasRRT().

void AddChart2Tree ( unsigned int  tree,
unsigned int  chartId,
Tatlasrrt ar 
)

The tree must not contain the chart

Parameters
treeThe identidier for the tree.
chartIdThe identifier for the chart.
arThe AtlasRRT.

Definition at line 2038 of file atlasrrt.c.

References Tatlasrrt::birrt, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, Tatlasrrt::mct1, Tatlasrrt::mct2, MEM_DUP, Tatlasrrt::nct1, Tatlasrrt::nct2, START2GOAL, and TChartInfo::tree.

Referenced by AddChart2AtlasRRT(), AddSample2AtlasRRT(), and PopulateWithSamples().

boolean PointTowardRandSample ( unsigned int  cId,
double  d,
double *  t,
unsigned int  samplingMode,
boolean  onManifold,
double *  origRandSample,
unsigned int *  randChart,
double *  randParam,
double *  randSample,
double *  deltaParam,
Tatlasrrt ar 
)

Determines a unitary vector on the current chart that points towards the random sample. Additionally, project the random sample to the current chart.

Parameters
cIdThe current chart.
dThe distance to the random sample. Should be preserved after projection.
tParameters on the current chart of the point from where to move toward the random sample.
samplingModeThe sampling mode (AMBIENT, KDTREE, TANGENT).
onManifoldTRUE if the goal is on the manifold (typically a point already in the RRT)
origRandSampleThe original random sample. Only used if onManifold is TRUE.
randChartChart on which the random sample is placed. This is mofified inside this function.
randParamParameters on the chart for the random sample. This is mofified inside this function.
randSampleThe random sample. This is modified projecting it to the current chart and moving it so that distance
deltaParamUnitary vector defining the direction from 't' to the random sample.
arThe atlasRRT including all elements.
Returns
TRUE if there is a problem in determining the direction of expansion (i.e., if the expansion is blocked).

Definition at line 2060 of file atlasrrt.c.

References Tatlasrrt::atlas, DifferenceVector(), FALSE, GetAtlasChart(), Tatlasrrt::k, Local2Global(), Manifold2Chart(), Norm(), Normalize(), SumVectorScale(), TANGENT_SAMPLING, and TRUE.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

unsigned int ReconstructAtlasRRTPath ( Tparameters pr,
unsigned int  sID,
double *  pl,
unsigned int *  ns,
double ***  path,
Tatlasrrt ar 
)

Reconstruct the path from the tree root to the sample in the tree closer to the goal. This is typically used to get the targed is reached.

Parameters
prThe set of parameters.
sIDSample to connect to the root of the tree.
plThe length of the output path, if any. If no path is found. this is undefined.
nsNumber of samples in the output path.
pathThe output path.
arThe AtlasRRT structure to query.
Returns
The size of each path step (system variables stored for each step).

Definition at line 2357 of file atlasrrt.c.

References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteSamples(), FALSE, GetRRTParent(), InitSamples(), NO_UINT, ReverseSamples(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TRUE, Tatlasrrt::w, and Warning().

Referenced by AtlasRRTstar().

unsigned int Steps2PathinAtlasRRT ( Tparameters pr,
Tvector steps,
double *  pl,
double *  pc,
unsigned int *  ns,
double ***  path,
Tatlasrrt ar 
)

Defines a path (a collection of close enough samples) from a set of steps defined as nodes in a AtlasRRT.

Parameters
prThe set of parameters.
stepsThe vector of steps to follow.
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 samples in the output path.
pathThe output path.
arThe AtalsRRT structure to query.
Returns
The size of each path step (system variables stored for each step).

Definition at line 2424 of file atlasrrt.c.

References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_EPSILON, DeleteSamples(), FALSE, GetParameter(), GetRRTNodeCost(), GetVectorElement(), InitSamples(), NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TRUE, VectorSize(), Tatlasrrt::w, and Warning().

Referenced by AtlasBiRRTstar().

void SmoothPathInAtlasRRT ( Tparameters pr,
unsigned int  sID,
Tatlasrrt ar 
)

Local optimiziation of the length of the path to a node. Tries to re-wire the tree to minimize the path from the root to a given node.

Parameters
prThe set of parameters.
sIDThe node.
arThe atlasRRT.

Definition at line 2503 of file atlasrrt.c.

References ADD_NONE, AddBranchToAtlasRRT(), FALSE, GetRRTNodeCost(), GetRRTParent(), NEW, NO_UINT, randomMax(), Tatlasrrt::rrt, TSampleInfo::s, SetRRTNodeCost(), SetRRTParent(), Tatlasrrt::si, StepsToRoot(), and TRUE.

unsigned int AddStepToAtlasRRTstar ( Tparameters pr,
unsigned int  it,
boolean  expand2Goal,
unsigned int  i_near,
double *  q_rand,
unsigned int *  id_goal,
double *  goal,
TAtlasRRTStatistics arst,
Tatlasrrt ar 
)

Tries to generate a node from i_near toward q_rand and, if possible, adds it to the AtlasRRT and updates the cost of the new node. Moreover, we check if the goal is actually reached. If so, we add another node to the tree representing the goal.

Parameters
prThe set of parameters.
itCurrent iteration.
expand2GoalTRUE if q_rand is actually the goal.
i_nearNode from which to expand.
q_randTarged for the expansion.
id_goalIdentifier of the goal sample, if any. Otherwise NO_UINT. Only used if goal is not NULL.
goalThe goal sample. NULL if no goal is given.
arstStatistics on the AtlasRRT construction process.
arThe AtlasRRT.

Definition at line 3561 of file atlasrrt.c.

References ADD_LAST, ADD_LAST_NO_REP, AddBranchToAtlasRRT(), CT_EPSILON, DistanceTopology(), Error(), FALSE, GetParameter(), GetRRTNodeCost(), GetRRTParent(), Tatlasrrt::m, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, SetRRTNodeCost(), Tatlasrrt::si, Tatlasrrt::tp, and TRUE.

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

void WireAtlasRRTstar ( 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,
TAtlasRRTStatistics arst,
Tatlasrrt ar 
)

Selects the best parent among all neighbouring nodes for a node just added to the tree.

Parameters
prThe set of parameters.
id_newIdentifier of the last node added to the tree.
i_nearParent of the node just added to the tree.
gammaRadius of search used to determine the neighbours.
nnNumber of neighbours.
nArray with the indices of the neighbouring nodes.
cArray where we store the cost to the neighbours. This is useful in re-wire to avoid recomputing symmetrical costs. (see RRTSTAR_SYMMETRIC_COST).
hHeuristic cost to the goal. Only used if the RRT is in graph mode.
qPriority queue to propagate changes in the tree. Only used the RRT is in graph mode.
tThe trees of the neighbouring nodes. This in only relevant in bi-directional RRTs
arstStatistics on the AtlasRRT construction process.
arThe AtlasRRT.

Definition at line 3623 of file atlasrrt.c.

References ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddElement2Heap(), Tatlasrrt::ambient, ConnectSamplesChart(), CT_EPSILON, Error(), FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, IsRRTGraph(), Tatlasrrt::k, Tatlasrrt::m, NEW, NewAtlasRRTBranch(), NewAtlasRRTNoEmptyBranch(), NewDoublePair(), NO_UINT, Tatlasrrt::parallel, Tatlasrrt::rrt, TSampleInfo::s, SetRRTCostAndParent(), Tatlasrrt::si, Tatlasrrt::tp, TRUE, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and Tatlasrrt::w.

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

void ReWireAtlasRRTstar ( Tparameters pr,
unsigned int  id_new,
double  gamma,
unsigned int  nn,
unsigned int *  n,
double *  c,
Tvector steps,
double *  l,
TAtlasRRTStatistics arst,
Tatlasrrt ar 
)

Uses the last added node to the AtlasRRT to try to reduce the cost of the neighbouring nodes.

Parameters
prThe set of parameters.
id_newIdentifier of the last node added to the tree.
gammaRadius of search used to determine the neighbours.
nnNumber of neighbours.
nArray with the indices of the neighbouring nodes.
cArray where we with the cost to the neighbours. Computed in WireAtlasRRTstar if RRTSTAR_SYMMETRIC_COST is 1.
stepsThe steps forming the solution path.
lCost of the path from start to goal via s1 and s2. Only used in mode==TWO_TREES_WITH_SWAP.
arstStatistics on the AtlasRRT construction process.
arThe AtlasRRT.

Definition at line 3764 of file atlasrrt.c.

References ADD_NONE, AddBranchToAtlasRRT(), ChangeBiRRTSteps(), CT_EPSILON, FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, SetRRTCostAndParent(), Tatlasrrt::si, TRUE, TWO_TREES_WITH_SWAP, and UpdateCostAndTree().

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

void AtlasRRTstarCloseIteration ( unsigned int  it,
unsigned int  id_goal,
double  time,
double  gamma,
double *  times,
double *  costs,
Tatlasrrt ar 
)

Prints information about the AtlasRRT* iteration.

Parameters
itThe current iteration.
id_goalThe identifier of the goal. If not found yet, NO_UINT.
timeElapsed time from the start of the AtlasRRT*
gammaValue of the threshold for nearest neighbours.
timesArray to store information about the execution time per iteration.
costsArray to store information about the cost to the goal per iteration.
arThe AtlasRRT.

Definition at line 3843 of file atlasrrt.c.

References ATLASRRT_VERBOSE, CostToRoot(), GetRRTNodeCost(), Tatlasrrt::nc, NO_UINT, Tatlasrrt::ns, and Tatlasrrt::rrt.

Referenced by AtlasRRTstar().

void AtlasBiRRTstarCloseIteration ( unsigned int  it,
double  l,
double  time,
double  gamma,
double *  times,
double *  costs,
Tatlasrrt ar 
)

Prints information about the Bi-directional AtlasRRT* iteration.

Parameters
itThe current iteration.
lCurrent cost to goal. INF if no path found yet.
timeElapsed time from the start of the AtlasRRT*
gammaValue of the threshold for nearest neighbours.
timesArray to store information about the execution time per iteration.
costsArray to store information about the cost to the goal per iteration.
arThe AtlasRRT.

Definition at line 3878 of file atlasrrt.c.

References ATLASRRT_VERBOSE, INF, Tatlasrrt::nc, and Tatlasrrt::ns.

Referenced by AtlasBiRRTstar().

void SaveAtlasRRTSampleInfo ( FILE *  f,
TSampleInfo si,
Tatlasrrt ar 
)

Saves the information relating a given sample to charts in an AtlasRRT.

Parameters
fThe file where to store the information.
siThe sample information to store.
arThe AtlasRRT.

Definition at line 2808 of file atlasrrt.c.

References TSampleInfo::c, TSampleInfo::generateChart, TSampleInfo::id, Tatlasrrt::k, TSampleInfo::lsc, TSampleInfo::pc, and TSampleInfo::t.

Referenced by SaveAtlasRRT().

void SaveAtlasRRTChartInfo ( FILE *  f,
TChartInfo ci,
Tatlasrrt ar 
)

Saves the information relating a given chart to the samples it contains

Parameters
fThe file where to store the information.
ciThe chart information to store.
arThe AtlasRRT.

Definition at line 2822 of file atlasrrt.c.

References TChartInfo::it, TChartInfo::lc, and TChartInfo::tree.

Referenced by SaveAtlasRRT().

void LoadAtlasRRTSampleInfo ( FILE *  f,
TSampleInfo si,
Tatlasrrt ar 
)

Reads the information relating a given sample to charts in an AtlasRRT.

Parameters
fThe file where to store the information.
siThe sample information to initialize.
arThe AtlasRRT.

Definition at line 2829 of file atlasrrt.c.

References TSampleInfo::c, TSampleInfo::generateChart, GetRRTNode(), TSampleInfo::id, Tatlasrrt::k, TSampleInfo::lsc, NEW, TSampleInfo::pc, Tatlasrrt::rrt, TSampleInfo::s, and TSampleInfo::t.

Referenced by LoadAtlasRRT().

void LoadAtlasRRTChartInfo ( FILE *  f,
TChartInfo ci,
Tatlasrrt ar 
)

Reads the information relating a given chart to the samples it contains

Parameters
fThe file where to store the information.
ciThe chart information to initialize.
arThe AtlasRRT.

Definition at line 2844 of file atlasrrt.c.

References TChartInfo::it, TChartInfo::lc, and TChartInfo::tree.

Referenced by LoadAtlasRRT().

void PlotQrand ( Tparameters pr,
char *  prefix,
unsigned int  inear,
unsigned int  c_rand,
double *  t_rand,
double *  q_rand,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tatlasrrt ar 
)

Small debug utility to plot the position of q_rand.

Parameters
prThe set of parameters.
prefixPrefix used to generate the plot file name.
inearNearest neighbour.
c_randThe chart where q_rand was selected.
t_randThe parameters for q_rand
q_randThe point in ambient coordinates
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.

Definition at line 2652 of file atlasrrt.c.

References Tatlasrrt::atlas, Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetAtlasChart(), GetChartCenter(), GetFileFullName(), InitPlot3d(), Local2Global(), Tatlasrrt::m, NEW, NewColor(), PLOT3D_EXT, PlotVect3d(), TSampleInfo::s, Tatlasrrt::si, StartNew3dObject(), Tatlasrrt::tp, and Tatlasrrt::w.

void PlotConnection ( Tparameters pr,
char *  prefix,
unsigned int  target,
unsigned int  near,
unsigned int  end,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tatlasrrt ar 
)

Graphical representation of the attempt to connect two trees.

Parameters
prThe set of parameters.
prefixPrefix used to generate the plot file name.
targetPoint to reach in the tree
nearPoint from where to extend the connection branch
endPoint finally reached in the connection attempt.
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.

Definition at line 2749 of file atlasrrt.c.

References Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetFileFullName(), InitPlot3d(), NewColor(), PLOT3D_EXT, PlotVect3d(), TSampleInfo::s, Tatlasrrt::si, StartNew3dObject(), and Tatlasrrt::w.

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().

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().

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().

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().

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.

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