SE3RigidBodyPlanningBenchmark.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: Ioan Sucan */
12 
13 #include <ompl/geometric/planners/est/EST.h>
14 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
15 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
16 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
17 #include <ompl/geometric/planners/prm/PRM.h>
18 #include <ompl/geometric/planners/rrt/RRT.h>
19 #include <ompl/geometric/planners/rrt/RRTConnect.h>
20 #include <ompl/geometric/planners/sbl/SBL.h>
21 #include <ompl/tools/benchmark/Benchmark.h>
22 #include <omplapp/apps/SE3RigidBodyPlanning.h>
23 #include <omplapp/config.h>
24 
25 #include <ompl/base/samplers/BridgeTestValidStateSampler.h>
26 #include <ompl/base/samplers/GaussianValidStateSampler.h>
27 #include <ompl/base/samplers/MaximizeClearanceValidStateSampler.h>
28 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
29 #include <ompl/base/samplers/UniformValidStateSampler.h>
30 using namespace ompl;
31 
32 void benchmark0(std::string &benchmark_name, app::SE3RigidBodyPlanning &setup, double &runtime_limit,
33  double &memory_limit, int &run_count)
34 {
35  benchmark_name = std::string("cubicles");
36  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_robot.dae";
37  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_env.dae";
38  setup.setRobotMesh(robot_fname);
39  setup.setEnvironmentMesh(env_fname);
40 
41  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
42  start->setX(-4.96);
43  start->setY(-40.62);
44  start->setZ(70.57);
45  start->rotation().setIdentity();
46 
48  goal->setX(200.49);
49  goal->setY(-40.62);
50  goal->setZ(70.57);
51  goal->rotation().setIdentity();
52 
53  setup.setStartAndGoalStates(start, goal);
54  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
55  setup.setup();
56 
57  std::vector<double> cs(3);
58  cs[0] = 35;
59  cs[1] = 35;
60  cs[2] = 35;
61  setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);
62 
63  runtime_limit = 10.0;
64  memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
65  run_count = 500;
66 }
67 
68 void benchmark1(std::string &benchmark_name, app::SE3RigidBodyPlanning &setup, double &runtime_limit,
69  double &memory_limit, int &run_count)
70 {
71  benchmark_name = std::string("Twistycool");
72  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Twistycool_robot.dae";
73  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Twistycool_env.dae";
74  setup.setRobotMesh(robot_fname);
75  setup.setEnvironmentMesh(env_fname);
76 
77  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
78  start->setX(270.);
79  start->setY(160.);
80  start->setZ(-200.);
81  start->rotation().setIdentity();
82 
84  goal->setX(270.);
85  goal->setY(160.);
86  goal->setZ(-400.);
87  goal->rotation().setIdentity();
88 
89  base::RealVectorBounds bounds(3);
90  bounds.setHigh(0, 350.);
91  bounds.setHigh(1, 250.);
92  bounds.setHigh(2, -150.);
93  bounds.setLow(0, 200.);
94  bounds.setLow(1, 75.);
95  bounds.setLow(2, -450.);
96  setup.getStateSpace()->as<base::SE3StateSpace>()->setBounds(bounds);
97 
98  setup.setStartAndGoalStates(start, goal);
99  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
100 
101  runtime_limit = 60.0;
102  memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
103  run_count = 50;
104 }
105 
106 void preRunEvent(const base::PlannerPtr & /*planner*/)
107 {
108 }
109 
110 void postRunEvent(const base::PlannerPtr & /*planner*/, tools::Benchmark::RunProperties & /*run*/)
111 {
112 }
113 
114 int main(int argc, char **argv)
115 {
117  std::string benchmark_name;
118  double runtime_limit, memory_limit;
119  int run_count;
120  int benchmark_id = argc > 1 ? ((argv[1][0] - '0') % 2) : 0;
121 
122  if (benchmark_id == 0)
123  benchmark0(benchmark_name, setup, runtime_limit, memory_limit, run_count);
124  else
125  benchmark1(benchmark_name, setup, runtime_limit, memory_limit, run_count);
126 
127  // create the benchmark object and add all the planners we'd like to run
128  tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
129  tools::Benchmark b(setup, benchmark_name);
130 
131  // optionally set pre & pos run events
132  b.setPreRunEvent([](const base::PlannerPtr &planner) { preRunEvent(planner); });
133  b.setPostRunEvent(
134  [](const base::PlannerPtr &planner, tools::Benchmark::RunProperties &run) { postRunEvent(planner, run); });
135 
136  b.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
137  b.addPlanner(std::make_shared<geometric::RRT>(setup.getSpaceInformation()));
138  b.addPlanner(std::make_shared<geometric::BKPIECE1>(setup.getSpaceInformation()));
139  b.addPlanner(std::make_shared<geometric::LBKPIECE1>(setup.getSpaceInformation()));
140  b.addPlanner(std::make_shared<geometric::KPIECE1>(setup.getSpaceInformation()));
141  b.addPlanner(std::make_shared<geometric::SBL>(setup.getSpaceInformation()));
142  b.addPlanner(std::make_shared<geometric::EST>(setup.getSpaceInformation()));
143  b.addPlanner(std::make_shared<geometric::PRM>(setup.getSpaceInformation()));
144 
145  int sampler_id = argc > 2 ? ((argv[2][0] - '0') % 5) : -1;
146 
147  if (sampler_id == 0 || sampler_id < 0)
148  {
149  // run all planners with a uniform valid state sampler on the benchmark problem
150  setup.getSpaceInformation()->setValidStateSamplerAllocator(
152  return std::make_shared<base::UniformValidStateSampler>(si);
153  });
154  b.addExperimentParameter("sampler_id", "INTEGER", "0");
155  b.benchmark(request);
156  b.saveResultsToFile();
157  }
158 
159  if (sampler_id == 1 || sampler_id < 0)
160  {
161  // run all planners with a Gaussian valid state sampler on the benchmark problem
162  setup.getSpaceInformation()->setValidStateSamplerAllocator(
164  return std::make_shared<base::GaussianValidStateSampler>(si);
165  });
166  b.addExperimentParameter("sampler_id", "INTEGER", "1");
167  b.benchmark(request);
168  b.saveResultsToFile();
169  }
170 
171  if (sampler_id == 2 || sampler_id < 0)
172  {
173  // run all planners with a obstacle-based valid state sampler on the benchmark problem
174  setup.getSpaceInformation()->setValidStateSamplerAllocator(
176  return std::make_shared<base::ObstacleBasedValidStateSampler>(si);
177  });
178  b.addExperimentParameter("sampler_id", "INTEGER", "2");
179  b.benchmark(request);
180  b.saveResultsToFile();
181  }
182 
183  if (sampler_id == 3 || sampler_id < 0)
184  {
185  // run all planners with a maximum-clearance valid state sampler on the benchmark problem
186  setup.getSpaceInformation()->setValidStateSamplerAllocator(
188  auto vss = std::make_shared<base::MaximizeClearanceValidStateSampler>(si);
189  vss->setNrImproveAttempts(5);
190  return vss;
191  });
192  b.addExperimentParameter("sampler_id", "INTEGER", "3");
193  b.benchmark(request);
194  b.saveResultsToFile();
195  }
196 
197  if (sampler_id == 4 || sampler_id < 0)
198  {
199  // run all planners with a maximum-clearance valid state sampler on the benchmark problem
200  setup.getSpaceInformation()->setValidStateSamplerAllocator(
202  return std::make_shared<base::BridgeTestValidStateSampler>(si);
203  });
204  b.addExperimentParameter("sampler_id", "INTEGER", "4");
205  b.benchmark(request);
206  b.saveResultsToFile();
207  }
208  return 0;
209 }
A shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of a scoped state.
Definition: ScopedState.h:56
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
std::map< std::string, std::string > RunProperties
The data collected from a run of a planner is stored as key-value pairs.
Definition: Benchmark.h:79
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::Planner.
The base class for space information. This contains all the information about the space planning is d...
Representation of a benchmark request.
Definition: Benchmark.h:156
The lower and upper bounds for an Rn space.
A state space representing SE(3)
Definition: SE3StateSpace.h:49