Detailed Description

Definition of a rrt on a point on a manifold. This is implemented for comparison with atlas and, eventually to complement atlas.

See Also: Trrt,rrt.c.

Definition in file rrt.h.

Data Structures
struct	TRRTStatistics
	Statistics on the RRT construction. More...

struct	TRRTStep
	Step in a solution path. More...

struct	TRRTSampleInfo
	Information for each sample in a RRT. More...

struct	Trrt
	A RRT on a manifold. More...

Macros
#define	RRT_VERBOSE 0
	Vebosity of the RRT operations. More...

#define	GET_RRT_STATISTICS 1
	Set this to one to gather statistics of RRT construction. More...

#define	HEURISTIC_RRT_STAR (0)
	Whether to use an heuristic in AtlarRRT*. More...

#define	RRTSTAR_UPDATE_COSTS 0
	On the fly update of the cost in the RRTstar. More...

#define	RRTSTAR_SYMMETRIC_COST 1
	TRUE (or 1) if cost(a,b)=cost(b,a) More...

#define	TEMPERATURE_INIT 0.001
	Initial temperature used in Transition based rrt searches.

#define	EXPLORATION_RRT 0
	Select between exploration and goal driven RRT. More...

#define	RRT_PLOT_NODES 1
	Selects the way RRTs are plotted. More...

#define	RRT_NN_TOPOLOGY 1
	Set to 1 to use the topology in the search for nearest-neighbours. More...

#define	INIT_NUM_SAMPLES_RRT 100
	Initial room for samples of an rrt. More...

#define	ONE_TREE 0
	One of the modes for the RRT. More...

#define	TWO_TREES 1
	One of the modes for the RRT. More...

#define	TWO_TREES_WITH_SWAP 2
	One of the modes for the RRT. More...

#define	NOTREE 0
	Used for samples not assigned to any tree. More...

#define	START2GOAL 1
	One of the possible trees. More...

#define	GOAL2START 2
	One of the possible trees. More...

#define	BOTHTREES (START2GOAL\|GOAL2START)
	Used for samples in both trees. More...

Functions
void	CopyRRTStep (void a, void b)
	Copy constructor for RRTSteps. More...

void	InitRRTStatistics (TRRTStatistics *rst)
	Init the RRT statistics. More...

void	AccumulateRRTStatistics (TRRTStatistics rst1, TRRTStatistics rst2)
	Accumulates two sets of RRT statistics. More...

void	PrintRRTStatistics (Trrt rrt, TRRTStatistics rst)
	Prints the summary of RRT statistics. More...

void	DeleteRRTStatistics (TRRTStatistics *rst)
	Destructor. More...

void	InitRRT (Tparameters pr, boolean parallel, boolean simp, double ps, unsigned int mode, boolean graph, double pg, unsigned int m, TAtlasBase w, Trrt *rrt)
	Defines a RRT from a given point. More...

double	GetTRRTTemperature (Trrt *rrt)
	Temperature of the T-RRT. More...

boolean	IsRRTGraph (Trrt *rrt)
	Identifies RRT with a graph structure. More...

boolean	RRTSample (unsigned int samplingMode, unsigned int tree, double goal, double q_rand, TRRTStatistics rst, Trrt rrt)
	Generates a random sample to expand the RRT. More...

boolean	RRTValidateSample (Tparameters pr, double q_rand, unsigned int tree, boolean expand2goal, double goal, double l, double h, unsigned int i_near, TRRTStatistics rst, Trrt *rrt)
	Validates a sample generate with RRTSample. More...

unsigned int	GetRRTNN (unsigned int tree, double q_rand, Trrt rrt)
	Locates the nearest sample in the tree of samples. More...

void	GetRRTNNInBall (unsigned int tree, double q_rand, double r, unsigned int nn, unsigned int *n, Trrt rrt)
	Locates the all the sample closer than a given distance. More...

void	GetRRTNNInBranch (unsigned int tree, unsigned int n1, unsigned int n2, unsigned int n, unsigned int nn, Trrt *rrt)
	Locates the nearest sample in the tree to a branch. More...

boolean	AddNodeToRRT (Tparameters pr, unsigned int tree, unsigned int i_near, double sample, double goal, unsigned int lastSample, void info, double costp, double cost, TRRTStatistics rst, Trrt *rrt)
	Adds a point to a RRT. More...

void	AddEdgeToRRT (unsigned int i, unsigned int j, double c, Trrt *rrt)
	Adds a neighbouring relation to an RRT. More...

boolean	TransitionTestRRT (Tparameters pr, unsigned int parent, double q_next, double deltaStep, double cost, double(costF)(Tparameters , boolean, double , void ), void costData, Trrt *rrt)
	transition Test for Transition RRT More...

void	RecursiveReWireRRTstar (Tparameters pr, Theap q, double g, Tvector steps, double l, Trrt rrt)
	A rewire that is propagates as much as necessary over the graph. More...

boolean	RRTstar (Tparameters pr, double pg, unsigned int it, double times, double costs, double planningTime, double pl, unsigned int ns, double **path, TRRTStatistics grst, Trrt *rrt)
	Optimal RRT on manifolds. More...

boolean	BiRRTstar (Tparameters pr, double pg, unsigned int it, double times, double costs, double planningTime, double pl, unsigned int ns, double **path, TRRTStatistics grst, Trrt *rrt)
	Optimal RRT on manifolds. More...

boolean	ccRRT (Tparameters pr, double pg, double time, double pl, unsigned int ns, double *path, TRRTStatistics grst, Trrt *rrt)
	Extends a RRT until we reach a targed point. More...

boolean	ccTRRT (Tparameters pr, double pg, double time, double pl, double pc, unsigned int ns, double **path, double(costF)(Tparameters , boolean, double , void ), void costData, TRRTStatistics grst, Trrt rrt)
	Extends a T-RRT until we reach a targed point. More...

boolean	cBiRRT (Tparameters pr, double pg, double time, double pl, unsigned int ns, double *path, TRRTStatistics grst, Trrt *rrt)
	Extends a RRT until we reach a targed point. More...

double	RRTPathSteps (unsigned int sID, Tvector path, Trrt rrt)
	Produces a vector with the nodes in a given branch. More...

double	ChangeBiRRTSteps (unsigned int l1, unsigned int l2, double c12, Tvector steps, Trrt rrt)
	Changes the optimal path in a bidirectional RRT. More...

double	UpdateBiRRTSteps (Tvector steps, Trrt rrt)
	Updates the current optimal path. More...

boolean	PathStart2GoalInRRT (Tparameters pr, double pgs, unsigned int l1, unsigned int l2, double pl, double pc, unsigned int ns, double *path, Trrt rrt)
	Determines the path from start to goal using an RRT. More...

