cuikatlasrrtstar.c
Go to the documentation of this file.
1 
2 #include "world.h"
3 #include "parameters.h"
4 
5 #include "defines.h"
6 #include "error.h"
7 #include "filename.h"
8 #include "atlasrrt.h"
9 #include "random.h"
10 #include "geom.h"
11 #include "samples.h"
12 #include "averages.h"
13 
14 #include <stdlib.h>
15 #include <string.h>
16 #include <time.h>
17 #include <math.h>
18 
84 int main(int argc, char **arg)
85 {
86  TAtlasBase world; /* The set of mechanism and obstacles. */
87  Tparameters parameters; /* Parameters used in the Cuik process. */
88 
89  Tfilename fparam;
90 
91  double *s1,*s2; /* Origin/goal of the AtlasRRT. */
92 
93  unsigned int nvs;
94 
95  Tatlasrrt atlasrrt;
96 
97  boolean connected;
98  double planningTime;
99  double pl;
100  unsigned int ns;
101  double **path;
102  boolean birrt;
103  boolean rrtgraph;
104 
105  unsigned int it,nRepetitions;
106  Taverages averages;
107 
108  unsigned int ri;
109  time_t t;
110 
111  /* Statistics about the time take up to each iteration and
112  the path lengh at that point */
113  unsigned int maxIt,execIt;
114  double *times,*costs;
115 
116  TAtlasRRTStatistics *arst;
117 
118  if (argc>1)
119  {
120  if (argc>2)
121  {
122  nRepetitions=atoi(arg[2]);
123  if (nRepetitions==0)
124  Error("Second parameter for cuikatlasrrtstar is wrong");
125  }
126  else
127  nRepetitions=1;
128 
129  if ((nRepetitions>1)&&(RRT_VERBOSE))
130  Warning("To get accurate execution time statistics, set GET_ATLASRRT_STATISTICS to 0");
131 
132  if ((GET_ATLASRRT_STATISTICS)&&(nRepetitions>1))
133  {
134  NEW(arst,1,TAtlasRRTStatistics);
136  }
137  else
138  arst=NULL;
139 
140  /*Init parameters*/
141  CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);
142  fprintf(stderr,"Reading parameters from : %s\n",GetFileFullName(&fparam));
143  InitParametersFromFile(GetFileFullName(&fparam),&parameters);
144 
145  birrt=(GetParameter(CT_BI_RRT,&parameters)>0.5);
146  rrtgraph=(GetParameter(CT_RRT_GRAPH,&parameters)>0.5);
147 
148  /*Read the world/cuik from file*/
149  CS_WD_INIT(&parameters,arg[1],&world);
150 
151  /* Read samples */
152  nvs=ReadTwoSamples(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1,&s2);
153 
154  /* Random seed initialization */
155  t=time(NULL); /* Get the time at which input files have been read */
156  ri=(unsigned int)t;
157  //ri=1341338413; // with gamma=0 the c8 give a long path
158  //ri=1341346253; // seed used to generate the growing RRTs for c8
159  ri=1416402361;
160  randomSet(ri);
161  fprintf(stderr,"Random seed : %u\n",ri);
162 
163  /* Maximum of iteraitons (set to NO_UINT to limit execution only in time) */
164  maxIt=(unsigned int)GetParameter(CT_MAX_PLANNING_ITERATIONS,&parameters);
165 
166  /* Start the process to connect the two samples */
167  InitAverages(nRepetitions,TRUE,TRUE,maxIt,&averages);
168 
169  /* Buffer to store statistics about the path length */
170  if (maxIt!=NO_UINT)
171  {
172  NEW(times,maxIt,double);
173  NEW(costs,maxIt,double);
174  }
175 
176  for(it=0;it<nRepetitions;it++)
177  {
178  /* Init an AtlasRRT (bi-directional or with graph structure depending on parameters) */
179  InitAtlasRRT(&parameters,TRUE/*parallel*/,s1,(birrt?TWO_TREES_WITH_SWAP:ONE_TREE),rrtgraph,s2,&world,&atlasrrt);
180  fprintf(stderr,"************************************************\n");
181 
182  /* Try to connect the goal with a tree from the start with
183  an optimal path */
184  connected=AtlasRRTstar(&parameters,s2,
185  &execIt,
186  times,costs,
187  &planningTime,&pl,&ns,&path,arst,&atlasrrt);
188 
189  /* Save the results (only if one shot execution) */
190  if (nRepetitions==1)
191  {
192  if (connected)
193  SaveSamples(arg[1],FALSE,nvs,ns,path);
194 
195  SaveAtlasRRT(&parameters,arg[1],&atlasrrt);
196  }
197 
198  /* Summarize and release allocated objects for this repetition*/
199  if (connected)
200  {
201  NewSuccesfulExperiment(planningTime,AtlasRRTMemSize(&atlasrrt),pl,0,
202  (double)GetAtlasRRTNumCharts(&atlasrrt),
203  (double)GetAtlasRRTNumNodes(&atlasrrt),
204  (execIt==maxIt?times:NULL),
205  (execIt==maxIt?costs:NULL),
206  &averages);
207 
208  DeleteSamples(ns,path);
209  }
210  else
211  fprintf(stderr," Execution failed\n");
212 
213  DeleteAtlasRRT(&atlasrrt);
214 
215  fprintf(stderr,"Execution compleated %u/%u\n",it+1,nRepetitions);
216  }
217 
218  /* Print data about the path length at each iteration. 0 means no path
219  to goal found yet */
220 
221  /* Print statistics about the execution (only if many iterations) */
222 
223  if (nRepetitions>1)
224  {
225  PrintAveragesHeader(stderr,argc,arg,&averages);
226 
227  fprintf(stderr,"%% **************************************************\n");
228  fprintf(stderr,"Random seed : %u\n",ri);
229  fprintf(stderr,"Update_Costs: %u Symmetric_Cost: %u Heuristic: %u Exploration: %u Adjust_SA: %u (%.2f--%.2f) GlobalNN: %u GlobalCurv: %u Tree_ATLAS: %u\n",
231  PrintParameters(stderr,&parameters);
232 
233  #if (GET_ATLASRRT_STATISTICS)
234  PrintAtlasRRTStatistics(NULL,arst);
236  free(arst);
237  #endif
238 
239  PrintAverages(stderr,&averages);
240 
241  fprintf(stderr,"%% **************************************************\n");
242  }
243 
244  /* Release memory */
245  if (maxIt!=NO_UINT)
246  {
247  free(times);
248  free(costs);
249  }
250 
251  DeleteAverages(&averages);
252 
253  free(s1);
254  free(s2);
255 
256  DeleteParameters(&parameters);
257 
258  CS_WD_DELETE(&world);
259 
260  DeleteFileName(&fparam);
261  }
262  else
263  {
264  fprintf(stderr," Wrong number of parameters.\n");
265  fprintf(stderr," Use:\n");
266  fprintf(stderr," cuikatlasrrtstar <problem filename>.%s [num Repetitions]\n",CS_WD_EXT);
267  fprintf(stderr," where <problem filename> the equations/world description\n");
268  fprintf(stderr," <num Repetitions> experiment repetitions to gather statistics\n");
269  fprintf(stderr," This is optional.\n");
270  fprintf(stderr," (the '.%s' extension is not required)\n",CS_WD_EXT);
271  }
272  return(EXIT_SUCCESS);
273 }
274 
Definition of the combination of an atlas with a RRT.
Definition of basic functions.
#define FALSE
FALSE.
Definition: boolean.h:30
boolean AtlasRRTstar(Tparameters *pr, double *pg, unsigned int *it, double *times, double *costs, double *planningTime, double *pl, unsigned int *ns, double ***path, TAtlasRRTStatistics *str, Tatlasrrt *ar)
Optimal AtlasRRT on manifolds.
Definition: atlasrrt.c:5285
void PrintParameters(FILE *f, Tparameters *p)
Prints a parameter set.
Definition: parameters.c:181
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
Data structure to hold the information about the name of a file.
Definition: filename.h:271
void NewSuccesfulExperiment(double t, unsigned int mem, double pl, double pc, unsigned int nc, unsigned int ns, double *time, double *cost, Taverages *av)
Adds data of a new experiment.
Definition: averages.c:75
#define RRT_VERBOSE
Vebosity of the RRT operations.
Definition: rrt.h:32
#define USE_ATLAS_TREE
Whether to use a binary tree to search for neighbouring charts.
Definition: atlas.h:78
#define CT_RRT_GRAPH
Graph RRT.
Definition: parameters.h:518
Definition of the Tfilename type and the associated functions.
#define ONE_TREE
One of the modes for the RRT.
Definition: rrt.h:132
void DeleteAtlasRRT(Tatlasrrt *ar)
Destructor.
Definition: atlasrrt.c:6206
#define TRUE
TRUE.
Definition: boolean.h:21
void InitAtlasRRTStatistics(TAtlasRRTStatistics *arst)
Init the Atlas RRT statistics.
Definition: atlasrrt.c:553
void Error(const char *s)
General error function.
Definition: error.c:80
#define PARAM_EXT
File extension for parameter files.
Definition: filename.h:132
#define CS_WD_DELETE(wcs)
Destructor of the equation structure.
Definition: wcs.h:592
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
Definition of the Tworld type and the associated functions.
#define CT_MAX_PLANNING_ITERATIONS
Maximum iterations for path planning.
Definition: parameters.h:483
#define CT_BI_RRT
Bi-directional RRT.
Definition: parameters.h:508
Error and warning functions.
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
#define RRTSTAR_UPDATE_COSTS
On the fly update of the cost in the RRTstar.
Definition: rrt.h:66
void PrintAverages(FILE *f, Taverages *av)
Prints the averages of a set of experiments.
Definition: averages.c:170
void PrintAveragesHeader(FILE *f, int argc, char **arg, Taverages *av)
Prints a header to the averages results.
Definition: averages.c:149
unsigned int AtlasRRTMemSize(Tatlasrrt *ar)
Memory used by a given atlasRRT.
Definition: atlasrrt.c:6066
unsigned int ReadTwoSamples(Tparameters *p, char *fname, unsigned int nvs, double **s1, double **s2)
Reads two samples from a file.
Definition: samples.c:2803
Definitions of constants and macros used in several parts of the cuik library.
#define ATLASRRT_GLOBAL_NN
Set to 1 if nearest neighbours are searched without using the chart relations.
Definition: atlasrrt.h:83
#define HEURISTIC_RRT_STAR
Whether to use an heuristic in AtlarRRT*.
Definition: rrt.h:54
int main(int argc, char **arg)
Main body of the cuikatlasrrtstar application.
Auxiliary functions to deal averages of path planner executions.
#define MOV_AVG_UP
Weight of new data when computing moving averages.
Definition: defines.h:451
void Warning(const char *s)
General warning function.
Definition: error.c:116
A table of parameters.
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn)
Constructor.
Definition: filename.c:22
void DeleteAtlasRRTStatistics(TAtlasRRTStatistics *arst)
Destructor.
Definition: atlasrrt.c:973
void InitParametersFromFile(char *file, Tparameters *p)
Constructor from a file.
Definition: parameters.c:51
Type defining the equations on which the atlas is defined.
Definition: wcs.h:30
char * GetFileFullName(Tfilename *fn)
Gets the file full name (paht+name+extension).
Definition: filename.c:151
#define EXPLORATION_RRT
Select between exploration and goal driven RRT.
Definition: rrt.h:98
unsigned int GetAtlasRRTNumCharts(Tatlasrrt *ar)
Number of charts in the AtlasRRT.
Definition: atlasrrt.c:5794
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
#define CS_WD_EXT
Possible extensions for the equation files.
Definition: wcs.h:48
void DeleteParameters(Tparameters *p)
Destructor.
Definition: parameters.c:294
Statistics on the AtlasRRT constrution.
Definition: atlasrrt.h:109
#define CS_WD_INIT(pr, name, wcs)
Initializes the equations from a file.
Definition: wcs.h:89
Structure to store expeeriment results.
Definition: averages.h:32
RRT with an atlas for sampling.
Definition: atlasrrt.h:203
#define RRTSTAR_SYMMETRIC_COST
TRUE (or 1) if cost(a,b)=cost(b,a)
Definition: rrt.h:74
#define TWO_TREES_WITH_SWAP
One of the modes for the RRT.
Definition: rrt.h:154
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
#define ADJUST_SA
Set this to one to adjust the sampling area.
Definition: atlasrrt.h:70
#define CS_WD_GET_NUM_SYSTEM_VARS(wcs)
Gets the number of system variables.
Definition: wcs.h:264
Auxiliary functions to deal with sets of samples.
#define GET_ATLASRRT_STATISTICS
Set this to one to gather statistics of AtlasRRT construction.
Definition: atlasrrt.h:49
void InitAtlasRRT(Tparameters *pr, boolean parallel, double *ps, unsigned int mode, boolean graph, double *pg, TAtlasBase *w, Tatlasrrt *ar)
Defines a Atlas-RRT from a given point.
Definition: atlasrrt.c:4114
Definition of basic randomization functions.
#define GET_ATLASRRT_GLOBAL_CURV_CHECK
Set this to one to check the global curvature tolerences between charts.
Definition: atlasrrt.h:91
void DeleteAverages(Taverages *av)
Deletes the space used by a set of averages.
Definition: averages.c:263
void DeleteSamples(unsigned int ns, double **path)
Deletes the space used by a set of samples.
Definition: samples.c:3159
Definition of the Tparameters type and the associated functions.
unsigned int GetAtlasRRTNumNodes(Tatlasrrt *ar)
Number of nodes in the AtlasRRT.
Definition: atlasrrt.c:5789
void InitAverages(unsigned int m, boolean useCharts, boolean useSamples, unsigned int maxIt, Taverages *av)
Initializes a set of averages.
Definition: averages.c:21
void PrintAtlasRRTStatistics(Tatlasrrt *ar, TAtlasRRTStatistics *arst)
Prints the summary of atlasRRT statistics.
Definition: atlasrrt.c:766
void SaveAtlasRRT(Tparameters *pr, char *prefix, Tatlasrrt *ar)
Stores the Atlas-RRT information on a file.
Definition: atlasrrt.c:6076
void randomSet(unsigned int seed)
Sets the random seed.
Definition: random.c:25
#define MOV_AVG_DOWN
Weight of new data when computing moving averages.
Definition: defines.h:467