Loading...
Searching...
No Matches
MultiLevelPlanningHyperCubeCommon.h
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 <ompl/base/SpaceInformation.h>
39#include <ompl/tools/benchmark/Benchmark.h>
40
41#include <boost/math/constants/constants.hpp>
42#include <boost/range/irange.hpp>
43#include <boost/range/algorithm_ext/push_back.hpp>
44#include <boost/format.hpp>
45#include <boost/lexical_cast.hpp>
46
47using namespace ompl::base;
48
49const double edgeWidth = 0.1; // original STRIDE paper had edgewidth = 0.1
50
51std::vector<int> getHypercubeAdmissibleProjection(int dim)
52{
53 std::vector<int> discrete;
54 boost::push_back(discrete, boost::irange(2, dim + 1));
55 return discrete;
56}
57
58// Only states near some edges of a hypercube are valid. The valid edges form a
59// narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
60// a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
61// s[i]>=1-edgewidth.
62class HyperCubeValidityChecker : public StateValidityChecker
63{
64public:
65 HyperCubeValidityChecker(const SpaceInformationPtr &si, int dimension)
66 : StateValidityChecker(si), dimension_(dimension)
67 {
68 si->setStateValidityCheckingResolution(0.001);
69 }
70
71 bool isValid(const State *state) const override
72 {
73 const auto *s = state->as<RealVectorStateSpace::StateType>();
74 bool foundMaxDim = false;
75
76 for (int i = dimension_ - 1; i >= 0; i--)
77 if (!foundMaxDim)
78 {
79 if ((*s)[i] > edgeWidth)
80 foundMaxDim = true;
81 }
82 else if ((*s)[i] < (1. - edgeWidth))
83 return false;
84 return true;
85 }
86
87protected:
88 int dimension_;
89};
90
91template <typename T>
92PlannerPtr GetMultiLevelPlanner(std::vector<int> sequenceLinks, SpaceInformationPtr si, std::string name = "Planner")
93{
94 std::vector<SpaceInformationPtr> si_vec;
95
96 for (unsigned int k = 0; k < sequenceLinks.size() - 1; k++)
97 {
98 int links = sequenceLinks.at(k);
99
100 auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(links));
101 ompl::base::RealVectorBounds bounds(links);
102 bounds.setLow(0.);
103 bounds.setHigh(1.);
104 spaceK->setBounds(bounds);
105
106 auto siK = std::make_shared<SpaceInformation>(spaceK);
107 siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, links));
108
109 spaceK->setup();
110 si_vec.push_back(siK);
111 }
112 si_vec.push_back(si);
113
114 auto planner = std::make_shared<T>(si_vec, name);
115
116 return planner;
117}
bool isValid(const State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
A shared pointer wrapper for ompl::base::Planner.
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::base::SpaceInformation.
StateValidityChecker(SpaceInformation *si)
Constructor.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
A shared pointer wrapper for ompl::control::SpaceInformation.
This namespace contains sampling based planning routines shared by both planning under geometric cons...