boolean	Bidirectional (Trrt *rrt)
	Identifies bidirectional RRTs. More...

unsigned int	GetRRTMode (Trrt *rrt)
	Identifies mode of the RRTs. More...

boolean	InDynamicDomain (unsigned int i, double q, Trrt rrt)
	Checks if a sample is in the dynamic domaiin of a given node. More...

void	AdjustDynamicDomain (unsigned int i, boolean collision, Trrt *rrt)
	Sets the dynamic domain. More...

double	GetDynamicDomainRadius (unsigned int i, Trrt *rrt)
	Returns the current dynamic domain radius for a given sample. More...

boolean	DynamicDomainRRT (Trrt *rrt)
	TRUE if the dynamic domain is active. More...

unsigned int	GetRRTNumNodes (Trrt *rrt)
	Number of nodes in the RRT. More...

double *	GetRRTNode (unsigned int i, Trrt *rrt)
	Returns one of the nodes of the RRT. More...

unsigned int	GetRRTNodeTree (unsigned int i, Trrt *rrt)
	Returns the tree for a given node. More...

unsigned int	GetRRTParent (unsigned int i, Trrt *rrt)
	Returns identifier of the parent of one of the nodes of the RRT. More...

void	SetRRTParent (unsigned int i, unsigned int p, Trrt *rrt)
	Changes the parent for a given node. More...

void *	GetRRTNodeInfo (unsigned int i, Trrt *rrt)
	Returns the user info associated with a node. More...

void	SetRRTNodeInfo (unsigned int i, void info, Trrt rrt)
	Changes the user info associated with a node. More...

double	GetRRTNodeCost (unsigned int i, Trrt *rrt)
	Returns the cost associated with a node. More...

double	GetRRTNodeCostFromParent (unsigned int i, Trrt *rrt)
	Returns the cost from the parent node, if defined. More...

void	SetRRTNodeCost (unsigned int i, double costp, double cost, Trrt *rrt)
	Changes the cost associated with a node. More...

void	SetRRTCostAndParent (unsigned int i, unsigned int p, double costp, double cost, Trrt *rrt)
	Changes the cost and the parent associated with a node. More...

unsigned int *	GetRRTTopology (Trrt *rrt)
	Returns a pointer to an array with the topology for each variable. More...

double	CostToRoot (unsigned int sID, Trrt *rrt)
	Computes the cost from a node to the root. More...

void	UpdateCostToRoot (unsigned int sID, Trrt *rrt)
	Updates the cost from a node to the root. More...

void	UpdateTree (unsigned int sID, Trrt *rrt)
	Updates the tree assignment a node to the root. More...

void	UpdateCostAndTree (unsigned int sID, Trrt *rrt)
	A combination of UpdateCostToRoot and UpdateTree. More...

unsigned int	StepsToRoot (unsigned int sID, Trrt *rrt)
	Computes the number of steps from a node to the root. More...

void	PlotRRT (char fname, int argc, char arg, Tparameters pr, unsigned int xID, unsigned int yID, unsigned int zID, Trrt *rrt)
	Pots a projection of a RRT. More...

void	SaveRRTNodes (Tparameters pr, char fname, boolean saveWithDummies, Trrt *rrt)
	Stores the nodes of the RRT. More...

void	SaveRRTCosts (Tparameters pr, char fname, Trrt *rrt)
	Stores the cost associated with the nodes of the RRT. More...

unsigned int	RRTMemSize (Trrt *rrt)
	Memory used by a given RRT. More...

void	SaveRRT (Tfilename fname, Trrt rrt)
	Stores the RRT information on a file. More...

void	LoadRRT (Tparameters pr, Tfilename fname, TAtlasBase w, Trrt rrt)
	Defines a RRT from the information on a file. More...

void	DeleteRRT (Trrt *rrt)
	Destructor. More...

Macro Definition Documentation

#define RRT_VERBOSE 0

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

Definition at line 32 of file rrt.h.

Referenced by BiRRTstarCloseIteration(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstarCloseIteration().

#define GET_RRT_STATISTICS 1

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

Definition at line 41 of file rrt.h.

Referenced by main().

#define HEURISTIC_RRT_STAR (0)

If true we use an heuristic stratetgy in the AtlasRRT*. There are four heuristic that can be applied separately

1: Reject to expand nodes that are already far from the root.
2: gamma is set to 0 until the goal is reached.

The different heuristics can be combined via HEURISTIC_ATLASRRT_STAR (1+2)

Definition at line 54 of file rrt.h.

Referenced by AtlasBiRRTstar(), AtlasRRTstar(), AtlasRRTValidateSample(), BiRRTstar(), main(), RRTstar(), and RRTValidateSample().

#define RRTSTAR_UPDATE_COSTS 0

On the fly update of the cost in the RRTstar. If set to one, we cost for the tree nodes for RRT* are updated for the nodes close to the just added node to the tree (in a gamma-ball). The cost is updated by accumulating cost to parent for all the neighbouring nodes.

This option also affects AtlasRRTStar.

Definition at line 66 of file rrt.h.

Referenced by main().

#define RRTSTAR_SYMMETRIC_COST 1

Set to true if the cost are symmetrical, i.e. if cost(a,b)=cost(b,a). This can save a significant amount of time in the RRT*.

Definition at line 74 of file rrt.h.

Referenced by main().

#define EXPLORATION_RRT 0

When 0 we use the normal RRT with bias to goal and that stops as soon as the goal is found.

If 1 the RRT is build without considering the goal and the construction stops when the tree includes the given number of samples (or nodes).

Normally this should be 0. We only use it to test the coverage properties of the RRT.

This is also used for AtlasRRT.

Todo:: Set this as a parameter and not as a compilation flag.

Definition at line 98 of file rrt.h.

Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), BiRRTstar(), ccRRT(), ccTRRT(), InitAtlasRRT(), main(), and RRTstar().

#define RRT_PLOT_NODES 1

RRTs are plotted with red lines connecting nodes and blue crosses defining each node. If this flag is set to 0, the crosses are not plotted (only the lines are shown).

Definition at line 107 of file rrt.h.

#define RRT_NN_TOPOLOGY 1

This flag can be used to activate of de-activate the topology in the search for nearest-neighbours.

This is basically used for testing purposes.

Definition at line 117 of file rrt.h.

Referenced by InitRRT(), and LoadRRT().

#define INIT_NUM_SAMPLES_RRT 100

Initial room for samples in th rrt. It will be expanded as needed. Should be at least 1.

Definition at line 125 of file rrt.h.

