samples.h File Reference

Introduction

Auxiliary functions to deal with sets of samples.

Samples are used as start/goal for the planning and solution paths are sets of samples.

See also
samples.c

Definition in file samples.h.

Macros

#define INIT_NUM_SAMPLES   50
 Initial number of samples (or steps) in a path. More...
 
#define PATH_NODES   1
 Selects the way to plot paths. More...
 
#define SMOOTH_RANDOM   0
 One of the possible smoothing methods. More...
 
#define SMOOTH_GRADIENT   1
 One of the possible smoothing methods. More...
 
#define SMOOTH_SHORTCUT   2
 One of the possible smoothing methods. More...
 
#define SMOOTH_EFFORT   3
 One of the possible smoothing methods. More...
 
#define SMOOTH_DISPERSION   4
 One of the possible smoothing methods. More...
 
#define DEFAULT_CONNECT   ConnectSamplesChart
 Macro to easily switch the connection method. More...
 
#define KINEMATIC_SUBSPACE   0
 Focus on the kinematic sub-space. More...
 
#define CORRECT_POINT_PLOT(cx, cy, cz, x, y, z)   { if ((cx<0)&&(x<cx)) x+=M_2PI;if ((cx>0)&&(x>cx)) x-=M_2PI;if ((cy<0)&&(y<cy)) y+=M_2PI;if ((cy>0)&&(y>cy)) y-=M_2PI;if ((cz<0)&&(z<cz)) z+=M_2PI;if ((cz>0)&&(z>cz)) z-=M_2PI; }
 Corrects the 3d position of a point. More...
 

Typedefs

typedef double(* TStepCost) (Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, unsigned int, Tchart *, Tchart *, Tchart *, Tworld *)
 Template of step costs. More...
 
typedef boolean(* TStepCostGradient) (Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, double *, double *, Tchart *, Tchart *, Tchart *, Tchart *, Tchart *, TJacobian *, TStepCost, double *, Tworld *)
 Template of step cost gradients. More...
 

Functions

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

◆ INIT_NUM_SAMPLES

#define INIT_NUM_SAMPLES   50

Initial number of samples (or steps) in a path. This will be enlarged if necessary.

Definition at line 30 of file samples.h.

◆ PATH_NODES

#define PATH_NODES   1

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

Definition at line 39 of file samples.h.

◆ SMOOTH_RANDOM

#define SMOOTH_RANDOM   0

Using this method paths are shortened trying to define shorcuts between randomly selected elements in the original path. This methods is quite effective.

Definition at line 49 of file samples.h.

◆ SMOOTH_GRADIENT

#define SMOOTH_GRADIENT   1

Using this method paths the length of the path is reduced using gradient descent. This produces very smooth paths but it is slow reaching a local minimum. Moreover, this minimum is typically local even in the current homotopy class.

Definition at line 59 of file samples.h.

◆ SMOOTH_SHORTCUT

#define SMOOTH_SHORTCUT   2

This is like the SMOOTH_RANDOM but the shotcuts are defined in a systematic way: try to reach as far as possible with a direct path from the first sample and so on.

Definition at line 69 of file samples.h.

◆ SMOOTH_EFFORT

#define SMOOTH_EFFORT   3

This is SMOOTH_GRADIENT but the measure to optimize is the control effort instead than the path lenght. This only make sense in tensegrity structures since they have an associated potential energy. In other structures this is very close the the path lenght optimization.

Definition at line 81 of file samples.h.

◆ SMOOTH_DISPERSION

#define SMOOTH_DISPERSION   4

This is SMOOTH_GRADIENT but the measure to optimize is the dispersion of the points along the path (i.e., try to make all steps of the same size). This is only used for debug since the dispersion is typically a secondary objective.

Definition at line 92 of file samples.h.

◆ DEFAULT_CONNECT

#define DEFAULT_CONNECT   ConnectSamplesChart

We have to modes to connect samples moving on the manifold of constratints: ConnectSamples and ConnectSamplesChart. The first one has been extensively tested. The second one is faster but less robust (we are working on improving this). This macro is just used to compare and debug these two connection methods.

Definition at line 104 of file samples.h.

◆ KINEMATIC_SUBSPACE

#define KINEMATIC_SUBSPACE   0

