Loading...
Searching...
No Matches
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
50const double edgeWidth = 0.1;
51const unsigned int ndim = 10;
52const double runtime_limit = 10;
53const double memory_limit = 4096;
54const 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.
60class HyperCubeValidityChecker : public ompl::base::StateValidityChecker
61{
62public:
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
84protected:
85 int nDim_;
86};
87
88void 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
96ob::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
128int 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}
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
A shared pointer wrapper for ompl::base::SpaceInformation.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
StateValidityChecker(SpaceInformation *si)
Constructor.
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Main namespace. Contains everything in this library.
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