samples.c
Go to the documentation of this file.
803 //fprintf(stderr," New step-> dg:%g (%g) st:%g tn:%g tg:%g (%g)\n",nt,DistanceTopology(m,tp,s2,s),nt,Norm(k,t_next),Norm(k,t_goal),Distance(k,t_next,t_goal));
1892 if ((lastPath!=NULL)&&(c<lastS-1)&&(lastNS<lastS-c+1)&&(nl<ll)) /* replace only if theres is a minimum gap and the num
1975 /* Select the start/end of the tentative shortcuts (note that we can have repeated random numbers!) */
2004 newLocalLength[j]=DEFAULT_CONNECT(pr,tp,sv,&domain,m,ne,(*point)[n1],(*point)[n2],localLength[j],
2089 fprintf(stderr,"Test %u->%u. pl:%g st:%u -> nl:%g nst:%u %s\n",n1,n2,pl,n2-n1+2,nl,lns,(((reached)&&(nl<pl))?"(*)":" "));
2369 fprintf(stderr,"Iteration %u Path steps: %u Step size: %g Path cost: %g Time: %g Norm g: %.12f\n",
2803 unsigned int ReadTwoSamples(Tparameters *p,char *fname,unsigned int nvs,double **s1,double **s2)
unsigned int RegenerateWorldSolutionPoint(Tparameters *pr, double *p, double **v, Tworld *w) Computes the missing values in a kinematic solution. Definition: world.c:3468 void First(Titerator *i) Moves an iterator to the first position of its associated list. Definition: list.c:356 void PlotVect3d(unsigned int n, double *x, double *y, double *z, Tplot3d *p) Adds a polyline to the current object. Definition: plot3d.c:447 CBLAS_INLINE void ScaleVector(double f, unsigned int s, double *v) Scales a vector. Definition: basic_algebra.c:30 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. Definition: samples.c:1275 double DistanceTopologySubset(unsigned int s, unsigned int *tp, boolean *subset, double *v1, double *v2) Computes the distance of two points along a subset of components. Definition: basic_algebra.c:522 #define REP_JOINTS One of the possible values of the REPRESENTATION parameter. Definition: parameters.h:60 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. Definition: samples.c:2149 #define CS_WD_GENERATE_SIMPLIFIED_POINT(pr, p, r, wcs) Generates a simplified point from an original one. Definition: wcs.h:495 unsigned int ReadTwoSamples(Tparameters *p, char *fname, unsigned int nvs, double **s1, double **s2) Reads two samples from a file. Definition: samples.c:2803 double DistanceTopology(unsigned int s, unsigned int *tp, double *v1, double *v2) Computes the distance of two points. Definition: basic_algebra.c:498 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. Definition: samples.h:235 Data structure to hold the information about the name of a file. Definition: filename.h:271 void ArrayPi2Pi(unsigned int n, unsigned int *t, double *a) Applies PI2PI to an array. Definition: basic_algebra.c:646 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. Definition: samples.c:1927 unsigned int randomMax(unsigned int m) Returns a random integer in the range [0,m]. Definition: random.c:77 double(* TStepCost)(Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, unsigned int, Tchart *, Tchart *, Tchart *, Tworld *) Template of step costs. Definition: samples.h:183 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. Definition: samples.c:2960 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. Definition: samples.c:966 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. Definition: samples.c:1658 void InitStatistics(unsigned int np, double v, Tstatistics *t) Constructor. Definition: statistics.c:21 CBLAS_INLINE double Norm(unsigned int s, double *v) Computes the norm of a vector. Definition: basic_algebra.c:356 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. Definition: samples.c:694 void SaveSamples(char *fname, char *suffix, unsigned int nvs, unsigned int ns, double **path) Saves a set of samples to a file. Definition: samples.c:2879 CBLAS_INLINE void SumVector(unsigned int s, double *v1, double *v2, double *v) Adds two vectors. Definition: basic_algebra.c:67 void DifferenceVectorTopology(unsigned int s, unsigned int *tp, double *v1, double *v2, double *v) Substracts two vectors. Definition: basic_algebra.c:306 CBLAS_INLINE void AccumulateVector(unsigned int s, double *v1, double *v2) Adds a vector to another vectors. Definition: basic_algebra.c:55 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. Definition: samples.c:1693 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. Definition: samples.c:2727 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. Definition: samples.c:1096 Definition of the Tfilename type and the associated functions. int cmpUInt(const void *a, const void *b) Compares two unsigned in integers. Definition: samples.c:1803 unsigned int WorldManifoldDimension(Tparameters *p, double *point, Tworld *w) Dimensionality of the solution manifold.+. Definition: world.c:3404 double Chart2Manifold(Tparameters *pr, TJacobian *sJ, double *t, unsigned int *tp, double *pInit, double *p, Tchart *c) Returns the point in the manifold for a given set of parameteres. Definition: chart.c:1069 void DifferenceVectorTopologySubset(unsigned int s, unsigned int *tp, boolean *subset, double *v1, double *v2, double *v) Substracts two vectors along some of its components. Definition: basic_algebra.c:325 void PlotLine(double x0, double y0, double x1, double y1, Tplot *p) Plots a segment. Definition: plot.c:215 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. Definition: samples.c:1611 void GetJacobianSize(unsigned int *nr, unsigned int *nc, TJacobian *j) Returns the size of the Jacobian. Definition: jacobian.c:49 boolean ReadListOfBoxes(char *filename, Tlist *l) Reads a list of boxes from a file. Definition: box_list.c:286 double PathEffort(Tparameters *p, unsigned int m, unsigned int np, double **point, Tworld *w) Approximated control effort of a path. Definition: samples.c:1708 double Distance(unsigned int s, double *v1, double *v2) Computes the distance of two points. Definition: basic_algebra.c:438 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. Definition: samples.c:1112 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. Definition: samples.c:3056 unsigned int InitChart(Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double e, double eCurv, double r, TJacobian *sJ, TAtlasBase *w, Tchart *c) Constructor. Definition: chart.c:795 CBLAS_INLINE double GeneralDotProduct(unsigned int s, double *v1, double *v2) Computes the dot product of two general vectors. Definition: basic_algebra.c:15 CBLAS_INLINE void MatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o) Product of a matrix and a vector. Definition: basic_algebra.c:749 boolean EndOfList(Titerator *i) Checks if an iterator is pointing at the end of the list. Definition: list.c:445 #define CS_WD_SIMP_INEQUALITIES_HOLD(pr, p, wcs) Cheks if all inequalities hold. Definition: wcs.h:207 void DifferenceVector(unsigned int s, double *v1, double *v2, double *v) Substracts two vectors. Definition: basic_algebra.c:264 Definition of the Tlist type and the associated functions. #define CS_WD_NEWTON_IN_SIMP(pr, p, wcs) Applies a Newton procedure to move a point towads the manifold. Definition: wcs.h:580 #define CS_WD_MANIFOLD_DIMENSION(pr, p, wcs) Computes the dimension of the solution set. Definition: wcs.h:565 void WorldForceFieldProjectedGradient(Tparameters *p, boolean simp, double *proj, double *sol, double **g, void *w) The projected gradient of the force field. Definition: world.c:2776 unsigned int GetWorldSimpTopology(Tparameters *p, unsigned int **t, Tworld *w) Gets the topology of the variables. Definition: world.c:3153 A table of parameters. void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn) Constructor. Definition: filename.c:22 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. Definition: samples.c:1135 #define CS_WD_GET_SIMP_JACOBIAN(pr, J, wcs) Computes the Jacobian of the simplified system. Definition: wcs.h:553 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. Definition: samples.c:1558 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. Definition: samples.c:1203 void ConcatSamples(unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path) Concats two path. Definition: samples.c:2706 void WorldForceField(Tparameters *p, boolean simp, double *sol, double **g, void *w) Evaluates the gradient of the potential energy of a configuration. Definition: world.c:2722 char * GetFileFullName(Tfilename *fn) Gets the file full name (paht+name+extension). Definition: filename.c:151 CBLAS_INLINE void TMatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o) Product of a transposed matrix and a vector. Definition: basic_algebra.c:765 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. Definition: samples.c:2541 unsigned int InitTrustedChart(Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double e, double eCurv, double r, TJacobian *sJ, TAtlasBase *w, Tchart *c) Constructor. Definition: chart.c:819 #define CS_WD_REGENERATE_ORIGINAL_POINT(pr, p, o, wcs) Completes an original point from a simplified one. Definition: wcs.h:293 #define JOINTS_EXT File extension for files with samples represented by the joint values. Definition: filename.h:188 #define MEM_DUP(_var, _n, _type) Duplicates a previously allocated memory space. Definition: defines.h:414 #define NO_UINT Used to denote an identifier that has not been initialized. Definition: defines.h:435 CBLAS_INLINE void SumVectorScale(unsigned int s, double *v1, double w, double *v2, double *v) Adds two vectors with a scale. Definition: basic_algebra.c:86 void InitBoxFromPoint(unsigned int dim, double *p, Tbox *b) Initializes a box from a point. Definition: box.c:43 void ShortcutSmooth(Tparameters *pr, boolean *sv, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w) Fixed-cut based smoothing. Definition: samples.c:1819 void DeleteSamples(unsigned int ns, double **path) Deletes the space used by a set of samples. Definition: samples.c:3159 unsigned int ReadOneSample(Tparameters *p, char *fname, unsigned int nvs, double **s) Reads one sample from a file. Definition: samples.c:2772 unsigned int GetWorldSystemVars(boolean **sv, Tworld *w) Gets the system vars of the kinematic cuiksystem. Definition: world.c:2383 void SaveSamplesInt(Tfilename *fpath, unsigned int nvs, unsigned int ns, double **path) Internal function to save a set of samples to a file. Definition: samples.c:2845 double Manifold2Chart(double *p, unsigned int *tp, double *t, Tchart *c) Returns the parametrization of a point. Definition: chart.c:1039 double GetParameter(unsigned int n, Tparameters *p) Gets the value for a particular parameter. Definition: parameters.c:93 #define CS_WD_REGENERATE_SOLUTION_POINT(pr, p, r, wcs) Compleates a solution point with the dummy variables. Definition: wcs.h:480 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. Definition: samples.c:1668 #define CS_WD_IN_COLLISION(f, pr, s, sPrev, wcs) Checks if a configuration is in collision. Definition: wcs.h:355 void GetWorldSimpJacobian(Tparameters *p, TJacobian *J, Tworld *w) Gets the simplified kinematic Jacobian. Definition: world.c:3145 unsigned int WorldGenerateSimplifiedPoint(Tparameters *p, double *o, double **s, Tworld *w) Reconstruct a point in simplified kinematic system from an original point. Definition: world.c:3362 #define CT_DELTA Size of the steps in the path connecting two configurations. Definition: parameters.h:282 #define CT_N_DOF Dimensionality of the solution space for the mechanism at hand. Definition: parameters.h:318 void GetBoxCenter(boolean *used, double *c, Tbox *b) Returns the box center along the selected dimensions. Definition: box.c:721 Auxiliary functions to deal with sets of samples. #define CS_WD_FROM_WORLD(ptr, wcs) Initializes the equations from a world structure. Definition: wcs.h:108 void InitSamples(unsigned int *ms, unsigned int *ns, double ***path) Initializes a set of samples. Definition: samples.c:665 Definition of basic randomization functions. 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. Definition: samples.c:1344 #define CS_WD_GENERATE_SIMP_INITIAL_BOX(pr, b, wcs) Computes the global box for the simplified system. Definition: wcs.h:539 unsigned int WorldSimpKinematicVars(Tparameters *p, boolean **kv, Tworld *w) Creates a boolean array to identify kinematic variables. Definition: world.c:2616 Definition of the Tparameters type and the associated functions. void ChangeParameter(unsigned int n, double v, Tparameters *p) Sets the value for a particular parameter. Definition: parameters.c:164 unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p) Start a composed object. Definition: plot3d.c:157 void GetWorldSimpInitialBox(Tparameters *p, Tbox *b, Tworld *w) Gets the kinematic simplified search space for a given problem. Definition: world.c:3396 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. Definition: samples.c:1622 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. Definition: samples.c:672 #define LINKS_EXT File extension for files with samples represented by the link poses. Definition: filename.h:181 boolean Advance(Titerator *i) Moves an iterator to the next position of its associated list. Definition: list.c:373 #define CS_WD_GET_SIMP_TOPOLOGY(pr, tp, wcs) Gets the simplified variable topology. Definition: wcs.h:450 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. Definition: samples.c:1224 boolean LoadSamples(Tfilename *fname, unsigned int *nvs, unsigned int *ns, double ***path) Reads a set of samples from file. Definition: samples.c:2913 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. Definition: samples.c:2894 boolean PointInBoxTopology(boolean *used, boolean update, unsigned int n, double *v, double tol, unsigned int *tp, Tbox *b) Checks if a point is included in a(sub-) box. Definition: box.c:374 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. Definition: samples.c:1683 void ReverseSamples(unsigned int ns, double **path) Reverses a set of samples. Definition: samples.c:2752 |
Follow us!