If set to 1, functions like path length or some aspects of the path effort or even smooth path (using gradient descent!) focus on the kinematic sub-space, not considering the force-related variables.

Obviously, this only has an effect if the problem includes force-related variables (i.e., in tensegrity problems).

Definition at line 119 of file samples.h.

◆ CORRECT_POINT_PLOT

#define CORRECT_POINT_PLOT (   cx,
  cy,
  cz,
  x,
  y,
 
)    { if ((cx<0)&&(x<cx)) x+=M_2PI;if ((cx>0)&&(x>cx)) x-=M_2PI;if ((cy<0)&&(y<cy)) y+=M_2PI;if ((cy>0)&&(y>cy)) y-=M_2PI;if ((cz<0)&&(z<cz)) z+=M_2PI;if ((cz>0)&&(z>cz)) z-=M_2PI; }

When ploting a point we have to take into account the cuts in the 3D space introduced to avoid cuts in the middle of a cyclic manifold.

Parameters
cxCut along the x dimension.
cyCut along the y dimension.
czCut along the z dimension.
xx dimension to correct.
yy dimension to correct.
zz dimension to correct.

Definition at line 135 of file samples.h.

Typedef Documentation

◆ TStepCost

typedef double(* TStepCost) (Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, unsigned int, Tchart *, Tchart *, Tchart *, Tworld *)

Functions that evaluate the cost of a path step should be of this type.

Cost functions can involve up to 5 points around a given point

  • [xpp xp x xn xnn]

but each elementary cost evaluation only involves 3 of these points. With this, x can be in up to three triplets

  • [xpp xp x]
  • [xp x xn]
  • [x xn xnn]

In other words, the "variation" of x can affect the cost over these three triplets.

Cost functions are typically evaluated over paths (not only for individual steps). Care must be taken for the cost function not to return twice the cost of a given step. For instance, the path length is the sum of the step lengts and the step length ony computes the distance between the first and second paramter for the triplets

  • [xp x xn]
  • [x xn xnn]

but returns 0 for the triplet [xpp xp x]. When evaluated in sequence this efectively gives the path lenght. In contrast, the path dispersion returns values for the tree triplets. The step length is a 3-step function but the step dispersion is a 5-step function.

See StepLength or StepEffort for examples of path step costs.

This is used in GradientSmooth.

The parameteres of a cost function are

  • The set of parameters.
  • The topology of the variables (in simplified system).
  • The selected variables to compute the path length component of the effort.
  • The size of the points (in simplified system!)
  • First point in the path (simplified).
  • Second point in the path
  • Third point along the path.
  • Which of the three points is the current point (0,1,2) Which of the three triplets defined above are we passing to the cost function.
  • A chart defined on the first point in the path.
  • A chart defined on the second point in the path.
  • A chart defined on the third point in the path.
  • The world structure.

Some of them might not be used but are given with the aim of permetting very general cost functions.

Definition at line 200 of file samples.h.

◆ TStepCostGradient

typedef boolean(* TStepCostGradient) (Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, double *, double *, Tchart *, Tchart *, Tchart *, Tchart *, Tchart *, TJacobian *, TStepCost, double *, Tworld *)

Functions that evaluate the gradient of the cost of a path step should be of this type.

Gradients are not evaluated in sequence and accumulated but called for individual points. Thus, the output of the gradient is the accumulated gradient for the tree triplets

  • [xpp xp x]
  • [xp x xn]
  • [x xn xnn]

with [xpp xp x xn xnn] the five points around x.

See StepLengthGradient or StepEffortGradient for example of path step cost gradients.

This is used in GradientSmooth.

The parameters of a gradient function are:

  • The set of parameters.
  • The topology of the variables (in simplified system).
  • The selected variables to compute the gradient.
  • The size of the points (in simplified system!)
  • Previous of the previous point in the path.
  • Previous point in the path.
  • Current point. We compute the gradient with respect to this point.
  • Next point in the path.
  • Next of the next point in the path.
  • A chart defined on x-2
  • A chart defined on x-1.
  • A chart defined on x.
  • A chart defined on x+1.
  • A chart defined on x+2
  • The Jacobian of the constraints. Only used for numerical gradients.
  • The cost function (only used in numerical evaluation).
  • The output gradient (in tangent space)
  • The world structure.

