Introduction

Implementation of the AtlasRRT functions.

See also: atlasrrt.h, Tatlasrrt

Definition in file atlasrrt.c.

Data Structures
struct	TAtlasRRTBranchConfig
	Configuration of the branch extension. More...

struct	TAtlasRRTBranchState
	Satus of the branch extension. More...

struct	TDynamicBranches
	Information necessary when extending a tree in dynamic mode. More...

Macros
#define	ADD_ALL 0
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	ADD_ALL_NO_REP 1
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	ADD_LAST 2
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	ADD_LAST_NO_REP 3
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	ADD_CHART_AND_LAST_NO_REP 4
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	ADD_NONE 5
	One of the addition modes for AddBranchToAtlasRRT. More...

#define	EXTEND_RRT 0
	One of the pre-defined branch configurations. More...

#define	CONNECT_RRTs 1
	One of the pre-defined branch configurations. More...

#define	CONNECT_SAMPLES 2
	One of the pre-defined branch configurations. More...

#define	CONNECT_SAFE 3
	One of the pre-defined branch configurations. More...

#define	SIMULATE_DYNAMICS 4
	One of the pre-defined branch configurations. More...

#define	EXTEND_RRT_DYNAMICS 5
	One of the pre-defined branch configurations. More...

#define	CONNECT_RRT_DYNAMICS 6
	One of the pre-defined branch configurations. More...

#define	NO_BLOCKED 0
	Branch blocked state. More...

#define	NO_DIR (1<<0)
	Branch blocked state. More...

#define	NO_POLICY (1<<1)
	Branch blocked state. More...

#define	NO_REPLAY (1<<2)
	Branch blocked state. More...

#define	SINGULARITY (1<<3)
	Branch blocked state. More...

#define	REPEATED_SAMPLE (1<<4)
	Branch blocked state. More...

#define	THE_IMPOSSIBLE (1<<5)
	Branch blocked state. More...

#define	REVISITED_CHART (1<<6)
	Branch blocked state. More...

#define	REPEATED_CHART (1<<7)
	Branch blocked state. More...

#define	TRANSITION_TEST_FAILS (1<<8)
	Branch blocked state. More...

#define	TOO_FAR (1<<9)
	Branch blocked state. More...

#define	TOO_LONG (1<<10)
	Branch blocked state. More...

#define	COLLISION (1<<11)
	Branch blocked state. More...

#define	NOT_IN_DOMAIN (1<<12)
	Branch blocked state. More...

#define	DYNAMIC_ERROR (1<<13)
	Branch blocked state. More...

#define	MAX_INT_TIME (1<<14)
	Branch blocked state. More...

#define	MAX_OPT_TIME (1<<15)
	Branch blocked state. More...

Functions
void	InitBranchConfig (unsigned int mode, TAtlasRRTBranchConfig *config)
	Initializes a branch configuration. More...

void	InitBranchState (Tparameters pr, TAtlasRRTBranchConfig config, TSampleInfo near, double randSample, double goalSample, TDynamicSpace ds, Tatlasrrt ar, TAtlasRRTBranchState state)
	Initializes the branch state. More...

boolean	canContinueBranch (boolean dynamics, TAtlasRRTBranchConfig config, TAtlasRRTBranchState state)
	Tests if the branch can be continued. More...

void	PrintBranchBlockedState (FILE f, char prefix, TAtlasRRTBranchConfig config, TAtlasRRTBranchState state)
	Prints information about the blocked status. More...

void	DeleteBranchState (TAtlasRRTBranchState *state)
	Destructor. More...

void	InitDynamicBranches (Tparameters pr, TDynamicBranches db, Tatlasrrt *ar)
	Allocates space for the generation of dynamic branches. More...

void	DeleteDynamicBranches (TDynamicBranches db, Tatlasrrt ar)
	Release memory. More...

void	InitSampleInfo (unsigned int m, unsigned int k, TSampleInfo *si)
	Initializes the space to hold a parametrized point. More...

void	SetSampleInfo (unsigned int m, unsigned int k, TSampleInfo si_dst, TSampleInfo si_src)
	Initializes a parametrized point from another parametrized point. More...

void	SetSampleInfoFromNode (unsigned int n, Tatlasrrt ar, TSampleInfo si)
	Sets the sample info to taking data from an AtlasRRT node. More...

void	DeleteSampleInfo (TSampleInfo *si)
	Deallocates a paramterized point. More...

void	NewAtlasRRTBranch (TAtlasRRTStatistics *arst)
	New branch creation. More...

void	NewAtlasRRTDynamicExtension (TAtlasRRTStatistics *arst)
	New dynamic extension. More...

void	NewAtlasRRTImprovedDynamicExtension (TAtlasRRTStatistics *arst)
	New improved dynamic extensions. More...

void	NewAtlasRRTNoEmptyBranch (TAtlasRRTStatistics *arst)
	New no empty branch. More...

void	NewAtlasRRTTreeConnection (TAtlasRRTStatistics *arst)
	New attempt to connect the two trees. More...

void	NewAtlasRRTNoEmptyTreeConnection (TAtlasRRTStatistics *arst)
	New non-void attempt to connect the two trees. More...

void	NewAtlasRRTDistanceQrand (double dqr, TAtlasRRTStatistics *arst)
	New distance to x_rand. More...

void	NewAtlasRRTStep (TAtlasRRTStatistics *arst)
	New step in branch extension. More...

void	NewAtlasRRTNoConvergentStep (TAtlasRRTStatistics *arst)
	Problems with convergence. More...

void	NewAtlasRRTQrandReached (double dqr, TAtlasRRTStatistics *arst)
	Random sample reached. More...

void	NewAtlasRRTNotInDomain (TAtlasRRTStatistics *arst)
	New node not in the given domain. More...

void	NewAtlasRRTCollision (TAtlasRRTStatistics *arst)
	New collision. More...

void	NewAtlasRRTTooLong (TAtlasRRTStatistics *arst)
	New long branch. More...

void	NewAtlasRRTNoAdvanceDirection (TAtlasRRTStatistics *arst)
	New lack of advance direction. More...

void	NewAtlasRRTNoLQRPolicy (TAtlasRRTStatistics *arst)
	New lack of LQR policy. More...

void	NewAtlasRRTLQRToptIncreases (TAtlasRRTStatistics *arst)
	New increment in LQR optimal time. More...

void	NewAtlasRRTImpossible (TAtlasRRTStatistics *arst)
	New impossible situation. More...

void	NewAtlasRRTNoReplay (TAtlasRRTStatistics *arst)
	New replay error. More...

void	NewAtlasRRTTooFar (TAtlasRRTStatistics *arst)
	New large step. More...

void	NewAtlasRRTRevisitedChart (TAtlasRRTStatistics *arst)
	New revisited chart. More...

void	NewAtlasRRTRepeatedChart (TAtlasRRTStatistics *arst)
	New repeated chart. More...

void	NewAtlasRRTChartBlocked (TAtlasRRTStatistics *arst)
	New branch blocked at chart creation. More...

void	NewAtlasRRTIntTimeReached (TAtlasRRTStatistics *arst)
	New branch stopped due to time limits. More...

void	NewAtlasRRTNoAction (TAtlasRRTStatistics *arst)
	New branch stopped for lack of valid action. More...

void	NewAtlasRRTLQRPolicy (double s, TAtlasRRTStatistics *arst)
	New action computation from an LQR policy. More...

void	NewAtlasRRTLQRinput (double s, TAtlasRRTStatistics *arst)
	New LQR input executed. More...

void	NewAtlasRRTLQRComputePolicy (double t, TAtlasRRTStatistics *arst)
	New LQR steer executed. More...

void	NewAtlasRRTBroyden (unsigned int it, TAtlasRRTStatistics *arst)
	New call to Broyden. More...

void	NewAtlasRRTTransTestFail (TAtlasRRTStatistics *arst)
	The transition test failed. More...

void	NewAtlasRRTDynamicError (TAtlasRRTStatistics *arst)
	Error in the dynamic computations. More...

void	NewAtlasRRTInitChartError (TAtlasRRTStatistics *arst)
	New chart due to error when trying to create a chart. More...

void	NewAtlasRRTOutOfChart (TAtlasRRTStatistics *arst)
	New chart due to radius. More...

void	NewAtlasRRTLargeCurvature (TAtlasRRTStatistics *arst)
	New chart due large curvature between two charts. More...

void	NewAtlasRRTDirLargeCurvature (TAtlasRRTStatistics *arst)
	New chart due to the curvature in the expansion direction. More...

void	NewAtlasRRTStepReduction (TAtlasRRTStatistics *arst)
	Step reduction. More...

void	NewAtlasRRTSample (TAtlasRRTStatistics *arst)
	New sample. More...

void	NewAtlasRRTRepeatedSample (TAtlasRRTStatistics *arst)
	New repeated sample. More...

void	NewAtlasRRTChart (TAtlasRRTStatistics *arst)
	New Chart. More...

void	NewAtlasRRTNoConnectToParent (TAtlasRRTStatistics *arst)
	A chart that does not intersect with its parent. More...

void	NewAtlasRRTBlockBySingularity (TAtlasRRTStatistics *arst)
	Number of times we try to create a char at a singular regions. More...

void	NewAtlasRRTRandomSample (TAtlasRRTStatistics *arst)
	New random sample. More...

void	NewAtlasRRTSampleRejection1 (TAtlasRRTStatistics *arst)
	New sample rejection. More...

void	NewAtlasRRTSampleRejection2 (TAtlasRRTStatistics *arst)
	New sample rejection. More...

void	NewAtlasRRTSampleRejection3 (TAtlasRRTStatistics *arst)
	New sample rejection. More...

void	NewAtlasRRTSampleRejection4 (TAtlasRRTStatistics *arst)
	New sample rejection. More...

void	NewAtlasRRTCollisionCheck (TAtlasRRTStatistics *arst)
	New collision check. More...

void	NewAverageNumNeighbours (double an, TAtlasRRTStatistics *arst)
	Number of neighbours per chart. More...

void	InitAtlasRRTStatistics (TAtlasRRTStatistics *arst)
	Init the Atlas RRT statistics. More...

void	AccumulateAtlasRRTStatistics (TAtlasRRTStatistics arst1, TAtlasRRTStatistics arst2)
	Accumulates two sets of Atlas RRT statistics. More...

