Loading...
Searching...
No Matches
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
50unsigned ndim = 6;
51const 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.
57bool 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
74void 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
82int 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));
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}
Maintain a set of parameters.
Definition: GenericParam.h:226
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
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...
A shared pointer wrapper for ompl::base::Planner.
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
Definition of an abstract state.
Definition: State.h:50
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:49
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
Representation of a benchmark request.
Definition: Benchmark.h:157