cuikcbirrt.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 "rrt.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 
80 int main(int argc,char **arg)
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  Trrt rrt;
92 
93  boolean connected;
94  double pl;
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;
104  time_t t;
105 
106  TRRTStatistics *rst;
107 
108  #if (EXPLORATION_RRT)
109  Error("cuikcbirrt can not be used in exploration mode (use ccrrt)");
110  #endif
111 
112 
113  if (argc>1)
114  {
115  if (argc>2)
116  {
117  nRepetitions=atoi(arg[2]);
118  if (nRepetitions==0)
119  Error("Second parameter for cuikcbirrt is wrong");
120  }
121  else
122  nRepetitions=1;
123 
124  if ((nRepetitions>1)&&((RRT_VERBOSE)||(GET_RRT_STATISTICS)))
125  Warning("To get accurate execution time statistics, set RRT_VERBOSE and GET_RRT_STATISTICS to 0");
126 
127  if ((GET_RRT_STATISTICS)&&(nRepetitions>1))
128  {
129  NEW(rst,1,TRRTStatistics);
130  InitRRTStatistics(rst);
131  }
132  else
133  rst=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  /*Read the world from file*/
141  CS_WD_INIT(&parameters,arg[1],&world);
142 
143  /* Read samples */
144  nvs=ReadTwoSamples(&parameters,arg[1],CS_WD_GET_NUM_SYSTEM_VARS(&world),&s1,&s2);
145 
146  /* Random seed initialization */
147  t=time(NULL); /* Get the time at which input files have been read */
148  ri=(unsigned int)t;
149  randomSet(ri);
150  fprintf(stderr,"Random seed : %u\n",ri);
151 
152  /* Start the process to connect the two samples */
153  InitAverages(nRepetitions,FALSE,TRUE,NO_UINT,&averages);
154 
155  for(it=0;it<nRepetitions;it++)
156  {
157  fprintf(stderr,"**************************************************\n");
158 
159  /* Init a bi-directional RRT (bi-directional despite the value of BI_RRT parameter) */
160  InitRRT(&parameters,FALSE,FALSE,s1,TWO_TREES,FALSE,s2,nvs,&world,&rrt);
161 
162  /* Try to connect the two trees */
163  connected=cBiRRT(&parameters,s2,
164  &planningTime,
165  &pl,&ns,&path,
166  rst,&rrt);
167 
168  /* Save the results (only if one shot execution) */
169  if (nRepetitions==1)
170  {
171  Tfilename frrt;
172 
173  if (connected)
174  SaveSamples(arg[1],FALSE,nvs,ns,path);
175 
176  CreateFileName(NULL,arg[1],NULL,RRT_EXT,&frrt);
177  fprintf(stderr,"Writing RRT to : %s\n",GetFileFullName(&frrt));
178  SaveRRT(&frrt,&rrt);
179  DeleteFileName(&frrt);
180  }
181 
182  /* Summarize and release allocated objects for this repetition*/
183  if (connected)
184  {
185  NewSuccesfulExperiment(planningTime,RRTMemSize(&rrt),pl,0,
186  NO_UINT,
187  (double)GetRRTNumNodes(&rrt),
188  NULL,NULL,
189  &averages);
190  DeleteSamples(ns,path);
191  }
192  else
193  fprintf(stderr," Execution failed (%f sec)\n",planningTime);
194 
195  DeleteRRT(&rrt);
196 
197  fprintf(stderr,"Execution compleated %u/%u\n",it+1,nRepetitions);
198  }
199 
200  /* Print statistics about the execution (only if many iterations) */
201  if (nRepetitions>1)
202  {
203  PrintAveragesHeader(stderr,argc,arg,&averages);
204 
205  fprintf(stderr,"%% **************************************************\n");
206  fprintf(stderr,"Random seed : %u\n",ri);
207  PrintParameters(stderr,&parameters);
208 
209  #if (GET_RRT_STATISTICS)
210  PrintRRTStatistics(NULL,rst);
211  DeleteRRTStatistics(rst);
212  free(rst);
213  #endif
214 
215  PrintAverages(stderr,&averages);
216 
217  fprintf(stderr,"%% **************************************************\n");
218  }
219 
220  /* Release memory */
221  DeleteAverages(&averages);
222 
223  free(s1);
224  free(s2);
225 
226  DeleteParameters(&parameters);
227 
228  CS_WD_DELETE(&world);
229 
230  DeleteFileName(&fparam);
231  }
232  else
233  {
234  fprintf(stderr," Wrong number of parameters.\n");
235  fprintf(stderr," Use:\n");
236  fprintf(stderr," cuikcbirrt <problem filename>.%s [num Repetitions]\n",CS_WD_EXT);
237  fprintf(stderr," where <problem filename> the equations/world description\n");
238  fprintf(stderr," <num Repetitions> experiment repetitions to gather statistics\n");
239  fprintf(stderr," This is optional.\n");
240  fprintf(stderr," (the '.%s' extension is not required)\n",CS_WD_EXT);
241  }
242  return(EXIT_SUCCESS);
243 }
244 
Statistics on the RRT construction.
Definition: rrt.h:199
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
void DeleteRRT(Trrt *rrt)
Destructor.
Definition: rrt.c:4196
#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:248
#define RRT_EXT
File extension for files storing rrt's.
Definition: filename.h:205
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
void DeleteRRTStatistics(TRRTStatistics *rst)
Destructor.
Definition: rrt.c:499
void InitRRT(Tparameters *pr, boolean parallel, boolean simp, double *ps, unsigned int mode, boolean graph, double *pg, unsigned int m, TAtlasBase *w, Trrt *rrt)
Defines a RRT from a given point.
Definition: rrt.c:1204
Definition of the Tfilename type and the associated functions.
#define TRUE
TRUE.
Definition: boolean.h:21
void Error(const char *s)
General error function.
Definition: error.c:80
#define PARAM_EXT
File extension for parameter files.
Definition: filename.h:131
A RRT on a manifold.
Definition: rrt.h:329
#define CS_WD_DELETE(wcs)
Destructor of the equation structure.
Definition: wcs.h:500
unsigned int RRTMemSize(Trrt *rrt)
Memory used by a given RRT.
Definition: rrt.c:3968
Definition of the Tworld type and the associated functions.
Definition of a rrt on a manifold.
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
void InitRRTStatistics(TRRTStatistics *rst)
Init the RRT statistics.
Definition: rrt.c:195
#define TWO_TREES
One of the modes for the RRT.
Definition: rrt.h:139
unsigned int ReadTwoSamples(Tparameters *p, char *fname, unsigned int nvs, double **s1, double **s2)
Reads two samples from a file.
Definition: samples.c:1247
Definitions of constants and macros used in several parts of the cuik library.
Auxiliary functions to deal averages of path planner executions.
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 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
int main(int argc, char **arg)
Main body of the cuikcbirrt application.
Definition: cuikcbirrt.c:80
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
void SaveRRT(Tfilename *fname, Trrt *rrt)
Stores the RRT information on a file.
Definition: rrt.c:3985
#define CS_WD_EXT
Possible extensions for the equation files.
Definition: wcs.h:48
void DeleteParameters(Tparameters *p)
Destructor.
Definition: parameters.c:295
void SaveSamples(char *fname, boolean smooth, unsigned int nvs, unsigned int ns, double **path)
Saves a set of samples to a file.
Definition: samples.c:1318
#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
unsigned int GetRRTNumNodes(Trrt *rrt)
Number of nodes in the RRT.
Definition: rrt.c:3521
#define GET_RRT_STATISTICS
Set this to one to gather statistics of RRT construction.
Definition: rrt.h:41
#define CS_WD_GET_NUM_SYSTEM_VARS(wcs)
Gets the number of system variables.
Definition: wcs.h:250
Auxiliary functions to deal with sets of samples.
Definition of basic randomization functions.
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:1495
void PrintRRTStatistics(Trrt *rrt, TRRTStatistics *rst)
Prints the summary of RRT statistics.
Definition: rrt.c:352
Definition of the Tparameters type and the associated functions.
void InitAverages(unsigned int m, boolean useCharts, boolean useSamples, unsigned int maxIt, Taverages *av)
Initializes a set of averages.
Definition: averages.c:21
boolean cBiRRT(Tparameters *pr, double *pg, double *time, double *pl, unsigned int *ns, double ***path, TRRTStatistics *grst, Trrt *rrt)
Extends a RRT until we reach a targed point.
Definition: rrt.c:3099
void randomSet(unsigned int seed)
Sets the random seed.
Definition: random.c:25