void	PrintAtlasRRTStatistics (Tatlasrrt ar, TAtlasRRTStatistics arst)
	Prints the summary of atlasRRT statistics. More...

void	DeleteAtlasRRTStatistics (TAtlasRRTStatistics *arst)
	Destructor. More...

void	UpdateLQRPolicy (Tparameters pr, TAtlasRRTBranchConfig config, Tchart c_lqr, double t_near, double t_rand, TAtlasRRTBranchState state, void st, TDynamicSpace ds, Tatlasrrt *ar)
	Recomputes the LQR policy. More...

double	AddBranchToAtlasRRT (Tparameters pr, unsigned int it, TAtlasRRTBranchConfig config, unsigned int nsReplay, double *replayPath, boolean replayNewChart, double replayTime, double replayCost, double action, unsigned int i_near, double randSample, double goalSample, void st, TAtlasRRTBranchState state, unsigned int ns, double *path, boolean newChart, double tStep, double cStep, TSampleInfo lastSample, TDynamicSpace ds, Tatlasrrt *ar)
	Adds a new branch to the AtlasRRT. More...

double	AddBranchToAtlasDynamicRRT (Tparameters pr, unsigned int oneAction, unsigned int t2g, unsigned int it, TAtlasRRTBranchConfig config, unsigned int i_near, double randSample, double goalSample, void st, TDynamicBranches db, unsigned int lastSample, boolean reachedGoal, Tatlasrrt *ar)
	Adds a new branch to the AtlasRRT for problems with dynamics. More...

void	NewTemptativeSample (Tparameters pr, unsigned int it, TAtlasRRTBranchConfig config, TAtlasRRTBranchState state, TSampleInfo current, double action, TSampleInfo next, TSampleInfo rand, double randSample, void st, TDynamicSpace ds, Tatlasrrt *ar)
	Generates a new sample in between x_near and x_rand. More...

double	AddSample2AtlasRRT (Tparameters pr, unsigned int tree, unsigned int currentID, unsigned int nextChart, double nextParam, double nextSample, double dp, double cost, boolean blocked, double action, double time, unsigned int ns, double path, double actions, double times, Tatlasrrt *ar)
	Adds a sample to the AtlasRRT. More...

boolean	AddChart2AtlasRRT (Tparameters pr, unsigned int tree, unsigned int it, TAtlasRRTBranchConfig config, TAtlasRRTBranchState state, TSampleInfo currentSampleInfo, boolean intersectParent, Tatlasrrt ar)
	Adds a chart to the Atlas RRT. More...

void	PopulateWithSamples (Tparameters pr, unsigned int id, Tatlasrrt ar)
	Assign samples to a newly created chart. More...

void	AddChart2Tree (unsigned int tree, unsigned int chartId, Tatlasrrt *ar)
	Add a chart to a tree. More...

boolean	PointTowardRandSample (TAtlasRRTBranchConfig config, unsigned int samplingMode, double randSample, TAtlasRRTBranchState state, TSampleInfo current, TSampleInfo rand, Tatlasrrt ar)
	Determines the motion on the current chart to approach x_rand. More...

unsigned int	ReconstructAtlasRRTPath (Tparameters pr, unsigned int sID, double pl, unsigned int ns, double *path, Tatlasrrt ar)
	Collects samples from the tree root to a given sample. More...

unsigned int	Steps2PathinAtlasRRT (Tparameters pr, Tvector steps, double pl, double pc, unsigned int ns, double *path, Tatlasrrt ar)
	Defines a path from a vector of steps. More...

void	SmoothPathInAtlasRRT (Tparameters pr, unsigned int sID, Tatlasrrt ar)
	Local optimiziation of the path to a node. More...

unsigned int	AddStepToAtlasRRTstar (Tparameters pr, unsigned int it, boolean expand2Goal, boolean onManifold, unsigned int i_near, double x_rand, unsigned int id_goal, double goal, TAtlasRRTStatistics arst, Tatlasrrt ar)
	Adds a node to an AtlasRRT*. More...

void	WireAtlasRRTstar (Tparameters pr, unsigned int id_new, unsigned int i_near, double gamma, unsigned int nn, unsigned int n, double *c, double h, Theap q, unsigned int t, TAtlasRRTStatistics arst, Tatlasrrt *ar)
	Wires a node to the AtlasRRT star. More...

void	ReWireAtlasRRTstar (Tparameters pr, unsigned int id_new, double gamma, unsigned int nn, unsigned int n, double c, Tvector steps, double l, TAtlasRRTStatistics arst, Tatlasrrt *ar)
	Rewires an AtlasRRT star. More...

void	AtlasRRTstarCloseIteration (unsigned int it, unsigned int id_goal, double time, double gamma, double times, double costs, Tatlasrrt *ar)
	Prints information about the AtlasRRT* iteration. More...

void	AtlasBiRRTstarCloseIteration (unsigned int it, double l, double time, double gamma, double times, double costs, Tatlasrrt *ar)
	Prints information about the BiAtlasRRT* iteration. More...

void	SaveAtlasRRTSampleInfo (FILE f, TSampleInfo si, Tatlasrrt *ar)
	Saves the information associated with a sample in an AtlasRRT. More...

void	SaveAtlasRRTChartInfo (FILE f, TChartInfo ci, Tatlasrrt *ar)
	Saves the information associated with a chart in an AtlasRRT. More...

void	LoadAtlasRRTSampleInfo (FILE f, TSampleInfo si, Tatlasrrt *ar)
	Reads the information associated with a sample in an AtlasRRT. More...

void	LoadAtlasRRTChartInfo (FILE f, TChartInfo ci, Tatlasrrt *ar)
	Reads the information associated with a chart in an AtlasRRT. More...

void	PlotQrand (Tparameters pr, char prefix, unsigned int inear, unsigned int c_rand, double t_rand, double x_rand, unsigned int xID, unsigned int yID, unsigned int zID, Tatlasrrt *ar)
	Plots a point where x_rang is and a line to its chart. More...

void	PlotConnection (Tparameters pr, char prefix, unsigned int target, unsigned int near, unsigned int end, unsigned int xID, unsigned int yID, unsigned int zID, Tatlasrrt *ar)
	Graphical representation of the attempt to connect two trees. More...

boolean	RandomPointInAtlasTree (Tparameters pr, double scale, unsigned int tree, unsigned int nm, double t, double p, Tatlasrrt *ar)
	Selects a random point in the set of charts of the tree. More...

void	InitAtlasRRT (Tparameters pr, boolean parallel, double ps, unsigned int mode, boolean graph, double pg, TAtlasBase w, Tatlasrrt *ar)
	Defines a Atlas-RRT from a given point. More...

boolean	AtlasRRTSample (Tparameters pr, unsigned int samplingMode, unsigned int it, unsigned int tree, double goal, double scale, boolean exploration, boolean onManifold, unsigned int c_rand, double t_rand, double x_rand, TAtlasRRTStatistics arst, Tatlasrrt *ar)
	Generates a random sample to expand the AtlasRRT. More...

boolean	AtlasRRTValidateSample (Tparameters pr, unsigned int c_rand, double t_rand, double x_rand, unsigned int tree, boolean expand2goal, unsigned int lastNN2Goal, double goal, double l, double h, unsigned int i_near, TAtlasRRTStatistics arst, Tatlasrrt ar)
	Validates a sample generate with AtlasRRTSample. More...

void	RefineTrajectory (Tparameters pr, boolean id, unsigned int ns, double *path, double actions, double times, Tatlasrrt ar)
	Increases the resolution of a trajectory. More...

void	AtlasRRTSimulate (Tparameters pr, unsigned int da, double u, unsigned int steps, unsigned int ns, double path, double actions, double times, Tatlasrrt ar)
	Dynamic simulation with a given action. More...

boolean	AtlasRRT (Tparameters pr, double pg, double time, double pl, unsigned int ns, double *path, unsigned int da, double *actions, double times, TAtlasRRTStatistics str, Tatlasrrt ar)
	Extends a Atlas-RRT until we reach a targed point. More...

boolean	AtlasTRRT (Tparameters pr, double pg, double time, double pl, double pc, unsigned int ns, double **path, double(costF)(Tparameters , boolean, double , void ), void costData, TAtlasRRTStatistics str, Tatlasrrt ar)
	Extends a Atlas-TRRT until we reach a targed point. More...

boolean	AtlasRRTstar (Tparameters pr, double pg, unsigned int it, double times, double costs, double planningTime, double pl, unsigned int ns, double **path, TAtlasRRTStatistics str, Tatlasrrt *ar)
	Optimal AtlasRRT on manifolds. More...

boolean	AtlasBiRRTstar (Tparameters pr, double pg, unsigned int it, double times, double costs, double planningTime, double pl, unsigned int ns, double **path, TAtlasRRTStatistics str, Tatlasrrt *ar)
	Bidirectional version of AtlasRRTstar. More...

unsigned int	GetAtlasRRTNumNodes (Tatlasrrt *ar)
	Number of nodes in the AtlasRRT. More...

unsigned int	GetAtlasRRTNumCharts (Tatlasrrt *ar)
	Number of charts in the AtlasRRT. More...

unsigned int	GetRRTNNInChart (Tparameters pr, unsigned int t2g, unsigned int tree, unsigned int chartId, double x_rand, double d, Tatlasrrt ar)
	Selects the nearest neighbour from the random point in a given chart. More...

unsigned int	Time2GoNNToTree (Tparameters pr, unsigned int tree, unsigned int goal, Tatlasrrt ar)
	Time to go nearest neighbour of a tree to a given sample. More...

void	PlotAtlasRRT (char prefix, int argc, char arg, Tparameters pr, unsigned int xID, unsigned int yID, unsigned int zID, double p1, double p2, Tatlasrrt *ar)
	Pots a projection of a Atlas-RRT. More...

unsigned int	AtlasRRTMemSize (Tatlasrrt *ar)
	Memory used by a given atlasRRT. More...

void	SaveAtlasRRT (Tparameters pr, char prefix, Tatlasrrt *ar)
	Stores the Atlas-RRT information on a file. More...

void	LoadAtlasRRT (Tparameters pr, char prefix, TAtlasBase w, Tatlasrrt ar)
	Defines an atlasRRT from the information on a file. More...

