AnytimePathShorteningDemo.cpp
1 /*********************************************************************
2  * Rice University Software Distribution License
3  *
4  * Copyright (c) 2010, 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 "AnytimePathShorteningDemo.h"
14 
15 #include <ompl/geometric/planners/AnytimePathShortening.h>
16 #include <ompl/tools/benchmark/Benchmark.h>
17 #include <ompl/util/Console.h>
18 #include <omplapp/apps/SE2RigidBodyPlanning.h>
19 #include <omplapp/apps/SE3MultiRigidBodyPlanning.h>
20 #include <omplapp/apps/SE3RigidBodyPlanning.h>
21 #include <omplapp/config.h>
22 
23 #include <cmath>
24 
25 using namespace ompl;
26 
27 std::shared_ptr<geometric::SimpleSetup> allocProblem(const ProblemType &probType)
28 {
29  // Explicitly use the FCL collision checker. PQP is not multi-threaded, and
30  // multiple planners using the same SpaceInformation instance simultaneously
31  // will serialize when checking if a state is in collision.
32 
33  switch (probType)
34  {
35  case BARRIERS:
36  return std::make_shared<app::SE2RigidBodyPlanning>();
37  case CUBICLES:
38  return std::make_shared<app::SE3RigidBodyPlanning>();
39  case EASY:
40  return std::make_shared<app::SE3MultiRigidBodyPlanning>(2);
41  default:
42  OMPL_ERROR("Unknown problem type in allocProblem");
43  return nullptr;
44  }
45 }
46 
47 void setStartAndGoalStates(const ProblemType &probType, std::shared_ptr<geometric::SimpleSetup> &setup)
48 {
49  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR);
50  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR);
51 
52  switch (probType)
53  {
54  case BARRIERS:
55  {
56  robot_fname += "/2D/Barriers_easy_robot.dae";
57  env_fname += "/2D/Barriers_easy_env.dae";
58 
59  // define start state
60  base::ScopedState<base::SE2StateSpace> start(setup->getSpaceInformation());
61  start->setX(34.81);
62  start->setY(-75.0);
63  start->setYaw(0.0);
64 
65  // define goal state
67  goal->setX(620.0);
68  goal->setY(-375.0);
69  goal->setYaw(3.14159);
70 
71  // set the start & goal states
72  setup->setStartAndGoalStates(start, goal);
73  }
74  break;
75 
76  case CUBICLES:
77  {
78  robot_fname += "/3D/cubicles_robot.dae";
79  env_fname += "/3D/cubicles_env.dae";
80 
81  // define start state
82  base::ScopedState<base::SE3StateSpace> start(setup->getSpaceInformation());
83  start->setX(-4.96);
84  start->setY(-40.62);
85  start->setZ(70.57);
86  start->rotation().setIdentity();
87 
88  // define goal state
90  goal->setX(200.49);
91  goal->setY(-40.62);
92  goal->setZ(70.57);
93  goal->rotation().setIdentity();
94 
95  // set the start & goal states
96  setup->setStartAndGoalStates(start, goal);
97  }
98  break;
99 
100  case EASY:
101  {
102  robot_fname += "/3D/Easy_robot.dae";
103  env_fname += "/3D/Easy_env.dae";
104 
105  base::ScopedState<base::CompoundStateSpace> start(setup->getSpaceInformation());
107 
108  // Robot 1
109  start[0] = 270.0;
110  start[1] = 100.0;
111  start[2] = -150.0;
112  start[3] = 1.0;
113  start[4] = 0.0;
114  start[5] = 0.0;
115  start[6] = 0.0;
116 
117  // Robot 2
118  start[7] = 270.0;
119  start[8] = 100.0;
120  start[9] = -425.0;
121  start[10] = 1.0;
122  start[11] = 0.0;
123  start[12] = 0.0;
124  start[13] = 0.0;
125 
126  // Robot 1
127  goal[0] = 270.0;
128  goal[1] = 100.0;
129  goal[2] = -425.0;
130  goal[3] = 1.0;
131  goal[4] = 0.0;
132  goal[5] = 0.0;
133  goal[6] = 0.0;
134 
135  goal[7] = 270.0;
136  goal[8] = 100.0;
137  goal[9] = -150.0;
138  goal[10] = 1.0;
139  goal[11] = 0.0;
140  goal[12] = 0.0;
141  goal[13] = 0.0;
142 
143  // set the start & goal states
144  setup->setStartAndGoalStates(start, goal);
145  }
146  break;
147 
148  default:
149  OMPL_ERROR("FATAL: Unknown problem type in setStartAndGoalStates");
150  break;
151  }
152 
153  dynamic_cast<app::RigidBodyGeometry *>(setup.get())->setRobotMesh(robot_fname);
154  dynamic_cast<app::RigidBodyGeometry *>(setup.get())->setEnvironmentMesh(env_fname);
155 }
156 
157 void solve(const ProblemType &probType, const OptimizationType &optType, const double &runtime,
158  const std::vector<PlannerType> &planners)
159 {
160  auto setup(allocProblem(probType));
161 
162  setStartAndGoalStates(probType, setup);
163 
164  // setting collision checking resolution to 1% of the space extent
165  setup->getSpaceInformation()->setStateValidityCheckingResolution(0.01);
166 
167  base::PlannerPtr planner;
168  switch (optType)
169  {
170  case SHORTCUT:
171  planner = std::make_shared<geometric::AnytimePathShortening>(setup->getSpaceInformation());
172  planner->as<geometric::AnytimePathShortening>()->setHybridize(false);
173  break;
174 
175  case HYBRIDIZE:
176  planner = std::make_shared<geometric::AnytimePathShortening>(setup->getSpaceInformation());
177  planner->as<geometric::AnytimePathShortening>()->setShortcut(false);
178  break;
179 
180  case ALTERNATE:
181  planner = std::make_shared<geometric::AnytimePathShortening>(setup->getSpaceInformation());
182  break;
183 
184  case NONE:
185  planner = allocPlanner(planners[0], setup->getSpaceInformation());
186  break;
187 
188  default:
189  OMPL_ERROR("Unhandled optimization case");
190  return;
191  }
192 
193  if (optType != NONE)
194  {
195  // Adding planners
196  for (auto i : planners)
197  {
198  base::PlannerPtr subPlanner = allocPlanner(i, setup->getSpaceInformation());
199  planner->as<geometric::AnytimePathShortening>()->addPlanner(subPlanner);
200  }
201 
202  std::cout << "Planner suite: " << std::endl;
203  for (size_t i = 0; i < planners.size(); ++i)
204  std::cout << i + 1 << ": " << planner->as<geometric::AnytimePathShortening>()->getPlanner(i)->getName()
205  << std::endl;
206  }
207  else
208  {
209  std::cout << "Planner suite: " << std::endl;
210  std::cout << "1: " << planner->getName() << std::endl;
211  }
212 
213  setup->setup();
214  setup->print();
215 
216  tools::Benchmark benchmark(*setup);
217  tools::Benchmark::Request request(runtime, 1e6, 50, 0.5, true, false, true);
218 
219  if (probType == BARRIERS)
220  benchmark.setExperimentName("Barriers*");
221  else if (probType == CUBICLES)
222  benchmark.setExperimentName("Cubicles*");
223  else
224  benchmark.setExperimentName("EasySwap*");
225  benchmark.addPlanner(planner);
226  benchmark.benchmark(request);
227  benchmark.saveResultsToFile();
228 }
229 
230 int main(int argc, char **argv)
231 {
232  // Expecting the following arguments:
233  // 1 - problem: barriers, cubicles, easy
234  // 2 - optimization type: shortcut, hybridize, alternate
235  // 3 - runtime, in seconds
236  // 4 - planner(s)
237 
238  if (argc < 5)
239  {
240  if (argc != 2)
241  {
242  std::cout << "Usage:" << std::endl;
243  std::cout << "demo_AnytimePathShortening ([barriers|cubicles|easy] [shortcut|hybridize|alternate|none] "
244  "[runtime(seconds)] [planner1] (planner2)...)|(input file)"
245  << std::endl;
246  return 0;
247  }
248  }
249 
250  ProblemType probType;
251  OptimizationType optType;
252  double runtime;
253  std::vector<PlannerType> planners;
254  parseArguments(argc, argv, probType, optType, runtime, planners);
255  if (!validArguments(probType, optType, runtime, planners))
256  return 1;
257 
258  solve(probType, optType, runtime, planners);
259 }
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
Representation of a benchmark request.
Definition: Benchmark.h:252
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:112
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Definition of a scoped state.
Definition: ScopedState.h:120
Main namespace. Contains everything in this library.
Definition: AppBase.h:21