atlasrrt.h File Reference

Introduction

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   (_DEBUG>1?_DEBUG-1:0)
	Vebosity of the AtlasRRT operations. More...
#define	REVISIT_CHARTS   1
	Revisit charts. More...
#define	REPEAT_CHARTS   0
	Repeat charts. More...
#define	RANDOM_INT_TIME   1
	Randomize the integration time for each planning step. More...
#define	RELINEALIZE_DYNAMICS   2
	Re-compute the LQR at each new chart. More...
#define	DYNAMICS_ADD_ALL   1
	Add all small steps along a dynamic branch. More...
#define	RANDOMIZED_LQR   (SATURATE_ACTIONS?0.01:0.0)
	LQR randomization. More...
#define	GET_ATLASRRT_GLOBAL_CURV_CHECK   0
	Set this to one to check the global curvature tolerences between charts. More...
#define	REJECT_USING_DYNAMIC_CONE   0
	Reject samples if they are not in the dynamic cone of the near sample. More...
#define	ATLASRRT_DYNAMIC_CONNECT   1
	Use the RRT connect strategy for problems wiht dynamics. More...
#define	DEBUG_SIMULATION   0
	Collect information when simulating. 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	NN_IN_CHART   0
	Set to 1 if nearest neighbours are searched in the chart including q_rand. More...
#define	NN_IN_NEIGHBOUR_CHART   0
	Look for nearest neighbour in neighbouring 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, boolean *onManifold, 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, unsigned int c_rand, double *t_rand, 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...
void	RefineTrajectory (Tparameters *pr, boolean id, unsigned int *ns, double ***path, double ***actions, double **times, Tatlasrrt *ar)
	Increases the resolution of a trajectory. More...
void	AtlasRRTSimulate (Tparameters *pr, unsigned int da, double *u, unsigned int steps, unsigned int *ns, double ***path, double ***actions, double **times, Tatlasrrt *ar)
	Dynamic simulation with a given action. More...
