CollisionCheckers.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2012, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #include <ompl/geometric/planners/rrt/RRT.h>
14 #include <ompl/util/Console.h>
15 #include <omplapp/config.h>
16 #include <omplapp/apps/SE3RigidBodyPlanning.h>
17 #include <omplapp/geometry/detail/FCLContinuousMotionValidator.h>
18 #include <vector>
19 
20 using namespace ompl;
21 
22 void configureApartmentProblem (app::SE3RigidBodyPlanning &setup)
23 {
24  std::string problem = "Apartment";
25  std::string environment_mesh = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_env.dae";
26  std::string robot_mesh = std::string (OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_robot.dae";
27 
28  // load the robot and the environment
29  setup.setEnvironmentMesh(environment_mesh.c_str());
30  setup.setRobotMesh(robot_mesh.c_str());
31 
32  // Bounds for Apartment environment
33  base::RealVectorBounds bounds(3);
34  bounds.low[0] = -73.76;
35  bounds.low[1] = -179.59;
36  bounds.low[2] = -0.03;
37  bounds.high[0] = 295.77;
38  bounds.high[1] = 168.26;
39  bounds.high[2] = 90.39;
40 
41  // Start/Goal pair for the Apartment environment
43  start->setX (-31.19);
44  start->setY (-99.85);
45  start->setZ (36.46);
46  start->rotation ().setIdentity ();
47 
49  goal->setX (140.0);
50  goal->setY (0.0);
51  goal->setZ (36.46);
52  goal->rotation ().setIdentity ();
53 
54  // Set start and goal
55  setup.setStartAndGoalStates(start, goal);
56 
57  // Bound the state space
58  setup.getSpaceInformation ()->getStateSpace ()->as <base::SE3StateSpace> ()->setBounds (bounds);
59 }
60 
61 void configureEasyProblem (app::SE3RigidBodyPlanning &setup)
62 {
63  std::string problem = "Easy";
64  std::string environment_mesh = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_env.dae";
65  std::string robot_mesh = std::string (OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_robot.dae";
66 
67  // load the robot and the environment
68  setup.setEnvironmentMesh(environment_mesh.c_str());
69  setup.setRobotMesh(robot_mesh.c_str());
70 
71  // Bounds for Easy/TwistyCool environment
72  base::RealVectorBounds bounds(3);
73  bounds.low[0] = 14.46;
74  bounds.low[1] = -24.25;
75  bounds.low[2] = -504.86;
76  bounds.high[0] = 457.96;
77  bounds.high[1] = 321.25;
78  bounds.high[2] = -72.86;
79 
80  // Start/Goal pair for the Easy/TwistyCool environment
82  start->setX (270.4);
83  start->setY (50.0);
84  start->setZ (-406.82);
85  start->rotation ().setIdentity ();
86 
88  goal->setX (270.4);
89  goal->setY (50.0);
90  goal->setZ (-186.82);
91  goal->rotation ().setIdentity ();
92 
93  // Set start and goal
94  setup.setStartAndGoalStates(start, goal);
95 
96  // Bound the state space
97  setup.getSpaceInformation ()->getStateSpace ()->as <base::SE3StateSpace> ()->setBounds (bounds);
98 }
99 
100 void configureCubiclesProblem (app::SE3RigidBodyPlanning &setup)
101 {
102  std::string problem = "cubicles";
103  std::string environment_mesh = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_env.dae";
104  std::string robot_mesh = std::string (OMPLAPP_RESOURCE_DIR) + "/3D/" + problem + "_robot.dae";
105 
106  // load the robot and the environment
107  setup.setEnvironmentMesh(environment_mesh.c_str());
108  setup.setRobotMesh(robot_mesh.c_str());
109 
110  // Bounds for cubicles environment
111  base::RealVectorBounds bounds(3);
112  bounds.low[0] = -508.88;
113  bounds.low[1] = -230.13;
114  bounds.low[2] = -123.75;
115  bounds.high[0] = 319.62;
116  bounds.high[1] = 531.87;
117  bounds.high[2] = 101.00;
118 
119  // Start/Goal pair for cubicles environment
121  start->setX (-4.96);
122  start->setY (-40.62);
123  start->setZ (70.57);
124  start->rotation ().setIdentity ();
125 
127  goal->setX (200.00);
128  goal->setY (-40.62);
129  goal->setZ (70.57);
130  goal->rotation ().setIdentity ();
131 
132  // Set start and goal
133  setup.setStartAndGoalStates(start, goal);
134 
135  // Bound the state space
136  setup.getSpaceInformation ()->getStateSpace ()->as <base::SE3StateSpace> ()->setBounds (bounds);
137 }
138 
139 void test (unsigned int tries, std::vector <std::vector<double> > &times, std::vector<int> &attempts, bool fcl=false, bool continuous=false)
140 {
141  std::cout << "Evaluating " << (continuous ? "continuous" : "discrete")
142  << " " << (fcl ? "FCL" : "PQP") << " checker" << std::endl;
143 
144  std::vector <double> time;
145  unsigned int successful = 0;
146  unsigned int problem = 0;
147 
148  while (problem != 3)
149  {
150  unsigned int nr_attempts = 0;
151 
152  if (problem == 0) std::cout << "- Apartment problem " << std::flush;
153  else if (problem == 1) std::cout << "- Cubicles problem " << std::flush;
154  else if (problem == 2) std::cout << R"(- 'Easy' problem )" << std::flush;
155 
156  // plan in SE3
158 
159  switch (problem)
160  {
161  case 0: configureApartmentProblem (setup); break;
162  case 1: configureCubiclesProblem (setup); break;
163  case 2: configureEasyProblem (setup); break;
164  }
165 
166  setup.setStateValidityCheckerType (fcl ? app::FCL : app::PQP);
167 
168  if (continuous)
169  {
170  setup.setup(); // FCLContinuousMotionValidator extracts goodies from the state validity checker. Instantiate the svc here.
171  setup.getSpaceInformation()->setMotionValidator(
172  std::make_shared<app::FCLContinuousMotionValidator>(
173  setup.getSpaceInformation(), setup.getMotionModel()));
174  }
175 
176  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
177  setup.setPlanner(std::make_shared<geometric::RRT>(setup.getSpaceInformation()));
178  setup.setup ();
179 
180  while (successful < tries)
181  {
182  setup.clear ();
183  setup.solve(30.0);
184 
185  // Retry if the planner failed, except for the continuous collision checker case.
186  if (setup.haveExactSolutionPath () || continuous)
187  {
188  successful++;
189  time.push_back (setup.getLastPlanComputationTime ());
190  std::cout << '.' << std::flush;
191  }
192  else
193  std::cout << 'x' << std::flush;
194  nr_attempts++;
195  }
196 
197  times.push_back (time);
198  time.clear ();
199  successful = 0;
200  problem++;
201  attempts.push_back (nr_attempts);
202  std::cout << std::endl;
203  }
204 }
205 
206 // Compares discrete and continuous collision checkers in OMPL.app
207 int main (int argc, char **argv)
208 {
209 #if OMPL_HAS_PQP
210  // User can supply number of tries as 2nd command line argument. Otherwise, use default NR_TRIES.
211  unsigned int nr_tries;
212  if (argc == 2)
213  {
214  nr_tries = atoi (argv[1]);
215 
216  // make sure command line input is valid
217  if (nr_tries == std::numeric_limits<unsigned int>::min() || nr_tries == std::numeric_limits<unsigned int>::max())
218  nr_tries = 20u;
219  }
220  else
221  nr_tries = 20u;
222 
223  ompl::msg::noOutputHandler(); // Disable console output from OMPL.
224 
225  std::vector <std::vector <double> > pqp_times;
226  std::vector <std::vector <double> > dfcl_times;
227  std::vector <std::vector <double> > cfcl_times;
228  std::vector <int> pqp_attempts;
229  std::vector <int> dfcl_attempts;
230  std::vector <int> cfcl_attempts;
231 
232  std::cout << "Comparing collision checkers:" << std::endl;
233  std::cout << "Each problem is executed until " << nr_tries << " attempts are successful (30 sec limit)" << std::endl;
234 
235  // PQP Test (discrete)
236  test (nr_tries, pqp_times, pqp_attempts);
237 
238  // Discrete FCL Test
239  test (nr_tries, dfcl_times, dfcl_attempts, true);
240 
241  // Continuous FCL Test
242  test (nr_tries, cfcl_times, cfcl_attempts, true, true);
243 
244  std::cout << std::endl << "Analysis:" << std::endl;
245 
246  // Assume size of all 3 time vectors are the same
247  for (size_t i = 0; i < pqp_times.size(); ++i)
248  {
249  bool isOdd;
250  auto pqp_time = pqp_times[i].begin() + pqp_times[i].size() / 2u;
251  auto dfcl_time = dfcl_times[i].begin() + dfcl_times[i].size() / 2u;
252  auto cfcl_time = cfcl_times[i].begin() + cfcl_times[i].size() / 2u;
253 
254  std::nth_element(pqp_times[i].begin(), pqp_time, pqp_times[i].end());
255  std::nth_element(dfcl_times[i].begin(), dfcl_time, dfcl_times[i].end());
256  std::nth_element(cfcl_times[i].begin(), cfcl_time, cfcl_times[i].end());
257 
258  std::cout << std::endl;
259  if (i == 0)
260  std::cout << " Apartment problem - Median Time (s)" << std::endl;
261  else if (i == 1)
262  std::cout << " Cubicles problem - Median Time (s)" << std::endl;
263  else if (i == 2)
264  std::cout << R"( 'Easy' problem - Median Time (s))" << std::endl;
265 
266  isOdd = pqp_times[i].size() % 2 == 1;
267  std::cout << " Discrete PQP: " << (isOdd ? *pqp_time : .5*(*pqp_time + *(pqp_time+1)))
268  << " " << nr_tries << "/" << pqp_attempts[i] << " planning attempts successful" << std::endl;
269  isOdd = dfcl_times[i].size() % 2 == 1;
270  std::cout << " Discrete FCL: " << (isOdd ? *dfcl_time : .5*(*dfcl_time + *(dfcl_time+1)))
271  << " " << nr_tries << "/" << dfcl_attempts[i] << " planning attempts successful" << std::endl;
272  isOdd = cfcl_times[i].size() % 2 == 1;
273  std::cout << " Continuous FCL: " << (isOdd ? *cfcl_time : .5*(*cfcl_time + *(cfcl_time+1)))
274  << " " << nr_tries << " total attempts" << std::endl;
275  }
276 
277 #else
278  std::cerr << "ERROR: PQP collision checker is not installed" << std::endl;
279 #endif
280 
281  return 0;
282 }
283 
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
Definition of a scoped state.
Definition: ScopedState.h:56
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
void noOutputHandler()
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(nul...
Definition: Console.cpp:95
void setPlanner(const base::PlannerPtr &planner)
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator...
Definition: SimpleSetup.h:208
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:75
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure.
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:239
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
The lower and upper bounds for an Rn space.
virtual void setStateValidityCheckerType(CollisionChecker ctype)
Change the type of collision checking for the rigid body.
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...
A state space representing SE(3)
Definition: SE3StateSpace.h:49
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:82