Referenced by InitAtlasRRT(), and InitRRT().

#define ONE_TREE 0

In this mode only one tree (start to goal) is defined.

Definition at line 132 of file rrt.h.

Referenced by Bidirectional(), BiRRTstar(), ccRRT(), ccTRRT(), GetRRTNN(), GetRRTNNInBall(), GetRRTNNInBranch(), GetRRTNodeTree(), InitAtlasRRT(), InitRRT(), main(), PathStart2GoalInRRT(), PlotRRT(), RRTSample(), and RRTstar().

#define TWO_TREES 1

In this mode only two trees are defined, one from start to goal and one from goal to start. Once assigned to a tree samples never move to the other tree.

Definition at line 141 of file rrt.h.

Referenced by AddNodeToRRT(), cBiRRT(), DeleteRRT(), GetRRTNN(), GetRRTNNInBall(), InitRRT(), LoadRRT(), and main().

#define TWO_TREES_WITH_SWAP 2

In this mode only two trees are defined, one from start to goal and one from goal to start. In this mode samples can change between trees. This is only used for bidirectional RRT*.

In this mode only one big pool of samples (i.e., one KD-tree) is maintained for NN searching. This is so since the kd-tree implementation does not allow samples to be removed from the kd-tree.

Definition at line 154 of file rrt.h.

Referenced by BiRRTstar(), GetRRTNN(), GetRRTNNInBall(), GetRRTNNInBranch(), main(), PlotRRT(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), SetRRTCostAndParent(), SetRRTParent(), UpdateTree(), WireAtlasRRTstar(), and WireRRTstar().

#define NOTREE 0

Used for samples not assigned to any tree.

Definition at line 161 of file rrt.h.

#define START2GOAL 1

Search direction of one of the possible trees. This one is used always.

Definition at line 168 of file rrt.h.

Referenced by AddChart2Tree(), AddNodeToRRT(), AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), ChangeBiRRTSteps(), GetRRTNN(), GetRRTNNInBall(), GetRRTNNInBranch(), GetRRTNodeTree(), InitAtlasRRT(), InitRRT(), LoadRRT(), PathStart2GoalInRRT(), PlotAtlasRRT(), PlotRRT(), RandomPointInAtlasTree(), RRTSample(), RRTstar(), UpdateCostAndTree(), and UpdateTree().

#define GOAL2START 2

Search direction of one of the possible trees. This one is only used when using the bidirectional search strategy.

Definition at line 176 of file rrt.h.

Referenced by AtlasBiRRTstar(), AtlasRRT(), BiRRTstar(), cBiRRT(), GetRRTNN(), GetRRTNNInBall(), InitAtlasRRT(), InitRRT(), PlotAtlasRRT(), PlotRRT(), UpdateCostAndTree(), and UpdateTree().

#define BOTHTREES (START2GOAL|GOAL2START)

Used for samples in both trees.

Definition at line 183 of file rrt.h.

Referenced by AddNodeToRRT(), AtlasBiRRTstar(), BiRRTstar(), GetRRTNN(), GetRRTNNInBall(), GetRRTNNInBranch(), RandomPointInAtlasTree(), and RRTSample().

Function Documentation

void CopyRRTStep	(	void *	a,
		void *	b
	)

Copy constructor for RRTSteps.

Parameters

a	Pointer to the RRTStep where to copy.
b	Pointer to the RRTStep from where to take the data.

Definition at line 750 of file rrt.c.

Referenced by AtlasBiRRTstar(), BiRRTstar(), and RRTPathSteps().

void InitRRTStatistics ( TRRTStatistics * rst )

Init the RRT statistics.

Parameters

rst	The RRT statistics to initialize.

Definition at line 195 of file rrt.c.

References TRRTStatistics::dQrand, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nConvergenceError, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nHighCost, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection.

Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstar().

void AccumulateRRTStatistics	(	TRRTStatistics *	rst1,
		TRRTStatistics *	rst2
	)

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

Parameters

rst1	The RRT statistics to accumulate.
rst2	The RRT statistics where to accumulate.

Definition at line 318 of file rrt.c.

References TRRTStatistics::dQrand, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection.

Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), and RRTstar().

void PrintRRTStatistics	(	Trrt *	rrt,
		TRRTStatistics *	rst
	)

Prints a report about the collected RRT statistics.

Parameters

rrt	The RRT (might be NULL).
rst	The RRT statistics to print.

Definition at line 352 of file rrt.c.

References TRRTStatistics::dQrand, DynamicDomainRRT(), GetDynamicDomainRadius(), INF, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nConvergenceError, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nHighCost, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, Trrt::ns, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection.

Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstar().

void DeleteRRTStatistics ( TRRTStatistics * rst )

Deletes the RRT statistics object.

Parameters

rst	The RRT statistics to delete.

Definition at line 499 of file rrt.c.

Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstar().

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

Defines a RRT with a single sample, the root.

Parameters

pr	The set of parameters.
parallel	TRUE if the RRT construction will involve any kind of parallelism via OpenMP. Up to now some algorithms (RRT*) include some degree of parallelism and others (eg. RRT) not.
simp	TRUE if the point is given in the simplified form and FALSE if it is in the global system (including redundant values). In any case, the sample includes only system variables.
ps	A point on the manifold from where to start the RRT. The RRT root.
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 point for the RRT. Only used when mode is not ONE_TREE.
m	The dimension of the sample space. Size of the samples. Only used if simp is TRUE.
w	The base type with the equations (and possibly obstacles).
rrt	The RRT to create.

Definition at line 1203 of file rrt.c.

References Trrt::ambient, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_IN_COLLISION, CS_WD_INIT_CD, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_DYNAMIC_DOMAIN, CT_EPSILON, CT_N_DOF, CT_SAMPLING, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, Error(), FALSE, TRRTSampleInfo::g, GetBoxInterval(), GetBoxNIntervals(), GetParameter(), GOAL2START, Trrt::graph, INIT_NUM_SAMPLES_RRT, Trrt::k, KDTREE_SAMPLING, LowerLimit(), TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, Trrt::n, Trrt::nCores, NEW, Trrt::nFail, TRRTSampleInfo::nn, NO_UINT, Trrt::ns, ONE_TREE, Trrt::parallel, TRRTSampleInfo::parent, PointInBoxTopology(), RRT_NN_TOPOLOGY, TRRTSampleInfo::samples, SetRRTTopology(), Trrt::si, START2GOAL, Trrt::temperature, TEMPERATURE_INIT, Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES, UpperLimit(), and Trrt::w.

Referenced by InitAtlasRRT(), and main().

double GetTRRTTemperature ( Trrt * rrt )