boolean	AtlasRRT (Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, unsigned int *da, double ***actions, double **times, 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...
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	GetRRTNNInChart (Tparameters *pr, unsigned int t2g, unsigned int tree, unsigned int chartId, double *q_rand, double *d, Tatlasrrt *ar)
	Selects the nearest neighbour from the random point in a given chart. More...
unsigned int	Time2GoNNToTree (Tparameters *pr, unsigned int tree, unsigned int goal, Tatlasrrt *ar)
	Time to go nearest neighbour of a tree to a given sample. More...
void	PlotAtlasRRT (char *prefix, int argc, char **arg, Tparameters *pr, unsigned int xID, unsigned int yID, unsigned int zID, double *p1, double *p2, Tatlasrrt *ar)
	Pots a projection of a Atlas-RRT. 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	PrintAtlasRRTDefines (FILE *f)
	Prints defines. More...
void	DeleteAtlasRRT (Tatlasrrt *ar)
	Destructor. More...

Macro Definition Documentation

ATLASRRT_VERBOSE

#define ATLASRRT_VERBOSE   (_DEBUG>1?_DEBUG-1: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 32 of file atlasrrt.h.

REVISIT_CHARTS

#define REVISIT_CHARTS   1

If set, RRT branches can enter previously existing charts (charts in the atlas before starting the branch).

This option is only taken into account when extending an RRT (towards and randomly generated sample) and not when aiming at a sample already in the RRT (when

Definition at line 44 of file atlasrrt.h.

REPEAT_CHARTS

#define REPEAT_CHARTS   0

If set, RRT branches can re-enter previously traversed charts (by the same branch).

Definition at line 51 of file atlasrrt.h.

RANDOM_INT_TIME

#define RANDOM_INT_TIME   1

If set to 1, the integration time for each planning step is set at random with a maximum set in the parameters (CT_INTEGRATION_TIME).

Definition at line 59 of file atlasrrt.h.

RELINEALIZE_DYNAMICS

#define RELINEALIZE_DYNAMICS   2

If set, we re-compute the LQR policy after the creation of a new chart. In this way, the linearization errors may be mitigated and the system may converge faster to the random state (or, eventually, the goal).

Possible values:

• 0: We do not re-compute the LQR policy when moving to a new chart.
• 1: We re-compute the policy when creating a new chart.
• 2: We re-compute the policy also when moving to an existing chart.

Definition at line 73 of file atlasrrt.h.

DYNAMICS_ADD_ALL

#define DYNAMICS_ADD_ALL   1

In dynamic branches we can add just the last step or all intermediate steps. This second option generates more nodes in the tree but provides more points for future three extension.

• When 0, only the last state of the simulation is stored in the three. This is the only state that can be used for future three extensions. The rest of the states generated in the simulation are stored in the RRT edge.
• When 1, we add the last state in the simulation plus all the states where a chart is generated.
• When 2, we add all the simulated states in the RRT. This enlarges the places where we can extend an RRT and may be necessary in hihghly dynamic problems.

Definition at line 93 of file atlasrrt.h.

RANDOMIZED_LQR

#define RANDOMIZED_LQR   (SATURATE_ACTIONS?0.01:0.0)

If set (i.e., larger than 0) some of the action are selected at random instead of using the LQR approach. This only holds when using LQR, of course. The value is in the range [0,1] and gives the probability of executing random actions (e.g., 0.1 means a 10% of chance of selecting the action at random).

Definition at line 104 of file atlasrrt.h.

GET_ATLASRRT_GLOBAL_CURV_CHECK

#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 112 of file atlasrrt.h.

REJECT_USING_DYNAMIC_CONE

#define REJECT_USING_DYNAMIC_CONE   0

Reject samples if they are not in the dynamic cone of the near sample.

Definition at line 119 of file atlasrrt.h.

ATLASRRT_DYNAMIC_CONNECT

#define ATLASRRT_DYNAMIC_CONNECT   1

If set, the RRT for problems with dynamics implement the connect stratey instead of the extend, i.e., braches are extended as far as they get closer to the random sample.

Definition at line 128 of file atlasrrt.h.

DEBUG_SIMULATION

#define DEBUG_SIMULATION   0

If set we collect statistics during the simulation. This is only used for debugging purposes and mainly to check the number of Broyden iterations.

Definition at line 137 of file atlasrrt.h.

GET_ATLASRRT_STATISTICS

#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 145 of file atlasrrt.h.

ADJUST_SA

#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.

Todo:

Set ADJUST_SA as a parameter so that it can be set without re-compiling. This implies to add an 'scale' field to the AtlarRRT structure and to modify some function interfaces and the SaveAtlasRRT (and the load) accordingly.

Definition at line 166 of file atlasrrt.h.

NN_IN_CHART

#define NN_IN_CHART   0

If set to 1 we search for nearest neighbour inside the chart including the random sample.

This should only be done in problems with dynamics and when using the time to go metric.

Definition at line 175 of file atlasrrt.h.

NN_IN_NEIGHBOUR_CHART

#define NN_IN_NEIGHBOUR_CHART   0

If NN_IN_CHART is set, we can additionally look for the nearest neighbours in the neighbouring chart just activating this flag.

Definition at line 183 of file atlasrrt.h.

Function Documentation

InitAtlasRRTStatistics()

void InitAtlasRRTStatistics

(

TAtlasRRTStatistics *

arst

)

Init the Atlas RRT statistics.

Parameters

arst	The AtlasRRT statistics to initialize.

Definition at line 1086 of file atlasrrt.c.

References TAtlasRRTStatistics::dQrand, TAtlasRRTStatistics::dQrandReached, TAtlasRRTStatistics::itB, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nAverageNeighbours, TAtlasRRTStatistics::nB, TAtlasRRTStatistics::nBranch, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nChartBlocked, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nDynamicErrors, TAtlasRRTStatistics::nDynamicExtension, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nImpossible, TAtlasRRTStatistics::nImprovedDynamicExtension, TAtlasRRTStatistics::nIntTimeReached, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nLQRComputePolicy, TAtlasRRTStatistics::nLQRPolicy, TAtlasRRTStatistics::nNoAction, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoDir, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNoPolicy, TAtlasRRTStatistics::nNoReplay, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections1, TAtlasRRTStatistics::nRejections2, TAtlasRRTStatistics::nRejections3, TAtlasRRTStatistics::nRejections4, TAtlasRRTStatistics::nRepeatedChart, TAtlasRRTStatistics::nRepeatedSample, TAtlasRRTStatistics::nRevisitedChart, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, TAtlasRRTStatistics::nToptIncrements, TAtlasRRTStatistics::nTransTestFailed, TAtlasRRTStatistics::nTreeConnection, TAtlasRRTStatistics::satAction, and TAtlasRRTStatistics::topt.

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

AccumulateAtlasRRTStatistics()

void AccumulateAtlasRRTStatistics	(	TAtlasRRTStatistics *	arst1,
		TAtlasRRTStatistics *	arst2
	)

Accumulates two sets of Atlas RRT statistics. This is used to accumulate statistics when executing a long sequence of experiments.

Parameters

arst1	The atlas RRT statistics to accumulate.
arst2	The atlas RRT statistics where to accumulate.

Definition at line 1405 of file atlasrrt.c.

References TAtlasRRTStatistics::dQrand, TAtlasRRTStatistics::dQrandReached, TAtlasRRTStatistics::itB, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nAverageNeighbours, TAtlasRRTStatistics::nB, TAtlasRRTStatistics::nBranch, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nChartBlocked, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nDynamicErrors, TAtlasRRTStatistics::nDynamicExtension, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nImpossible, TAtlasRRTStatistics::nImprovedDynamicExtension, TAtlasRRTStatistics::nIntTimeReached, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nLQRComputePolicy, TAtlasRRTStatistics::nLQRPolicy, TAtlasRRTStatistics::nNoAction, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoDir, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNoPolicy, TAtlasRRTStatistics::nNoReplay, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections1, TAtlasRRTStatistics::nRejections2, TAtlasRRTStatistics::nRejections3, TAtlasRRTStatistics::nRejections4, TAtlasRRTStatistics::nRepeatedChart, TAtlasRRTStatistics::nRepeatedSample, TAtlasRRTStatistics::nRevisitedChart, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, TAtlasRRTStatistics::nToptIncrements, TAtlasRRTStatistics::nTransTestFailed, TAtlasRRTStatistics::nTreeConnection, TAtlasRRTStatistics::satAction, and TAtlasRRTStatistics::topt.

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

PrintAtlasRRTStatistics()

void PrintAtlasRRTStatistics	(	Tatlasrrt *	ar,
		TAtlasRRTStatistics *	arst
	)

Prints a report about the collected atlasRRT statistics.

Parameters

ar	The AtlasRRT (might be NULL).
arst	The AtlasRRT statistics to print.

Definition at line 1482 of file atlasrrt.c.

References Tatlasrrt::atlas, TAtlasRRTStatistics::dQrand, TAtlasRRTStatistics::dQrandReached, DynamicDomainRRT(), Tatlasrrt::dynamics, GetAtlasChart(), GetChartSamplingRadius(), GetDynamicDomainRadius(), INF, TAtlasRRTStatistics::itB, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nAverageNeighbours, TAtlasRRTStatistics::nB, TAtlasRRTStatistics::nBranch, Tatlasrrt::nc, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nChartBlocked, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nDynamicErrors, TAtlasRRTStatistics::nDynamicExtension, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nImpossible, TAtlasRRTStatistics::nImprovedDynamicExtension, TAtlasRRTStatistics::nIntTimeReached, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nLQRComputePolicy, TAtlasRRTStatistics::nLQRPolicy, TAtlasRRTStatistics::nNoAction, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoDir, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNoPolicy, TAtlasRRTStatistics::nNoReplay, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections1, TAtlasRRTStatistics::nRejections2, TAtlasRRTStatistics::nRejections3, TAtlasRRTStatistics::nRejections4, TAtlasRRTStatistics::nRepeatedChart, TAtlasRRTStatistics::nRepeatedSample, TAtlasRRTStatistics::nRevisitedChart, Tatlasrrt::ns, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, TAtlasRRTStatistics::nToptIncrements, TAtlasRRTStatistics::nTransTestFailed, TAtlasRRTStatistics::nTreeConnection, Tatlasrrt::parallel, Tatlasrrt::rrt, TAtlasRRTStatistics::satAction, and TAtlasRRTStatistics::topt.

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

DeleteAtlasRRTStatistics()

void DeleteAtlasRRTStatistics

(

TAtlasRRTStatistics *

arst

)

Deletes the atlasRRT statistics object.

Parameters

arst	The AtlasRRT statistics to delete.

Definition at line 1822 of file atlasrrt.c.

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

InitAtlasRRT()

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

pr	The set of parameters.
parallel	TRUE 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.
ps	A point on the manifold from where to start the RRT. The RRT root. This is a sample including only system variables.
mode	Mode for the RRT: ONE_TREE, TWO_TREES, TWO_TREES_WITH_SWAP.
graph	If 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*.
pg	The goal for the planning. Only used if the AtlasRRT is bidirectional.
w	The base type with the equations (and possibly obstacles).
ar	The Atlas-RRT to create.

Definition at line 5667 of file atlasrrt.c.

References ActionSpaceDimension(), 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_DYNAMIC_GOAL_ERROR, CT_DYNAMICS, CT_E, CT_N_DOF, CT_N_DYNAMIC_ACTIONS, CT_R, Tatlasrrt::da, Tatlasrrt::dge, Tatlasrrt::ds, Tatlasrrt::dynamics, Tatlasrrt::e, Error(), EXPLORATION_RRT, FALSE, TSampleInfo::generateChart, GetAtlasNumCharts(), GetBoxNIntervals(), GetParameter(), GetRRTNode(), GetRRTNumNodes(), GetRRTTopology(), GetRRTWeights(), GOAL2START, TSampleInfo::id, INF, INIT_NUM_CHARTS, INIT_NUM_SAMPLES_RRT, InitAtlasFromPoint(), InitDynamicSpace(), 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, Tatlasrrt::nda, 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, Tatlasrrt::w, and Tatlasrrt::weights.

Referenced by main().

AtlasRRTSample()

boolean AtlasRRTSample	(	Tparameters *	pr,
		unsigned int	samplingMode,
		unsigned int	it,
		unsigned int	tree,
		double *	goal,
		double	scale,
		boolean *	exploration,
		boolean *	onManifold,
		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

pr	The set of parameters.
samplingMode	Policy 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
it	Current iteration.
tree	The tree to use in the sampling. Only used if mode is KDTREE_SAMPLING.
goal	Goal 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.
scale	The global scale factor for the sampling areas of the charts.
exploration	Set to TRUE if samplingMode is TANGENT_SAMPLING and the random sample is in the outer parts of the chart used for sampling.
onManifold	TRUE if the sample is on the manifold.
c_rand	The index of the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
t_rand	The parameters on the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
q_rand	Space to store the random sample.
arst	Atlas statistics. This is only used we are repeating experiments. Otherwise is is NULL.
ar	AtlasRRT to use.

Returns

TRUE if the goal is used as random sample.

Definition at line 5866 of file atlasrrt.c.

References AMBIENT_SAMPLING, Tatlasrrt::atlas, Tatlasrrt::birrt, Chart2Manifold(), CS_WD_SIMP_INEQUALITIES_HOLD, DecreaseChartSamplingRadius(), Error(), FALSE, GetAtlasChart(), IncreaseChartSamplingRadius(), INF, Tatlasrrt::J, Tatlasrrt::k, KDTREE_SAMPLING, Tatlasrrt::m, NEW, NewAtlasRRTRandomSample(), NewAtlasRRTSampleRejection1(), NO_UINT, Norm(), Tatlasrrt::r, randomDouble(), RandomPointInAtlasTree(), Tatlasrrt::rrt, RRTSample(), TANGENT_SAMPLING, Tatlasrrt::tp, TRUE, and Tatlasrrt::w.

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

AtlasRRTValidateSample()

boolean AtlasRRTValidateSample	(	Tparameters *	pr,
		unsigned int	c_rand,
		double *	t_rand,
		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

pr	The set of parameters.
c_rand	Chart used to generate the random sample.
t_rand	The parameters of the random sample in the chart.
q_rand	The random sample.
tree	The tree where to look for nearest neighbours.
expand2goal	TRUE if the random sample is the goal.
lastNN2Goal	Last 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.
goal	The goal.
l	The shortest path from start to goal so far. Only used for RRT*-like algorithms and it only has effect if it is not INF.
h	A lower estimate of the distance between the random sample and the goal. This is only relevant in RRT*-like algorhtms.
i_near	The nearest neighbour in the RRT.
arst	Statistics on the AtlasRRT construction process (only used if GET_RRT_STATISTICS is set).
ar	AtlasRRT to use.

Returns

TRUE if the random sample is valid.

Definition at line 5974 of file atlasrrt.c.

References Tatlasrrt::atlas, TSampleInfo::c, Chart2Manifold(), ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::da, DistanceTopology(), Tatlasrrt::ds, Tatlasrrt::dynamics, Error(), FALSE, GeneralDotProduct(), GetAtlasChart(), GetChartTangentSpace(), GetRRTNN(), GetRRTNNInChart(), Getxdot(), HEURISTIC_RRT_STAR, InDynamicDomain(), INF, Tatlasrrt::J, Tatlasrrt::k, Local2Global(), Tatlasrrt::m, Manifold2Chart(), NEW, NewAtlasRRTSampleRejection2(), NewAtlasRRTSampleRejection3(), NewAtlasRRTSampleRejection4(), NEWZ, NO_UINT, Norm(), Tatlasrrt::ns, PrintVector(), randomDouble(), Tatlasrrt::rrt, RRTBlockedProb(), TSampleInfo::s, Tatlasrrt::si, SubtractVector(), TSampleInfo::t, T2G_METRIC_NN_EXTEND, TMatrixVectorProduct(), Tatlasrrt::tp, and TRUE.

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

RefineTrajectory()

void RefineTrajectory	(	Tparameters *	pr,
		boolean	id,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		Tatlasrrt *	ar
	)

In problems with dynamics only the end-point of the simulated RRT branches are added to the tree. Thus, the resolution of the resulting trajectories are sometimes too coarse. This function simulates the trajectories between the steps in the trajectory to increase the time resolution of the trajectory. In this way, the animations are smoother.

Parameters

pr	The set of parameters.
id	TRUE to use inverse dynamics to adjust the actions.
ns	The number of steps in the trajectory.
path	The states in the trajectory.
actions	The actions executed along the trajectory.
times	The time for each action.
ar	The atlas RRT.

Definition at line 6177 of file atlasrrt.c.

References AddStep2Trajectory(), Tatlasrrt::ambient, ConnectDynamicStates(), ConnectDynamicStatesID(), CT_DELTA, Tatlasrrt::da, DeleteTrajectory(), Tatlasrrt::ds, FALSE, GetParameter(), INF, InitTrajectory(), Tatlasrrt::J, Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::tp, and Tatlasrrt::w.

Referenced by AtlasRRT(), and main().

AtlasRRTSimulate()

void AtlasRRTSimulate	(	Tparameters *	pr,
		unsigned int	da,
		double *	u,
		unsigned int	steps,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		Tatlasrrt *	ar
	)

Dynamic simulation from the first point in the atlasrrt (it is supposed to include only one point) applying a given action for the integration time fixed in the paremeter structure.

Parameters

pr	The set of parameters.
da	Dimension of the action space. Number of elements in u and in each vector in actions.
u	The action to apply.
steps	Steps to simulate (each of time INTEGRATION_TIME).
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
actions	The action executed at each configuration. In this particular function this is just a repetition of the input action u.
times	The time between each configuration.
ar	The Atlas-RRT.

Definition at line 6270 of file atlasrrt.c.

References ActionSpaceDimension(), AddBranchToAtlasRRT(), AddStep2Trajectory(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_DETECT_BIFURCATIONS, CT_INTEGRATION_TIME, TAtlasRRTBranchConfig::currentMaxIntTime, DeleteAtlasRRTStatistics(), DeleteSamples(), Tatlasrrt::ds, Error(), FALSE, GetParameter(), INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitTrajectory(), TAtlasRRTBranchState::intTime, TAtlasRRTBranchConfig::maxIntTime, TAtlasRRTBranchConfig::maxTopt, Tatlasrrt::nc, Tatlasrrt::nda, Tatlasrrt::ns, PrintAtlasRRTStatistics(), TAtlasRRTBranchState::reachedGoal, TSampleInfo::s, Tatlasrrt::si, SIMULATE_DYNAMICS, TAtlasRRTBranchConfig::topt, and Tatlasrrt::w.

Referenced by main().

AtlasRRT()

boolean AtlasRRT	(	Tparameters *	pr,
		double *	pg,
		double *	time,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		unsigned int *	da,
		double ***	actions,
		double **	times,
		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

pr	The set of parameters.
pg	A 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.
time	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
da	Dimension of the action space. Only used in dynamical systems. Otherwise is 0.
actions	Action executed to reach step in the soluiont. Only used in dynamical systems.
times	Time to reach each state. Only used in dynamical systems.
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used we are repeating experiments. Otherwise is is NULL.
ar	The Atlas-RRT to expand.

Definition at line 6380 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), AddBranchToAtlasDynamicRRT(), AddBranchToAtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, AtlasRRTSample(), AtlasRRTValidateSample(), AverageNumNeighbours(), Tatlasrrt::birrt, TSampleInfo::c, TAtlasRRTBranchConfig::checkCollisions, CONNECT_RRT_DYNAMICS, CONNECT_RRTs, CS_ANY_COLLISION, 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_INTEGRATION_TIME, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, TAtlasRRTBranchConfig::currentMaxIntTime, Tatlasrrt::da, DecreaseChartSamplingRadius(), DeleteAtlasRRTStatistics(), DeleteDynamicBranches(), DeleteStatistics(), DistanceTopology(), Tatlasrrt::dynamics, DYNAMICS_ONE_ACTION_CONNECT, DYNAMICS_ONE_ACTION_EXTEND, Error(), EXPLORATION_RRT, TAtlasRRTBranchConfig::explorationSample, EXTEND_RRT, EXTEND_RRT_DYNAMICS, FALSE, GetAtlasChart(), GetElapsedTime(), GetParameter(), GetRRTNNInBranch(), GOAL2START, IncreaseChartSamplingRadius(), INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitDynamicBranches(), InitStatistics(), Tatlasrrt::k, TAtlasRRTBranchState::lastID, Tatlasrrt::m, TAtlasRRTBranchConfig::maxIntTime, MOV_AVG_DOWN, MOV_AVG_UP, Tatlasrrt::nc, NEW, NewAtlasRRTBranch(), NewAtlasRRTChart(), NewAtlasRRTDistanceQrand(), NewAtlasRRTNoEmptyBranch(), NewAtlasRRTNoEmptyTreeConnection(), NewAtlasRRTSample(), NewAtlasRRTTreeConnection(), NewAverageNumNeighbours(), NO_UINT, Tatlasrrt::ns, TAtlasRRTBranchConfig::onManifold, Tatlasrrt::parallel, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), PrintVector(), randomDouble(), TAtlasRRTBranchState::reachedGoal, RefineTrajectory(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, T2G_METRIC_INSIDE_CONNECT, T2G_METRIC_INSIDE_EXTEND, TANGENT_SAMPLING, Time2GoNNToTree(), Tatlasrrt::tp, TRUE, and Tatlasrrt::w.

Referenced by main().

AtlasTRRT()

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

pr	The set of parameters.
pg	A 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.
time	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
pc	The cost of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
costF	The cost function, if any.
costData	The last parameter of the cost funtion, if any.
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The Atlas-RRT to expand.

Definition at line 6777 of file atlasrrt.c.

References AccumulateAtlasRRTStatistics(), AddBranchToAtlasRRT(), Tatlasrrt::ambient, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, TAtlasRRTBranchConfig::costData, TAtlasRRTBranchConfig::costF, 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(), Tatlasrrt::dynamics, Error(), EXPLORATION_RRT, TAtlasRRTBranchConfig::explorationSample, EXTEND_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), GetTRRTTemperature(), INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitStatistics(), Tatlasrrt::k, TAtlasRRTBranchState::lastID, Tatlasrrt::m, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, TAtlasRRTBranchConfig::onManifold, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), TAtlasRRTBranchState::reachedGoal, Tatlasrrt::rrt, SetRRTNodeCost(), START2GOAL, Tatlasrrt::tp, TRUE, and Tatlasrrt::w.

Referenced by main().

AtlasRRTstar()

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

pr	The set of parameters.
pg	A 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.
times	Optional table to store the time at each iteration. If not used set to NULl at call.
costs	Optional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTime	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The AtlasRRT to expand.

Returns

TRUE if the path exists.

Definition at line 7304 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(), DeleteHeap(), DeleteStatistics(), DistanceTopology(), Tatlasrrt::dynamics, 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().

AtlasBiRRTstar()

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

pr	The set of parameters.
pg	A 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.
times	Optional table to store the time at each iteration. If not used set to NULl at call.
costs	Optional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTime	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The AtlasRRT to expand.

Returns

TRUE if the path exists.

Definition at line 7546 of file atlasrrt.c.

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

Referenced by AtlasRRTstar().

GetAtlasRRTNumNodes()

unsigned int GetAtlasRRTNumNodes

(

Tatlasrrt *

ar

)

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

Parameters

ar	The AtlasRRT to query.

Returns

The number of samples stored in the AtlasRRT so far.

Definition at line 7802 of file atlasrrt.c.

References Tatlasrrt::ns.

Referenced by main().

GetAtlasRRTNumCharts()

unsigned int GetAtlasRRTNumCharts

(

Tatlasrrt *

ar

)

Returns the number of charts in the AtlasRRT.

Parameters

ar	The AtlasRRT to query.

Returns

The number of charts in the AtlasRRT so far.

Definition at line 7807 of file atlasrrt.c.

References Tatlasrrt::nc.

Referenced by main().

RandomPointInAtlasTree()

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

pr	The set of parameters.
tree	The tree.
nm	The indice of selected chart.
scale	The global scale factor for the sampling areas of the charts.
t	The parameters of the random point on the chart.
p	The random point on the manifold.
ar	The atlasRRT to query

Returns

TRUE if the random point could be actually determined.

Definition at line 5375 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().

GetRRTNNInChart()

unsigned int GetRRTNNInChart	(	Tparameters *	pr,
		unsigned int	t2g,
		unsigned int	tree,
		unsigned int	chartId,
		double *	q_rand,
		double *	d,
		Tatlasrrt *	ar
	)

Selects the nearest neighbour from the random point in the chart including the random sample.

Parameters

pr	The set of parameters.
t2g	Not zero if using a time2go metric.
tree	The tree. Only samples of this tree are considered as potential nearest-neighbours.
chartId	The chart including the random sample.
q_rand	The random sample.
d	The distance to the nearest neighbour. At input: minimu distance to considere points. At output: minimum distance to a neighbour.
ar	The atlasRRT to query.

Returns

The identifier of the nearest-neighbour of NO_UINT if no sample is a valid neighbour.

Definition at line 7812 of file atlasrrt.c.

References Tatlasrrt::atlas, Tatlasrrt::birrt, Tatlasrrt::ci, DistanceTopologyMin(), Tatlasrrt::ds, GetAtlasChart(), GetRRTNodeTree(), INF, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Manifold2Chart(), NEW, NO_UINT, PrintVector(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, Time2Go(), and Tatlasrrt::tp.

Referenced by AtlasRRTValidateSample(), and Time2GoNNToTree().

Time2GoNNToTree()

unsigned int Time2GoNNToTree	(	Tparameters *	pr,
		unsigned int	tree,
		unsigned int	goal,
		Tatlasrrt *	ar
	)

Determines the closest sample in a given tree to another sample (in the other tree) using the time2go metric. This gives the point of the tree from where it is more likely to connec to the given sample.

Parameters

pr	The set of parameters.
tree	The tree where to look for nearest neighbours.
goal	The identifier of the sample ot reach from the tree.
ar	The atlas RRT.

Returns

The index of the sample in the tree closer (in time to go metric) the goal sample. Can be NO_UINT if there is no good candidate for connection.

Definition at line 7869 of file atlasrrt.c.

References Tatlasrrt::atlas, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ds, Error(), GetAtlasChart(), GetChartCenter(), GetRRTNNInChart(), GetRRTNodeTree(), GOAL2START, INF, Tatlasrrt::k, Manifold2Chart(), Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, NEWZ, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, T2G_METRIC_NN_CONNECT, Time2Go(), and Tatlasrrt::tp.

Referenced by AtlasRRT().

PlotAtlasRRT()

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

Combines the plot of the Atlas and the RRT.

Parameters

prefix	Prefix for the name of the file where to store the plots.
argc	Number 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.
arg	Arguments 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.
pr	The set of parameters.
xID	The first ambient dimension where to project (in the original system including system vars and dummies).
yID	The second ambient dimension where to project (in the original system including system vars and dummies).
zID	The third ambient dimension where to project (in the original system including system vars and dummies).
p1	Root sample of the RRT (vector with 3 components, i.e., already in the projected space). NULL if not given.
p2	Goal sample of the RRT (vector with 3 components, i.e., already in the projected space). NULL if not given.
ar	The Atlas-RRT to plot.

See also

PlotAtlas, PlotRRT.

Definition at line 7955 of file atlasrrt.c.

References Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Close3dObject(), ClosePlot3d(), CORRECT_POINT_PLOT, CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeleteColor(), DeleteFileName(), FALSE, GetAtlasChart(), GetFileFullName(), GetParameter(), 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().

AtlasRRTMemSize()

unsigned int AtlasRRTMemSize

(

Tatlasrrt *

ar

)

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

Parameters

ar	The atlasRRT.

Returns

The size of the atlasRRT in bytes.

Definition at line 8182 of file atlasrrt.c.

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

Referenced by main().

SaveAtlasRRT()

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

pr	The parameter set.
prefix	Prefix for the name of the files where to store the RRT, the Atlas and the information linking the RRT and the Atlas.
ar	The Atlas-RRT to store.

Definition at line 8192 of file atlasrrt.c.

References Tatlasrrt::atlas, ATLAS_EXT, ATLAS_RRT_EXT, Tatlasrrt::birrt, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CreateFileName(), Tatlasrrt::da, DeleteFileName(), Tatlasrrt::dge, Tatlasrrt::dynamics, 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().

LoadAtlasRRT()

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

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

Parameters

pr	The set of parameters.
prefix	Prefix for the name of the files where to store the RRT the Atlas and the information linking the RRT and the Atlas.
w	The world with the equations defining the manifold where the atlas is defined. This is not stored in the file and must be provided externally.
ar	The atlasRRT to define.

Definition at line 8255 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, Tatlasrrt::da, DeleteFileName(), Tatlasrrt::dge, Tatlasrrt::ds, Tatlasrrt::dynamics, Tatlasrrt::e, Error(), FALSE, GetFileFullName(), GetRRTTopology(), GetRRTWeights(), INF, InitDynamicSpace(), 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, Tatlasrrt::w, and Tatlasrrt::weights.

Referenced by main().

PrintAtlasRRTDefines()

void PrintAtlasRRTDefines

(

FILE *

f

)

Prints the defines in atlasrrt.h

This is used only for debug purposes.

Parameters

f	The file where to store the values.

Definition at line 8359 of file atlasrrt.c.

References ADJUST_SA, ATLASRRT_DYNAMIC_CONNECT, ATLASRRT_VERBOSE, DEBUG_SIMULATION, DYNAMICS_ADD_ALL, GET_ATLASRRT_GLOBAL_CURV_CHECK, GET_ATLASRRT_STATISTICS, NN_IN_CHART, NN_IN_NEIGHBOUR_CHART, PrintAtlasDefines(), PrintDynamicsDefines(), PrintRRTDefines(), RANDOM_INT_TIME, RANDOMIZED_LQR, REJECT_USING_DYNAMIC_CONE, RELINEALIZE_DYNAMICS, REPEAT_CHARTS, and REVISIT_CHARTS.

Referenced by main().

DeleteAtlasRRT()

void DeleteAtlasRRT

(

Tatlasrrt *

ar

)

Deletes the information stored in the RRT.

Parameters

ar	The RRT to delete.

Definition at line 8382 of file atlasrrt.c.

References Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, DeleteAtlas(), DeleteBox(), DeleteDynamicSpace(), DeleteJacobian(), DeleteRRT(), Tatlasrrt::ds, Tatlasrrt::dynamics, Tatlasrrt::J, Tatlasrrt::nc, Tatlasrrt::ns, Tatlasrrt::parallel, Tatlasrrt::rrt, Tatlasrrt::si, and TSampleInfo::t.

Referenced by main().