void	PrintAtlasRRTDefines (FILE *f)
	Prints defines. More...

void	DeleteAtlasRRT (Tatlasrrt *ar)
	Destructor. More...

Macro Definition Documentation

◆ ADD_ALL

#define ADD_ALL 0

Adds all nodes generated in the branch extension.

Definition at line 32 of file atlasrrt.c.

◆ ADD_ALL_NO_REP

#define ADD_ALL_NO_REP 1

Adds all nodes generated in the branch extension, but avoiding repetitions.

Definition at line 40 of file atlasrrt.c.

◆ ADD_LAST

#define ADD_LAST 2

Adds anly the last generated node during the branch extension.

Definition at line 47 of file atlasrrt.c.

◆ ADD_LAST_NO_REP

#define ADD_LAST_NO_REP 3

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

Definition at line 55 of file atlasrrt.c.

◆ ADD_CHART_AND_LAST_NO_REP

#define ADD_CHART_AND_LAST_NO_REP 4

Adds the last generated node during the branch extension and all the samples where a chart is generated. In all cases repetitions are avoided.

Definition at line 64 of file atlasrrt.c.

◆ ADD_NONE

#define ADD_NONE 5

Do not add any node during the branch extension.

Definition at line 71 of file atlasrrt.c.

◆ EXTEND_RRT

#define EXTEND_RRT 0

Extends a AtlasRRT towards a random sample.

All the generated samples are added to the RRT.

Definition at line 84 of file atlasrrt.c.

◆ CONNECT_RRTs

#define CONNECT_RRTs 1

Extends a AtlasRRT towards a node already in a RRT (i.e., tries to connect a the two RRTs in a bi-directinal RRT).

All the generated samples are added to the RRT.

Definition at line 94 of file atlasrrt.c.

◆ CONNECT_SAMPLES

#define CONNECT_SAMPLES 2

Checks if it is possible to connect two samples on the manifold.

Definition at line 101 of file atlasrrt.c.

◆ CONNECT_SAFE

#define CONNECT_SAFE 3

Checks if it is possible to connect two samples on the manifold but without checking for collisions (the connection is already known to be collision free).

Definition at line 110 of file atlasrrt.c.

◆ SIMULATE_DYNAMICS

#define SIMULATE_DYNAMICS 4

Simulates the effects of a particular action taking into account dynamics.

Definition at line 117 of file atlasrrt.c.

◆ EXTEND_RRT_DYNAMICS

#define EXTEND_RRT_DYNAMICS 5

Simulates the effects of a particular action taking into account dynamics. This is used when extending an RRT towards a random sample.

Definition at line 125 of file atlasrrt.c.

◆ CONNECT_RRT_DYNAMICS

#define CONNECT_RRT_DYNAMICS 6

Simulates the effects of a particular action taking into account dynamics. This is used when extending an RRT towards a point already in the RRT (i.e., when trying to connect trees)

Definition at line 134 of file atlasrrt.c.

◆ NO_BLOCKED

#define NO_BLOCKED 0

The blocked state is defined as a bit-wise and of different possible reasons to stop the branch extension.

The branch is not blocked so far.

Definition at line 148 of file atlasrrt.c.

◆ NO_DIR

#define NO_DIR (1<<0)

The branch is blocked because we can not determine a clear advance direction (the x_rand projects too close to x_near).

Definition at line 156 of file atlasrrt.c.

◆ NO_POLICY

#define NO_POLICY (1<<1)

The branch is blocked because we can not determine a LQR policy.

Definition at line 163 of file atlasrrt.c.

◆ NO_REPLAY

#define NO_REPLAY (1<<2)

The branch is blocked because we do not have more data to replay a previous branch extension.

Definition at line 170 of file atlasrrt.c.

◆ SINGULARITY

#define SINGULARITY (1<<3)

The branch is blocked because we reached a singular region (a region where we can not define charts).

Definition at line 178 of file atlasrrt.c.

◆ REPEATED_SAMPLE

#define REPEATED_SAMPLE (1<<4)

The branch is blocked because we repeated a point in the RRT.

Definition at line 185 of file atlasrrt.c.

◆ THE_IMPOSSIBLE

#define THE_IMPOSSIBLE (1<<5)

The branch is blocked because the impossible happened. We have several imposible situations that may appear due to (numerical) errors. For instance, we must always be able to create a new chart close enough to a previous chart. If this is not true, the impossible happened.

Typically an error is triggered in this cases.

Definition at line 196 of file atlasrrt.c.

◆ REVISITED_CHART

#define REVISITED_CHART (1<<6)

The branch is blocked because the branch entered in a previously created chart and the current branch extension configuration prevends this to happen. This is typically used to avoid branch crossing in kinematic planning.

Definition at line 205 of file atlasrrt.c.

◆ REPEATED_CHART

#define REPEATED_CHART (1<<7)

The branch is blocked because the branch entered in a chart already visited along the branch and the current branch extension configuration prevends this to happen. This is typically used to avoid branch crossing in kinematic planning.

Definition at line 214 of file atlasrrt.c.

◆ TRANSITION_TEST_FAILS

#define TRANSITION_TEST_FAILS (1<<8)

The branch is blocked because a transition test failed.

Definition at line 221 of file atlasrrt.c.

◆ TOO_FAR

#define TOO_FAR (1<<9)

The branch is blocked because it is already too far from the origin.

Definition at line 228 of file atlasrrt.c.

◆ TOO_LONG

#define TOO_LONG (1<<10)

The branch is blocked because it is already too long.

Definition at line 235 of file atlasrrt.c.

◆ COLLISION

#define COLLISION (1<<11)

The branch is blocked because a collision is detected.

Definition at line 242 of file atlasrrt.c.

◆ NOT_IN_DOMAIN

#define NOT_IN_DOMAIN (1<<12)

The branch is blocked because the branch reaches the limit of the domain.

Definition at line 249 of file atlasrrt.c.

◆ DYNAMIC_ERROR

#define DYNAMIC_ERROR (1<<13)

The branch is blocked because there is an error in a dynamic-related computation.

Definition at line 256 of file atlasrrt.c.

◆ MAX_INT_TIME

#define MAX_INT_TIME (1<<14)

The branch is blocked because we reached the maximum integration time.

Definition at line 263 of file atlasrrt.c.

◆ MAX_OPT_TIME

#define MAX_OPT_TIME (1<<15)

The branch is blocked because the LQR optimal policy would take too long to reach the goal.

Definition at line 270 of file atlasrrt.c.

Function Documentation

◆ InitBranchConfig()

void InitBranchConfig	(	unsigned int	mode,
		TAtlasRRTBranchConfig *	config
	)

Initializes a branch configuration with one of the predefined pre-sets.

Note that the particular algorithms may use one of the pre-sets and then adjust several of the fields at its convenience.

Parameters

mode	The mode. See EXTEND_RRT, CONNECT_RRTs, CONNECT_SAMPLES, CONNECT_SAFE SIMULATE_DYNAMICS
config	The branch configuration to initizlize.

Definition at line 2586 of file atlasrrt.c.

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

◆ InitBranchState()

void InitBranchState	(	Tparameters *	pr,
		TAtlasRRTBranchConfig *	config,
		TSampleInfo *	near,
		double *	randSample,
		double *	goalSample,
		TDynamicSpace *	ds,
		Tatlasrrt *	ar,
		TAtlasRRTBranchState *	state
	)

Sets the values of the state structure to start a branch creation.

Parameters

pr	The set of parameters.
config	The branch mode configuration.
near	The information of the nearest node (node to extend) from the RRT.
randSample	Ramdom sample guiding the expansion.
goalSample	Goal sample of the path planning process.
ds	Space for dynamic computations. Only required if a time2go metric is used (in problems with dynamics!). Otherwise it can be NULL.
ar	The AtlasRRT
state	The branch state structure to initialize.

Definition at line 2403 of file atlasrrt.c.

Referenced by AddBranchToAtlasRRT().

◆ canContinueBranch()

boolean canContinueBranch	(	boolean	dynamics,
		TAtlasRRTBranchConfig *	config,
		TAtlasRRTBranchState *	state
	)

Tests if the branch can be further extended.

Parameters

dynamics	TRUE if the problem has dynamic constratins.
config	The branch extension configuration.
state	The current branch state.

Returns: TRUE if everything is ok and the branch can be further extended.

Definition at line 2576 of file atlasrrt.c.

References TAtlasRRTBranchState::blocked, NO_BLOCKED, TAtlasRRTBranchState::reachedGoal, TAtlasRRTBranchState::reachedQrand, and TAtlasRRTBranchState::reachedTmpQrand.

Referenced by AddBranchToAtlasRRT().

◆ PrintBranchBlockedState()

void PrintBranchBlockedState	(	FILE *	f,
		char *	prefix,
		TAtlasRRTBranchConfig *	config,
		TAtlasRRTBranchState *	state
	)

Prints the reason to stop a branch extension.

If the branch is not blocked, nothing is printed.

Parameters

f	The file where to write the information (typically stderr).
prefix	String to print before the information (typically white spaces used for indent).
config	Branch configuration.
state	The state with the information to print.

Definition at line 2525 of file atlasrrt.c.

References TAtlasRRTBranchState::blocked, COLLISION, TAtlasRRTBranchConfig::currentMaxIntTime, DYNAMIC_ERROR, TAtlasRRTBranchState::intTime, MAX_INT_TIME, MAX_OPT_TIME, NO_DIR, NO_POLICY, NO_REPLAY, NOT_IN_DOMAIN, TAtlasRRTBranchState::reachedGoal, TAtlasRRTBranchState::reachedQrand, TAtlasRRTBranchState::reachedTmpQrand, REPEATED_CHART, REPEATED_SAMPLE, REVISITED_CHART, SINGULARITY, THE_IMPOSSIBLE, TOO_FAR, TOO_LONG, and TRANSITION_TEST_FAILS.

Referenced by AddBranchToAtlasDynamicRRT(), and AddBranchToAtlasRRT().

◆ DeleteBranchState()

void DeleteBranchState ( TAtlasRRTBranchState * state )

Releases the memory allocated by InitBranchState but keeps all the information flags.

Parameters

state The structure to deallocate.

Definition at line 2568 of file atlasrrt.c.