Returns the current value for the temperature of the T-RRT.

Parameters

rrt	The T-RRT to query.

Returns: The temperature.

Definition at line 1460 of file rrt.c.

References Trrt::temperature.

Referenced by AtlasTRRT().

boolean IsRRTGraph ( Trrt * rrt )

Identifies RRTs where we store the whole set of neighnbours for each node. This is used in some variants of RRT* to accelerate the propagation of the path improvements.

Parameters

rrt	The T-RRT to query.

Returns: TRUE if the RRT has graph structure.

Definition at line 1465 of file rrt.c.

References Trrt::graph.

Referenced by AtlasBiRRTstar(), AtlasRRTstar(), and WireAtlasRRTstar().

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

Generates a random sample to expand the RRT. 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

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). In this second option the random sample is 'near' the existing nodes in the tree. This option can only be used if the cuik-kdtree library is available.
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.
q_rand	Space to store the random sample.
rst	Statistics on the RRT construction process (only used if GET_RRT_STATISTICS is set).
rrt	RRT to use.

Returns: TRUE if the goal is used as random sample.

Definition at line 1470 of file rrt.c.

References Trrt::ambient, BOTHTREES, KDTREE_SAMPLING, Trrt::m, Trrt::mode, NewRRTRandomSample(), ONE_TREE, randomDouble(), RandomPointInBox(), and START2GOAL.

Referenced by AtlasRRTSample(), BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), and RRTstar().

boolean RRTValidateSample	(	Tparameters *	pr,
		double *	q_rand,
		unsigned int	tree,
		boolean	expand2goal,
		double *	goal,
		double	l,
		double *	h,
		unsigned int *	i_near,
		TRRTStatistics *	rst,
		Trrt *	rrt
	)

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

Parameters

pr	The set of parameters.
q_rand	The random sample.
tree	The tree where to look for nearest neighbours.
expand2goal	TRUE if the random sample is the goal.
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.
rst	Statistics on the RRT construction process (only used if GET_RRT_STATISTICS is set).
rrt	RRT to use.

Returns: TRUE if the random sample is valid.

Definition at line 1509 of file rrt.c.

References CS_WD_SIMP_INEQUALITIES_HOLD, Trrt::dd, DistanceTopology(), GetRRTNN(), HEURISTIC_RRT_STAR, InDynamicDomain(), INF, Trrt::m, NewRRTSampleRejection(), TRRTSampleInfo::samples, Trrt::si, Trrt::tp, TRUE, and Trrt::w.

Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), and RRTstar().

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

Finds the sample in the tree that is closer (in the Euclidean distance) to the given point.

Parameters

tree	Which tree (START2GOAL or GOAL2START) to use in the search. Only considered in bidirectional RRTs. Otherwise all points are added in the START2GOAL tree (the only one defined).
q_rand	The point (in the simplified system).
rrt	The RRT to query.

Returns: The index of the tree node closer to q_rand.

Definition at line 1546 of file rrt.c.

References BOTHTREES, DistanceTopologyMin(), GOAL2START, INF, Trrt::m, Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, TRRTSampleInfo::samples, Trrt::si, SquaredDistanceTopology(), START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, and TWO_TREES_WITH_SWAP.

Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRTValidateSample(), BiRRTstar(), cBiRRT(), New_AddBranchToAtlasRRT(), PathStart2GoalInRRT(), and RRTValidateSample().

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

Finds the set of samples inside a ball of radious 'r' centered at a given point.

Parameters

tree	Which tree (START2GOAL or GOAL2START) to use in the search. Only considered in bidirectional RRTs. Otherwise all points are added in the START2GOAL tree (the only one defined).
q_rand	The point (in the simplified system).
r	The radius of the search ball.
nn	(output) The number of returned points.
n	(output) The array of indices for the in-ball nodes.
rrt	The RRT to query.

Definition at line 1653 of file rrt.c.

References BOTHTREES, DistanceTopologyMin(), GOAL2START, Trrt::m, MEM_DUP, Trrt::mode, NEW, Trrt::ns, ONE_TREE, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, and TWO_TREES_WITH_SWAP.

Referenced by AtlasBiRRTstar(), AtlasRRTstar(), BiRRTstar(), and RRTstar().

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

Finds the sample in the given tree that is close to a given branch in another tree. The branch is defined by samples n1 and n2 where n1 is antecesor of n2.

Parameters

tree	Which tree (START2GOAL or GOAL2START) where to search for the nearest sample. This must be different from the tree including the query samples (n1, n2).
n1	First node (parent) defining the branch in the other tree.
n2	Second node (child) defining the branch in the other tree.
n	The index of the element in the branch (i.e., node in between n1 and n2 both included) close to the other 'tree'.
nn	The index of the node in 'tree' closer to the branch (i.e., to the node whose index is returned in 'n').
rrt	The RRT to query.

Definition at line 1774 of file rrt.c.

References BOTHTREES, DistanceTopologyMin(), Error(), FALSE, INF, Trrt::m, Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP.

Referenced by AtlasRRT().

boolean AddNodeToRRT	(	Tparameters *	pr,
		unsigned int	tree,
		unsigned int	i_near,
		double *	sample,
		double *	goal,
		unsigned int *	lastSample,
		void *	info,
		double	costp,
		double	cost,
		TRRTStatistics *	rst,
		Trrt *	rrt
	)

A debug utility to add points to RRTs without checking for collisions nor moving slowly toward to q_rand. It just adds the point and links it to the parent (i_near)

Parameters

pr	The set of parameters.
i_near	Index of the sample in the RRT from where to start the extension. The parent for the sample to add.
tree	The tree that should include the new sample. In must be the same tree as that of the parent sample, if defined.
sample	The sample to add.
goal	Global goal for the RRT (also given in simplified form). If sample is close to goal the function returns TRUE.
lastSample	Identifier given to the sample added to the RRT.
info	User information associated to the node.
costp	The distance to the parent.
cost	The cost of the node to be added. This can mean different things: The cost of the node evaluated with a user-given function (in T-RRT) or the distance from the root of the tree (in RRT*).
rst	Statistics on the RRT construction process (only used if GET_RRT_STATISTICS is set).
rrt	the RRT to extend.

Returns: TRUE if the goal has been reached (sufficiently close) or if the maximum number of samples in the RRT have been reached. If the goal is NULL then we return FALSE.

Definition at line 1868 of file rrt.c.

