Detailed Description

Implements the functions defining a RRT on a variety.

See Also: Trrt,rrt.h

Definition in file rrt.c.

Functions
void	NewRRTBranch (TRRTStatistics *rst)
	New branch creation. More...

void	NewRRTNoEmptyBranch (TRRTStatistics *rst)
	New no empty branch. More...

void	NewRRTTreeConnection (TRRTStatistics *rst)
	New attempt to connect the two trees. More...

void	NewRRTNoEmptyTreeConnection (TRRTStatistics *rst)
	New non-void attempt to connect the two trees. More...

void	NewRRTStep (TRRTStatistics *rst)
	New step in branch extension. More...

void	NewRRTDistanceQrand (double dqr, TRRTStatistics *rst)
	New distance to q_rand. More...

void	NewRRTQrandReached (TRRTStatistics *rst)
	Random sample reached. More...

void	NewRRTConvergenceError (TRRTStatistics *rst)
	Convergence error. More...

void	NewRRTHighCost (TRRTStatistics *rst)
	A high cost configuration is reached. More...

void	NewRRTOutOfDomain (TRRTStatistics *rst)
	The branch leaves the domain. More...

void	NewRRTDistanceIncreases (TRRTStatistics *rst)
	The distance to q_rand increase. More...

void	NewRRTCollision (TRRTStatistics *rst)
	New collision. More...

void	NewRRTTooFar (TRRTStatistics *rst)
	New large step. More...

void	NewRRTStalled (TRRTStatistics *rst)
	New stalled branch. More...

void	NewRRTSample (TRRTStatistics *rst)
	New sample. More...

void	NewRRTRandomSample (TRRTStatistics *rst)
	New random sample. More...

void	NewRRTSampleRejection (TRRTStatistics *rst)
	New sample rejection. More...

void	NewRRTCollisionCheck (TRRTStatistics *rst)
	New collision check. 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	SetRRTTopology (Tparameters pr, Trrt rrt)
	Initializes the topology array in the RRT. More...

boolean	AddBranchToRRT (Tparameters pr, boolean ccRRT, unsigned int tree, unsigned int i_near, double q_rand, double goal, unsigned int lastSample, double(costF)(Tparameters , boolean, double , void ), void costData, TRRTStatistics rst, Trrt *rrt)
	Extends a RRT as much as possible towards a given sample. More...

unsigned int	ReconstructRRTPath (Tparameters pr, unsigned int sID, double pl, double pc, unsigned int ns, double **path, Trrt rrt)
	Collects samples from the tree root to a given sample. More...

unsigned int	Steps2PathinRRT (Tparameters pr, Tvector steps, double pl, double pc, unsigned int ns, double *path, Trrt rrt)
	Defines a path from a vector of steps. More...

double	RRTPathLength (Tparameters pr, unsigned int sID, Trrt rrt)
	Path length from a sample to the root. More...

unsigned int	AddStepToRRTstar (Tparameters pr, unsigned int i_near, double q_rand, double goal, unsigned int i_goal, TRRTStatistics rst, Trrt rrt)
	Extend strategy for the RRT. More...

void	WireRRTstar (Tparameters pr, unsigned int id_new, unsigned int i_near, double gamma, unsigned int nn, unsigned int n, double *c, double h, Theap q, unsigned int t, TRRTStatistics rst, Trrt *rrt)
	Wires a node to the RRT star. More...

void	ReWireRRTstar (Tparameters pr, unsigned int id_new, double gamma, unsigned int nn, unsigned int n, double c, Tvector steps, double l, Trrt rrt)
	Rewires an RRT star. More...

void	RRTstarCloseIteration (unsigned int it, unsigned int id_goal, double time, double gamma, double times, double costs, Trrt *rrt)
	Prints information about the RRT* iteration. More...

void	BiRRTstarCloseIteration (unsigned int it, double l, double time, double gamma, double times, double costs, Trrt *rrt)
	Prints information about the BiRRT* iteration. More...

void	CopyRRTStep (void a, void b)
	Copy constructor for RRTSteps. 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	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	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	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	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	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...

Function Documentation

void NewRRTBranch ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 228 of file rrt.c.

References TRRTStatistics::nBranch.

Referenced by cBiRRT(), ccRRT(), ccTRRT(), and WireRRTstar().

void NewRRTNoEmptyBranch ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 233 of file rrt.c.

References TRRTStatistics::nNoEmptyBranch.

Referenced by cBiRRT(), ccRRT(), ccTRRT(), and WireRRTstar().

void NewRRTTreeConnection ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 238 of file rrt.c.

References TRRTStatistics::nTreeConnection.

Referenced by cBiRRT().

void NewRRTNoEmptyTreeConnection ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 243 of file rrt.c.

References TRRTStatistics::nNoEmptyTreeConnection.

Referenced by cBiRRT().