References TAtlasRRTBranchState::c, TAtlasRRTBranchState::chartInAtlas, DeleteChart(), DeleteVector(), TAtlasRRTBranchState::deltaParam, and TAtlasRRTBranchState::traversedCharts.

Referenced by AddBranchToAtlasRRT().

◆ InitDynamicBranches()

void InitDynamicBranches	(	Tparameters *	pr,
		TDynamicBranches *	db,
		Tatlasrrt *	ar
	)

Allocates space for the generation of dynamic branches (one for each possible action). Such branches are simulated when extending RRTs on problems with dynamic constraints.

Parameters

pr	The set of parameters.
db	The dinamic branch structure to initialize.
ar	The atlasRRT.

Definition at line 3552 of file atlasrrt.c.

References TDynamicBranches::action, TDynamicBranches::costReplay, Tatlasrrt::da, TDynamicBranches::dAction, InitSampleInfo(), Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::nda, NEW, TDynamicBranches::newChartReplay, TDynamicBranches::nsReplay, TDynamicBranches::qrandb, TDynamicBranches::sample, TDynamicBranches::state, TDynamicBranches::statesReplay, TDynamicBranches::timeReplay, and TDynamicBranches::tr.

Referenced by AtlasRRT().

◆ DeleteDynamicBranches()

void DeleteDynamicBranches	(	TDynamicBranches *	db,
		Tatlasrrt *	ar
	)

Releases the memory allocated wiht InitDynamicBranches.

Parameters

db	The dinamic branch structure to release.
ar	The atlasRRT.

Definition at line 3596 of file atlasrrt.c.

References TDynamicBranches::action, TDynamicBranches::costReplay, TDynamicBranches::dAction, DeleteSampleInfo(), Tatlasrrt::nda, TDynamicBranches::newChartReplay, TDynamicBranches::nsReplay, TDynamicBranches::qrandb, TDynamicBranches::sample, TDynamicBranches::state, TDynamicBranches::statesReplay, TDynamicBranches::timeReplay, and TDynamicBranches::tr.

Referenced by AtlasRRT().

◆ InitSampleInfo()

void InitSampleInfo	(	unsigned int	m,
		unsigned int	k,
		TSampleInfo *	si
	)

Allocates space for a parametrized point.

Parameters

m	The dimension of the ambien space.
k	The dimension of the manifold.
si	The parametrized point to allocate.

Definition at line 2728 of file atlasrrt.c.

References TSampleInfo::c, TSampleInfo::generateChart, TSampleInfo::id, TSampleInfo::lsc, NEW, NO_UINT, TSampleInfo::pc, TSampleInfo::s, and TSampleInfo::t.

Referenced by AddBranchToAtlasRRT(), and InitDynamicBranches().

◆ SetSampleInfo()

void SetSampleInfo	(	unsigned int	m,
		unsigned int	k,
		TSampleInfo *	si_dst,
		TSampleInfo *	si_src
	)

Copy constructor.

Parameters

m	The dimension of the ambien space.
k	The dimension of the manifold.
si_dst	The point to set. It is assumed to be initialzied.
si_src	The point from where to get the data.

Definition at line 2739 of file atlasrrt.c.

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

Referenced by AddBranchToAtlasRRT(), AddChart2AtlasRRT(), and SetSampleInfoFromNode().

◆ SetSampleInfoFromNode()

void SetSampleInfoFromNode	(	unsigned int	n,
		Tatlasrrt *	ar,
		TSampleInfo *	si
	)

Copies the informaion of an AtlasRRT node.

Note that this does not allocate space.

Parameters

n	Index of the node to use in the initialization.
ar	The Atlas RRT.
si	The parametrized point to allocate.

Definition at line 2751 of file atlasrrt.c.

References Tatlasrrt::k, Tatlasrrt::m, SetSampleInfo(), and Tatlasrrt::si.

Referenced by AddBranchToAtlasRRT().

◆ DeleteSampleInfo()

void DeleteSampleInfo ( TSampleInfo * si )

Destructor.

Parameters

si	The parametrized point to deallocate.

Definition at line 2756 of file atlasrrt.c.

References TSampleInfo::s, and TSampleInfo::t.

Referenced by AddBranchToAtlasRRT(), and DeleteDynamicBranches().

◆ NewAtlasRRTBranch()

void NewAtlasRRTBranch ( TAtlasRRTStatistics * arst )

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

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1159 of file atlasrrt.c.

References TAtlasRRTStatistics::nBranch.

Referenced by AtlasRRT(), and WireAtlasRRTstar().

◆ NewAtlasRRTDynamicExtension()

void NewAtlasRRTDynamicExtension ( TAtlasRRTStatistics * arst )

To be called every time we start a new extension to an atlasRRT branch.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1164 of file atlasrrt.c.

References TAtlasRRTStatistics::nDynamicExtension.

Referenced by AddBranchToAtlasDynamicRRT().

◆ NewAtlasRRTImprovedDynamicExtension()

void NewAtlasRRTImprovedDynamicExtension ( TAtlasRRTStatistics * arst )

To be called every time a dynamic extension gets closer to the random sample (distance from the end of the branch to x_rand is smaller than the distance from x_near to x_rand).

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1169 of file atlasrrt.c.

References TAtlasRRTStatistics::nImprovedDynamicExtension.

Referenced by AddBranchToAtlasDynamicRRT().

◆ NewAtlasRRTNoEmptyBranch()

void NewAtlasRRTNoEmptyBranch ( TAtlasRRTStatistics * arst )

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

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1174 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoEmptyBranch.

Referenced by AtlasRRT(), and WireAtlasRRTstar().

◆ NewAtlasRRTTreeConnection()

void NewAtlasRRTTreeConnection ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1179 of file atlasrrt.c.

References TAtlasRRTStatistics::nTreeConnection.

Referenced by AtlasRRT().

◆ NewAtlasRRTNoEmptyTreeConnection()

void NewAtlasRRTNoEmptyTreeConnection ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1184 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoEmptyTreeConnection.

Referenced by AtlasRRT().

◆ NewAtlasRRTDistanceQrand()

void NewAtlasRRTDistanceQrand	(	double	dqr,
		TAtlasRRTStatistics *	arst
	)

New distance from x_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-x_rand distance.
arst	The AtlasRRT statistics to modify.

Definition at line 1189 of file atlasrrt.c.

References TAtlasRRTStatistics::dQrand.

Referenced by AtlasRRT().

◆ NewAtlasRRTStep()

void NewAtlasRRTStep ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1194 of file atlasrrt.c.

References TAtlasRRTStatistics::nStep.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTNoConvergentStep()

void NewAtlasRRTNoConvergentStep ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1199 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoConvergent.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTQrandReached()

void NewAtlasRRTQrandReached	(	double	dqr,
		TAtlasRRTStatistics *	arst
	)

To be called every time the random sample is reached.

Parameters

dqr	Distance to x_rand when we considere it reached. This is only relevant in problems with dynamics and using LQR as a steering method.
arst	The AtlasRRT statistics to modify.

Definition at line 1204 of file atlasrrt.c.

References TAtlasRRTStatistics::dQrandReached, and TAtlasRRTStatistics::nQrandReached.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTNotInDomain()

void NewAtlasRRTNotInDomain ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1213 of file atlasrrt.c.

References TAtlasRRTStatistics::nNotInDomain.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTCollision()

void NewAtlasRRTCollision ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1218 of file atlasrrt.c.

References TAtlasRRTStatistics::nCollision.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTTooLong()

void NewAtlasRRTTooLong ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1223 of file atlasrrt.c.

References TAtlasRRTStatistics::nTooLong.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTNoAdvanceDirection()

void NewAtlasRRTNoAdvanceDirection ( TAtlasRRTStatistics * arst )

The advance direction can not be determined (the projection is too close to current sample).

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1228 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoDir.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

◆ NewAtlasRRTNoLQRPolicy()

void NewAtlasRRTNoLQRPolicy ( TAtlasRRTStatistics * arst )

The LQR policy can not be computed safely

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1233 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoPolicy.

Referenced by UpdateLQRPolicy().

◆ NewAtlasRRTLQRToptIncreases()

void NewAtlasRRTLQRToptIncreases ( TAtlasRRTStatistics * arst )

The LQR policy is not applied since the optimal time increases.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1238 of file atlasrrt.c.

References TAtlasRRTStatistics::nToptIncrements.

Referenced by UpdateLQRPolicy().

◆ NewAtlasRRTImpossible()

void NewAtlasRRTImpossible ( TAtlasRRTStatistics * arst )

An impossible situation is detected.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1243 of file atlasrrt.c.

References TAtlasRRTStatistics::nImpossible.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTNoReplay()

void NewAtlasRRTNoReplay ( TAtlasRRTStatistics * arst )

A replay branch is stopped due to lack of data.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1248 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoReplay.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTTooFar()

void NewAtlasRRTTooFar ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1253 of file atlasrrt.c.

References TAtlasRRTStatistics::nTooFar.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTRevisitedChart()

void NewAtlasRRTRevisitedChart ( TAtlasRRTStatistics * arst )

To be called every time a branch extension is stopped to avoid revisiting a previously existing chart.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1258 of file atlasrrt.c.

References TAtlasRRTStatistics::nRevisitedChart.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTRepeatedChart()

void NewAtlasRRTRepeatedChart ( TAtlasRRTStatistics * arst )

To be called every time a branch extension is stopped to avoid re-entering a chart already visited along the branch (aka repated chart).

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1263 of file atlasrrt.c.

References TAtlasRRTStatistics::nRepeatedChart.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTChartBlocked()

void NewAtlasRRTChartBlocked ( TAtlasRRTStatistics * arst )

To be called every time a branch extension is stopped after creating a chart. This typically happens when the projection of the random sample in the new chart is not giving a clear direction to advance (it is on the center of the new chart).

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1268 of file atlasrrt.c.

References TAtlasRRTStatistics::nChartBlocked.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTIntTimeReached()

void NewAtlasRRTIntTimeReached ( TAtlasRRTStatistics * arst )

To be called every time a branch extension is stopped when the integration time is reached.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1273 of file atlasrrt.c.

References TAtlasRRTStatistics::nIntTimeReached.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTNoAction()

void NewAtlasRRTNoAction ( TAtlasRRTStatistics * arst )