References AddEdgeToRRT(), ArrayPi2Pi(), BOTHTREES, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, TRRTSampleInfo::ddr, DistanceTopology(), Error(), FALSE, TRRTSampleInfo::g, GetParameter(), Trrt::graph, INF, TRRTSampleInfo::m, Trrt::m, MEM_DUP, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, NEW, NewRRTSample(), TRRTSampleInfo::nn, Trrt::ns, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, and TRRTSampleInfo::userInfo.

Referenced by AddBranchToRRT(), AddSample2AtlasRRT(), AddStepToRRTstar(), and PathStart2GoalInRRT().

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

Stores an edge between nodes 'i' and 'j' with cost 'c'

This only has effect if the RRT is in graph mode.

Parameters

i	The first node of the edge.
j	The second node of the edge.
c	The cost of the edge.
rrt	The rrt to update.

Definition at line 781 of file rrt.c.

References TRRTSampleInfo::cn, Error(), Trrt::graph, TRRTSampleInfo::m, MEM_DUP, MEM_EXPAND, TRRTSampleInfo::n, TRRTSampleInfo::nn, Trrt::ns, and Trrt::si.

Referenced by AddNodeToRRT(), AtlasBiRRTstar(), BiRRTstar(), WireAtlasRRTstar(), and WireRRTstar().

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

transition Test for Transition RRT

Parameters

pr	The set of parameters.
parent	The parent node of the transition.
q_next	The configuration to be reached.
deltaStep	The distance step for the transition.
cost	The cost of q_next to be returned.
costF	The cost function used to evaluate the configuration cost.
costData	To be used as last parameter for the cost funtion.
rrt	The RRT.

Definition at line 806 of file rrt.c.

References TRRTSampleInfo::cost, CT_COEF_TEMP, CT_EPSILON, CT_NFAIL_MAX, GetParameter(), Trrt::nFail, randomDouble(), Trrt::si, Trrt::temperature, and TRUE.

Referenced by AddBranchToRRT(), New_NewTemptativeSample(), and NewTemptativeSample().

void RecursiveReWireRRTstar	(	Tparameters *	pr,
		Theap *	q,
		double *	g,
		Tvector *	steps,
		double *	l,
		Trrt *	rrt
	)

A version of ReWireRRTstar to be used when the RRT is in graph mode. This re-wire uses the neighbours and cost-to-neighbour stored in the RRT nodes and propagages the recursively propagates the updates to all the necessary nodes in the graph.

Parameters

pr	The set of parameters.
q	The priority queue with the nodes to update.
g	The goal configuration.
steps	Steps forming the solution. Only used in bi-directional trees.
l	Best estimation of the cost start-goal up to now. Only updated in bidirectional trees.
rrt	The RRT to rewire.

Definition at line 2144 of file rrt.c.

References ChangeBiRRTSteps(), TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, DistanceTopology(), Error(), ExtractMinElement(), FirstInPair(), TRRTSampleInfo::g, GetHeapElement(), GetParameter(), Trrt::graph, HeapEmpty(), Trrt::m, Trrt::mode, TRRTSampleInfo::n, NewDoublePair(), TRRTSampleInfo::nn, NO_UINT, TRRTSampleInfo::parent, TRRTSampleInfo::samples, SetRRTCostAndParent(), Trrt::si, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and UpdateHeapElement().

Referenced by AtlasBiRRTstar(), AtlasRRTstar(), BiRRTstar(), and RRTstar().

boolean RRTstar	(	Tparameters *	pr,
		double *	pg,
		unsigned int *	it,
		double *	times,
		double *	costs,
		double *	planningTime,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		TRRTStatistics *	grst,
		Trrt *	rrt
	)

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.

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).
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
rrt	The RRT to expand.

Returns: TRUE if the path exists.

Definition at line 2351 of file rrt.c.

References AccumulateRRTStatistics(), AddStepToRRTstar(), Trrt::ambient, BiRRTstar(), CopyDoublePair(), TRRTSampleInfo::cost, 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_EPSILON, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteDoublePair(), DeleteHeap(), DeleteRRTStatistics(), DeleteStatistics(), Trrt::delta, DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNNInBall(), Trrt::graph, HEURISTIC_RRT_STAR, INF, InitHeap(), InitRRTStatistics(), InitStatistics(), Trrt::k, LessThanDoublePair(), Trrt::m, Trrt::mode, Trrt::ms, Trrt::n, Trrt::nCores, NEW, NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PointInBoxTopology(), PrintRRTStatistics(), ReconstructRRTPath(), RecursiveReWireRRTstar(), ReWireRRTstar(), RRTSample(), RRTstarCloseIteration(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, UpdateCostAndTree(), Trrt::w, and WireRRTstar().

Referenced by BiRRTstar(), and main().

boolean BiRRTstar	(	Tparameters *	pr,
		double *	pg,
		unsigned int *	it,
		double *	times,
		double *	costs,
		double *	planningTime,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		TRRTStatistics *	grst,
		Trrt *	rrt
	)

This is the same as RRTstar but implements a bi-directional search.

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).
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
rrt	The RRT to expand.

Returns: TRUE if the path exists.

Definition at line 2583 of file rrt.c.

References AccumulateRRTStatistics(), AddEdgeToRRT(), AddStepToRRTstar(), Trrt::ambient, BiRRTstarCloseIteration(), BOTHTREES, ChangeBiRRTSteps(), ConnectSamples(), CopyDoublePair(), CopyRRTStep(), TRRTSampleInfo::cost, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteDoublePair(), DeleteHeap(), DeleteRRTStatistics(), DeleteStatistics(), DeleteVector(), Trrt::delta, DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GetRRTNNInBall(), GOAL2START, Trrt::graph, HEURISTIC_RRT_STAR, INF, InitHeap(), InitRRTStatistics(), InitStatistics(), InitVector(), Trrt::k, LessThanDoublePair(), Trrt::m, Trrt::mode, Trrt::ms, Trrt::n, Trrt::nCores, NEW, NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PrintRRTStatistics(), RecursiveReWireRRTstar(), ReWireRRTstar(), RRTSample(), RRTstar(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Steps2PathinRRT(), Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES_WITH_SWAP, UpdateBiRRTSteps(), Trrt::w, and WireRRTstar().

Referenced by RRTstar().

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

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

The tree extension is performed using the strategy defined in Dalibard, S., Nakhaei, A., Lamiraux, F., and Laumond, J.-P. (2009). Whole-body task planning for a humanoid robot: a way to integrate collision avoidance. In IEEE-RAS International Conference on Humanoid Robots, pages 355-360.

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.
time	Actual time 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).
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
rrt	The RRT to expand.

Returns: TRUE if the path exists.

Definition at line 2965 of file rrt.c.

References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, 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_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, Trrt::w, and Warning().

Referenced by main().

