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/apps/SE3RigidBodyPlanning.h>
16 #include <omplapp/config.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);
30  setup.setRobotMesh(robot_mesh);
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
42  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
43  start->setX(-31.19);
44  start->setY(-99.85);
45  start->setZ(36.46);
46  start->rotation().setIdentity();
47 
48  base::ScopedState<base::SE3StateSpace> goal(setup.getSpaceInformation());
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);
69  setup.setRobotMesh(robot_mesh);
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
81  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
82  start->setX(270.4);
83  start->setY(50.0);
84  start->setZ(-406.82);
85  start->rotation().setIdentity();
86 
87  base::ScopedState<base::SE3StateSpace> goal(setup.getSpaceInformation());
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);
108  setup.setRobotMesh(robot_mesh);
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
120  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
121  start->setX(-4.96);
122  start->setY(-40.62);
123  start->setZ(70.57);
124  start->rotation().setIdentity();
125 
126  base::ScopedState<base::SE3StateSpace> goal(setup.getSpaceInformation());
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,
140  bool continuous = false)
141 {
142  std::cout << "Evaluating " << (continuous ? "continuous" : "discrete") << " " << (fcl ? "FCL" : "PQP") << " checker"
143  << std::endl;
144 
145  std::vector<double> time;
146  unsigned int successful = 0;
147  unsigned int problem = 0;
148 
149  while (problem != 3)
150  {
151  unsigned int nr_attempts = 0;
152 
153  if (problem == 0)
154  std::cout << "- Apartment problem " << std::flush;
155  else if (problem == 1)
156  std::cout << "- Cubicles problem " << std::flush;
157  else if (problem == 2)
158  std::cout << R"(- 'Easy' problem )" << std::flush;
159 
160  // plan in SE3
162 
163  switch (problem)
164  {
165  case 0:
166  configureApartmentProblem(setup);
167  break;
168  case 1:
169  configureCubiclesProblem(setup);
170  break;
171  case 2:
172  configureEasyProblem(setup);
173  break;
174  }
175 
176  setup.setStateValidityCheckerType(fcl ? app::FCL : app::PQP);
177 
178  if (continuous)
179  {
180  setup.setup(); // FCLContinuousMotionValidator extracts goodies from the state validity checker.
181  // Instantiate the svc here.
182  setup.getSpaceInformation()->setMotionValidator(std::make_shared<app::FCLContinuousMotionValidator>(
183  setup.getSpaceInformation(), setup.getMotionModel()));
184  }
185 
186  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
187  setup.setPlanner(std::make_shared<geometric::RRT>(setup.getSpaceInformation()));
188  setup.setup();
189 
190  while (successful < tries)
191  {
192  setup.clear();
193  setup.solve(30.0);
194 
195  // Retry if the planner failed, except for the continuous collision checker case.
196  if (setup.haveExactSolutionPath() || continuous)
197  {
198  successful++;
199  time.push_back(setup.getLastPlanComputationTime());
200  std::cout << '.' << std::flush;
201  }
202  else
203  std::cout << 'x' << std::flush;
204  nr_attempts++;
205  }
206 
207  times.push_back(time);
208  time.clear();
209  successful = 0;
210  problem++;
211  attempts.push_back(nr_attempts);
212  std::cout << std::endl;
213  }
214 }
215 
216 // Compares discrete and continuous collision checkers in OMPL.app
217 int main(int argc, char **argv)
218 {
219 #if OMPL_HAS_PQP
220  // User can supply number of tries as 2nd command line argument. Otherwise, use default NR_TRIES.
221  unsigned int nr_tries;
222  if (argc == 2)
223  {
224  nr_tries = atoi(argv[1]);
225 
226  // make sure command line input is valid
227  if (nr_tries == std::numeric_limits<unsigned int>::min() ||
228  nr_tries == std::numeric_limits<unsigned int>::max())
229  nr_tries = 20u;
230  }
231  else
232  nr_tries = 20u;
233 
234  ompl::msg::noOutputHandler(); // Disable console output from OMPL.
235 
236  std::vector<std::vector<double>> pqp_times;
237  std::vector<std::vector<double>> dfcl_times;
238  std::vector<std::vector<double>> cfcl_times;
239  std::vector<int> pqp_attempts;
240  std::vector<int> dfcl_attempts;
241  std::vector<int> cfcl_attempts;
242 
243  std::cout << "Comparing collision checkers:" << std::endl;
244  std::cout << "Each problem is executed until " << nr_tries << " attempts are successful (30 sec limit)"
245  << std::endl;
246 
247  // PQP Test (discrete)
248  test(nr_tries, pqp_times, pqp_attempts);
249 
250  // Discrete FCL Test
251  test(nr_tries, dfcl_times, dfcl_attempts, true);
252 
253  // Continuous FCL Test
254  test(nr_tries, cfcl_times, cfcl_attempts, true, true);
255 
256  std::cout << std::endl << "Analysis:" << std::endl;
257 
258  // Assume size of all 3 time vectors are the same
259  for (size_t i = 0; i < pqp_times.size(); ++i)
260  {
261  bool isOdd;
262  auto pqp_time = pqp_times[i].begin() + pqp_times[i].size() / 2u;
263  auto dfcl_time = dfcl_times[i].begin() + dfcl_times[i].size() / 2u;
264  auto cfcl_time = cfcl_times[i].begin() + cfcl_times[i].size() / 2u;
265 
266  std::nth_element(pqp_times[i].begin(), pqp_time, pqp_times[i].end());
267  std::nth_element(dfcl_times[i].begin(), dfcl_time, dfcl_times[i].end());
268  std::nth_element(cfcl_times[i].begin(), cfcl_time, cfcl_times[i].end());
269 
270  std::cout << std::endl;
271  if (i == 0)
272  std::cout << " Apartment problem - Median Time (s)" << std::endl;
273  else if (i == 1)
274  std::cout << " Cubicles problem - Median Time (s)" << std::endl;
275  else if (i == 2)
276  std::cout << R"( 'Easy' problem - Median Time (s))" << std::endl;
277 
278  isOdd = pqp_times[i].size() % 2 == 1;
279  std::cout << " Discrete PQP: " << (isOdd ? *pqp_time : .5 * (*pqp_time + *(pqp_time + 1))) << " "
280  << nr_tries << "/" << pqp_attempts[i] << " planning attempts successful" << std::endl;
281  isOdd = dfcl_times[i].size() % 2 == 1;
282  std::cout << " Discrete FCL: " << (isOdd ? *dfcl_time : .5 * (*dfcl_time + *(dfcl_time + 1))) << " "
283  << nr_tries << "/" << dfcl_attempts[i] << " planning attempts successful" << std::endl;
284  isOdd = cfcl_times[i].size() % 2 == 1;
285  std::cout << " Continuous FCL: " << (isOdd ? *cfcl_time : .5 * (*cfcl_time + *(cfcl_time + 1))) << " "
286  << nr_tries << " total attempts" << std::endl;
287  }
288 
289 #else
290  std::cerr << "ERROR: PQP collision checker is not installed" << std::endl;
291 #endif
292 
293  return 0;
294 }
Definition of a scoped state.
Definition: ScopedState.h:56
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
void noOutputHandler()
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(nul...
Definition: Console.cpp:95
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
The lower and upper bounds for an Rn space.
A state space representing SE(3)
Definition: SE3StateSpace.h:49