Some of them might not be used but are given with the aim of permetting very general cost functions.

Definition at line 252 of file samples.h.

Function Documentation

◆ InitSamples()

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

Initializes the space where to store the samples.

Parameters
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe 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
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe sample set.
newChartTRUE if a chart is generated at the corresponding step. If this is NULL, it is not used.
timeTime lapse from the previous step. If NULL, it is not used.
costCost 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
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe sample set.
daDimesion of the action space (of the action vectors).
actionsThe action executed at each step.
timesThe 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().

◆ 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
prThe set of parameters.
parallelTRUE if we have to execute a parallel vesion of smoothing process. Not relevant in SMOOTH_CUT.
modeSmoothing mode to be used: SMOOTH_RANDOM, SMOOTH_GRADIENT, or SMOOTH_CUT.
maxIterationsMaximum number of iterations. To be scaled by the number of steps in the path.
nsThe numer of samples in the input path.
pathThe samples in the input path.
snsThe number of samples in the output (smoothed) path.
spathThe samples in the output (smoothed) path.
wThe 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
nvsThe number of values per sample.
ns1The number of samples defining the first path.
path1The collection of samples defining the first path.
ns2The number of samples defining the second path.
path2The collection of samples defining the second path.
nsThe number of samples defining the output path.
pathThe 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
nvsThe number of values per sample.
ns1The number of samples defining the first path.
path1The collection of samples defining the first path.
ns2The number of samples defining the second path.
path2The collection of samples defining the second path.
nsThe number of samples defining the output path.
pathThe 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
nvsThe number of values per sample.
daDimension of the action space.
ns1The number of samples defining the first path.
path1The collection of samples defining the first path.
action1The action executed to reach the corresponding state in path1
time1The time to execute the corresponding action in action1.
ns2The number of samples defining the second path.
path2The collection of samples defining the second path.
action2The 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.
time2The time to execute the corresponding action in action2.
nsThe number of samples defining the output path.
pathThe collection of samples defining the output path.
actionAction executed to reach each sample.
timeDuration 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
nsNumber of samples.
pathThe samples to reverse.

Definition at line 3101 of file samples.c.

Referenced by ReconstructAtlasRRTPath().

◆ 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
nvNumber of variables.
sampleThe sample to add.
nvsNumber of system variables.
systemVarsArray to identify the system variables. Use NULL if all variables are to be included in the sample.
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe 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
nvNumber of variables.
sampleThe sample to add.
ncTRUE if a chart is created at the new sample.
hThe time from the previous sample.
cThe cost of the transition.
nvsNumber of system variables.
systemVarsArray to identify the system variables. Use NULL if all variables are to be included in the sample.
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe sample set.
newChartTRUE if a chart is created at this step. May be NULL and then it is not used.
timeTime lapse from the previous step. May be NULL and then it is not used.
costCost 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
nvNumber of variables.
sampleThe sample to add.
nvsNumber of system variables.
systemVarsArray to identify the system variables. Use NULL if all variables are to be included in the sample.
actionThe executed action. Vector of size da.
timeThe duration of the action.
msMax number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary.
nsNumber of samples in the set. 0 after initialization.
pathThe sample set.
daThe dimension of the action space. The size of the action vectors
actionsThe action executed at each time step.
timesThe 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
prThe set of parameters.
tpThe topology of the variables.
ambientThe ambient space limits.
collisionTRUE if collisions have to be avoided.
mThe 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.
kThe dimension of the tangent space.
daThe dimension of the action space.
stateThe state from where to start the simulation.
actionThe action to execute.
timeThe time to execute the action.
goalThe goal to be reached.
nsThe number of stats of the integrated trajectory.
pathThe states in the integrated trajectory.
actionsThe action in the integrated trajectory. This is just a repetition of the input action.
timesThe integration step of each step.
JThe Jacobian of the system of equations.
dsThe dynamics space (memory space used during integration).
wThe 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
prThe set of parameters.
tpThe topology of the variables.
ambientThe ambient space limits.
collisionTRUE if collisions have to be avoided.
mThe 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.
kThe dimension of the tangent space.
daThe dimension of the action space.
stateThe state from where to start the simulation.
actionThe action to execute.
timeThe time to execute the action.
goalThe goal to be reached.
nsThe number of stats of the integrated trajectory.
pathThe states in the integrated trajectory.
actionsThe action in the integrated trajectory. This is just a repetition of the input action.
timesThe integration step of each step.
JThe Jacobian of the system of equations.
dsThe dynamics space (memory space used during integration).
wThe 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
prThe set of parameters.
tpThe topology for the variables.
svComponents 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.
domainThe domain for the variables.
mThe 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.
nThe number of equations in the problem. This is actually not used in this function but we keep this parameter for compatibility with ConnectSamples.
s1The first sample to connect.
s2The second sample to connect.
mdMaximum displacement between the two samples (along the components selected by 'sv').
checkCollisionsTRUE if the path has to be collision free.
sJThe 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.
lastSampleBuffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered.
nsNumber of steps of the path connecting the two samples.
pathSamples 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.
wThe 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
prThe set of parameters.
tpThe topology for the variables.
svComponents 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.
domainThe domain for the variables.
mThe 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.
nThe number of equations in the problem.
s1The first sample to connect.
s2The second sample to connect.
mdMaximum displacement between the two samples (along the components selected by 'sv').
checkCollisionsTRUE if the path has to be collision free.
sJThe 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.
lastSampleBuffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered.
nsNumber of steps of the path connecting the two samples.
pathSamples 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.
wThe 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
tpThe topology of the variables.
svThe components to use in the distance computation.
mThe size of each point.
npNumber of points in the path.
pointThe 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
pThe set of parameters.
mThe size of each point.
npNumber of points in the path.
pointThe set of points.
wThe 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().