boolean ccTRRT	(	Tparameters *	pr,
		double *	pg,
		double *	time,
		double *	pl,
		double *	pc,
		unsigned int *	ns,
		double ***	path,
		double()(Tparameters , boolean, double , void )	costF,
		void *	costData,
		TRRTStatistics *	grst,
		Trrt *	rrt
	)

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

The difference with respect ccRRT 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 RRT. This is also a sample including only system variables.
time	Actual time 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 path (if any).
path	Configurations defining the output path (points on the manifold).
costF	The cost function.
costData	User data to be provided to the cost function as a last parameter.
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
rrt	The RRT to expand.

Returns: TRUE if the path exists.

Definition at line 2822 of file rrt.c.

References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, TRRTSampleInfo::cost, 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_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, Trrt::w, and Warning().

Referenced by main().

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

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

The tree extension is performed using the strategy defined in 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.

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.
time	Actual time 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).
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
rrt	The RRT to expand.

Returns: TRUE if the path exists.

Definition at line 3098 of file rrt.c.

References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, 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_EPSILON, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GOAL2START, INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTNoEmptyTreeConnection(), NewRRTSample(), NewRRTTreeConnection(), Trrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, TWO_TREES, Trrt::w, and Warning().

Referenced by main().

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

Produces a vector with the nodes connecting a given node with the root of the RRT.

The vector is created inside this function but must be destroyed externally.

Parameters

sID	The sample to connect to the root.
path	The vector with the samples to visit along the path.
rrt	The RRT.

Returns: The length of the path.

Definition at line 3276 of file rrt.c.

References CopyRRTStep(), TRRTStep::cost, TRRTSampleInfo::costp, TRRTStep::id, InitVector(), NewVectorElement(), NO_UINT, TRRTSampleInfo::parent, and Trrt::si.

Referenced by ChangeBiRRTSteps(), and ReconstructRRTPath().

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

Replaces a given path by another passing by two nodes in the two trees of a bidirectional RRT.

Note that in this case the path is just a list of nodes to be visited in order. The actual path between them is not computed, but will be computed at the end of the planning (this saves time during planning).

Parameters

l1	Sample in the first tree that can be connected to the second tree.
l2	Sample in the second tree that can be connected to the first tree.
c12	Cost of the transition from l1 to l2.
steps	Vector of nodes forming the path. This vector must be initialitzed externally.
rrt	The RRT to query.

Returns: The length of the new path.

Definition at line 3297 of file rrt.c.

References ConcatVectors(), TRRTStep::cost, DeleteVector(), GetVectorElement(), TRRTStep::id, NewVectorElement(), ResetVector(), RRTPathSteps(), Trrt::si, START2GOAL, TRRTSampleInfo::tree, and VectorSize().

Referenced by AtlasBiRRTstar(), BiRRTstar(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), and UpdateBiRRTSteps().

double UpdateBiRRTSteps	(	Tvector *	steps,
		Trrt *	rrt
	)

Tries to incorporate the recent changes in the trees to the optimal path.

The optimal paths in a BiRRT are defined from two nodes, each one in a different tree. This path is represented by a list of nodes. However, the optimal path can be improved taking into accont the wires and rewires in the trees. This functions takes the list of nodes that define the optimal path and incorporates the shortcuts discobered in the tree since the path was defined.

Parameters

steps	Vector of nodes forming the path. This vector must be initialitzed externally.
rrt	The RRT to query.

Returns: The new lenght of the optimal path.

Definition at line 3356 of file rrt.c.

References ChangeBiRRTSteps(), TRRTStep::cost, TRRTSampleInfo::cost, Error(), GetVectorElement(), TRRTStep::id, INF, NO_UINT, Trrt::si, TRRTSampleInfo::tree, UpdateCostAndTree(), and VectorSize().

Referenced by AtlasBiRRTstar(), and BiRRTstar().

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

Determines the path from start to goal using an RRT, if one exists.

Parameters

pr	The set of parameters.
pgs	A point on the manifold to reach in SIMPLIFIED form.
l1	Sample in the first tree close to the second tree. Used only for bidirectional RRTs.
l2	Sample in the second tree close to the first tree. Used only for bidirectional RRTs.
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 path (if any).
path	Configurations defining the output path (points on the manifold).
rrt	The RRT to query.

Returns: TRUE if the path exists.

Definition at line 3401 of file rrt.c.

References AddNodeToRRT(), DeleteSamples(), Trrt::delta, DistanceTopology(), GetRRTNN(), Trrt::m, Trrt::mode, Trrt::ns, ONE_TREE, ReconstructRRTPath(), ReverseConcatSamples(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, and TRUE.

Referenced by AtlasRRT(), AtlasTRRT(), cBiRRT(), ccRRT(), and ccTRRT().

boolean Bidirectional ( Trrt * rrt )

Identifies bidirectional RRTs.

Parameters

rrt	The RRT to query.

Returns: TRUE if the RRT is bidirectional (i.e., TWO_TREES, or TWO_TREES_WITH_SWAP).

Definition at line 3462 of file rrt.c.

References Trrt::mode, and ONE_TREE.

unsigned int GetRRTMode ( Trrt * rrt )

Identifies mode of the RRTs.

Parameters

rrt	The RRT to query.

Returns: ONE_TREE, TWO_TREES, or TWO_TREES_WITH_SWAP.

Definition at line 3468 of file rrt.c.

References Trrt::mode.

Referenced by ReWireAtlasRRTstar(), and WireAtlasRRTstar().

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

Checks if a sample is in the dynamic domaiin of a given node. Random samples out of the dynamic domain should be rejected.

Parameters

i	The index of the sample giving the dynamic domain.
q	The random sample.
rrt	The RRT to query.

Returns: TRUE if the random sample is in the dynamic domain.

Definition at line 3473 of file rrt.c.

References Trrt::dd, TRRTSampleInfo::ddr, DistanceTopology(), Trrt::m, TRRTSampleInfo::samples, Trrt::si, Trrt::tp, and TRUE.

Referenced by AtlasRRTValidateSample(), and RRTValidateSample().

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

If the dynamic domain technique is used, and a problem is detected when extending a branch from a given node, we set the dynamic domain for this node so that new random samples outside the dynamic domain are rejected.

Parameters

i	The node whose dynamic domain need to be adjusted.
collision	TRUE if the branch is stopped due to a collision.
rrt	The RRT to update.

Definition at line 3481 of file rrt.c.

References Trrt::dd, TRRTSampleInfo::ddr, MOV_AVG_DOWN, MOV_AVG_UP, and Trrt::si.

Referenced by AddBranchToAtlasRRT(), AddBranchToRRT(), AddStepToRRTstar(), and New_AddBranchToAtlasRRT().

double GetDynamicDomainRadius	(	unsigned int	i,
		Trrt *	rrt
	)

Return the dynamic domain radious for a given sample. If the dynamic domain technique is not active or the radiius for the sample has not been set yet, this function returns 0.

Parameters

i	The sample identifier.
rrt	The RRT structure to query.

Returns: The dynamic domain for sample 'i'.

Definition at line 3513 of file rrt.c.

References Trrt::dd, TRRTSampleInfo::ddr, and Trrt::si.

Referenced by PrintAtlasRRTStatistics(), and PrintRRTStatistics().

boolean DynamicDomainRRT ( Trrt * rrt )

TRUE if the dynamic domain technique is used. Using this technique the random sample are rejected if they are no in the dynamic domain of the nearest-neighbour sample.

Parameters

rrt	The RRT to query.

Returns: TRUE if the RRT uses the dynamic domain technique.

Definition at line 3521 of file rrt.c.

References Trrt::dd.

Referenced by PrintAtlasRRTStatistics(), and PrintRRTStatistics().

unsigned int GetRRTNumNodes ( Trrt * rrt )

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

Parameters

rrt	The RRT to query.

Returns: The number of samples stored in the RRT so far.

Definition at line 3526 of file rrt.c.

References Trrt::ns.

Referenced by InitAtlasRRT(), and main().

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

Returns a pointer to the sample in the RRT with the given identifier.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: A pointer to the sample or NULL if the samples does not exists in the tree.

Definition at line 3531 of file rrt.c.

References Trrt::ns, TRRTSampleInfo::samples, and Trrt::si.

Referenced by AddSample2AtlasRRT(), AtlasTRRT(), ccTRRT(), InitAtlasRRT(), and LoadAtlasRRTSampleInfo().

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

In bidirectional RRTs, identifies the tree including a given node.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: The tree (START2GOAL or GOAL2START) including the given node.

Definition at line 3539 of file rrt.c.

References Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, Trrt::si, START2GOAL, and TRRTSampleInfo::tree.

Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), GetRRTNNInChart(), InitBranchStatus(), PlotAtlasRRT(), PopulateWithSamples(), ReWireAtlasRRTstar(), and WireAtlasRRTstar().

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