void NewRRTStep ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 253 of file rrt.c.

References TRRTStatistics::nStep.

void NewRRTDistanceQrand	(	double	dqr,
		TRRTStatistics *	rst
	)

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

Parameters

dqr	New tree-q_rand distance.
rst	The RRT statistics to modify.

Definition at line 248 of file rrt.c.

References TRRTStatistics::dQrand.

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

void NewRRTQrandReached ( TRRTStatistics * rst )

To be called every time the random sample is reached.

Parameters

rst	The RRT statistics to modify.

Definition at line 258 of file rrt.c.

References TRRTStatistics::nQrandReached.

Referenced by AddBranchToRRT().

void NewRRTConvergenceError ( TRRTStatistics * rst )

Error converging to the manifold.

Parameters

rst	The RRT statistics to modify.

Definition at line 263 of file rrt.c.

References TRRTStatistics::nConvergenceError.

Referenced by AddBranchToRRT().

void NewRRTHighCost ( TRRTStatistics * rst )

A high cost configuration is reached. Only possible in transition-based RRTs.

Parameters

rst	The RRT statistics to modify.

Definition at line 268 of file rrt.c.

References TRRTStatistics::nHighCost.

Referenced by AddBranchToRRT().

void NewRRTOutOfDomain ( TRRTStatistics * rst )

The branch extension is stopped because it leves the problem domain.

Parameters

rst	The RRT statistics to modify.

Definition at line 273 of file rrt.c.

References TRRTStatistics::nOutOfDomain.

Referenced by AddBranchToRRT().

void NewRRTDistanceIncreases ( TRRTStatistics * rst )

The branch extension is stopped because the distance to q_rand increases.

Parameters

rst	The RRT statistics to modify.

Definition at line 278 of file rrt.c.

References TRRTStatistics::nDistanceIncreases.

Referenced by AddBranchToRRT().

void NewRRTCollision ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 283 of file rrt.c.

References TRRTStatistics::nCollision.

Referenced by AddBranchToRRT().

void NewRRTTooFar ( TRRTStatistics * rst )

To be called every time a branch extension is stopped if a sample that is too far from the previous sample.

Parameters

rst	The RRT statistics to modify.

Definition at line 288 of file rrt.c.

References TRRTStatistics::nTooFar.

Referenced by AddBranchToRRT().

void NewRRTStalled ( TRRTStatistics * rst )

To be called every time a branch extension is stalled (it grows but too slow).

Parameters

rst	The RRT statistics to modify.

Definition at line 293 of file rrt.c.

References TRRTStatistics::nStalled.

Referenced by AddBranchToRRT().

void NewRRTSample ( TRRTStatistics * rst )

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

Parameters

rst	The RRT statistics to modify.

Definition at line 298 of file rrt.c.

References TRRTStatistics::nSample.

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

void NewRRTRandomSample ( TRRTStatistics * rst )

Counts the number of random samples generated.

Definition at line 303 of file rrt.c.

References TRRTStatistics::nRandom.

Referenced by RRTSample().

void NewRRTSampleRejection ( TRRTStatistics * rst )

Counts the number os random samples rejected.

Definition at line 308 of file rrt.c.

References TRRTStatistics::nRejections.

Referenced by RRTValidateSample().

void NewRRTCollisionCheck ( TRRTStatistics * rst )

New collision check.

Definition at line 313 of file rrt.c.

References TRRTStatistics::nCollisionChecks.

Referenced by AddBranchToRRT().

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 SetRRTTopology	(	Tparameters *	pr,
		Trrt *	rrt
	)

Initializes the topology array in the RRT.

Parameters

pr	The set of parameters.
rrt	The RRT where to initialize the topology.

Definition at line 757 of file rrt.c.

References CS_WD_GET_SIMP_TOPOLOGY, Error(), FALSE, Trrt::m, TOPOLOGY_S, Trrt::tp, and Trrt::w.

Referenced by InitRRT(), and LoadRRT().

boolean AddBranchToRRT	(	Tparameters *	pr,
		boolean	ccRRT,
		unsigned int	tree,
		unsigned int	i_near,
		double *	q_rand,
		double *	goal,
		unsigned int *	lastSample,
		double()(Tparameters , boolean, double , void )	costF,
		void *	costData,
		TRRTStatistics *	rst,
		Trrt *	rrt
	)

As a single branch to the RRT with the purpose of reaching a given sample.

The branch might include many samples if many small steps are performed toward de targed.

The extension proceeds as far as there is no collision, and the distance to the targed sample decreases.

During the generation of the branch we check if the global goal is reached.

Parameters