To be called every time a branch extension is stopped when there is no valid action to keep on growing the branch. This may potentially happen when using the time2go metric.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1278 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoAction.

Referenced by AddBranchToAtlasDynamicRRT().

◆ NewAtlasRRTLQRPolicy()

void NewAtlasRRTLQRPolicy	(	double	s,
		TAtlasRRTStatistics *	arst
	)

To be called every time compute the action to execute from a policy computed using LQR.

Parameters

s	Number of saturated actions.
arst	The AtlasRRT statistics to modify.

Definition at line 1283 of file atlasrrt.c.

References TAtlasRRTStatistics::nLQRPolicy, and TAtlasRRTStatistics::satAction.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTLQRinput()

void NewAtlasRRTLQRinput	(	double	s,
		TAtlasRRTStatistics *	arst
	)

Collects statistics on whether or not the LQR actions are saturated. If so, the action cost (parameter TAU) must be increased.

Parameters

s	The ratio of saturated actions (0=none 1=all).
arst	The AtlasRRT statistics to modify.

◆ NewAtlasRRTLQRComputePolicy()

void NewAtlasRRTLQRComputePolicy	(	double	t,
		TAtlasRRTStatistics *	arst
	)

Collects statistic on the value of the optimal time resulting from calling LQRComputePolicy.

Parameters

t	The optimal time as determined by LQRComputePolicy.
arst	The AtlasRRT statistics to modify.

Definition at line 1292 of file atlasrrt.c.

References TAtlasRRTStatistics::nLQRComputePolicy, and TAtlasRRTStatistics::topt.

Referenced by UpdateLQRPolicy().

◆ NewAtlasRRTBroyden()

void NewAtlasRRTBroyden	(	unsigned int	it,
		TAtlasRRTStatistics *	arst
	)

For debugging purposes, se collect statistics on the number of iterations of the Broyden process. In this way we can compare different methods to compute the gradient (via nJacobian, via HandC, etc).

Parameters

it	Number of iterations of the last call to Broyden (returned by NextDynamicState).
arst	The AtlasRRT statistics to modify.

Definition at line 1301 of file atlasrrt.c.

References TAtlasRRTStatistics::itB, and TAtlasRRTStatistics::nB.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTTransTestFail()

void NewAtlasRRTTransTestFail ( TAtlasRRTStatistics * arst )

To be called every time the transition test fails and the branch extension is stopped.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1310 of file atlasrrt.c.

References TAtlasRRTStatistics::nTransTestFailed.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTDynamicError()

void NewAtlasRRTDynamicError ( TAtlasRRTStatistics * arst )

To be called every time there is an error in the dynamics compuations.

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1315 of file atlasrrt.c.

References TAtlasRRTStatistics::nDynamicErrors.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTInitChartError()

void NewAtlasRRTInitChartError ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1325 of file atlasrrt.c.

References TAtlasRRTStatistics::nErrorNewChart.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTOutOfChart()

void NewAtlasRRTOutOfChart ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1320 of file atlasrrt.c.

References TAtlasRRTStatistics::nOutOfChart.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTLargeCurvature()

void NewAtlasRRTLargeCurvature ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1330 of file atlasrrt.c.

References TAtlasRRTStatistics::nLargeCurvature.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTDirLargeCurvature()

void NewAtlasRRTDirLargeCurvature ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1335 of file atlasrrt.c.

References TAtlasRRTStatistics::nDirLargeCurvature.

Referenced by NewTemptativeSample().

◆ NewAtlasRRTStepReduction()

void NewAtlasRRTStepReduction ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1340 of file atlasrrt.c.

References TAtlasRRTStatistics::nStepReduction.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTSample()

void NewAtlasRRTSample ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1345 of file atlasrrt.c.

References TAtlasRRTStatistics::nSample.

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

◆ NewAtlasRRTRepeatedSample()

void NewAtlasRRTRepeatedSample ( TAtlasRRTStatistics * arst )

To be called every time an RRT extension end up in repeated sample

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1350 of file atlasrrt.c.

References TAtlasRRTStatistics::nRepeatedSample.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTChart()

void NewAtlasRRTChart ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1355 of file atlasrrt.c.

References TAtlasRRTStatistics::nChart.

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

◆ NewAtlasRRTNoConnectToParent()

void NewAtlasRRTNoConnectToParent ( TAtlasRRTStatistics * arst )

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

Parameters

arst	The statistics to update.

Definition at line 1360 of file atlasrrt.c.

References TAtlasRRTStatistics::nNoConnect.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTBlockBySingularity()

void NewAtlasRRTBlockBySingularity ( TAtlasRRTStatistics * arst )

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

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

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

Parameters

arst	The AtlasRRT statistics to modify.

Definition at line 1365 of file atlasrrt.c.

References TAtlasRRTStatistics::nSingular.

Referenced by AddBranchToAtlasRRT().

◆ NewAtlasRRTRandomSample()

void NewAtlasRRTRandomSample ( TAtlasRRTStatistics * arst )

Counts the number of random samples generated.

Parameters

arst	The statistics to update.

Definition at line 1370 of file atlasrrt.c.

References TAtlasRRTStatistics::nRandom.

Referenced by AtlasRRTSample().

◆ NewAtlasRRTSampleRejection1()

void NewAtlasRRTSampleRejection1 ( TAtlasRRTStatistics * arst )

Counts the number os random samples rejected. In particular the ones rejected directly at the generation process.

Parameters

arst	The statistics to update.

Definition at line 1375 of file atlasrrt.c.

References TAtlasRRTStatistics::nRejections1.

Referenced by AtlasRRTSample().

◆ NewAtlasRRTSampleRejection2()

void NewAtlasRRTSampleRejection2 ( TAtlasRRTStatistics * arst )

Counts the number os random samples rejected. In particular the ones rejected after the generation process (dynamic domain and alike).

Parameters

arst	The statistics to update.

Definition at line 1380 of file atlasrrt.c.

References TAtlasRRTStatistics::nRejections2.

Referenced by AtlasRRTValidateSample().

◆ NewAtlasRRTSampleRejection3()

void NewAtlasRRTSampleRejection3 ( TAtlasRRTStatistics * arst )

Counts the number os random samples rejected. In particular the ones rejected because blocked nodes. This is a subset of those counted by NewAtlasRRTSampleRejection2.

Parameters

arst	The statistics to update.

Definition at line 1385 of file atlasrrt.c.

References TAtlasRRTStatistics::nRejections3.

Referenced by AtlasRRTValidateSample().

◆ NewAtlasRRTSampleRejection4()

void NewAtlasRRTSampleRejection4 ( TAtlasRRTStatistics * arst )

Counts the number os random samples rejected. In particular the ones rejected because the dynamic cone is not well oriented with the radom sample. This is a subset of those counted by NewAtlasRRTSampleRejection2.

Parameters

arst	The statistics to update.

Definition at line 1390 of file atlasrrt.c.

References TAtlasRRTStatistics::nRejections4.

Referenced by AtlasRRTValidateSample().

◆ NewAtlasRRTCollisionCheck()

void NewAtlasRRTCollisionCheck ( TAtlasRRTStatistics * arst )

New collision check.

Parameters

arst	The statistics to update.

Definition at line 1395 of file atlasrrt.c.

References TAtlasRRTStatistics::nCollisionChecks.

Referenced by NewTemptativeSample().

◆ NewAverageNumNeighbours()

void NewAverageNumNeighbours	(	double	an,
		TAtlasRRTStatistics *	arst
	)

This is used to store the average number of neighbours for each charts after the generation of an atlasrrt (after planning).

Parameters

an	The average number of neighbours.
arst	The statistics to update.

Definition at line 1400 of file atlasrrt.c.

References TAtlasRRTStatistics::nAverageNeighbours.

Referenced by AtlasRRT().

◆ InitAtlasRRTStatistics()

void InitAtlasRRTStatistics ( TAtlasRRTStatistics * arst )

Init the Atlas RRT statistics.

Parameters

arst	The AtlasRRT statistics to initialize.

Definition at line 1086 of file atlasrrt.c.

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

◆ AccumulateAtlasRRTStatistics()

void AccumulateAtlasRRTStatistics	(	TAtlasRRTStatistics *	arst1,
		TAtlasRRTStatistics *	arst2
	)

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

Parameters

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

Definition at line 1405 of file atlasrrt.c.

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

◆ PrintAtlasRRTStatistics()

void PrintAtlasRRTStatistics	(	Tatlasrrt *	ar,
		TAtlasRRTStatistics *	arst
	)

Prints a report about the collected atlasRRT statistics.

Parameters

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

Definition at line 1482 of file atlasrrt.c.

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

◆ DeleteAtlasRRTStatistics()

void DeleteAtlasRRTStatistics ( TAtlasRRTStatistics * arst )

Deletes the atlasRRT statistics object.

Parameters

arst	The AtlasRRT statistics to delete.

Definition at line 1822 of file atlasrrt.c.

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

◆ UpdateLQRPolicy()

void UpdateLQRPolicy	(	Tparameters *	pr,
		TAtlasRRTBranchConfig *	config,
		Tchart *	c_lqr,
		double *	t_near,
		double *	t_rand,
		TAtlasRRTBranchState *	state,
		void *	st,
		TDynamicSpace *	ds,
		Tatlasrrt *	ar
	)

Recomputes the LQR policy.

Parameters

pr	The set of parameters.
config	The branch configuration.
c_lqr	The chart where to compute the policy.
t_near	The parameters of the start point in the chart.
t_rand	The parameters of the goal point in the chart.
state	The branch state.
st	The statistics, if any.
ds	The dynamics space to use.
ar	Tha atlas rrt structure.

Definition at line 2763 of file atlasrrt.c.

References TAtlasRRTBranchState::blocked, CT_DELTA, CT_EPSILON, CT_LQR_PLANNING_TIME, TAtlasRRTBranchConfig::currentMaxIntTime, Tatlasrrt::da, Distance(), Tatlasrrt::ds, GetChartCenter(), GetChartTangentSpace(), GetInverseActionCostMatrix(), GetLinearizedDynamics(), GetParameter(), INF, TAtlasRRTBranchState::intTime, Tatlasrrt::k, LinearizeDynamics(), LQR_COMPUTE_POLICY, Tatlasrrt::m, MAX_OPT_TIME, TAtlasRRTBranchConfig::maxIntTime, TAtlasRRTBranchConfig::maxTopt, NewAtlasRRTLQRComputePolicy(), NewAtlasRRTLQRToptIncreases(), NewAtlasRRTNoLQRPolicy(), NO_POLICY, PrintMatrix(), PrintVector(), SetLinearizedDynamics(), START2GOAL, TAtlasRRTBranchConfig::tLastLinearization, TAtlasRRTBranchConfig::topt, and TAtlasRRTBranchState::tree.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

