cuikatlastrrt.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, pc;
95  unsigned int ns;
96  double **path;
97 
98  double planningTime;
99 
100  unsigned int it,nRepetitions;
101  Taverages averages;
102 
103  unsigned int ri; /* random seed*/
104  time_t t; /* Used to timestamp the results */
105 
106  unsigned int db;
107 
108  TAtlasRRTStatistics *arst;
109 
110  if (argc>1)
111  {
112  if (argc>2)
113  {
114  nRepetitions=atoi(arg[2]);
115  if (nRepetitions==0)
116  Error("Second parameter for cuikatlastrrt is wrong");
117  }
118  else
119  nRepetitions=1;
120 
121  if ((nRepetitions>1)&&(GET_ATLASRRT_STATISTICS))
122  Warning("To get accurate execution time statistics, set GET_ATLASRRT_STATISTICS to 0");
123 
124  if ((nRepetitions>1)&&(ATLASRRT_VERBOSE))
125  Warning("To get accurate execution time statistics, set GET_ATLASRRT_STATISTICS to 0");
126 
127  if ((GET_ATLASRRT_STATISTICS)&&(nRepetitions>1))
128  {
129  NEW(arst,1,TAtlasRRTStatistics);
131  }
132  else
133  arst=NULL;
134 
135  /*Init parameters*/
136  CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);
137  fprintf(stderr,"Reading parameters from : %s\n",GetFileFullName(&fparam));
138  InitParametersFromFile(GetFileFullName(&fparam),&parameters);
139 
140  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,&parameters);
141  if (db>0)
142  Error("cuikatlastrrt does not deal with bifurcations (yet)");
143 
144  /*Read the world/cuik from file*/
145  CS_WD_INIT(&parameters,arg[1],&world);
146 
147  /* Read samples */
148  #if (EXPLORATION_RRT)
149  nvs=ReadOneSample(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1);
150  NEW(s2,nvs,double);
151  memcpy(s2,s1,sizeof(double)*nvs);
152  #else
153  nvs=ReadTwoSamples(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1,&s2);
154  #endif
155 
156  /* Random seed initialization */
157  t=time(NULL); /* Get the time at which input files have been read */
158  ri=(unsigned int)t;
159  randomSet(ri);
160  fprintf(stderr,"Random seed : %u\n",ri);
161 
162  /* Start the process to connect the two samples */
163  InitAverages(nRepetitions,TRUE,TRUE,NO_UINT,&averages);
164 
165  for(it=0;it<nRepetitions;it++)
166  {
167  /* Initialize the atlas */
168  InitAtlasRRT(&parameters,FALSE/*parallel*/,s1,ONE_TREE,FALSE,s2,&world,&atlasrrt);
169  fprintf(stderr,"************************************************\n");
170 
171  /* Define the path using the atlas */
172  connected=AtlasTRRT(&parameters,s2,
173  &planningTime,
174  &pl,&pc,&ns,&path,CS_WD_COST_FN(&world),
175  CS_WD_COST_PTR(&world),arst,&atlasrrt);
176 
177  /* Save the results (only if one shot execution) */
178  if (nRepetitions==1)
179  {
180  if (connected)
181  SaveSamples(arg[1],FALSE,nvs,ns,path);
182 
183  SaveAtlasRRT(&parameters,arg[1],&atlasrrt);
184  }
185 
186  /* Summarize and release allocated objects for this repetition*/
187 
188  if ((EXPLORATION_RRT)||(connected))
189  {
190  NewSuccesfulExperiment(planningTime,AtlasRRTMemSize(&atlasrrt),pl,pc,
191  (double)GetAtlasRRTNumCharts(&atlasrrt),
192  (double)GetAtlasRRTNumNodes(&atlasrrt),
193  NULL,NULL,
194  &averages);
195  DeleteSamples(ns,path);
196  }
197  else
198  fprintf(stderr," Execution failed (%f sec)\n",planningTime);
199 
200  DeleteAtlasRRT(&atlasrrt);
201 
202  fprintf(stderr,"Execution compleated %u/%u\n",it+1,nRepetitions);
203  }
204 
205  /* Print statistics about the execution (only if many iterations) */
206  if (nRepetitions>1)
207  {
208  PrintAveragesHeader(stderr,argc,arg,&averages);
209 
210  fprintf(stderr,"%% **************************************************\n");
211  fprintf(stderr,"Random seed : %u\n",ri);
212  fprintf(stderr,"Exploration: %u Adjust_SA: %u (%.2f--%.2f) GlobalNN: %u GlobalCurv: %u Tree_ATLAS: %u\n",
214  PrintParameters(stderr,&parameters);
215 
216  #if (GET_ATLASRRT_STATISTICS)
217  PrintAtlasRRTStatistics(NULL,arst);
219  free(arst);
220  #endif
221 
222  PrintAverages(stderr,&averages);
223 
224  fprintf(stderr,"%% **************************************************\n");
225  }
226 
227  /* Release memory */
228  DeleteAverages(&averages);
229 
230  /* Release memory */
231  free(s1);
232  free(s2);
233 
234  DeleteParameters(&parameters);
235 
236  CS_WD_DELETE(&world);
237 
238  DeleteFileName(&fparam);
239  }
240  else
241  {
242  fprintf(stderr," Wrong number of parameters.\n");
243  fprintf(stderr," Use:\n");
244  fprintf(stderr," cuikatlastrrt <problem filename>.%s [num Repetitions]\n",CS_WD_EXT);
245  fprintf(stderr," where <problem filename> the equations/world description\n");
246  fprintf(stderr," <num Repetitions> experiment repetitions to gather statistics\n");
247  fprintf(stderr," This is optional.\n");
248  fprintf(stderr," (the '.%s' extension is not required)\n",CS_WD_EXT);
249  }
250  return(EXIT_SUCCESS);
251 }
252 
int main(int argc, char **arg)
Main body of the cuikatlastrrt application.
Definition: cuikatlastrrt.c:79
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 CS_WD_COST_FN(wcs)
Cost function for a given configution.
Definition: wcs.h:418
#define USE_ATLAS_TREE
Whether to use a binary tree to search for neighbouring charts.
Definition: atlas.h:78
boolean AtlasTRRT(Tparameters *pr, double *pg, double *time, double *pl, double *pc, unsigned int *ns, double ***path, double(*costF)(Tparameters *, boolean, double *, void *), void *costData, TAtlasRRTStatistics *str, Tatlasrrt *ar)
Extends a Atlas-TRRT until we reach a targed point.
Definition: atlasrrt.c:4688
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
#define CS_WD_COST_PTR(wcs)
Pointer to the base type.
Definition: wcs.h:408
Definition of the Tworld type and the associated functions.
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
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
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