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 <omplapp/config.h>
14 #include <omplapp/apps/SE3RigidBodyPlanning.h>
15 #include <ompl/tools/benchmark/Benchmark.h>
16 #include <ompl/geometric/planners/rrt/RRTConnect.h>
17 #include <ompl/geometric/planners/rrt/RRT.h>
18 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
19 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
20 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
21 #include <ompl/geometric/planners/sbl/SBL.h>
22 #include <ompl/geometric/planners/est/EST.h>
23 #include <ompl/geometric/planners/prm/PRM.h>
24 
25 #include <ompl/base/samplers/UniformValidStateSampler.h>
26 #include <ompl/base/samplers/GaussianValidStateSampler.h>
27 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
28 #include <ompl/base/samplers/MaximizeClearanceValidStateSampler.h>
29 using namespace ompl;
30 
31 void benchmark0(std::string& benchmark_name, app::SE3RigidBodyPlanning& setup,
32  double& runtime_limit, double& memory_limit, int& run_count)
33 {
34  benchmark_name = std::string("cubicles");
35  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_robot.dae";
36  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_env.dae";
37  setup.setRobotMesh(robot_fname.c_str());
38  setup.setEnvironmentMesh(env_fname.c_str());
39 
41  start->setX(-4.96);
42  start->setY(-40.62);
43  start->setZ(70.57);
44  start->rotation().setIdentity();
45 
47  goal->setX(200.49);
48  goal->setY(-40.62);
49  goal->setZ(70.57);
50  goal->rotation().setIdentity();
51 
52  setup.setStartAndGoalStates(start, goal);
53  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
54  setup.setup();
55 
56  std::vector<double> cs(3);
57  cs[0] = 35; cs[1] = 35; cs[2] = 35;
58  setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);
59 
60  runtime_limit = 10.0;
61  memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
62  run_count = 500;
63 }
64 
65 void benchmark1(std::string& benchmark_name, app::SE3RigidBodyPlanning& setup,
66  double& runtime_limit, double& memory_limit, int& run_count)
67 {
68  benchmark_name = std::string("Twistycool");
69  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Twistycool_robot.dae";
70  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Twistycool_env.dae";
71  setup.setRobotMesh(robot_fname.c_str());
72  setup.setEnvironmentMesh(env_fname.c_str());
73 
75  start->setX(270.);
76  start->setY(160.);
77  start->setZ(-200.);
78  start->rotation().setIdentity();
79 
81  goal->setX(270.);
82  goal->setY(160.);
83  goal->setZ(-400.);
84  goal->rotation().setIdentity();
85 
86  base::RealVectorBounds bounds(3);
87  bounds.setHigh(0,350.);
88  bounds.setHigh(1,250.);
89  bounds.setHigh(2,-150.);
90  bounds.setLow(0,200.);
91  bounds.setLow(1,75.);
92  bounds.setLow(2,-450.);
93  setup.getStateSpace()->as<base::SE3StateSpace>()->setBounds(bounds);
94 
95  setup.setStartAndGoalStates(start, goal);
96  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
97 
98  runtime_limit = 60.0;
99  memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
100  run_count = 50;
101 }
102 
103 void preRunEvent(const base::PlannerPtr& /*planner*/)
104 {
105 }
106 
107 void postRunEvent(const base::PlannerPtr& /*planner*/, tools::Benchmark::RunProperties& /*run*/)
108 {
109 }
110 
111 int main(int argc, char **argv)
112 {
114  std::string benchmark_name;
115  double runtime_limit, memory_limit;
116  int run_count;
117  int benchmark_id = argc > 1 ? ((argv[1][0] - '0') % 2) : 0;
118 
119  if (benchmark_id==0)
120  benchmark0(benchmark_name, setup, runtime_limit, memory_limit, run_count);
121  else
122  benchmark1(benchmark_name, setup, runtime_limit, memory_limit, run_count);
123 
124  // create the benchmark object and add all the planners we'd like to run
125  tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
126  tools::Benchmark b(setup, benchmark_name);
127 
128  // optionally set pre & pos run events
129  b.setPreRunEvent([](const base::PlannerPtr& planner) { preRunEvent (planner); });
130  b.setPostRunEvent([](const base::PlannerPtr& planner, tools::Benchmark::RunProperties& run)
131  { postRunEvent(planner, run); });
132 
133  b.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
134  b.addPlanner(std::make_shared<geometric::RRT>(setup.getSpaceInformation()));
135  b.addPlanner(std::make_shared<geometric::BKPIECE1>(setup.getSpaceInformation()));
136  b.addPlanner(std::make_shared<geometric::LBKPIECE1>(setup.getSpaceInformation()));
137  b.addPlanner(std::make_shared<geometric::KPIECE1>(setup.getSpaceInformation()));
138  b.addPlanner(std::make_shared<geometric::SBL>(setup.getSpaceInformation()));
139  b.addPlanner(std::make_shared<geometric::EST>(setup.getSpaceInformation()));
140  b.addPlanner(std::make_shared<geometric::PRM>(setup.getSpaceInformation()));
141 
142  int sampler_id = argc > 2 ? ((argv[2][0] - '0') % 4) : -1;
143 
144  if (sampler_id == 0 || sampler_id < 0)
145  {
146  // run all planners with a uniform valid state sampler on the benchmark problem
147  setup.getSpaceInformation()->setValidStateSamplerAllocator(
149  {
150  return std::make_shared<base::UniformValidStateSampler>(si);
151  });
152  b.setExperimentName(benchmark_name + "_uniform_sampler");
153  b.benchmark(request);
154  b.saveResultsToFile();
155  }
156 
157  if (sampler_id == 1 || sampler_id < 0)
158  {
159  // run all planners with a Gaussian valid state sampler on the benchmark problem
160  setup.getSpaceInformation()->setValidStateSamplerAllocator(
162  {
163  return std::make_shared<base::GaussianValidStateSampler>(si);
164  });
165  b.setExperimentName(benchmark_name + "_gaussian_sampler");
166  b.benchmark(request);
167  b.saveResultsToFile();
168  }
169 
170  if (sampler_id == 2 || sampler_id < 0)
171  {
172  // run all planners with a obstacle-based valid state sampler on the benchmark problem
173  setup.getSpaceInformation()->setValidStateSamplerAllocator(
175  {
176  return std::make_shared<base::ObstacleBasedValidStateSampler>(si);
177  });
178  b.setExperimentName(benchmark_name + "_obstaclebased_sampler");
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  {
189  auto vss = std::make_shared<base::MaximizeClearanceValidStateSampler>(si);
190  vss->setNrImproveAttempts(5);
191  return vss;
192  });
193  b.setExperimentName(benchmark_name + "_maxclearance_sampler");
194  b.benchmark(request);
195  b.saveResultsToFile();
196  }
197 
198  return 0;
199 }
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
A shared pointer wrapper for ompl::base::ValidStateSampler.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:87
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
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
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.
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.
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