◆ 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
pThe set of parameters.
fnameFile from where to read the sample.
nvsNumber of system variables in the problem = Number of values in the sample.
sWhere 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
pThe set of parameters.
fnameFile from where to read the sample.
nvsNumber of system variables in the problem = Number of values in the sample.
s1Where to store the first sample. The space is allocated internally.
s2Where 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
fnameThe file where to store the samples.
suffixSuffix to be added to the filename. It is used when generating smoothed paths.
nvsThe dimensionality of each sample.
nsThe number of samples defining the path.
pathThe 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
fnameThe file where to store the samples.
suffixSuffix to be added to the filename. It is used when generating smoothed paths.
nvsThe dimensionality of each sample.
nsThe number of samples defining the path.
pathThe collection of samples defining the path.
daThe dimension of the action space (of the action vectors).
actionActions executed at each time step.
timeDuration 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
fnameThe file where to store the samples.
smoothTRUE if the set of samples is the result of a smoothing process.
nThe number of this path. Used to define a different name for a sequence of paths to store.
nvsThe dimensionality of each sample.
nsThe number of samples defining the path.
pathThe 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
fnameName of the file containing the samples.
nvsNumber of entries for each sample.
nsNumber of samples (output).
pathThe 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
fnameName of the file containing the samples.
nvsNumber of entries for each sample.
nsNumber of samples (output).
pathThe set of samples (output).
daDimension of the action space (output).
actionThe action applied at each step (output).
timeThe 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
tmThe minimum time in between steps.
idThe identifiers of the steps that are keep in the sparsified trajectory.
nsNumber of samples.
pathThe set of samples.
actionThe action applied at each step.
timeThe 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
pThe set of parametres.
p3dThe 3d plot where to add the plot.
xIDFirst dimension for the projection (in the original system including system vars only).
yIDSecond dimension for the projection (in the original system including system vars only).
zIDThird dimension for the projection (in the original system including system vars only).
nsThe number of samples.
pathThe 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
pThe set of parametres.
p3dThe 3d plot where to add the plot.
xIDFirst dimension for the projection (in the original system including system vars only).
yIDSecond dimension for the projection (in the original system including system vars only).
zIDThird dimension for the projection (in the original system including system vars only).
wThe world from where to compute the force field.
nsThe number of solution points.
solsThe 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
nsThe number of samples.
pathThe 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
nsThe number of samples.
pathThe set of samples.
newChartTRUE if a chart is created at the corresponding step. Can be NULL.
timeTime lapse from the previous step. Can be NULL.
costCost 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
nsThe number of samples.
pathThe set of samples.
actionThe action applied at each step.
timeThe 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().