Loading...
Searching...
No Matches
PropositionalTriangularDecomposition.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: Matt Maly */
36
37#include "ompl/extensions/triangle/PropositionalTriangularDecomposition.h"
38#include "ompl/extensions/triangle/TriangularDecomposition.h"
39#include "ompl/control/planners/ltl/PropositionalDecomposition.h"
40#include "ompl/control/planners/ltl/World.h"
41#include "ompl/util/RandomNumbers.h"
42#include "ompl/base/State.h"
43#include "ompl/base/StateSampler.h"
44#include "ompl/base/spaces/RealVectorBounds.h"
45#include <ostream>
46#include <set>
47#include <vector>
48
49namespace ob = ompl::base;
50namespace oc = ompl::control;
51
52namespace
53{
54 /* PropositionalTriangularDecomposition creates a WrapperDecomposition under-the-hood
55 (which represents a triangulation over the given bounds that uses
56 PropositionalTriangularDecomposition's user-defined project and sample methods)
57 and hands it upward to the PropositionalDecomposition superclass constructor. */
58 class WrapperDecomposition : public oc::TriangularDecomposition
59 {
60 public:
61 using Polygon = TriangularDecomposition::Polygon;
62 using Vertex = TriangularDecomposition::Vertex;
63 WrapperDecomposition(const oc::Decomposition *decomp, const ob::RealVectorBounds &bounds,
64 const std::vector<Polygon> &holes, const std::vector<Polygon> &props);
65 ~WrapperDecomposition() override = default;
66 void project(const ob::State *s, std::vector<double> &coord) const override;
67 void sampleFromRegion(int rid, ompl::RNG &rng, std::vector<double> &coord) const override;
68 void sampleFullState(const ob::StateSamplerPtr &sampler, const std::vector<double> &coord,
69 ob::State *s) const override;
70
71 protected:
72 const oc::Decomposition *decomp_;
73 };
74}
75
76oc::PropositionalTriangularDecomposition::PropositionalTriangularDecomposition(const ob::RealVectorBounds &bounds,
77 const std::vector<Polygon> &holes,
78 const std::vector<Polygon> &props)
79 : PropositionalDecomposition(std::make_shared<WrapperDecomposition>(this, bounds, holes, props))
80 , triDecomp_(static_cast<TriangularDecomposition *>(decomp_.get()))
81{
82}
83
84int oc::PropositionalTriangularDecomposition::getNumProps() const
85{
86 return triDecomp_->getNumRegionsOfInterest();
87}
88
89oc::World oc::PropositionalTriangularDecomposition::worldAtRegion(int triID)
90{
91 int numProps = getNumProps();
92 World world(numProps);
93 for (int p = 0; p < numProps; ++p)
94 world[p] = false;
95 if (triID == -1)
96 return world;
97 int prop = triDecomp_->getRegionOfInterestAt(triID);
98 if (prop >= 0)
99 world[prop] = true;
100 return world;
101}
102
103void oc::PropositionalTriangularDecomposition::setup()
104{
105 triDecomp_->setup();
106}
107
108void oc::PropositionalTriangularDecomposition::addHole(const Polygon &hole)
109{
110 triDecomp_->addHole(hole);
111}
112
113void oc::PropositionalTriangularDecomposition::addProposition(const Polygon &prop)
114{
115 triDecomp_->addRegionOfInterest(prop);
116}
117
118const std::vector<oc::PropositionalTriangularDecomposition::Polygon> &
119oc::PropositionalTriangularDecomposition::getHoles() const
120{
121 return triDecomp_->getHoles();
122}
123
124const std::vector<oc::PropositionalTriangularDecomposition::Polygon> &
125oc::PropositionalTriangularDecomposition::getPropositions() const
126{
127 return triDecomp_->getAreasOfInterest();
128}
129
130void oc::PropositionalTriangularDecomposition::print(std::ostream &out) const
131{
132 triDecomp_->print(out);
133}
134
135namespace
136{
137 WrapperDecomposition::WrapperDecomposition(const oc::Decomposition *decomp, const ob::RealVectorBounds &bounds,
138 const std::vector<Polygon> &holes, const std::vector<Polygon> &props)
139 : oc::TriangularDecomposition(bounds, holes, props), decomp_(decomp)
140 {
141 }
142
143 void WrapperDecomposition::project(const ob::State *s, std::vector<double> &coord) const
144 {
145 decomp_->project(s, coord);
146 }
147
148 void WrapperDecomposition::sampleFromRegion(int rid, ompl::RNG &rng, std::vector<double> &coord) const
149 {
150 decomp_->sampleFromRegion(rid, rng, coord);
151 }
152
153 void WrapperDecomposition::sampleFullState(const ob::StateSamplerPtr &sampler, const std::vector<double> &coord,
154 ob::State *s) const
155 {
156 decomp_->sampleFullState(sampler, coord, s);
157 }
158}
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
The lower and upper bounds for an Rn space.
Definition of an abstract state.
Definition: State.h:50
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
Definition: Decomposition.h:63
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition.
A propositional decomposition wraps a given Decomposition with a region-to-proposition assignment ope...
A TriangularDecomposition is a triangulation that ignores obstacles.
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition: World.h:72
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:45
STL namespace.