Introduction

Implementation of the auxiliary functions to deal with sets of samples.

See also: samples.h

Definition in file samples.c.

Macros
#define	DISPERSION 1
	Evenly distribute points on the path. More...

#define	REMOVE_POINTS ((DISPERSION)&&(1))
	To remove points when optimizint paths via gradient. More...

#define	APPROXIMATED_CHARTS_IN_GRADIENT 1
	Speed up gradient smooth by using approximated charts. More...

Functions
double	StepDispersion (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Dispersion path step. More...

boolean	StepDispersionGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Gradient of the step dispersion penalty term. More...

double	StepLength (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Length path step. More...

boolean	StepLengthGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Gradient of the length of a path step. More...

double	StepEffort (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Control effort of a path step. More...

boolean	StepEffortGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Gradient of the control effort of a path step. More...

boolean	StepCostNumericalGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Generic numerical gradient. More...

double	CombineCosts (TStepCost c1, TStepCost c2, Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Combines two cost functions. More...

boolean	CombineGradients (TStepCostGradient g1, TStepCostGradient g2, Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Combines two gradients. More...

double	LengthAndDispersion (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Combines length and dispersion cost functions. More...

boolean	LengthAndDispersionGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Gradient of the length and dispersion. More...

double	EffortAndDispersion (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xp, double x, double xn, unsigned int current, Tchart cp, Tchart c, Tchart cn, Tworld w)
	Combines control effort and dispersion cost functions. More...

boolean	EffortAndDispersionGradient (Tparameters p, unsigned int tp, boolean sv, unsigned int m, double xpp, double xp, double x, double xn, double xnn, Tchart cpp, Tchart cp, Tchart c, Tchart cn, Tchart cnn, TJacobian sJ, TStepCost cost, double u, Tworld w)
	Gradient of the effort and dispersion. More...

int	cmpUInt (const void a, const void b)
	Compares two unsigned in integers. More...

void	ShortcutSmooth (Tparameters pr, boolean sv, Tstatistics stime, unsigned int np, double **point, TAtlasBase w)
	Fixed-cut based smoothing. More...

void	RandomSmooth (Tparameters pr, unsigned int nCores, unsigned int maxIterations, boolean sv, Tstatistics stime, unsigned int np, double **point, TAtlasBase w)
	Random-based path smoothing. More...

void	GradientSmooth (Tparameters pr, unsigned int nCores, unsigned int maxIterations, boolean sv, Tstatistics stime, unsigned int np, double **point, TStepCost stepCost, TStepCostGradient stepCostGradient, TAtlasBase w)
	Gradient-based path smoothing. More...

void	SaveSamplesInt (Tfilename fpath, unsigned int nvs, unsigned int ns, double path, unsigned int da, double action, double time)
	Internal function to save a set of samples to a file. More...

void	InitSamples (unsigned int ms, unsigned int ns, double ***path)
	Initializes a set of samples. More...

void	InitSamplesReplay (unsigned int ms, unsigned int ns, double *path, boolean newChart, double time, double cost)
	Initializes a set of samples with flag. More...

void	InitTrajectory (unsigned int ms, unsigned int ns, double *path, unsigned int da, double actions, double *times)
	Initializes a trajectory. More...

void	AddSample2Samples (unsigned int nv, double sample, unsigned int nvs, boolean systemVars, unsigned int ms, unsigned int ns, double ***path)
	Adds a sample to a set of samples. More...

void	AddSample2SamplesReplay (unsigned int nv, double sample, boolean nc, double h, double c, unsigned int nvs, boolean systemVars, unsigned int ms, unsigned int ns, double *path, boolean newChart, double time, double cost)
	Adds a sample to a set of samples for replay. More...

void	AddStep2Trajectory (unsigned int nv, double sample, unsigned int nvs, boolean systemVars, double action, double time, unsigned int ms, unsigned int ns, double path, unsigned int da, double actions, double *times)
	Adds a step to a trajectory. More...

double	ConnectDynamicStatesID (Tparameters pr, unsigned int tp, Tbox ambient, boolean collision, unsigned int m, unsigned int k, unsigned int da, double state, double action, double time, double goal, unsigned int ns, double path, double actions, double times, TJacobian J, TDynamicSpace ds, TAtlasBase w)
	Forwarnd integration of the equations of motion. More...

double	ConnectDynamicStates (Tparameters pr, unsigned int tp, Tbox ambient, boolean collision, unsigned int m, unsigned int k, unsigned int da, double state, double action, double time, double goal, unsigned int ns, double path, double actions, double times, TJacobian J, TDynamicSpace ds, TAtlasBase w)
	Forwarnd integration of the equations of motion. More...

double	ConnectSamplesChart (Tparameters pr, unsigned int tp, boolean sv, Tbox domain, unsigned int m, unsigned int n, double s1, double s2, double md, boolean checkCollisions, TJacobian sJ, boolean reached, boolean collision, double lastSample, unsigned int ns, double *path, TAtlasBase w)
	Determines the connection between two points on the manifold. More...

double	ConnectSamples (Tparameters pr, unsigned int tp, boolean sv, Tbox domain, unsigned int m, unsigned int n, double s1, double s2, double md, boolean checkCollisions, TJacobian sJ, boolean reached, boolean collision, double lastSample, unsigned int ns, double *path, TAtlasBase w)
	Determines the connection between two points on the manifold. More...

double	PathLength (unsigned int tp, boolean sv, unsigned int m, unsigned int np, double **point)
	Length of a path formed by a set of samples. More...

double	PathEffort (Tparameters p, unsigned int m, unsigned int np, double point, Tworld w)
	Approximated control effort of a path. More...

void	SmoothSamples (Tparameters pr, boolean parallel, int mode, unsigned int maxIterations, unsigned int ns, double path, unsigned int sns, double **spath, TAtlasBase w)
	Path smoothing. More...

void	ConcatSamples (unsigned int nvs, unsigned int ns1, double path1, unsigned int ns2, double path2, unsigned int ns, double **path)
	Concats two path. More...

void	ReverseConcatSamples (unsigned int nvs, unsigned int ns1, double path1, unsigned int ns2, double path2, unsigned int ns, double **path)
	Reverses and concats a path. More...

void	ReverseConcatTrajectories (unsigned int nvs, unsigned int da, unsigned int ns1, double path1, double action1, double time1, unsigned int ns2, double path2, double action2, double time2, unsigned int ns, double path, double action, double *time)
	Reverses and concats a trajectory. More...

void	ReverseSamples (unsigned int ns, double **path)
	Reverses a set of samples. More...

unsigned int	ReadOneSample (Tparameters p, char fname, unsigned int nvs, double **s)
	Reads one sample from a file. More...

unsigned int	ReadTwoSamples (Tparameters p, char fname, unsigned int nvs, double s1, double s2)
	Reads two samples from a file. More...

void	SaveSamples (char fname, char suffix, unsigned int nvs, unsigned int ns, double **path)
	Saves a set of samples to a file. More...

void	SaveTrajectory (char fname, char suffix, unsigned int nvs, unsigned int ns, double path, unsigned int da, double action, double *time)
	Saves a set of samples to a file including action and timing information. More...

void	SaveSamplesN (char fname, boolean smooth, unsigned int n, unsigned int nvs, unsigned int ns, double *path)
	Saves a set of samples to a file. More...

boolean	LoadSamples (Tfilename fname, unsigned int nvs, unsigned int ns, double **path)
	Reads a set of samples from file. More...

boolean	LoadTrajectory (Tfilename fname, unsigned int nvs, unsigned int ns, double *path, unsigned int da, double *action, double time)
	Reads a trajectory from file. More...

void	SparsifyTrajectory (double tm, unsigned int *id, unsigned int ns, double path, double action, double *time)
	Ensures a mininum time between two consecutive trajectory steps. More...

void	PlotSamples (Tparameters p, Tplot3d p3d, unsigned int xID, unsigned int yID, unsigned int zID, unsigned int ns, double **path)
	Plots a 3D projection of a path. More...

void	PlotForceField (Tparameters p, Tplot3d p3d, unsigned int xID, unsigned int yID, unsigned int zID, Tworld w, unsigned int ns, double *sols)
	Plots the force-field on a set of points. More...

void	DeleteSamples (unsigned int ns, double **path)
	Deletes the space used by a set of samples. More...

void	DeleteSamplesReplay (unsigned int ns, double *path, boolean newChart, double time, double cost)
	Deletes the space used by a set of samples with flags. More...

void	DeleteTrajectory (unsigned int ns, double path, double action, double *time)
	Deletes the space used by a trajectory. More...

Macro Definition Documentation

◆ DISPERSION

#define DISPERSION 1

If set, the cost and the gradient are complemented with a penalty term that penalized path steps with different lengths. In other words, we try to evenly distribute the steps along the path.

Long steps are too far from the manifold and, thus, they introduce large errors, but they are shorter that paths that closely follow the manifold. This is way long steps often appear when minimizing the path length (and related measures) since the error with respect to the manifold is not taken into account.

Long steps appear more often if we remove steps along the paths (if we remove the points that are very close the its neightours). In this case the minimization of the dispersion is a must

See also: REMOVE_POINTS.

See StepDispersion and StepDispersionGradient.

Definition at line 46 of file samples.c.

◆ REMOVE_POINTS

#define REMOVE_POINTS ((DISPERSION)&&(1))

If set, we remove the points that are too close each other during the path gradient optimization.

It can only be set if DISPERSION is also set.

Definition at line 56 of file samples.c.

◆ APPROXIMATED_CHARTS_IN_GRADIENT

#define APPROXIMATED_CHARTS_IN_GRADIENT 1

When displacing a point along a gradient we have to compute the chart at this point. Howerver, this chart is very close to the chart at the original point (the point before the gradient displacement. If this flag is set, we use the already computed chart as an approximation of the chart at the new point.

Todo:: Verify that this results in actual speed up.

Definition at line 71 of file samples.c.

Function Documentation

◆ StepDispersion()

double StepDispersion	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

Penalizes path steps that have different length. This is used to evenly distribute the path step along the path.

For a path sequence (x_1,x_2,x_3), the formula to optimize is

(|x_2-x_1|-|x_3-x_2|)^2

i.e., penalizes the difference in length of the two path steps around x_2. This is applied for all points on the path (except for 'stalled' points, i.e., points that can not be moved due to collision, for instance).

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	First point in the path (simplified).
x	Second point along the path. This is the point used to compute the cost.
xn	Third point along the path.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	Chart on c1. Atually not used. Can be NULL.
c	Chart on c2. Atually not used. Can be NULL.
cn	Chart on c3. Atually not used. Can be NULL.
w	The world structure.

Definition at line 1380 of file samples.c.

References DistanceTopologySubset().

Referenced by CombineGradients(), EffortAndDispersion(), LengthAndDispersion(), and SmoothSamples().

◆ StepDispersionGradient()

boolean StepDispersionGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

Gradient of the StepDispersion cost funtion.

Note that the actual position of a point (say x_3) affects the cost of different triplets of points

(x_1,x_2,x_3)
(x_2,x_3,x_4)
(x_3,x_4,x_5)

We have to accumulate the gradient in the three cases. This is why this function consideres sequences of 5 points (x_1,x_2,x_3,x_4,x_5), but returns the derivative w.r.t x_3.

Note that the step dispersion is orthogonal ot any other cost to evaluate on the path. Thus it can be applied on the top of different cost functions (path length, path effort, ...).

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	First point in the sequence. NULL at the beginning of the path.
xp	Second point in the sequence.
x	Third point in the sequence.
xn	Fourth point in the sequence.
xnn	Fith point in the sequence. NULL at the end of the path.
cpp	Chart at xpp.
cp	Chart at xp.
c	Chart at x.
cn	Chart at xn.
cnn	Chart at xnn.
sJ	The symbolic Jacobian of the constraints. Not used. Can be NULL.
cost	Cost function. Not used. Can be NULL.
u	The resulting gradient (in tangent space!) of the cost function w.r.t. x_3.
w	The world structure.

Returns: TRUE if the gradient could be computed.

Definition at line 1403 of file samples.c.

References CT_EPSILON, CT_N_DOF, DifferenceVectorTopologySubset(), DistanceTopologySubset(), FALSE, GetChartTangentSpace(), GetParameter(), NEW, Norm(), ScaleVector(), SumVectorScale(), TMatrixVectorProduct(), and TRUE.

Referenced by EffortAndDispersionGradient(), LengthAndDispersionGradient(), and SmoothSamples().

◆ StepLength()

double StepLength	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

Auxiliary function used to compute the path length.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	Fist poinnt in the path. Not used. Can be NULL.
x	Second point in the path (simplified).
xn	point along the path. Not used. Can be NULL.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	A chart on xp
c	A chart defined on x.
cn	A chart defined on xn. Not used. Cam be NULL.
w	The world structure.

Definition at line 1471 of file samples.c.

References DistanceTopologySubset(), and Error().

Referenced by CombineGradients(), EffortAndDispersionGradient(), LengthAndDispersion(), LengthAndDispersionGradient(), and SmoothSamples().

◆ StepLengthGradient()

boolean StepLengthGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

Auxiliary function used to compute the gradient of a path step length (see StepLength).

It is used also when smoothing a path via gradient descent.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1492 of file samples.c.

References CT_EPSILON, CT_N_DOF, DifferenceVectorTopologySubset(), FALSE, GetChartTangentSpace(), GetParameter(), NEW, Norm(), ScaleVector(), SumVectorScale(), TMatrixVectorProduct(), and TRUE.

Referenced by LengthAndDispersionGradient(), and SmoothSamples().

◆ StepEffort()

double StepEffort	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

Auxiliary function used to compute the path control effort (see PathEffort).

Computes the control effort of moving from x1 to x2 taking into account the drift introduced by the potential energy gradient. If there is no such a gradient, the output is close to the step length.

Note that we actually compute and approximation of the control effort. The shorter the steps in the path, the closer the approximation.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	Previous point in the path. Actually not used. Can be NULL.
x	Current point in the path (simplified).
xn	Next point along the path.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn. Actually not used. Can be NULL.
w	The world structure.

Definition at line 1543 of file samples.c.

References CT_EPSILON, CT_N_DOF, DifferenceVector(), DifferenceVectorTopologySubset(), Error(), GeneralDotProduct(), GetChartTangentSpace(), GetParameter(), KINEMATIC_SUBSPACE, NEW, Norm(), ScaleVector(), TMatrixVectorProduct(), TRUE, and WorldForceField().

Referenced by EffortAndDispersion(), PathEffort(), and SmoothSamples().

◆ StepEffortGradient()

boolean StepEffortGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

Auxiliary function used to compute the gradient of a path step control effort (see StepEffort).

This is also used to smooth a path via gradient descent.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1612 of file samples.c.

References CT_EPSILON, CT_N_DOF, DifferenceVector(), DifferenceVectorTopologySubset(), Error(), FALSE, GeneralDotProduct(), GetChartTangentSpace(), GetParameter(), MatrixVectorProduct(), NEW, Norm(), ScaleVector(), SumVectorScale(), TMatrixVectorProduct(), TRUE, WorldForceField(), and WorldForceFieldProjectedGradient().

Referenced by EffortAndDispersionGradient(), and SmoothSamples().

◆ StepCostNumericalGradient()

boolean StepCostNumericalGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

Numerical gradient. Works for any cost function but it is slower than a specific gradient.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1826 of file samples.c.

References Chart2Manifold(), CT_EPSILON, CT_N_DOF, FALSE, GetParameter(), INF, NEW, and TRUE.

Referenced by GradientSmooth().

◆ CombineCosts()

double CombineCosts	(	TStepCost	c1,
		TStepCost	c2,
		Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

inline

Adds two cost functions.

Parameters

c1	The first cost function to add.
c2	The second cost function to add.
p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	Previous point in the path. Actually not used. Can be NULL.
x	Current point in the path (simplified).
xn	Next point along the path.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn. Actually not used. Can be NULL.
w	The world structure.

Definition at line 1879 of file samples.c.

Referenced by EffortAndDispersion(), and LengthAndDispersion().

◆ CombineGradients()

boolean CombineGradients	(	TStepCostGradient	g1,
		TStepCostGradient	g2,
		Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

inline

Gradient of CombineCosts (if called with the right set of parameters).

Parameters

g1	The first gradient to add.
g2	Teh second gradient to add.
p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1890 of file samples.c.

References CT_N_DOF, GetParameter(), NEW, StepDispersion(), StepLength(), and SumVector().

Referenced by EffortAndDispersionGradient(), and LengthAndDispersionGradient().

◆ LengthAndDispersion()

double LengthAndDispersion	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

inline

Adds the length and dispersion cost functions. See CombineCosts.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	Previous point in the path. Actually not used. Can be NULL.
x	Current point in the path (simplified).
xn	Next point along the path.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn. Actually not used. Can be NULL.
w	The world structure.

Definition at line 1926 of file samples.c.

References CombineCosts(), StepDispersion(), and StepLength().

Referenced by SmoothSamples().

◆ LengthAndDispersionGradient()

boolean LengthAndDispersionGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

inline

Gradient of LengthAndDispersion.

See CombineGradients.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1936 of file samples.c.

References CombineGradients(), StepDispersionGradient(), StepLength(), and StepLengthGradient().

Referenced by SmoothSamples().

◆ EffortAndDispersion()

double EffortAndDispersion	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xp,
		double *	x,
		double *	xn,
		unsigned int	current,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tworld *	w
	)

inline

Adds the control effort and dispersion cost functions.

See CombineCosts.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xp	Previous point in the path. Actually not used. Can be NULL.
x	Current point in the path (simplified).
xn	Next point along the path.
current	Which of the three points is the current (i.e., the point for which we evaluate the cost).
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn. Actually not used. Can be NULL.
w	The world structure.

Definition at line 1951 of file samples.c.

References CombineCosts(), StepDispersion(), and StepEffort().

Referenced by SmoothSamples().

◆ EffortAndDispersionGradient()

boolean EffortAndDispersionGradient	(	Tparameters *	p,
		unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		double *	xpp,
		double *	xp,
		double *	x,
		double *	xn,
		double *	xnn,
		Tchart *	cpp,
		Tchart *	cp,
		Tchart *	c,
		Tchart *	cn,
		Tchart *	cnn,
		TJacobian *	sJ,
		TStepCost	cost,
		double *	u,
		Tworld *	w
	)

inline

Gradient of EffortAndDispersion.

See CombineGradients.

Parameters

p	The set of parameters.
tp	The topology of the variables (in simplified system).
sv	The selected variables to compute the path length component of the effort.
m	The size of the points (in simplified system!)
xpp	Previous of the previous point in the path. Not used. Can be NULL.
xp	Previous point in the path.
x	Current point. We compute the gradient with respect to this point.
xn	Next point in the path.
xnn	Next of the next point in the path. Not used. Can be NULL.
cpp	Chart at xpp. Not used. Can be NULL.
cp	A chart defined on xp.
c	A chart defined on x.
cn	A chart defined on xn.
cnn	Chart at xnn. Not used. Can be NULL.
sJ	The symbolic Jacobian of the constraints.
cost	Cost function.
u	The output gradient (in tangent space!)
w	The world structure.

Returns: TRUE if the gradient can be actually computed.

Definition at line 1961 of file samples.c.

References CombineGradients(), StepDispersionGradient(), StepEffortGradient(), and StepLength().

Referenced by SmoothSamples().

◆ cmpUInt()

int cmpUInt	(	const void *	a,
		const void *	b
	)

Auxiliary function used for qsort inside RandomSmooth when we exploit parallelism to speed up the smoothing.

Parameters

a	Pointer to the first unsigned integer.
b	Pointer to the second unsigned integer.

Returns: -1 if a<b, 0 if a==b, or 1 if a>b.

Definition at line 2071 of file samples.c.

Referenced by RandomSmooth().

◆ ShortcutSmooth()

void ShortcutSmooth	(	Tparameters *	pr,
		boolean *	sv,
		Tstatistics *	stime,
		unsigned int *	np,
		double ***	point,
		TAtlasBase *	w
	)

This is like the RandomSmooth but the tentative shortcuts are selected systematically: from the first sample as far as possible and so on.

Parameters

pr	The set of parameters.
sv	The variables to use in the distance computations.
stime	Statistics initilized at the beginning of the smoothing process.
np	Number of points defining the path.
point	The points defining the path.
w	The world/cuiksystem on which the sample is defined.

Definition at line 2087 of file samples.c.

References CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_INIT_CD, CT_N_DOF, DEFAULT_CONNECT, DeleteBox(), DeleteJacobian(), DeleteSamples(), FALSE, GetElapsedTime(), GetParameter(), PathLength(), and TRUE.

Referenced by SmoothSamples().

◆ RandomSmooth()

void RandomSmooth	(	Tparameters *	pr,
		unsigned int	nCores,
		unsigned int	maxIterations,
		boolean *	sv,
		Tstatistics *	stime,
		unsigned int *	np,
		double ***	point,
		TAtlasBase *	w
	)

Tries to reduce the lenght of a path by ramdomly selecting points on the path and verifying if a direct collision free path between them exists. If so, the new path replaces a segment of the previous path.

IMPORTANT: this is a sub-process of SmoothSamples and it is intended for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.

Parameters

pr	The set of parameters.
nCores	Number of computing cores available.
maxIterations	Maximum number of iterations. To be scaled by the number of steps in the path.
sv	Array with the components to use distance computations.
stime	Statistics initilized at the beginning of the smoothing process.
np	Number of points defining the path.
point	The points defining the path.
w	The world/cuiksystem on which the sample is defined.

Definition at line 2195 of file samples.c.

References cmpUInt(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_INIT_CD, CT_N_DOF, DEFAULT_CONNECT, DeleteBox(), DeleteJacobian(), DeleteSamples(), FALSE, GetElapsedTime(), GetParameter(), NEW, PathLength(), randomMax(), and TRUE.

Referenced by SmoothSamples().

◆ GradientSmooth()

void GradientSmooth	(	Tparameters *	pr,
		unsigned int	nCores,
		unsigned int	maxIterations,
		boolean *	sv,
		Tstatistics *	stime,
		unsigned int *	np,
		double ***	point,
		TStepCost	stepCost,
		TStepCostGradient	stepCostGradient,
		TAtlasBase *	w
	)

Reduces the length of a path by applying a movement to the points forming the path according the the gradient of the path lenght for w.r.t. the parameters of this points.

IMPORTANT: this is a sub-process of SmoothSamples and it is intended for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.

Parameters

pr	The set of parameters.
nCores	Number of computing cores available. Up to now this is not used.
maxIterations	Maximum number of iterations. To be scaled by the number of steps in the path.
sv	Array with the components to use distance computations.
stime	Statistics initilized at the beginning of the smoothing process.
np	Number of points defining the path.
point	The points defining the path.
stepCost	Function giving the cost of moving between two path steps See TStepCost
stepCostGradient	Function giving the gradient of the cost when moving a point in between two other points along the path. See TStepCostGradient If NULL a numerical gradient is used.
w	The world/cuiksystem on which the sample is defined.

Todo:: Implement a parallel version of the GradientSmooth function.

Definition at line 2419 of file samples.c.

References Chart2Manifold(), CopyChart(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_IN_COLLISION, CS_WD_INIT_CD, CT_CE, CT_DELTA, CT_E, CT_EPSILON, CT_N_DOF, CT_R, DeleteBox(), DeleteChart(), DeleteJacobian(), DistanceTopology(), Error(), FALSE, GeneralDotProduct(), GET_WORLD, GetElapsedTime(), GetJacobianSize(), GetParameter(), INF, InitTrustedChart(), NEW, NEWZ, PointInBoxTopology(), ScaleVector(), StepCostNumericalGradient(), and TRUE.

Referenced by SmoothSamples().

◆ SaveSamplesInt()

void SaveSamplesInt	(	Tfilename *	fpath,
		unsigned int	nvs,
		unsigned int	ns,
		double **	path,
		unsigned int	da,
		double **	action,
		double *	time
	)

Internal function to save a set of samples to a file. This includes code shared by SaveSamples and SaveSamplesN.

Parameters

fpath	Name of the file where to store the samples.
nvs	The dimensionality of each sample.
ns	The number of samples defining the path.
path	The collection of samples defining the path.
da	Dimension of the action space.
action	Action executed at each time step. Optional. NULL if not given.
time	Execution time for each action. Optional. NULL if not given.

Definition at line 3194 of file samples.c.

References DeleteBox(), Error(), GetFileFullName(), InitBoxFromPoint(), and PrintBox().

Referenced by SaveSamples(), SaveSamplesN(), and SaveTrajectory().

◆ InitSamples()

void InitSamples	(	unsigned int *	ms,
		unsigned int *	ns,
		double ***	path
	)

Initializes the space where to store the samples.

Parameters

ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.

Definition at line 672 of file samples.c.

References INIT_NUM_SAMPLES, and NEW.

Referenced by ConnectSamples(), ConnectSamplesChart(), InitSamplesReplay(), InitTrajectory(), MinimizeOnAtlas(), ReconstructAtlasPath(), ReconstructAtlasRRTPath(), Steps2PathinAtlasRRT(), and Steps2PathinRRT().

◆ InitSamplesReplay()

void InitSamplesReplay	(	unsigned int *	ms,
		unsigned int *	ns,
		double ***	path,
		boolean **	newChart,
		double **	time,
		double **	cost
	)

Initializes the space where to store the samples used to reply a simulation (e.g., to re-grow an RRT branch without the computational burden of re-generating the samples, which is particularly expensive in problems with dynamics).

Parameters

ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.
newChart	TRUE if a chart is generated at the corresponding step. If this is NULL, it is not used.
time	Time lapse from the previous step. If NULL, it is not used.
cost	Cost of the transition. If NULL, it is not used.

Definition at line 679 of file samples.c.

References InitSamples(), and NEW.

Referenced by AddBranchToAtlasRRT().

◆ InitTrajectory()

void InitTrajectory	(	unsigned int *	ms,
		unsigned int *	ns,
		double ***	path,
		unsigned int	da,
		double ***	actions,
		double **	times
	)

Initializes the space where to store the trajectory.

Parameters

ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.
da	Dimesion of the action space (of the action vectors).
actions	The action executed at each step.
times	The time for each step.

Definition at line 691 of file samples.c.

References InitSamples(), and NEW.

Referenced by AddBranchToAtlasRRT(), AtlasRRTSimulate(), ConnectDynamicStates(), ConnectDynamicStatesID(), NumericIntegration(), RefineTrajectory(), Simulate(), and Steps2PathinRRT().

◆ AddSample2Samples()

void AddSample2Samples	(	unsigned int	nv,
		double *	sample,
		unsigned int	nvs,
		boolean *	systemVars,
		unsigned int *	ms,
		unsigned int *	ns,
		double ***	path
	)

Adds a sample to a set of samples. The sample is given in full form (including all variables) but we only store the system variables.

Parameters

nv	Number of variables.
sample	The sample to add.
nvs	Number of system variables.
systemVars	Array to identify the system variables. Use NULL if all variables are to be included in the sample.
ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.

Definition at line 699 of file samples.c.

References MEM_DUP, and NEW.

Referenced by ConnectSamples(), ConnectSamplesChart(), MinimizeOnAtlas(), PathInChart(), ReconstructAtlasPath(), ReconstructAtlasRRTPath(), SmoothSamples(), Steps2PathinAtlasRRT(), and Steps2PathinRRT().

◆ AddSample2SamplesReplay()

void AddSample2SamplesReplay	(	unsigned int	nv,
		double *	sample,
		boolean	nc,
		double	h,
		double	c,
		unsigned int	nvs,
		boolean *	systemVars,
		unsigned int *	ms,
		unsigned int *	ns,
		double ***	path,
		boolean **	newChart,
		double **	time,
		double **	cost
	)

Adds a sample to a set of samples used to reply simulations (i.e., to re-simulate a brach without the expensive process to generate the samples). See InitSamplesReplay

Parameters

nv	Number of variables.
sample	The sample to add.
nc	TRUE if a chart is created at the new sample.
h	The time from the previous sample.
c	The cost of the transition.
nvs	Number of system variables.
systemVars	Array to identify the system variables. Use NULL if all variables are to be included in the sample.
ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.
newChart	TRUE if a chart is created at this step. May be NULL and then it is not used.
time	Time lapse from the previous step. May be NULL and then it is not used.
cost	Cost of the transition. May be NULL and then it is not used.

Definition at line 721 of file samples.c.

References MEM_DUP, MEM_EXPAND, and NEW.

Referenced by AddBranchToAtlasRRT().

◆ AddStep2Trajectory()

void AddStep2Trajectory	(	unsigned int	nv,
		double *	sample,
		unsigned int	nvs,
		boolean *	systemVars,
		double *	action,
		double	time,
		unsigned int *	ms,
		unsigned int *	ns,
		double ***	path,
		unsigned int	da,
		double ***	actions,
		double **	times
	)

Adds a step to a trajectory.

Parameters

nv	Number of variables.
sample	The sample to add.
nvs	Number of system variables.
systemVars	Array to identify the system variables. Use NULL if all variables are to be included in the sample.
action	The executed action. Vector of size da.
time	The duration of the action.
ms	Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
ns	Number of samples in the set. 0 after initialization.
path	The sample set.
da	The dimension of the action space. The size of the action vectors
actions	The action executed at each time step.
times	The duration of each action.

Definition at line 758 of file samples.c.

References MEM_DUP, MEM_EXPAND, and NEW.

Referenced by AddBranchToAtlasRRT(), AtlasRRTSimulate(), ConnectDynamicStates(), ConnectDynamicStatesID(), NumericIntegration(), RefineTrajectory(), Simulate(), and Steps2PathinRRT().

◆ ConnectDynamicStatesID()

double ConnectDynamicStatesID	(	Tparameters *	pr,
		unsigned int *	tp,
		Tbox *	ambient,
		boolean	collision,
		unsigned int	m,
		unsigned int	k,
		unsigned int	da,
		double *	state,
		double *	action,
		double	time,
		double *	goal,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		TJacobian *	J,
		TDynamicSpace *	ds,
		TAtlasBase *	w
	)

Version of ConnectDynamicStates where the action is adjusted at each step using inverse dynamics.

Parameters

pr	The set of parameters.
tp	The topology of the variables.
ambient	The ambient space limits.
collision	TRUE if collisions have to be avoided.
m	The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.
k	The dimension of the tangent space.
da	The dimension of the action space.
state	The state from where to start the simulation.
action	The action to execute.
time	The time to execute the action.
goal	The goal to be reached.
ns	The number of stats of the integrated trajectory.
path	The states in the integrated trajectory.
actions	The action in the integrated trajectory. This is just a repetition of the input action.
times	The integration step of each step.
J	The Jacobian of the system of equations.
ds	The dynamics space (memory space used during integration).
w	The world/cuiksystem on which the sample is defined.

Returns: The distance to the goal or INF if the integration fails for any reason (for instance if a collision is detected).

Definition at line 793 of file samples.c.

References AddStep2Trajectory(), ComputeInverseDynamics(), CS_WD_IN_COLLISION, CT_DELTA, DeleteChart(), DistanceTopology(), FALSE, GetParameter(), INF, InitChart(), InitTrajectory(), Local2Global(), Manifold2Chart(), NEWZ, NextDynamicState(), ScaleVector(), START2GOAL, and TRUE.

Referenced by RefineTrajectory().

◆ ConnectDynamicStates()

double ConnectDynamicStates	(	Tparameters *	pr,
		unsigned int *	tp,
		Tbox *	ambient,
		boolean	collision,
		unsigned int	m,
		unsigned int	k,
		unsigned int	da,
		double *	state,
		double *	action,
		double	time,
		double *	goal,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		TJacobian *	J,
		TDynamicSpace *	ds,
		TAtlasBase *	w
	)

Integrates the dynamic constraints with the aim of reaching a particular goal This is the dynamic version of ConnectSamples.

Parameters

pr	The set of parameters.
tp	The topology of the variables.
ambient	The ambient space limits.
collision	TRUE if collisions have to be avoided.
m	The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.
k	The dimension of the tangent space.
da	The dimension of the action space.
state	The state from where to start the simulation.
action	The action to execute.
time	The time to execute the action.
goal	The goal to be reached.
ns	The number of stats of the integrated trajectory.
path	The states in the integrated trajectory.
actions	The action in the integrated trajectory. This is just a repetition of the input action.
times	The integration step of each step.
J	The Jacobian of the system of equations.
ds	The dynamics space (memory space used during integration).
w	The world/cuiksystem on which the sample is defined.

Returns: The distance to the goal or INF if the integration fails for any reason (for instance if a collision is detected).

Definition at line 887 of file samples.c.

References AddStep2Trajectory(), CS_WD_IN_COLLISION, CT_DELTA, DeleteChart(), DistanceTopology(), FALSE, GetParameter(), INF, InitChart(), InitTrajectory(), NEWZ, NextDynamicState(), START2GOAL, and TRUE.

Referenced by RefineTrajectory().

◆ ConnectSamplesChart()

double ConnectSamplesChart	(	Tparameters *	pr,
		unsigned int *	tp,
		boolean *	sv,
		Tbox *	domain,
		unsigned int	m,
		unsigned int	n,
		double *	s1,
		double *	s2,
		double	md,
		boolean	checkCollisions,
		TJacobian *	sJ,
		boolean *	reached,
		boolean *	collision,
		double *	lastSample,
		unsigned int *	ns,
		double ***	path,
		TAtlasBase *	w
	)

Determines if two points on the manifold can be connected with a path of length below a given threshold.

This is the same as ConnectSamples but using a chart-based way to move over the manifold from s1 to s2.

As in the case of ConnectSamples the connection can fail depending on the shape of the manifold. However, for close samples it typically works fine.

This can be seen as a simple version of AddBranchToAtlasRRT. The main difference is that here we do not maintain an atlas: we generate charts as we advance over the manifold but we release them when they are not any longer useful.

IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.

Parameters

pr	The set of parameters.
tp	The topology for the variables.
sv	Components of the configuration to use to compute the length of the connection. If NULL all components are used. Up to now, it is only used for path smoothing purposes.
domain	The domain for the variables.
m	The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.
n	The number of equations in the problem. This is actually not used in this function but we keep this parameter for compatibility with ConnectSamples.
s1	The first sample to connect.
s2	The second sample to connect.
md	Maximum displacement between the two samples (along the components selected by 'sv').
checkCollisions	TRUE if the path has to be collision free.
sJ	The symbolic Jacobian.
reached	(output) TRUE if s2 can be reached form s1.
collision	(output) TRUE if the connection can not be established due to a collision.
lastSample	Buffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered.
ns	Number of steps of the path connecting the two samples.
path	Samples along the path connecting the two samples. Set ns or path to NULL if you are not interested in the path but only in verifying that the two samples can be connected. Note that the two given samples are NOT included in this output path.
w	The world/cuiksystem on which the sample is defined.

Returns: The maximum distance travelled form s1 to s2. The distance from s1 to the sample stored in 'lastSample', if any.

Definition at line 961 of file samples.c.

References AccumulateVector(), AddSample2Samples(), ArrayPi2Pi(), Chart2Manifold(), CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_CE, CT_DELTA, CT_E, CT_EPSILON, CT_N_DOF, CT_R, DeleteChart(), DeleteSamples(), DifferenceVector(), DifferenceVectorTopology(), DistanceTopology(), DistanceTopologySubset(), Error(), FALSE, GetJacobianSize(), GetParameter(), InitChart(), InitSamples(), Manifold2Chart(), NEW, Norm(), PointInBoxTopology(), ScaleVector(), SumVectorScale(), and TRUE.

Referenced by WireAtlasRRTstar().

◆ ConnectSamples()

double ConnectSamples	(	Tparameters *	pr,
		unsigned int *	tp,
		boolean *	sv,
		Tbox *	domain,
		unsigned int	m,
		unsigned int	n,
		double *	s1,
		double *	s2,
		double	md,
		boolean	checkCollisions,
		TJacobian *	sJ,
		boolean *	reached,
		boolean *	collision,
		double *	lastSample,
		unsigned int *	ns,
		double ***	path,
		TAtlasBase *	w
	)

Determines if two points on the manifold can be connected with a path of length below a given threshold.

The method used to determine the path does not return the sortest path and it can fail in many cases, specially when the samples are far away.

IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.

Parameters

pr	The set of parameters.
tp	The topology for the variables.
sv	Components of the configuration to use to compute the length of the connection. If NULL all components are used. Up to now, it is only used for path smoothing purposes.
domain	The domain for the variables.
m	The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the non-simplified one, as it is the case of many of the functions on samples.
n	The number of equations in the problem.
s1	The first sample to connect.
s2	The second sample to connect.
md	Maximum displacement between the two samples (along the components selected by 'sv').
checkCollisions	TRUE if the path has to be collision free.
sJ	The symbolic Jacobian. It is actually not used in this function but the parameter is keep for compatibility with ConnectSamplesChart. You can safely set it to NULL in this case.
reached	(output) TRUE if s2 can be reached form s1.
collision	(output) TRUE if the connection can not be established due to a collision.
lastSample	Buffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered.
ns	Number of steps of the path connecting the two samples.
path	Samples along the path connecting the two samples. Set ns or path to NULL if you are not interested in the path but only in verifying that the two samples can be connected. Note that the two given samples are NOT included in this output path.
w	The The world/cuiksystem on which the sample is defined.

Returns: The maximum distance travelled form s1 to s2. The distance from s1 to the sample stored in 'lastSample', if any.

Definition at line 1234 of file samples.c.

References AddSample2Samples(), ArrayPi2Pi(), CS_WD_IN_COLLISION, CS_WD_NEWTON_IN_SIMP, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_EPSILON, DeleteSamples(), DifferenceVectorTopology(), DistanceTopology(), DistanceTopologySubset(), DIVERGED, FALSE, GetParameter(), InitSamples(), NEW, Norm(), PointInBoxTopology(), SumVectorScale(), and TRUE.

Referenced by AddStepToRRTstar(), BiRRTstar(), ReWireRRTstar(), Steps2PathinRRT(), and WireRRTstar().

◆ PathLength()

double PathLength	(	unsigned int *	tp,
		boolean *	sv,
		unsigned int	m,
		unsigned int	np,
		double **	point
	)

Computes the length of a path defined by a set of points. IMPORTANT: this is for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.

Parameters

tp	The topology of the variables.
sv	The components to use in the distance computation.
m	The size of each point.
np	Number of points in the path.
point	The set of points.

Returns: The path length.

Definition at line 1364 of file samples.c.

References DistanceTopologySubset().

Referenced by main(), RandomSmooth(), ShortcutSmooth(), and SmoothSamples().

◆ PathEffort()

double PathEffort	(	Tparameters *	p,
		unsigned int	m,
		unsigned int	np,
		double **	point,
		Tworld *	w
	)

Computes the approximated control effort of a path. This only makes sense in problems where there is an associated energy which generates a force field (i.e., the gradient of the energy). The control effort is the effort to compensate this force field and to drive the robot along the path. If no force filed is defined, the control effort gives the path length (see PathLength).

The control effort is defined over the continuous path and we approximate it at discrete times (i.e., an integral approximated as a discrete sum).

Parameters

p	The set of parameters.
m	The size of each point.
np	Number of points in the path.
point	The set of points.
w	The world structure giving semantic to the path points.

Returns: The (approximated) control effort along the path.

Definition at line 1976 of file samples.c.

References ChangeParameter(), CS_WD_FROM_WORLD, CT_CE, CT_E, CT_N_DOF, CT_R, DeleteBox(), DeleteChart(), DeleteJacobian(), Error(), GetParameter(), GetWorldSimpInitialBox(), GetWorldSimpJacobian(), GetWorldSimpTopology(), InitTrustedChart(), KINEMATIC_SUBSPACE, NEW, RegenerateWorldSolutionPoint(), StepEffort(), TRUE, WorldGenerateSimplifiedPoint(), WorldManifoldDimension(), and WorldSimpKinematicVars().

Referenced by main().

◆ SmoothSamples()

void SmoothSamples	(	Tparameters *	pr,
		boolean	parallel,
		int	mode,
		unsigned int	maxIterations,
		unsigned int	ns,
		double **	path,
		unsigned int *	sns,
		double ***	spath,
		TAtlasBase *	w
	)

Produces a path that (locally) minimizes the length and connects the two extremes of the given path.

This implements a basic smoothing structure transforming the input samples (in the original system) to the simplified system and then call a speciallized smoothing procedure.

Only path with more than 2 steps are suitable of optimization. Otherwise this function triggers an error.

Parameters

pr	The set of parameters.
parallel	TRUE if we have to execute a parallel vesion of smoothing process. Not relevant in SMOOTH_CUT.
mode	Smoothing mode to be used: SMOOTH_RANDOM, SMOOTH_GRADIENT, or SMOOTH_CUT.
maxIterations	Maximum number of iterations. To be scaled by the number of steps in the path.
ns	The numer of samples in the input path.
path	The samples in the input path.
sns	The number of samples in the output (smoothed) path.
spath	The samples in the output (smoothed) path.
w	The The world/cuiksystem on which the sample is defined.

Definition at line 2811 of file samples.c.

References AddSample2Samples(), ChangeParameter(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_GET_SYSTEM_VARS, CS_WD_MANIFOLD_DIMENSION, CS_WD_REGENERATE_ORIGINAL_POINT, CS_WD_REGENERATE_SOLUTION_POINT, CT_DELTA, CT_N_DOF, DeleteBox(), DeleteStatistics(), EffortAndDispersion(), EffortAndDispersionGradient(), Error(), GET_WORLD, GetElapsedTime(), GetParameter(), GradientSmooth(), InitStatistics(), KINEMATIC_SUBSPACE, LengthAndDispersion(), LengthAndDispersionGradient(), NEW, ON_CUIKSYSTEM, PathLength(), RandomSmooth(), ShortcutSmooth(), SMOOTH_DISPERSION, SMOOTH_EFFORT, SMOOTH_GRADIENT, SMOOTH_RANDOM, SMOOTH_SHORTCUT, StepDispersion(), StepDispersionGradient(), StepEffort(), StepEffortGradient(), StepLength(), StepLengthGradient(), and WorldSimpKinematicVars().

Referenced by main().

◆ ConcatSamples()

void ConcatSamples	(	unsigned int	nvs,
		unsigned int	ns1,
		double **	path1,
		unsigned int	ns2,
		double **	path2,
		unsigned int *	ns,
		double ***	path
	)

Produces a path that is the concatenation of two paths.

Parameters

nvs	The number of values per sample.
ns1	The number of samples defining the first path.
path1	The collection of samples defining the first path.
ns2	The number of samples defining the second path.
path2	The collection of samples defining the second path.
ns	The number of samples defining the output path.
path	The collection of samples defining the output path.

Definition at line 2981 of file samples.c.

References NEW.

◆ ReverseConcatSamples()

void ReverseConcatSamples	(	unsigned int	nvs,
		unsigned int	ns1,
		double **	path1,
		unsigned int	ns2,
		double **	path2,
		unsigned int *	ns,
		double ***	path
	)

Produces a path that is the concatenation of a path and the reverse of a second path. This is basically used to reconstruct paths for bidirectional (Atlas)RRTs.

Parameters

nvs	The number of values per sample.
ns1	The number of samples defining the first path.
path1	The collection of samples defining the first path.
ns2	The number of samples defining the second path.
path2	The collection of samples defining the second path.
ns	The number of samples defining the output path.
path	The collection of samples defining the output path.

Definition at line 3002 of file samples.c.

References NEW.

Referenced by ReverseConcatTrajectories().

◆ ReverseConcatTrajectories()

void ReverseConcatTrajectories	(	unsigned int	nvs,
		unsigned int	da,
		unsigned int	ns1,
		double **	path1,
		double **	action1,
		double *	time1,
		unsigned int	ns2,
		double **	path2,
		double **	action2,
		double *	time2,
		unsigned int *	ns,
		double ***	path,
		double ***	action,
		double **	time
	)

Version of ReverseConcatSamples but for trajectories.

We assume that the first path is from the START2GOAL tree and that the second path is from the GOAL2START tree. This is critical since the semantic of the actions is different in each tree.

Parameters

nvs	The number of values per sample.
da	Dimension of the action space.
ns1	The number of samples defining the first path.
path1	The collection of samples defining the first path.
action1	The action executed to reach the corresponding state in path1
time1	The time to execute the corresponding action in action1.
ns2	The number of samples defining the second path.
path2	The collection of samples defining the second path.
action2	The action executed to reach the next state in path2. Note the difference with respect to action1. Action1 is the action from the previous state to the current and action2 is the action from the current state to the next.
time2	The time to execute the corresponding action in action2.
ns	The number of samples defining the output path.
path	The collection of samples defining the output path.
action	Action executed to reach each sample.
time	Duration of each action.

Definition at line 3027 of file samples.c.

References Error(), NEW, NEWZ, and ReverseConcatSamples().

Referenced by PathStart2GoalInRRT().

◆ ReverseSamples()

void ReverseSamples	(	unsigned int	ns,
		double **	path
	)

Reverses a set of samples.

Parameters

ns	Number of samples.
path	The samples to reverse.

Definition at line 3101 of file samples.c.

Referenced by ReconstructAtlasRRTPath().

◆ ReadOneSample()

unsigned int ReadOneSample	(	Tparameters *	p,
		char *	fname,
		unsigned int	nvs,
		double **	s
	)

Reads one sample from a file.

If the mechanisms representation is DOF-based (see REP_JOINTS) the samples are read from a .dof file. Otherwise they are read from a .samples file.

Parameters

p	The set of parameters.
fname	File from where to read the sample.
nvs	Number of system variables in the problem = Number of values in the sample.
s	Where to store the sample. The space is allocated internally.

Returns: The dimensionality of the sample. Number of system variables of w.

Definition at line 3121 of file samples.c.

References CreateFileName(), CT_REPRESENTATION, DeleteFileName(), Error(), GetFileFullName(), GetParameter(), JOINTS_EXT, LINKS_EXT, NEW, and REP_JOINTS.

Referenced by main().

◆ ReadTwoSamples()

unsigned int ReadTwoSamples	(	Tparameters *	p,
		char *	fname,
		unsigned int	nvs,
		double **	s1,
		double **	s2
	)

Reads two sample from a file.

If the mechanisms representation is DOF-based (see REP_JOINTS) the samples are read from a .dof file. Otherwise they are read from a .samples file.

Parameters

p	The set of parameters.
fname	File from where to read the sample.
nvs	Number of system variables in the problem = Number of values in the sample.
s1	Where to store the first sample. The space is allocated internally.
s2	Where to store the second sample. The space is allocated internally.

Returns: The dimensionality of the sample. Number of system variables of w.

Definition at line 3152 of file samples.c.

References CreateFileName(), CT_EPSILON, CT_REPRESENTATION, DeleteFileName(), Distance(), Error(), GetFileFullName(), GetParameter(), JOINTS_EXT, LINKS_EXT, NEW, and REP_JOINTS.

Referenced by main().

◆ SaveSamples()

void SaveSamples	(	char *	fname,
		char *	suffix,
		unsigned int	nvs,
		unsigned int	ns,
		double **	path
	)

Saves a set of samples (possibly defining a solution path) to a file.

Parameters

fname	The file where to store the samples.
suffix	Suffix to be added to the filename. It is used when generating smoothed paths.
nvs	The dimensionality of each sample.
ns	The number of samples defining the path.
path	The collection of samples defining the path.

Definition at line 3248 of file samples.c.

References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.

Referenced by main(), and MinimizeOnAtlas().

◆ SaveTrajectory()

void SaveTrajectory	(	char *	fname,
		char *	suffix,
		unsigned int	nvs,
		unsigned int	ns,
		double **	path,
		unsigned int	da,
		double **	action,
		double *	time
	)

Saves a set of samples (possibly defining a solution path) to a file including action and timing information. Thus, the result is a trajectory.

Parameters

fname	The file where to store the samples.
suffix	Suffix to be added to the filename. It is used when generating smoothed paths.
nvs	The dimensionality of each sample.
ns	The number of samples defining the path.
path	The collection of samples defining the path.
da	The dimension of the action space (of the action vectors).
action	Actions executed at each time step.
time	Duration of the execution of each action.

Definition at line 3263 of file samples.c.

References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.

Referenced by main().

◆ SaveSamplesN()

void SaveSamplesN	(	char *	fname,
		boolean	smooth,
		unsigned int	n,
		unsigned int	nvs,
		unsigned int	ns,
		double **	path
	)

Saves a set of samples (possibly defining a solution path) to a file. This function is used to save sets of paths.

Parameters

fname	The file where to store the samples.
smooth	TRUE if the set of samples is the result of a smoothing process.
n	The number of this path. Used to define a different name for a sequence of paths to store.
nvs	The dimensionality of each sample.
ns	The number of samples defining the path.
path	The collection of samples defining the path.

Definition at line 3284 of file samples.c.

References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.

Referenced by MinimizeOnAtlas().

◆ LoadSamples()

boolean LoadSamples	(	Tfilename *	fname,
		unsigned int *	nvs,
		unsigned int *	ns,
		double ***	path
	)

Reads a set of samples from a file created using SaveSamples.

Parameters

fname	Name of the file containing the samples.
nvs	Number of entries for each sample.
ns	Number of samples (output).
path	The set of samples (output).

Returns: TRUE if the sample set could be actually read.

Definition at line 3303 of file samples.c.

References Advance(), DeleteListOfBoxes(), EndOfList(), Error(), FALSE, First(), GetBoxCenter(), GetBoxNIntervals(), GetCurrent(), GetFileFullName(), InitIterator(), ListSize(), NEW, ReadListOfBoxes(), and TRUE.

Referenced by main().

◆ LoadTrajectory()

boolean LoadTrajectory	(	Tfilename *	fname,
		unsigned int *	nvs,
		unsigned int *	ns,
		double ***	path,
		unsigned int *	da,
		double ***	action,
		double **	time
	)

Reads a trajectory SaveTrajectory.

Parameters

fname	Name of the file containing the samples.
nvs	Number of entries for each sample.
ns	Number of samples (output).
path	The set of samples (output).
da	Dimension of the action space (output).
action	The action applied at each step (output).
time	The time each action is executed (output).

Returns: TRUE if the sample set could be actually read.

Definition at line 3356 of file samples.c.

References Error(), FALSE, GetFileFullName(), NEW, and TRUE.

Referenced by main().

◆ SparsifyTrajectory()

void SparsifyTrajectory	(	double	tm,
		unsigned int **	id,
		unsigned int *	ns,
		double **	path,
		double **	action,
		double *	time
	)

Ensures a mininum time between two consecutive trajectory steps. This is use before visualizing a trajectory since if the time between frames is too short there are delays in the visualization and it looks unnatural.

Parameters

tm	The minimum time in between steps.
id	The identifiers of the steps that are keep in the sparsified trajectory.
ns	Number of samples.
path	The set of samples.
action	The action applied at each step.
time	The time each action is executed.

Returns: TRUE if the sample set could be actually read.

Definition at line 3435 of file samples.c.

References NEW.

Referenced by main().

◆ PlotSamples()

void PlotSamples	(	Tparameters *	p,
		Tplot3d *	p3d,
		unsigned int	xID,
		unsigned int	yID,
		unsigned int	zID,
		unsigned int	ns,
		double **	path
	)

Plots a 3D projection of a set of samples representing a path. The path is represented by a green line.

Parameters

p	The set of parametres.
p3d	The 3d plot where to add the plot.
xID	First dimension for the projection (in the original system including system vars only).
yID	Second dimension for the projection (in the original system including system vars only).
zID	Third dimension for the projection (in the original system including system vars only).
ns	The number of samples.
path	The set of samples.

Definition at line 3472 of file samples.c.

References Close3dObject(), CORRECT_POINT_PLOT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeleteColor(), GetParameter(), M_PI, NEW, NewColor(), PlotSphere(), PlotVect3d(), and StartNew3dObject().

Referenced by main().

◆ PlotForceField()

void PlotForceField	(	Tparameters *	p,
		Tplot3d *	p3d,
		unsigned int	xID,
		unsigned int	yID,
		unsigned int	zID,
		Tworld *	w,
		unsigned int	ns,
		double **	sols
	)

Plots a 3D projection of the force field on a set of points. The force filed is represented as a short line on each point pointing in the direction of the force field and with length proportional to the force-field magnitude.

Parameters

p	The set of parametres.
p3d	The 3d plot where to add the plot.
xID	First dimension for the projection (in the original system including system vars only).
yID	Second dimension for the projection (in the original system including system vars only).
zID	Third dimension for the projection (in the original system including system vars only).
w	The world from where to compute the force field.
ns	The number of solution points.
sols	The set of solution points.

Definition at line 3583 of file samples.c.

References Close3dObject(), CT_DELTA, CT_EPSILON, DeleteColor(), Error(), FALSE, GetParameter(), GetWorldSystemVars(), INF, NEW, NewColor(), NO_UINT, Norm(), PlotLine(), RegenerateWorldSolutionPoint(), ScaleVector(), StartNew3dObject(), Warning(), and WorldForceField().

Referenced by main().

◆ DeleteSamples()

void DeleteSamples	(	unsigned int	ns,
		double **	path
	)

Release the memory used by a set of samples.

Parameters

ns	The number of samples.
path	The set of samples.

Definition at line 3686 of file samples.c.

Referenced by AtlasRRTSimulate(), ConnectSamples(), ConnectSamplesChart(), DeleteRRT(), DeleteSamplesReplay(), DeleteTrajectory(), main(), RandomSmooth(), ReconstructAtlasRRTPath(), ShortcutSmooth(), Steps2PathinAtlasRRT(), and Steps2PathinRRT().

◆ DeleteSamplesReplay()

void DeleteSamplesReplay	(	unsigned int	ns,
		double **	path,
		boolean *	newChart,
		double *	time,
		double *	cost
	)

Release the memory used by a set of samples with flags. See InitSamplesReplay.

Parameters

ns	The number of samples.
path	The set of samples.
newChart	TRUE if a chart is created at the corresponding step. Can be NULL.
time	Time lapse from the previous step. Can be NULL.
cost	Cost of the transition. Can be NULL.

Definition at line 3698 of file samples.c.

References DeleteSamples().

Referenced by AddBranchToAtlasDynamicRRT().

◆ DeleteTrajectory()

void DeleteTrajectory	(	unsigned int	ns,
		double **	path,
		double **	action,
		double *	time
	)

Release the memory used by a trajectory.

Parameters

ns	The number of samples.
path	The set of samples.
action	The action applied at each step.
time	The time each action is executed.

Definition at line 3710 of file samples.c.

References DeleteSamples().

Referenced by AddBranchToAtlasRRT(), AddNodeToRRT(), ApproachState(), DeleteRRT(), kinobiRRT(), kinoEST(), kinoRRT(), main(), PathStart2GoalInRRT(), and RefineTrajectory().

Institut de Robòtica i Informàtica Industrial

Introduction

Macros

Functions

Macro Definition Documentation

◆ DISPERSION

◆ REMOVE_POINTS

◆ APPROXIMATED_CHARTS_IN_GRADIENT

Function Documentation

◆ StepDispersion()

◆ StepDispersionGradient()

◆ StepLength()

◆ StepLengthGradient()

◆ StepEffort()

◆ StepEffortGradient()

◆ StepCostNumericalGradient()

◆ CombineCosts()

◆ CombineGradients()

◆ LengthAndDispersion()

◆ LengthAndDispersionGradient()

◆ EffortAndDispersion()

◆ EffortAndDispersionGradient()

◆ cmpUInt()

◆ ShortcutSmooth()

◆ RandomSmooth()

◆ GradientSmooth()

◆ SaveSamplesInt()

◆ InitSamples()

◆ InitSamplesReplay()

◆ InitTrajectory()

◆ AddSample2Samples()

◆ AddSample2SamplesReplay()

◆ AddStep2Trajectory()

◆ ConnectDynamicStatesID()

◆ ConnectDynamicStates()

◆ ConnectSamplesChart()

◆ ConnectSamples()

◆ PathLength()

◆ PathEffort()

◆ SmoothSamples()

◆ ConcatSamples()

◆ ReverseConcatSamples()

◆ ReverseConcatTrajectories()

◆ ReverseSamples()

◆ ReadOneSample()

◆ ReadTwoSamples()

◆ SaveSamples()

◆ SaveTrajectory()

◆ SaveSamplesN()

◆ LoadSamples()

◆ LoadTrajectory()

◆ SparsifyTrajectory()

◆ PlotSamples()

◆ PlotForceField()

◆ DeleteSamples()

◆ DeleteSamplesReplay()

◆ DeleteTrajectory()

Institut de Robòtica
i Informàtica Industrial