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.
|
void | InitSamples (unsigned int *ms, unsigned int *ns, double ***path) |
| Initializes a set of samples. 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 | 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...
|
|
double | ConnectSamplesChart (Tparameters *pr, unsigned int *tp, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, 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, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w) |
| Determines the connection between two points on the manifold. 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, boolean smooth, unsigned int nvs, unsigned int ns, double **path) |
| Saves a set of samples to a file. 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...
|
|
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 | DeleteSamples (unsigned int ns, double **path) |
| Deletes the space used by a set of samples. More...
|
|
#define INIT_NUM_SAMPLES 100 |
Initial number of samples (or steps) in a path. This will be enlarged if necessary.
Definition at line 28 of file samples.h.
Referenced by InitSamples().
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 37 of file samples.h.
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 47 of file samples.h.
Referenced by main(), and SmoothSamples().
#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 57 of file samples.h.
Referenced by main(), and SmoothSamples().
#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 67 of file samples.h.
Referenced by main(), and SmoothSamples().
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 79 of file samples.h.
Referenced by RandomSmooth().
void InitSamples |
( |
unsigned int * |
ms, |
|
|
unsigned int * |
ns, |
|
|
double *** |
path |
|
) |
| |
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 1033 of file samples.c.
References AddSample2Samples(), CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CS_WD_REGENERATE_SOLUTION_POINT, CT_DELTA, DeleteStatistics(), Error(), GetElapsedTime(), GetParameter(), GradientSmooth(), InitStatistics(), NEW, PathLength(), RandomSmooth(), ShortcutSmooth(), SMOOTH_GRADIENT, SMOOTH_RANDOM, and SMOOTH_SHORTCUT.
Referenced by main().
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 1150 of file samples.c.
References NEW.
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 1171 of file samples.c.
References NEW.
Referenced by PathStart2GoalInRRT().
void ReverseSamples |
( |
unsigned int |
ns, |
|
|
double ** |
path |
|
) |
| |
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 150 of file samples.c.
References MEM_DUP, and NEW.
Referenced by AddBranchToAtlasRRT(), ConnectSamples(), ConnectSamplesChart(), MinimizeOnAtlas(), PathInChart(), ReconstructAtlasPath(), ReconstructAtlasRRTPath(), SmoothSamples(), Steps2PathinAtlasRRT(), and Steps2PathinRRT().
double ConnectSamplesChart |
( |
Tparameters * |
pr, |
|
|
unsigned int * |
tp, |
|
|
Tbox * |
domain, |
|
|
unsigned int |
m, |
|
|
unsigned int |
n, |
|
|
double * |
s1, |
|
|
double * |
s2, |
|
|
double |
md, |
|
|
boolean |
checkCollisions, |
|
|
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.
This can be seen as a simply 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. |
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. |
checkCollisions | TRUE if the path has to be collision free. |
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 172 of file samples.c.
References AccumulateVector(), AddSample2Samples(), ArrayPi2Pi(), Chart2Manifold(), CS_WD_GET_SIMP_JACOBIAN, CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_CE, CT_DELTA, CT_E, CT_EPSILON, CT_N_DOF, CT_R, DeleteChart(), DeleteJacobian(), DeleteSamples(), DifferenceVector(), DifferenceVectorTopology(), DistanceTopology(), Error(), FALSE, GetJacobianSize(), GetParameter(), InitChart(), InitSamples(), Manifold2Chart(), NEW, Norm(), PointInBoxTopology(), ScaleVector(), SumVectorScale(), and TRUE.
Referenced by ShortcutSmooth(), and WireAtlasRRTstar().
double ConnectSamples |
( |
Tparameters * |
pr, |
|
|
unsigned int * |
tp, |
|
|
Tbox * |
domain, |
|
|
unsigned int |
m, |
|
|
unsigned int |
n, |
|
|
double * |
s1, |
|
|
double * |
s2, |
|
|
double |
md, |
|
|
boolean |
checkCollisions, |
|
|
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. |
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. |
checkCollisions | TRUE if the path has to be collision free. |
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 434 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(), DIVERGED, FALSE, GetParameter(), InitSamples(), NEW, Norm(), PointInBoxTopology(), SumVectorScale(), and TRUE.
Referenced by AddStepToRRTstar(), BiRRTstar(), ReWireRRTstar(), Steps2PathinRRT(), and WireRRTstar().
unsigned int ReadOneSample |
( |
Tparameters * |
p, |
|
|
char * |
fname, |
|
|
unsigned int |
nvs, |
|
|
double ** |
s |
|
) |
| |
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 1247 of file samples.c.
References CreateFileName(), CT_REPRESENTATION, DeleteFileName(), Error(), GetFileFullName(), GetParameter(), JOINTS_EXT, LINKS_EXT, NEW, and REP_JOINTS.
Referenced by main().
void SaveSamples |
( |
char * |
fname, |
|
|
boolean |
smooth, |
|
|
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. |
smooth | TRUE if the set of samples is the result of a smoothing process. |
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 1318 of file samples.c.
References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.
Referenced by main(), and MinimizeOnAtlas().
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 1333 of file samples.c.
References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.
Referenced by MinimizeOnAtlas().
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 1352 of file samples.c.
References Advance(), DeleteListOfBoxes(), EndOfList(), Error(), FALSE, First(), GetBoxCenter(), GetBoxNIntervals(), GetCurrent(), GetFileFullName(), InitIterator(), ListSize(), NEW, ReadListOfBoxes(), and TRUE.
Referenced by main().
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 red 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 and dummies). |
yID | Second dimension for the projection (in the original system including system vars and dummies). |
zID | Third dimension for the projection (in the original system including system vars and dummies). |
ns | The number of samples. |
path | The set of samples. |
Definition at line 1399 of file samples.c.
References Close3dObject(), CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeleteColor(), GetParameter(), M_2PI, NEW, NewColor(), PlotVect3d(), and StartNew3dObject().
Referenced by main().
void DeleteSamples |
( |
unsigned int |
ns, |
|
|
double ** |
path |
|
) |
| |
|
Follow us!