pr	The set of parameters.
ccRRT	TRUE if the branch extension has to use the ccRRT strategy proposed by Dallibart et at. and FALSE if it has to use the one by Berenson et at. (see ccRRT and cBiRRT). Note that typically, in the first case the thee is one-directional and in the second it is bidirectional (but this is not compulsatory).
tree	The current tree. Only considered in bidirectional RRTs. Otherwise all points are added in the START2GOAL tree (the only one defined).
i_near	Index of the sample in the RRT from where to start the extension.
q_rand	The targed sample. This must be given in the simplefied system.
goal	Global goal for the RRT (also given in simplified form).
lastSample	Identifier of the last sample added along the new branch. It will be i_near if the branch is empty.
costF	The cost Function used in TRRT. NULL for normal RRT exploration.
costData	The last parameter for the cost function.
rst	Statistics about the RRT construction. 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.

Definition at line 847 of file rrt.c.

References AccumulateVector(), AddNodeToRRT(), AdjustDynamicDomain(), Trrt::ambient, CS_WD_IN_COLLISION, CS_WD_NEWTON_IN_SIMP, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, Trrt::dd, Trrt::delta, DifferenceVectorTopology(), DistanceTopology(), DIVERGED, FALSE, GetParameter(), Trrt::m, Trrt::n, NEW, NewRRTCollision(), NewRRTCollisionCheck(), NewRRTConvergenceError(), NewRRTDistanceIncreases(), NewRRTHighCost(), NewRRTOutOfDomain(), NewRRTQrandReached(), NewRRTStalled(), NewRRTTooFar(), Norm(), PointInBoxTopology(), TRRTSampleInfo::samples, ScaleVector(), Trrt::si, SumVectorScale(), Trrt::tp, TransitionTestRRT(), TRUE, and Trrt::w.

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

unsigned int ReconstructRRTPath	(	Tparameters *	pr,
		unsigned int	sID,
		double *	pl,
		double *	pc,
		unsigned int *	ns,
		double ***	path,
		Trrt *	rrt
	)

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

Parameters

pr	The set of parameters.
sID	Sample to connect to the root of the tree.
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 samples in the output path.
path	The output path.
rrt	The RRT structure to query.

Returns: The size of each path step (system variables stored for each step).

Definition at line 1076 of file rrt.c.

References DeleteVector(), ReverseSamples(), RRTPathSteps(), and Steps2PathinRRT().

Referenced by PathStart2GoalInRRT(), and RRTstar().

unsigned int Steps2PathinRRT	(	Tparameters *	pr,
		Tvector *	steps,
		double *	pl,
		double *	pc,
		unsigned int *	ns,
		double ***	path,
		Trrt *	rrt
	)

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

Parameters

pr	The set of parameters.
steps	The vector of steps to follow.
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 samples in the output path.
path	The output path.
rrt	The RRT structure to query.

Returns: The size of each path step (system variables stored for each step).

Definition at line 1093 of file rrt.c.

References AddSample2Samples(), Trrt::ambient, ConnectSamples(), TRRTSampleInfo::cost, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_EPSILON, DeleteSamples(), FALSE, GetParameter(), GetVectorElement(), TRRTStep::id, INF, InitSamples(), Trrt::m, Trrt::n, TRRTSampleInfo::samples, Trrt::si, Trrt::tp, VectorSize(), Trrt::w, and Warning().

Referenced by BiRRTstar(), and ReconstructRRTPath().

double RRTPathLength	(	Tparameters *	pr,
		unsigned int	sID,
		Trrt *	rrt
	)

Computes the length of the

Parameters

pr	The set of parameteres.
sID	Sample to connect to the root of the tree.
rrt	The RRT structure to query.

Returns: The length of the path.

Definition at line 1181 of file rrt.c.

References DistanceTopology(), Trrt::m, NO_UINT, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, and Trrt::tp.

unsigned int AddStepToRRTstar	(	Tparameters *	pr,
		unsigned int	i_near,
		double *	q_rand,
		double *	goal,
		unsigned int *	i_goal,
		TRRTStatistics *	rst,
		Trrt *	rrt
	)

Extends a RRT adding a node from q_near to q_rand. This implements a RRT-Extend strategy while AddBranchToRRT implements a RRT-Connect.

Right now this is only used for the RRT* but it could be used for other types of RRT without any problem.

Parameters

pr	The set of parameters.
i_near	Index of the node in the tree from where to extend it.
q_rand	Node giving the direction to extend the tree.
goal	Global goal for the RRT. Set to NULL if goal detection is deactivated.
i_goal	Identifier of the goal sample in the RRT. NO_UINT while the goal has not been reached. This is an input/output parameter. Must be initialized to NO_UINT before the first call to this function and it will be updated inside it. Only used if goal is not NULL.
rst	Statistics on the RRT construction process (only used if GET_RRT_STATISTICS is set).
rrt	The RRT to extend.

Returns: Identifier of the node added to the RRT.

Definition at line 1970 of file rrt.c.