◆ AddBranchToAtlasRRT()

double AddBranchToAtlasRRT	(	Tparameters *	pr,
		unsigned int	it,
		TAtlasRRTBranchConfig *	config,
		unsigned int	nsReplay,
		double **	replayPath,
		boolean *	replayNewChart,
		double *	replayTime,
		double *	replayCost,
		double *	action,
		unsigned int	i_near,
		double *	randSample,
		double *	goalSample,
		void *	st,
		TAtlasRRTBranchState *	state,
		unsigned int *	ns,
		double ***	path,
		boolean **	newChart,
		double **	tStep,
		double **	cStep,
		TSampleInfo *	lastSample,
		TDynamicSpace *	ds,
		Tatlasrrt *	ar
	)

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

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

Parameters

pr	The set of parametes.
it	The current iteration.
config	Configuration defining the brach extension mode. See TAtlasRRTBranchConfig.
nsReplay	Number of steps in the replay sequence.
replayPath	If not NULL, new samples are not computed on the fly but taken from this vector. This is useful to replicate replabranch extensions previously simulated.
replayNewChart	If not NULL indicates the steps in the branch replay where to create a new chart. This is only used if replayPath is also not NULL. CAUTION: This information is destroyed when replaying the trajectory and, thus the same trajectory can not be replayed twice. Make a copy of it, if necessary.
replayTime	If not NULL gives the time lapse for each element in replayPath with respect to the previous step.
replayCost	If not NULL gives the transition cost for each element in replayPath with respect to the previous step.
action	Action to execute (only used in dynamic systems).
i_near	Identifier of the node in the AtlasRRT from where to extend the branch.
randSample	The radom sample to reach (or at least to aim at).
goalSample	The goal sample of the path planning problem.
st	Structure to collect statistics about the path planning process.
state	State of the branch extension. Used to return information to the caller. Please see TAtlasRRTBranchState.
ns	Number of samples in the branch. Only updated if different from NULL.
path	Samples along the path between the two given points. Only used if different from NULL. This is typically used to recover paths on trees. In this case we do not want to store the nodes in the tree (i.e. addMode is ADD_NONE) but to retrive them. This can also be used as paramter replayPath to reproduce branches.
newChart	Arraly of flags indicating at which step in the path a new chart is created. Only set if it is different from NULL. This is used later on to reproduce branches, i.e., it is used as parameter replayNewChart.
tStep	Time lapse from the previous step. This can be used as input replayTime.
cStep	Cost of the step. This can be used as input replayCost.
lastSample	Buffer to store the last sample along the branch. NULL if the last sample is not relevant to the caller.
ds	Space for dynamic computations. Only required in problems with dynamics. Otherwise it can be NULL.
ar	The AtlasRRT to extend.

Returns: The branch length if the random sample is reached and INF otherwise.

Definition at line 2898 of file atlasrrt.c.

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

◆ AddBranchToAtlasDynamicRRT()

double AddBranchToAtlasDynamicRRT	(	Tparameters *	pr,
		unsigned int	oneAction,
		unsigned int	t2g,
		unsigned int	it,
		TAtlasRRTBranchConfig *	config,
		unsigned int	i_near,
		double *	randSample,
		double *	goalSample,
		void *	st,
		TDynamicBranches *	db,
		unsigned int *	lastSample,
		boolean *	reachedGoal,
		Tatlasrrt *	ar
	)

Variant of AddBranchToAtlasRRT for dynamical systems. Here instead of growing just one branch we grow a branch for each different acion and keep the one that is closer to x_rand

Parameters

pr	The set of parametes.
oneAction	If not 0, only one action (selected at random or with any available complex method) is used to extend the RRT. There is no simulation of alternative actions. It basically makes sense with Time2Go metric.
t2g	If set (not 0) the Time2Go metric is used.
it	The current iteration.
config	Configuration defining the brach extension mode. See TAtlasRRTBranchConfig.
i_near	Identifier of the node in the AtlasRRT from where to extend the branch.
randSample	The radom sample to reach (or at least to aim at).
goalSample	The goal sample of the path planning problem.
st	Structure to collect statistics about the path planning process.
db	Space for the simulatin of the different actions, one for each valid action. See InitDynamicBranches
lastSample	Identifer of the last sample added to the branh.
reachedGoal	TRUE if the goal is eventually reached.
ar	The AtlasRRT to extend.

Returns: The length of the adde branch or 0 if no action is good enough.

Definition at line 3632 of file atlasrrt.c.

Referenced by AtlasRRT().

◆ NewTemptativeSample()

void NewTemptativeSample	(	Tparameters *	pr,
		unsigned int	it,
		TAtlasRRTBranchConfig *	config,
		TAtlasRRTBranchState *	state,
		TSampleInfo *	current,
		double *	action,
		TSampleInfo *	next,
		TSampleInfo *	rand,
		double *	randSample,
		void *	st,
		TDynamicSpace *	ds,
		Tatlasrrt *	ar
	)

Generates a new sample interpolating in between x_near and x_rand and projecting to the manifold.

This is used to extend a AtlasRRT branch.

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

Parameters

pr	The set of parameters.
it	Current iteration. Used to stop the branch extension if we enter in an existing chart from a just created one.
config	The branch extension configuration.
state	The branch extension state.
current	The current point along the branch.
action	Action to execute (only used in dynamical systems).
next	The temptative next point to generate.
rand	The random point we are aiming at.
randSample	The random sample (as given in the input of the branch extnsion).
st	Statistics about chart construction.
ds	Space for dynamic computations. Only required in problems with dynamics. Otherwise it can be NULL.
ar	The atlas RRT.

Definition at line 4083 of file atlasrrt.c.

Referenced by AddBranchToAtlasRRT().

◆ AddSample2AtlasRRT()

double AddSample2AtlasRRT	(	Tparameters *	pr,
		unsigned int	tree,
		unsigned int *	currentID,
		unsigned int	nextChart,
		double *	nextParam,
		double *	nextSample,
		double	dp,
		double	cost,
		boolean	blocked,
		double *	action,
		double	time,
		unsigned int	ns,
		double **	path,
		double **	actions,
		double *	times,
		Tatlasrrt *	ar
	)

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

Parameters

pr	The set of parameteres.
tree	In which tree (START2GOAL or GOAL2START) to insert the sample. Only considered if the altasRRT is bidirectional.
currentID	Identifier of the current sample. Parent of the sample to add.
nextChart	Chart for the new sample.
nextParam	Parameters in the chart of the sample to add.
nextSample	Sample to add.
dp	Distance to parent sample.
cost	Cost for this node. This can be different thinks: the cost of the node evaluated with a user-given funcion (in TRRT) or the distance from the root of the tree (in RRT*)
blocked	TRUE for nodes close to domain borders or obstacles. Can be used later on to minimize expansions from this nodes.
action	Action executed to reach the node (only used in dynamical systems).
time	Time necessary to reach the node (only used in dynamical systems).
ns	Number of small steps from the parent to the state to add. May be NULL if not used.
path	The states in between the new node and its parent. May be NULL if not used. CAUTION: for efficiently, we directly point to the path given by the caller. Thus the caller should not deallocte the information (it will be de-allocated when deleting the RRT). The same applies to actions and times
actions	Action executed in between the parent node and the node to add. In general this is the same action as u, but it may be different. The actions are only used in problems with dynamics. May be NULL if not used.
times	Time of each step in between the parent and the new node. The sum of such times must be time (but the time to actually reach the node to add). The times are only used in problems with dynamics. May be NULL if not used.
ar	The AtlasRRT.

Returns: Distance from the new sample to its parent.

Definition at line 4725 of file atlasrrt.c.

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

Referenced by AddBranchToAtlasRRT().

◆ AddChart2AtlasRRT()

boolean AddChart2AtlasRRT	(	Tparameters *	pr,
		unsigned int	tree,
		unsigned int	it,
		TAtlasRRTBranchConfig *	config,
		TAtlasRRTBranchState *	state,
		TSampleInfo *	currentSampleInfo,
		boolean *	intersectParent,
		Tatlasrrt *	ar
	)

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

We can generate a chart on:

A sample already added to the RRT: The currentSampleInfo.id gives the identifier of this sample and currentSampleInfo is a pointer to the node in the tree.
A cophy of a sample already added to the RRT: The currentSampleInfo.id gives the identifier of this sample and currentSampleInfo is a pointer to the copy of the node in the tree.
A temporary sample not in the RRT: currentSampleInfo.id is NO_UINT.

Parameters

pr	The set of parameters.
tree	The identifier of the tree to which the sample belongs.
it	Current iteration. Used to timestamp the charts.
config	The branch configuration. Can be NULL if not given.
state	The branch state. Can be NULL if not given.
currentSampleInfo	Information relative to the sample from where to generate the next sample.
intersectParent	(Output) TRUE if the added chart intersects with its parent. Used for debug.
ar	The AtlasRRT.

Returns: TRUE if the chart can actually be generated. If not, it is most likely because the given point is at a (almost) singular region.

Definition at line 4837 of file atlasrrt.c.

References ADD_NONE, AddChart2Tree(), AddTrustedChart2Atlas(), Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, TAtlasRRTBranchState::c, Tatlasrrt::ce, TAtlasRRTBranchConfig::chartAddMode, TAtlasRRTBranchState::chartInAtlas, ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::ci, CopyChart(), DeleteChart(), Tatlasrrt::e, EnlargeChart(), Error(), FALSE, ForceChartCut(), TSampleInfo::generateChart, GetAtlasChart(), GetAtlasNumCharts(), TSampleInfo::id, INIT_NUM_CHARTS, InitTrustedChart(), TChartInfo::it, Tatlasrrt::J, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, Manifold2Chart(), Tatlasrrt::mc, MEM_EXPAND, Tatlasrrt::nc, NEW, NO_UINT, Tatlasrrt::parallel, TSampleInfo::pc, PopulateWithSamples(), PrintVector(), Tatlasrrt::r, TSampleInfo::s, SetSampleInfo(), Tatlasrrt::si, SingularChart(), TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, TRUE, Tatlasrrt::w, and Warning().

