cuikatlasrrt.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 "chart.h"
12 #include "samples.h"
13 #include "averages.h"
14 
15 #include <stdlib.h>
16 #include <string.h>
17 #include <time.h>
18 
79 int main(int argc, char **arg)
80 {
81 
82  TAtlasBase world; /* The set of mechanism and obstacles. */
83  Tparameters parameters; /* Parameters used in the Cuik process. */
84 
85  Tfilename fparam;
86 
87  double *s1,*s2; /* Origin/goal of the RRT. */
88 
89  unsigned int nvs;
90 
91  Tatlasrrt atlasrrt;
92 
93  boolean connected;
94  double pl;
95  unsigned int ns;
96  double **path;
97  boolean birrt;
98 
99  double planningTime;
100 
101  unsigned int it,nRepetitions;
102  Taverages averages;
103 
104  unsigned int ri; /* random seed*/
105  time_t t; /* Used to timestamp the results */
106 
107  unsigned int db;
108 
109  TAtlasRRTStatistics *arst;
110 
111  if (argc>1)
112  {
113  if (argc>2)
114  {
115  nRepetitions=atoi(arg[2]);
116  if (nRepetitions==0)
117  Error("Second parameter for cuikatlasrrt is wrong");
118  }
119  else
120  nRepetitions=1;
121 
122  if ((nRepetitions>1)&&(GET_ATLASRRT_STATISTICS))
123  Warning("To get accurate execution time statistics, set GET_ATLASRRT_STATISTICS to 0");
124 
125  if ((nRepetitions>1)&&(ATLASRRT_VERBOSE))
126  Warning("To get accurate execution time statistics, set ATLASRRT_VERBOSE to 0");
127 
128  if ((GET_ATLASRRT_STATISTICS)&&(nRepetitions>1))
129  {
130  NEW(arst,1,TAtlasRRTStatistics);
132  }
133  else
134  arst=NULL;
135 
136  /*Init parameters*/
137  CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);
138  fprintf(stderr,"Reading parameters from : %s\n",GetFileFullName(&fparam));
139  InitParametersFromFile(GetFileFullName(&fparam),&parameters);
140 
141  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,&parameters);
142  if (db>0)
143  Error("cuikatlasrrt does not deal with bifurcations (yet)");
144 
145  if (EXPLORATION_RRT)
146  birrt=FALSE;
147  else
148  birrt=(GetParameter(CT_BI_RRT,&parameters)>0.5);
149 
150  /*Read the world/cuik from file*/
151  CS_WD_INIT(&parameters,arg[1],&world);
152 
153  /* Read samples */
154  if (EXPLORATION_RRT)
155  {
156  nvs=ReadOneSample(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1);
157  s2=NULL;
158  }
159  else
160  nvs=ReadTwoSamples(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1,&s2);
161 
162  /* Random seed initialization */
163  t=time(NULL); /* Get the time at which input files have been read */
164  ri=(unsigned int)t;
165  //ri=1424256627;
166  randomSet(ri);
167  fprintf(stderr,"Random seed : %u\n",ri);
168 
169  /* Start the process to connect the two samples */
170  InitAverages(nRepetitions,TRUE,TRUE,NO_UINT,&averages);
171 
172  for(it=0;it<nRepetitions;it++)
173  {
174  /* Initialize the atlas */
175  InitAtlasRRT(&parameters,FALSE/*parallel*/,s1,birrt,FALSE,s2,&world,&atlasrrt);
176  fprintf(stderr,"************************************************\n");
177 
178  /* Define the path using the atlas */
179  connected=AtlasRRT(&parameters,s2,&planningTime,&pl,&ns,&path,arst,&atlasrrt);
180 
181  /* Save the results (only if one shot execution) */
182  if (nRepetitions==1)
183  {
184  if (connected)
185  SaveSamples(arg[1],FALSE,nvs,ns,path);
186 
187  SaveAtlasRRT(&parameters,arg[1],&atlasrrt);
188  }
189 
190  /* Summarize and release allocated objects for this repetition*/
191 
192  if (EXPLORATION_RRT)
193  fprintf(stderr," Exploration concluded in %f sec\n",planningTime);
194  else
195  {
196  if (connected)
197  {
198  NewSuccesfulExperiment(planningTime,AtlasRRTMemSize(&atlasrrt),pl,0,
199  (double)GetAtlasRRTNumCharts(&atlasrrt),
200  (double)GetAtlasRRTNumNodes(&atlasrrt),
201  NULL,NULL,
202  &averages);
203  DeleteSamples(ns,path);
204  }
205  else
206  fprintf(stderr," Execution failed (%f sec)\n",planningTime);
207  }
208 
209  DeleteAtlasRRT(&atlasrrt);
210 
211  fprintf(stderr,"Execution compleated %u/%u\n",it+1,nRepetitions);
212  }
213 
214  /* Print statistics about the execution (only if many iterations) */
215  if (nRepetitions>1)
216  {
217  PrintAveragesHeader(stderr,argc,arg,&averages);
218 
219  fprintf(stderr,"%% **************************************************\n");
220  fprintf(stderr,"Random seed : %u\n",ri);
221  fprintf(stderr,"Exploration: %u Adjust_SA: %u (%.2f--%.2f) GlobalNN: %u GlobalCurv: %u Tree_ATLAS: %u\n",
223  PrintParameters(stderr,&parameters);
224 
225  #if (GET_ATLASRRT_STATISTICS)
226  PrintAtlasRRTStatistics(NULL,arst);
228  free(arst);
229  #endif
230 
231  PrintAverages(stderr,&averages);
232 
233  fprintf(stderr,"%% **************************************************\n");
234  }
235 
236  /* Release memory */
237  DeleteAverages(&averages);
238 
239  /* Release memory */
240  free(s1);
241  free(s2);
242 
243  DeleteParameters(&parameters);
244 
245  CS_WD_DELETE(&world);
246 
247  DeleteFileName(&fparam);
248  }
249  else
250  {
251  fprintf(stderr," Wrong number of parameters.\n");
252  fprintf(stderr," Use:\n");
253  fprintf(stderr," cuikatlasrrt <problem filename>.%s [num Repetitions]\n",CS_WD_EXT);
254  fprintf(stderr," where <problem filename> the equations/world description\n");
255  fprintf(stderr," <num Repetitions> experiment repetitions to gather statistics\n");
256  fprintf(stderr," This is optional.\n");
257  fprintf(stderr," (the '.%s' extension is not required)\n",CS_WD_EXT);
258  }
259  return(EXIT_SUCCESS);
260 }
261 
Definition of the combination of an atlas with a RRT.
Definition of basic functions.
#define FALSE
FALSE.
Definition: boolean.h:30
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 USE_ATLAS_TREE
Whether to use a binary tree to search for neighbouring charts.
Definition: atlas.h:78
Definition of the Tfilename type and the associated functions.
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_BI_RRT
Bi-directional RRT.
Definition: parameters.h:508
Error and warning functions.
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
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
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
boolean AtlasRRT(Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, TAtlasRRTStatistics *str, Tatlasrrt *ar)
Extends a Atlas-RRT until we reach a targed point.
Definition: atlasrrt.c:4392
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
Definition of a local chart on a manifold.
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
#define ATLASRRT_VERBOSE
Vebosity of the AtlasRRT operations.
Definition: atlasrrt.h:41
RRT with an atlas for sampling.
Definition: atlasrrt.h:203
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
int main(int argc, char **arg)
Main body of the cuikatlasrrt application.
Definition: cuikatlasrrt.c:79
Auxiliary functions to deal with sets of samples.
#define CT_DETECT_BIFURCATIONS
TRUE (or 1) if bifurcation must be detected.
Definition: parameters.h:468
#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
unsigned int ReadOneSample(Tparameters *p, char *fname, unsigned int nvs, double **s)
Reads one sample from a file.
Definition: samples.c:2772
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