References AddNodeToRRT(), AdjustDynamicDomain(), Trrt::ambient, ConnectSamples(), TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, Trrt::dd, DistanceTopology(), GetParameter(), INF, Trrt::m, Trrt::n, NEW, NO_UINT, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, Trrt::tp, TRRTSampleInfo::tree, TRUE, and Trrt::w.

Referenced by BiRRTstar(), and RRTstar().

void WireRRTstar	(	Tparameters *	pr,
		unsigned int	id_new,
		unsigned int	i_near,
		double	gamma,
		unsigned int	nn,
		unsigned int *	n,
		double **	c,
		double	h,
		Theap *	q,
		unsigned int *	t,
		TRRTStatistics *	rst,
		Trrt *	rrt
	)

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

Parameters

pr	The set of parameters.
id_new	Identifier of the last node added to the tree.
i_near	Parent of the node just added to the tree.
gamma	Radius of search used to determine the neighbours.
nn	Number of neighbours.
n	Array with the indices of the neighbouring nodes.
c	Array where we store the cost to the neighbours. This is useful in re-wire to avoid recomputing symmetrical costs. (see RRTSTAR_SYMMETRIC_COST and the graph mode of Trrt).
h	Heuristic cost to the goal. Only used if the RRT is in graph mode.
q	Priority queue to propagate changes in the tree. Only used the RRT is in graph mode.
t	The trees of the neighbouring nodes. This in only relevant in bi-directional RRTs.
rst	Statistics on the RRT construction process (only used if GET_RRT_STATISTICS is set).
rrt	The RRT to wire.

Definition at line 2023 of file rrt.c.

References AddEdgeToRRT(), AddElement2Heap(), Trrt::ambient, ConnectSamples(), TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, Error(), FALSE, GetParameter(), Trrt::graph, INF, Trrt::m, Trrt::mode, Trrt::n, NEW, NewDoublePair(), NewRRTBranch(), NewRRTNoEmptyBranch(), TRRTSampleInfo::parent, TRRTSampleInfo::samples, SetRRTCostAndParent(), Trrt::si, Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and Trrt::w.

Referenced by BiRRTstar(), and RRTstar().

void ReWireRRTstar	(	Tparameters *	pr,
		unsigned int	id_new,
		double	gamma,
		unsigned int	nn,
		unsigned int *	n,
		double *	c,
		Tvector *	steps,
		double *	l,
		Trrt *	rrt
	)

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

Parameters

pr	The set of parameters.
id_new	Identifier of the last node added to the tree.
gamma	Radius of search used to determine the neighbours.
nn	Number of neighbours.
n	Array with the indices of the neighbouring nodes.
c	Array where we with the cost to the neighbours. Computed in WireRRTstar if RRTSTAR_SYMMETRIC_COST is 1 and RRT is not in graph mode.
steps	The steps forming the solution path.
l	Best estimation of the cost start-goal up to now. Only updated in bidirectional trees.
rrt	The RRT to rewire.

Definition at line 2212 of file rrt.c.

References Trrt::ambient, ChangeBiRRTSteps(), ConnectSamples(), TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, Error(), FALSE, GetParameter(), Trrt::graph, INF, Trrt::m, Trrt::mode, Trrt::n, TRRTSampleInfo::parent, TRRTSampleInfo::samples, SetRRTCostAndParent(), Trrt::si, Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and Trrt::w.

Referenced by BiRRTstar(), and RRTstar().

void RRTstarCloseIteration	(	unsigned int	it,
		unsigned int	id_goal,
		double	time,
		double	gamma,
		double *	times,
		double *	costs,
		Trrt *	rrt
	)

Prints information about the RRT* iteration.

Parameters

it	The current iteration.
id_goal	The identifier of the goal. If not found yet, NO_UINT.
time	Elapsed time from the start of the RRT*
gamma	Value of the threshold for nearest neighbours.
times	Array to store information about the execution time per iteration.
costs	Array to store information about the cost to the goal per iteration.
rrt	The RRT.

Definition at line 2294 of file rrt.c.

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

Referenced by RRTstar().

void BiRRTstarCloseIteration	(	unsigned int	it,
		double	l,
		double	time,
		double	gamma,
		double *	times,
		double *	costs,
		Trrt *	rrt
	)

Prints information about the BiRRT* iteration.

Parameters

it	The current iteration.
l	Cost of the path start/goal via s1 and s2.
time	Elapsed time from the start of the RRT*
gamma	Value of the threshold for nearest neighbours.
times	Array to store information about the execution time per iteration.
costs	Array to store information about the cost to the goal per iteration.
rrt	The RRT.

Definition at line 2323 of file rrt.c.

References INF, Trrt::ns, and RRT_VERBOSE.

Referenced by BiRRTstar().

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

Functions

Function Documentation

Institut de Robòtica
i Informàtica Industrial