Returns the identifier of the parent for a given RRT node.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: The parent identifier or NU_UINT if the node does not exists or if it is the root (it has not parent).

Definition at line 3552 of file rrt.c.

References NO_UINT, Trrt::ns, TRRTSampleInfo::parent, and Trrt::si.

Referenced by AddStepToAtlasRRTstar(), PlotAtlasRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), and WireAtlasRRTstar().

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

Changes the parent for a given node in the RRT. By changing the parent we also change the tree of the node to that of its new parent.

Parameters

i	The node whose parent we want to change.
p	The new parent node.
rrt	The RRT to modify.

Definition at line 3560 of file rrt.c.

References Error(), Trrt::mode, NO_UINT, Trrt::ns, TRRTSampleInfo::parent, Trrt::si, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP.

Referenced by SmoothPathInAtlasRRT().

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

Returns the user info associated with a node.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: The pointer to the info associated with the node.

Definition at line 3577 of file rrt.c.

References Trrt::si, and TRRTSampleInfo::userInfo.

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

Changes the user info associated with a node.

Parameters

i	The identifier of the node.
info	The new user info.
rrt	The RRT to query.

Definition at line 3585 of file rrt.c.

References Error(), Trrt::si, and TRRTSampleInfo::userInfo.

double GetRRTNodeCost	(	unsigned int	i,
		Trrt *	rrt
	)

Returns the cost associated with a node.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: The cost associated with the node.

Definition at line 3593 of file rrt.c.

References TRRTSampleInfo::cost, and Trrt::si.

Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRTstar(), AtlasRRTstarCloseIteration(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().

double GetRRTNodeCostFromParent	(	unsigned int	i,
		Trrt *	rrt
	)

Returns the cost from the parent node.

Parameters

i	The identifier of the node.
rrt	The RRT to query.

Returns: The cost associated with the node.

Definition at line 3601 of file rrt.c.

References TRRTSampleInfo::costp, and Trrt::si.

Referenced by ReWireAtlasRRTstar(), and WireAtlasRRTstar().

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

Changes the cost associated with a node.

Parameters

i	The identifier of the node.
costp	The new cost from the parent sample, if defined.
cost	The new cost.
rrt	The RRT to query.

Definition at line 3609 of file rrt.c.

References TRRTSampleInfo::cost, TRRTSampleInfo::costp, Error(), and Trrt::si.

Referenced by AddStepToAtlasRRTstar(), AtlasTRRT(), and SmoothPathInAtlasRRT().

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

Changes the cost and the parent associated with a node.

Parameters

i	The identifier of the node.
p	The parent node
costp	The new cost from the parent sample, if defined.
cost	The new cost.
rrt	The RRT to query.

Definition at line 3620 of file rrt.c.

References TRRTSampleInfo::cost, TRRTSampleInfo::costp, Error(), Trrt::mode, TRRTSampleInfo::parent, Trrt::si, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP.

Referenced by RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), WireAtlasRRTstar(), and WireRRTstar().

unsigned int* GetRRTTopology ( Trrt * rrt )

Gives access to the topology for each variable stored in the RRT

Parameters

rrt	The RRT to query.

Returns: A pointer to the RRT topology array.

Definition at line 3638 of file rrt.c.

References Trrt::tp.

Referenced by InitAtlasRRT(), and LoadAtlasRRT().

double CostToRoot	(	unsigned int	sID,
		Trrt *	rrt
	)

Computes the cost from a node to the root by adding the individual cost of the node-parent relations.

During re-wires the cost of a node can change without being reflected in the cost associated with this node due to changes in the branch including the node that are not yet propagated. This function computes the true cost with full propagation given the current tree.

Parameters

sID	Identifier of the node whose cost we have to computed.
rrt	The RRT.

Definition at line 3643 of file rrt.c.

References TRRTSampleInfo::costp, NO_UINT, TRRTSampleInfo::parent, and Trrt::si.

Referenced by AtlasRRTstarCloseIteration(), RRTstarCloseIteration(), and UpdateCostToRoot().

void UpdateCostToRoot	(	unsigned int	sID,
		Trrt *	rrt
	)

Updates the cost from a node to the root by adding the individual cost of the node-parent relations.

Parameters

sID	Identifier of the node whose cost we have to updated.
rrt	The RRT.

Definition at line 3658 of file rrt.c.

References TRRTSampleInfo::cost, CostToRoot(), and Trrt::si.

