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 #include <ompl/util/String.h>
45 
46 #include <boost/math/constants/constants.hpp>
47 #include <boost/format.hpp>
48 #include <fstream>
49 
50 unsigned ndim = 6;
51 const double edgeWidth = 0.1;
52 
53 // Only states near some edges of a hypercube are valid. The valid edges form a
54 // narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
55 // a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
56 // s[i]>=1-edgewidth.
57 bool isStateValid(const ompl::base::State *state)
58 {
59  const auto *s
60  = static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state);
61  bool foundMaxDim = false;
62 
63  for (int i = ndim - 1; i >= 0; i--)
64  if (!foundMaxDim)
65  {
66  if ((*s)[i] > edgeWidth)
67  foundMaxDim = true;
68  }
69  else if ((*s)[i] < (1. - edgeWidth))
70  return false;
71  return true;
72 }
73 
74 void addPlanner(ompl::tools::Benchmark& benchmark, const ompl::base::PlannerPtr& planner, double range)
75 {
76  ompl::base::ParamSet& params = planner->params();
77  if (params.hasParam(std::string("range")))
78  params.setParam(std::string("range"), ompl::toString(range));
79  benchmark.addPlanner(planner);
80 }
81 
82 int main(int argc, char **argv)
83 {
84  if(argc > 1)
85  ndim = std::stoul(argv[1]);
86 
87  double range = edgeWidth * 0.5;
88  auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
89  ompl::base::RealVectorBounds bounds(ndim);
91  ompl::base::ScopedState<> start(space), goal(space);
92 
93  bounds.setLow(0.);
94  bounds.setHigh(1.);
95  space->setBounds(bounds);
96  ss.setStateValidityChecker(&isStateValid);
97  ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
98  for(unsigned int i = 0; i < ndim; ++i)
99  {
100  start[i] = 0.;
101  goal[i] = 1.;
102  }
103  ss.setStartAndGoalStates(start, goal);
104 
105  // by default, use the Benchmark class
106  double runtime_limit = 1000, memory_limit = 4096;
107  int run_count = 20;
108  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
109  ompl::tools::Benchmark b(ss, "HyperCube");
110  b.addExperimentParameter("num_dims", "INTEGER", std::to_string(ndim));
111 
112  addPlanner(b, std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()), range);
113  addPlanner(b, std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()), range);
114  addPlanner(b, std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()), range);
115  addPlanner(b, std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()), range);
116  addPlanner(b, std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()), range);
117  b.benchmark(request);
118  b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());
119 
120  exit(0);
121 }
Definition of a scoped state.
Definition: ScopedState.h:56
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Maintain a set of parameters.
Definition: GenericParam.h:225
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.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82