HypercubeBenchmark.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Bryant Gipson, Mark Moll */
36 
37 #include <ompl/base/spaces/RealVectorStateSpace.h>
38 #include <ompl/geometric/planners/rrt/RRT.h>
39 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
40 #include <ompl/geometric/planners/est/EST.h>
41 #include <ompl/geometric/planners/prm/PRM.h>
42 #include <ompl/geometric/planners/stride/STRIDE.h>
43 #include <ompl/tools/benchmark/Benchmark.h>
44 
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/format.hpp>
47 #include <fstream>
48 
49 unsigned ndim = 6;
50 const double edgeWidth = 0.1;
51 
52 // Only states near some edges of a hypercube are valid. The valid edges form a
53 // narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
54 // a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
55 // s[i]>=1-edgewidth.
56 bool isStateValid(const ompl::base::State *state)
57 {
59  = static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state);
60  bool foundMaxDim = false;
61 
62  for (int i = ndim - 1; i >= 0; i--)
63  if (!foundMaxDim)
64  {
65  if ((*s)[i] > edgeWidth)
66  foundMaxDim = true;
67  }
68  else if ((*s)[i] < (1. - edgeWidth))
69  return false;
70  return true;
71 }
72 
73 void addPlanner(ompl::tools::Benchmark& benchmark, const ompl::base::PlannerPtr& planner, double range)
74 {
75  ompl::base::ParamSet& params = planner->params();
76  if (params.hasParam(std::string("range")))
77  params.setParam(std::string("range"), std::to_string(range));
78  benchmark.addPlanner(planner);
79 }
80 
81 int main(int argc, char **argv)
82 {
83  if(argc > 1)
84  ndim = boost::lexical_cast<size_t>(argv[1]);
85 
86  double range = edgeWidth * 0.5;
87  auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
88  ompl::base::RealVectorBounds bounds(ndim);
90  ompl::base::ScopedState<> start(space), goal(space);
91 
92  bounds.setLow(0.);
93  bounds.setHigh(1.);
94  space->setBounds(bounds);
95  ss.setStateValidityChecker(&isStateValid);
96  ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
97  for(unsigned int i = 0; i < ndim; ++i)
98  {
99  start[i] = 0.;
100  goal[i] = 1.;
101  }
102  ss.setStartAndGoalStates(start, goal);
103 
104  // by default, use the Benchmark class
105  double runtime_limit = 1000, memory_limit = 4096;
106  int run_count = 20;
107  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
108  ompl::tools::Benchmark b(ss, "HyperCube");
109  b.addExperimentParameter("num_dims", "INTEGER", std::to_string(ndim));
110 
111  addPlanner(b, std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()), range);
112  addPlanner(b, std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()), range);
113  addPlanner(b, std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()), range);
114  addPlanner(b, std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()), range);
115  addPlanner(b, std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()), range);
116  b.benchmark(request);
117  b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());
118 
119  exit(0);
120 }
Definition of a scoped state.
Definition: ScopedState.h:56
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
Definition: Benchmark.h:253
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Maintain a set of parameters.
Definition: GenericParam.h:226
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
A shared pointer wrapper for ompl::base::Planner.
bool setParam(const std::string &key, const std::string &value)
Algorithms in OMPL often have parameters that can be set externally. While each algorithm will have t...
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
Definition of an abstract state.
Definition: State.h:49
Representation of a benchmark request.
Definition: Benchmark.h:156
The lower and upper bounds for an Rn space.