Referenced by AddBranchToAtlasRRT(), and InitAtlasRRT().

◆ PopulateWithSamples()

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

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

Parameters

pr	The set of parameters.
id	Identifier of the new chart.
ar	The atlasRRT.

Definition at line 5065 of file atlasrrt.c.

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

Referenced by AddChart2AtlasRRT().

◆ AddChart2Tree()

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

Add a visited chart to the list of chart reached by a given tree. This is used to select charts when sampling. We avoid storing the same chart twice to avoid bias in the sampling.

Parameters

tree	The identidier for the tree.
chartId	The identifier for the chart.
ar	The AtlasRRT.

Definition at line 4798 of file atlasrrt.c.

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

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

◆ PointTowardRandSample()

boolean PointTowardRandSample	(	TAtlasRRTBranchConfig *	config,
		unsigned int	samplingMode,
		double *	randSample,
		TAtlasRRTBranchState *	state,
		TSampleInfo *	current,
		TSampleInfo *	rand,
		Tatlasrrt *	ar
	)

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

Parameters

config	The configuration determining how the branch has to be extended.
samplingMode	The sampling mode (AMBIENT, KDTREE, TANGENT).
randSample	The (original) random sample.
state	The current state of the branch extension.
current	The current sample.
rand	The random sample.
ar	The atlasrrt.

Definition at line 4631 of file atlasrrt.c.

References Tatlasrrt::atlas, TSampleInfo::c, TAtlasRRTBranchState::c, TAtlasRRTBranchState::chartInAtlas, TAtlasRRTBranchState::deltaParam, DifferenceVector(), TAtlasRRTBranchState::distQrand, Tatlasrrt::dynamics, FALSE, GetAtlasChart(), GetChartCenter(), GetChartTangentSpace(), Tatlasrrt::k, Local2Global(), Tatlasrrt::m, Manifold2Chart(), TAtlasRRTBranchState::maxLength, NO_UINT, Norm(), Normalize(), TAtlasRRTBranchConfig::onManifold, Tatlasrrt::parallel, PrintMatrix(), PrintVector(), Tatlasrrt::r, TSampleInfo::s, SumVectorScale(), TSampleInfo::t, TANGENT_SAMPLING, Tatlasrrt::tp, and TRUE.

Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().

◆ ReconstructAtlasRRTPath()

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

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

Parameters

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.
ns	Number of samples in the output path.
path	The output path.
ar	The AtlasRRT structure to query.

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

Definition at line 5155 of file atlasrrt.c.

References AddBranchToAtlasRRT(), AddSample2Samples(), CONNECT_SAFE, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteSamples(), GetRRTParent(), InitBranchConfig(), InitSamples(), NO_UINT, TAtlasRRTBranchState::reachedQrand, ReverseSamples(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, Tatlasrrt::w, and Warning().

Referenced by AtlasRRTstar().

◆ Steps2PathinAtlasRRT()

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

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

Parameters

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.
ar	The AtalsRRT structure to query.

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

Definition at line 5223 of file atlasrrt.c.

References AddBranchToAtlasRRT(), AddSample2Samples(), CONNECT_SAFE, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_DELTA, CT_EPSILON, DeleteSamples(), TAtlasRRTBranchState::distQrand, GetParameter(), GetRRTNodeCost(), GetVectorElement(), InitBranchConfig(), InitSamples(), NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, VectorSize(), Tatlasrrt::w, and Warning().

Referenced by AtlasBiRRTstar().

◆ SmoothPathInAtlasRRT()

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

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

Parameters

pr	The set of parameters.
sID	The node.
ar	The atlasRRT.

Definition at line 5315 of file atlasrrt.c.

References AddBranchToAtlasRRT(), CONNECT_SAMPLES, GetRRTNodeCost(), GetRRTParent(), INF, InitBranchConfig(), TAtlasRRTBranchConfig::maxLength, TAtlasRRTBranchConfig::maxTopt, NEW, NO_UINT, randomMax(), TAtlasRRTBranchState::reachedQrand, Tatlasrrt::rrt, TSampleInfo::s, SetRRTNodeCost(), SetRRTParent(), Tatlasrrt::si, and StepsToRoot().

◆ AddStepToAtlasRRTstar()

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

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

Parameters

pr	The set of parameters.
it	Current iteration.
expand2Goal	TRUE if x_rand is actually the goal.
onManifold	TRUE if the sample is on the manifold.
i_near	Node from which to expand.
x_rand	Targed for the expansion.
id_goal	Identifier of the goal sample, if any. Otherwise NO_UINT. Only used if goal is not NULL.
goal	The goal sample. NULL if no goal is given.
arst	Statistics on the AtlasRRT construction process.
ar	The AtlasRRT.

Definition at line 6946 of file atlasrrt.c.

References ADD_LAST, ADD_LAST_NO_REP, AddBranchToAtlasRRT(), TAtlasRRTBranchConfig::addMode, CONNECT_SAMPLES, CT_EPSILON, TAtlasRRTBranchState::distGoal, Error(), GetParameter(), GetRRTNodeCost(), GetRRTParent(), InitBranchConfig(), TAtlasRRTBranchState::lastID, NO_UINT, TAtlasRRTBranchConfig::onManifold, TAtlasRRTBranchState::reachedGoal, TAtlasRRTBranchState::reachedQrand, Tatlasrrt::rrt, SetRRTNodeCost(), and TRUE.

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

◆ WireAtlasRRTstar()

void WireAtlasRRTstar	(	Tparameters *	pr,
		unsigned int	id_new,
		unsigned int	i_near,
		double	gamma,
		unsigned int	nn,
		unsigned int *	n,
		double **	c,
		double	h,
		Theap *	q,
		unsigned int *	t,
		TAtlasRRTStatistics *	arst,
		Tatlasrrt *	ar
	)

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

Parameters

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).
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
arst	Statistics on the AtlasRRT construction process.
ar	The AtlasRRT.

Definition at line 7012 of file atlasrrt.c.

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

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

◆ ReWireAtlasRRTstar()

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

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

Parameters

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 WireAtlasRRTstar if RRTSTAR_SYMMETRIC_COST is 1.
steps	The steps forming the solution path.
l	Cost of the path from start to goal via s1 and s2. Only used in mode==TWO_TREES_WITH_SWAP.
arst	Statistics on the AtlasRRT construction process.
ar	The AtlasRRT.

Definition at line 7157 of file atlasrrt.c.

References AddBranchToAtlasRRT(), ChangeBiRRTSteps(), CONNECT_SAMPLES, CT_EPSILON, FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, InitBranchConfig(), TAtlasRRTBranchConfig::maxLength, NO_UINT, TAtlasRRTBranchState::reachedQrand, Tatlasrrt::rrt, TSampleInfo::s, SetRRTCostAndParent(), Tatlasrrt::si, TWO_TREES_WITH_SWAP, and UpdateCostAndTree().

Referenced by AtlasBiRRTstar(), and AtlasRRTstar().

◆ AtlasRRTstarCloseIteration()

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

Prints information about the AtlasRRT* iteration.

Parameters

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 AtlasRRT*
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.
ar	The AtlasRRT.

Definition at line 7239 of file atlasrrt.c.

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

Referenced by AtlasRRTstar().

◆ AtlasBiRRTstarCloseIteration()

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

Prints information about the Bi-directional AtlasRRT* iteration.

Parameters

it	The current iteration.
l	Current cost to goal. INF if no path found yet.
time	Elapsed time from the start of the AtlasRRT*
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.
ar	The AtlasRRT.

Definition at line 7274 of file atlasrrt.c.

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

Referenced by AtlasBiRRTstar().

◆ SaveAtlasRRTSampleInfo()

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

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

Parameters

f	The file where to store the information.
si	The sample information to store.
ar	The AtlasRRT.

Definition at line 5620 of file atlasrrt.c.

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

Referenced by SaveAtlasRRT().

◆ SaveAtlasRRTChartInfo()

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

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

Parameters

f	The file where to store the information.
ci	The chart information to store.
ar	The AtlasRRT.

Definition at line 5634 of file atlasrrt.c.

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

Referenced by SaveAtlasRRT().

◆ LoadAtlasRRTSampleInfo()

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

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

Parameters

f	The file where to store the information.
si	The sample information to initialize.
ar	The AtlasRRT.

Definition at line 5641 of file atlasrrt.c.

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

Referenced by LoadAtlasRRT().

◆ LoadAtlasRRTChartInfo()

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

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

Parameters

f	The file where to store the information.
ci	The chart information to initialize.
ar	The AtlasRRT.

Definition at line 5656 of file atlasrrt.c.

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

Referenced by LoadAtlasRRT().

◆ PlotQrand()

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

Small debug utility to plot the position of x_rand.

Parameters

pr	The set of parameters.
prefix	Prefix used to generate the plot file name.
inear	Nearest neighbour.
c_rand	The chart where x_rand was selected.
t_rand	The parameters for x_rand
x_rand	The point in ambient coordinates
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).
ar	The AtlasRRT.

Definition at line 5464 of file atlasrrt.c.

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

◆ PlotConnection()

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

Graphical representation of the attempt to connect two trees.

Parameters

pr	The set of parameters.
prefix	Prefix used to generate the plot file name.
target	Point to reach in the tree
near	Point from where to extend the connection branch
end	Point finally reached in the connection attempt.
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).
ar	The AtlasRRT.

Definition at line 5561 of file atlasrrt.c.

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

◆ RandomPointInAtlasTree()

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

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

Parameters

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

Returns: TRUE if the random point could be actually determined.

Definition at line 5375 of file atlasrrt.c.

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

Referenced by AtlasRRTSample().

◆ InitAtlasRRT()

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

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

Parameters

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

Definition at line 5667 of file atlasrrt.c.

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

Referenced by main().

◆ AtlasRRTSample()

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

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

Parameters

