QuotientSpacePlanningHyperCube.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18  * of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written
20  * permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andreas Orthey */
37 
38 #include "QuotientSpacePlanningCommon.h"
39 #include <ompl/base/spaces/RealVectorStateSpace.h>
40 
41 #include <ompl/geometric/planners/quotientspace/QRRT.h>
42 
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 const double edgeWidth = 0.1;
51 const unsigned int ndim = 10;
52 const double runtime_limit = 10;
53 const double memory_limit = 4096;
54 const int run_count = 10;
55 
56 // Only states near some edges of a hypercube are valid. The valid edges form a
57 // narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
58 // a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
59 // s[i]>=1-edgewidth.
60 class HyperCubeValidityChecker : public ompl::base::StateValidityChecker
61 {
62 public:
63  HyperCubeValidityChecker(const ompl::base::SpaceInformationPtr &si, int nDim)
64  : ompl::base::StateValidityChecker(si), nDim_(nDim)
65  {
66  }
67 
68  bool isValid(const ompl::base::State *state) const override
69  {
70  const auto *s = static_cast<const ompl::base::RealVectorStateSpace::StateType *>(state);
71  bool foundMaxDim = false;
72 
73  for (int i = nDim_ - 1; i >= 0; i--)
74  if (!foundMaxDim)
75  {
76  if ((*s)[i] > edgeWidth)
77  foundMaxDim = true;
78  }
79  else if ((*s)[i] < (1. - edgeWidth))
80  return false;
81  return true;
82  }
83 
84 protected:
85  int nDim_;
86 };
87 
88 void addPlanner(ompl::tools::Benchmark &benchmark, const ompl::base::PlannerPtr &planner, double range)
89 {
90  ompl::base::ParamSet &params = planner->params();
91  if (params.hasParam(std::string("range")))
92  params.setParam(std::string("range"), ompl::toString(range));
93  benchmark.addPlanner(planner);
94 }
95 
96 ob::PlannerPtr GetQRRT(ob::SpaceInformationPtr si, ob::ProblemDefinitionPtr pdef, unsigned int numLinks)
97 {
98  // ompl::msg::setLogLevel(ompl::msg::LOG_DEV2);
99  std::vector<ob::SpaceInformationPtr> si_vec;
100 
101  for (unsigned int k = 2; k < numLinks; k += 2)
102  {
103  OMPL_INFORM("Create QuotientSpace Chain with %d links.", k);
104 
105  auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(k));
107  bounds.setLow(0.);
108  bounds.setHigh(1.);
109  spaceK->setBounds(bounds);
110 
111  auto siK = std::make_shared<ob::SpaceInformation>(spaceK);
112  siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, k));
113  siK->setStateValidityCheckingResolution(0.001);
114 
115  spaceK->setup();
116  si_vec.push_back(siK);
117  }
118  OMPL_INFORM("Add Original Chain with %d links.", numLinks);
119  si_vec.push_back(si);
120 
121  auto planner = std::make_shared<og::QRRT>(si_vec);
122  planner->setProblemDefinition(pdef);
123  std::string qName = "QuotientSpaceRRT[" + std::to_string(si_vec.size()) + "lvl]";
124  planner->setName(qName);
125  return planner;
126 }
127 
128 int main()
129 {
130  double range = edgeWidth * 0.5;
131  auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
132  ompl::base::RealVectorBounds bounds(ndim);
134  ompl::base::ScopedState<> start(space), goal(space);
135 
136  bounds.setLow(0.);
137  bounds.setHigh(1.);
138  space->setBounds(bounds);
139  ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(ss.getSpaceInformation(), ndim));
140  ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
141  for (unsigned int i = 0; i < ndim; ++i)
142  {
143  start[i] = 0.;
144  goal[i] = 1.;
145  }
146  ss.setStartAndGoalStates(start, goal);
147 
148  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
149  ompl::tools::Benchmark b(ss, "HyperCube");
150  b.addExperimentParameter("num_dims", "INTEGER", std::to_string(ndim));
151 
152  ob::SpaceInformationPtr si = ss.getSpaceInformation();
153 
154  ob::PlannerPtr quotientSpacePlanner = GetQRRT(ss.getSpaceInformation(), ss.getProblemDefinition(), ndim);
155  addPlanner(b, quotientSpacePlanner, range);
156 
157  b.benchmark(request);
158  b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());
159 
160  printBenchmarkResults(b);
161 
162  return 0;
163 }
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.
Maintain a set of parameters.
Definition: GenericParam.h:290
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:114
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:127
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
A shared pointer wrapper for ompl::base::Planner.
StateValidityChecker(SpaceInformation *si)
Constructor.
Representation of a benchmark request.
Definition: Benchmark.h:253
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:113
Definition of a scoped state.
Definition: ScopedState.h:121
Abstract definition for a class checking the validity of states. The implementation of this class mus...
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
The lower and upper bounds for an Rn space.