void UpdateTree	(	unsigned int	sID,
		Trrt *	rrt
	)

Checks the tree including the current node and assigns it to this tree. During re-wires the actual tree of a node can change due to changes along the branch including this tree that are not propagated to it. This functions makes sure there is no inconsistency at least for the selected node.

Parameters

sID	Identifier of the node whose tree we have to computed.
rrt	The RRT.

Definition at line 3663 of file rrt.c.

References Error(), GOAL2START, Trrt::mode, NO_UINT, TRRTSampleInfo::parent, Trrt::si, START2GOAL, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP.

Referenced by PlotRRT().

void UpdateCostAndTree	(	unsigned int	sID,
		Trrt *	rrt
	)

A combination of UpdateCostToRoot and UpdateTree defined to gain some efficiency.

Parameters

sID	Identifier of the node to update.
rrt	The RRT.

Definition at line 3692 of file rrt.c.

References TRRTSampleInfo::cost, TRRTSampleInfo::costp, GOAL2START, NO_UINT, TRRTSampleInfo::parent, Trrt::si, START2GOAL, and TRRTSampleInfo::tree.

Referenced by AtlasRRTstar(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), RRTstar(), UpdateBiRRTSteps(), WireAtlasRRTstar(), and WireRRTstar().

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

Computes the number of steps from a node to the root.

Parameters

sID	Identifier of the node whose cost we have to computed.
rrt	The RRT.

Definition at line 3719 of file rrt.c.

References NO_UINT, TRRTSampleInfo::parent, and Trrt::si.

Referenced by SmoothPathInAtlasRRT().

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

Plots a 3d projection of a RRT defined on a manifold.

Although the ambient space can have arbitrary dimension we project it on 3 dimensions. Only 2D manifolds plots can be properly visualized.

The output plot can be visualized using geomview.

Parameters

fname	File where to store the plot.
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 thrid ambient dimension where to project (in the original system including system vars and dummies).
rrt	The RRT to plot.

See Also: PlotMap.

Definition at line 3734 of file rrt.c.

References Close3dObject(), ClosePlot3d(), CS_WD_REGENERATE_ORIGINAL_POINT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeleteColor(), FALSE, GetParameter(), GOAL2START, InitPlot3d(), M_2PI, Trrt::mode, NEW, NewColor(), Trrt::ns, ONE_TREE, TRRTSampleInfo::parent, PlotVect3d(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, StartNew3dObject(), TRRTSampleInfo::tree, TWO_TREES_WITH_SWAP, UpdateTree(), and Trrt::w.

Referenced by main(), and PlotAtlasRRT().

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

Stores the nodes of the RRT in the form of boxes.

Parameters

pr	The set of parameters.
fname	Name to use for the two output file.
saveWithDummies	TRUE if the output has to include de dummies. This is necessary only if you plan to unsimplify the output points.
rrt	The RRT process.

Definition at line 3883 of file rrt.c.

References CreateFileName(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteFileName(), Error(), GetFileFullName(), Trrt::ns, TRRTSampleInfo::samples, Trrt::si, SOL_EXT, SOL_WITH_DUMMIES_EXT, and Trrt::w.

Referenced by main().

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

Stores the cost associated with the nodes of the RRT. This is typically used when dealing with molecules. In this case the cost is the energy.

Parameters

pr	The set of parameters.
fname	Name to use for the two output file.
rrt	The RRT process.

Definition at line 3934 of file rrt.c.

References TRRTSampleInfo::cost, COST_EXT, CreateFileName(), DeleteFileName(), Error(), GetFileFullName(), INF, Trrt::ns, and Trrt::si.

Referenced by main().

unsigned int RRTMemSize ( Trrt * rrt )

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

Parameters

rrt The RRT.

Returns: The size of the RRT in bytes.

Definition at line 3973 of file rrt.c.

References Trrt::graph, Trrt::m, TRRTSampleInfo::nn, Trrt::ns, and Trrt::si.

Referenced by AtlasRRTMemSize(), and main().

void SaveRRT	(	Tfilename *	fname,
		Trrt *	rrt
	)

Stores all the information in the RRT in a file.

Todo:: Save RRT in binary format.

Parameters

fname	Name of the file where to store the RRT.
rrt	The RRT to store.

Definition at line 3990 of file rrt.c.

References TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, Error(), TRRTSampleInfo::g, GetFileFullName(), Trrt::graph, Trrt::k, TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, Trrt::n, Trrt::nFail, TRRTSampleInfo::nn, Trrt::ns, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, Trrt::temperature, and TRRTSampleInfo::tree.

Referenced by main(), and SaveAtlasRRT().

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

Construct a RRT from the information previously stored in a file by SaveRRT.

The user information stored in the node (see GetRRTNodeInfo and SetRRTNodeInfo) is lost when storing the RRT in a file. Right now we never store user info in the RRT nodes and, thus this limitation has no effect.

Parameters

pr	The set of parameters.
fname	The name of the file from where to get the information.
w	The world with the equations defining the manifold where the RRT is defined. This is not stored in the file and must be provided externally.
rrt	The RRT to define.

Definition at line 4041 of file rrt.c.

References Trrt::ambient, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CS_WD_GENERATE_SIMP_INITIAL_BOX, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, Error(), FALSE, TRRTSampleInfo::g, GetBoxInterval(), GetBoxNIntervals(), GetFileFullName(), Trrt::graph, Trrt::k, LowerLimit(), TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, Trrt::n, Trrt::nCores, NEW, Trrt::nFail, TRRTSampleInfo::nn, Trrt::ns, Trrt::parallel, TRRTSampleInfo::parent, RRT_NN_TOPOLOGY, TRRTSampleInfo::samples, SetRRTTopology(), Trrt::si, START2GOAL, Trrt::temperature, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, UpperLimit(), TRRTSampleInfo::userInfo, and Trrt::w.

Referenced by LoadAtlasRRT(), and main().

void DeleteRRT ( Trrt * rrt )

Deletes the information stored in the RRT.

Parameters

rrt	The RRT to delete.

Definition at line 4201 of file rrt.c.

References Trrt::ambient, TRRTSampleInfo::cn, DeleteBox(), Trrt::graph, Trrt::mode, TRRTSampleInfo::n, Trrt::ns, TRRTSampleInfo::samples, Trrt::si, Trrt::tp, and TWO_TREES.

Referenced by DeleteAtlasRRT(), and main().

Institut de Robòtica i Informàtica Industrial

Detailed Description

Data Structures

Macros

Functions

Macro Definition Documentation

Function Documentation

Institut de Robòtica
i Informàtica Industrial