pr	The set of parameters.
samplingMode	Policy to generate the random sample. This can be AMBIENT_SAMPLING (to generate it from the ambient space) or KDTREE_SAMPLING (to generate it using the kd-tree). or TANGENT_SAMPLING (to use the charts). The KDTREE_SAMPLING only works if you use the cuik-kdtree library. Otherwise AMBIENT_SAMPLING is used instead
it	Current iteration.
tree	The tree to use in the sampling. Only used if mode is KDTREE_SAMPLING.
goal	Goal node. NULL if we do not have goal (i.e. we are defining an exploration RRT) or if we can not use the goal as a random sample.
scale	The global scale factor for the sampling areas of the charts.
exploration	Set to TRUE if samplingMode is TANGENT_SAMPLING and the random sample is in the outer parts of the chart used for sampling.
onManifold	TRUE if the sample is on the manifold.
c_rand	The index of the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
t_rand	The parameters on the chart used for the sampling. Only set if samplingMode is TANGENT_SAMPLING.
q_rand	Space to store the random sample.
arst	Atlas statistics. This is only used we are repeating experiments. Otherwise is is NULL.
ar	AtlasRRT to use.

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

Definition at line 5866 of file atlasrrt.c.

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

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

◆ AtlasRRTValidateSample()

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

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

Parameters

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

Returns: TRUE if the random sample is valid.

Definition at line 5974 of file atlasrrt.c.

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

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

◆ RefineTrajectory()

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

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

Parameters

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

Definition at line 6177 of file atlasrrt.c.

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

Referenced by AtlasRRT(), and main().

◆ AtlasRRTSimulate()

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

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

Parameters

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

Definition at line 6270 of file atlasrrt.c.

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

Referenced by main().

◆ AtlasRRT()

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

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

Parameters

pr	The set of parameters.
pg	A point on the manifold to reach with the AtlasRRT. This is also a sample including only system variables. It is only used for one-directional RRTs. For bidirectional ones we use the one given in InitAtlasRRT.
time	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
da	Dimension of the action space. Only used in dynamical systems. Otherwise is 0.
actions	Action executed to reach step in the soluiont. Only used in dynamical systems.
times	Time to reach each state. Only used in dynamical systems.
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used we are repeating experiments. Otherwise is is NULL.
ar	The Atlas-RRT to expand.

Definition at line 6380 of file atlasrrt.c.

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

Referenced by main().

◆ AtlasTRRT()

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

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

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

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

Parameters

pr	The set of parameters.
pg	A point on the manifold to reach with the AtlasTRRT. This is also a sample including only system variables. It is only used for one-directional RRTs. No bidirectional search with AtlasTRRT.
time	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
pc	The cost of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
costF	The cost function, if any.
costData	The last parameter of the cost funtion, if any.
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The Atlas-RRT to expand.

Definition at line 6777 of file atlasrrt.c.

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

Referenced by main().

◆ AtlasRRTstar()

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

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

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

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

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

Parameters

pr	The set of parameters.
pg	A point on the manifold to reach with the RRT. This is also a sample including only system variables.
it	[output] Number of iterations actually executed.
times	Optional table to store the time at each iteration. If not used set to NULl at call.
costs	Optional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTime	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The AtlasRRT to expand.

Returns: TRUE if the path exists.

Definition at line 7304 of file atlasrrt.c.

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

Referenced by AtlasBiRRTstar(), and main().

◆ AtlasBiRRTstar()

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

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

Parameters

pr	The set of parameters.
pg	A point on the manifold to reach with the RRT. This is also a sample including only system variables.
it	[output] Number of iterations actually executed.
times	Optional table to store the time at each iteration. If not used set to NULl at call.
costs	Optional table to store the cost of the path at each interation. If not used set to NULl at call.
planningTime	Time actually used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
str	Structure to accumulate statistics on the Atlas RRT construction. This is only used whe repeating experiments. Otherwise is is NULL.
ar	The AtlasRRT to expand.

Returns: TRUE if the path exists.

Definition at line 7546 of file atlasrrt.c.

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

Referenced by AtlasRRTstar().

◆ GetAtlasRRTNumNodes()

unsigned int GetAtlasRRTNumNodes ( Tatlasrrt * ar )

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

Parameters

ar	The AtlasRRT to query.

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

Definition at line 7802 of file atlasrrt.c.

References Tatlasrrt::ns.

Referenced by main().

◆ GetAtlasRRTNumCharts()

unsigned int GetAtlasRRTNumCharts ( Tatlasrrt * ar )

Returns the number of charts in the AtlasRRT.

Parameters

ar	The AtlasRRT to query.

Returns: The number of charts in the AtlasRRT so far.

Definition at line 7807 of file atlasrrt.c.

References Tatlasrrt::nc.

Referenced by main().

◆ GetRRTNNInChart()

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

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

Parameters

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

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

Definition at line 7812 of file atlasrrt.c.

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

Referenced by AtlasRRTValidateSample(), and Time2GoNNToTree().

◆ Time2GoNNToTree()

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

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

Parameters

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

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

Definition at line 7869 of file atlasrrt.c.

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

Referenced by AtlasRRT().

◆ PlotAtlasRRT()

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

Combines the plot of the Atlas and the RRT.

Parameters

prefix	Prefix for the name of the file where to store the plots.
argc	Number arguments given to the program calling this function. This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
arg	Arguments given to the program calling this function. This is used This is used to write commnets in the output gcl file so that the plot can be easiy reproduced.
pr	The set of parameters.
xID	The first ambient dimension where to project (in the original system including system vars and dummies).
yID	The second ambient dimension where to project (in the original system including system vars and dummies).
zID	The third ambient dimension where to project (in the original system including system vars and dummies).
p1	Root sample of the RRT (vector with 3 components, i.e., already in the projected space). NULL if not given.
p2	Goal sample of the RRT (vector with 3 components, i.e., already in the projected space). NULL if not given.
ar	The Atlas-RRT to plot.

See also: PlotAtlas, PlotRRT.

Definition at line 7955 of file atlasrrt.c.

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

Referenced by main().

◆ AtlasRRTMemSize()

unsigned int AtlasRRTMemSize ( Tatlasrrt * ar )

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

Parameters

ar	The atlasRRT.

Returns: The size of the atlasRRT in bytes.

Definition at line 8182 of file atlasrrt.c.

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

Referenced by main().

◆ SaveAtlasRRT()

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

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

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

Parameters

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

Definition at line 8192 of file atlasrrt.c.

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

Referenced by main().

◆ LoadAtlasRRT()

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

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

Parameters

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

Definition at line 8255 of file atlasrrt.c.

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

Referenced by main().

◆ PrintAtlasRRTDefines()

void PrintAtlasRRTDefines ( FILE * f )

Prints the defines in atlasrrt.h

This is used only for debug purposes.

Parameters

f	The file where to store the values.

Definition at line 8359 of file atlasrrt.c.

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

Referenced by main().

◆ DeleteAtlasRRT()

void DeleteAtlasRRT ( Tatlasrrt * ar )

Deletes the information stored in the RRT.

Parameters

ar	The RRT to delete.

Definition at line 8382 of file atlasrrt.c.

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

Referenced by main().

Institut de Robòtica i Informàtica Industrial

Introduction

Data Structures

Macros

Functions

Macro Definition Documentation

◆ ADD_ALL

◆ ADD_ALL_NO_REP

◆ ADD_LAST

◆ ADD_LAST_NO_REP

◆ ADD_CHART_AND_LAST_NO_REP

◆ ADD_NONE

◆ EXTEND_RRT

◆ CONNECT_RRTs

◆ CONNECT_SAMPLES

◆ CONNECT_SAFE

◆ SIMULATE_DYNAMICS

◆ EXTEND_RRT_DYNAMICS

◆ CONNECT_RRT_DYNAMICS

◆ NO_BLOCKED

◆ NO_DIR

◆ NO_POLICY

◆ NO_REPLAY

◆ SINGULARITY

◆ REPEATED_SAMPLE

◆ THE_IMPOSSIBLE

◆ REVISITED_CHART

◆ REPEATED_CHART

◆ TRANSITION_TEST_FAILS

◆ TOO_FAR

◆ TOO_LONG

◆ COLLISION

◆ NOT_IN_DOMAIN

◆ DYNAMIC_ERROR

◆ MAX_INT_TIME

◆ MAX_OPT_TIME

Function Documentation

◆ InitBranchConfig()

◆ InitBranchState()

◆ canContinueBranch()

◆ PrintBranchBlockedState()

◆ DeleteBranchState()

◆ InitDynamicBranches()

◆ DeleteDynamicBranches()

◆ InitSampleInfo()

◆ SetSampleInfo()

◆ SetSampleInfoFromNode()

◆ DeleteSampleInfo()

◆ NewAtlasRRTBranch()

◆ NewAtlasRRTDynamicExtension()

◆ NewAtlasRRTImprovedDynamicExtension()

◆ NewAtlasRRTNoEmptyBranch()

◆ NewAtlasRRTTreeConnection()

◆ NewAtlasRRTNoEmptyTreeConnection()

◆ NewAtlasRRTDistanceQrand()

◆ NewAtlasRRTStep()

◆ NewAtlasRRTNoConvergentStep()

◆ NewAtlasRRTQrandReached()

◆ NewAtlasRRTNotInDomain()

◆ NewAtlasRRTCollision()

◆ NewAtlasRRTTooLong()

◆ NewAtlasRRTNoAdvanceDirection()

◆ NewAtlasRRTNoLQRPolicy()

◆ NewAtlasRRTLQRToptIncreases()

◆ NewAtlasRRTImpossible()

◆ NewAtlasRRTNoReplay()

◆ NewAtlasRRTTooFar()

◆ NewAtlasRRTRevisitedChart()

◆ NewAtlasRRTRepeatedChart()

◆ NewAtlasRRTChartBlocked()

◆ NewAtlasRRTIntTimeReached()

◆ NewAtlasRRTNoAction()

◆ NewAtlasRRTLQRPolicy()

◆ NewAtlasRRTLQRinput()

◆ NewAtlasRRTLQRComputePolicy()

◆ NewAtlasRRTBroyden()

◆ NewAtlasRRTTransTestFail()

◆ NewAtlasRRTDynamicError()

◆ NewAtlasRRTInitChartError()

◆ NewAtlasRRTOutOfChart()

Institut de Robòtica
i Informàtica Industrial