Loading...
Searching...
No Matches
Projection.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey */
38
39#include <ompl/multilevel/datastructures/Projection.h>
40#include <ompl/util/Exception.h>
41
42using namespace ompl::base;
43using namespace ompl::multilevel;
44
45Projection::Projection(ompl::base::StateSpacePtr bundleSpace, ompl::base::StateSpacePtr baseSpace)
46 : bundleSpace_(bundleSpace), baseSpace_(baseSpace)
47{
48}
49
50bool Projection::isAdmissible() const
51{
52 OMPL_WARN("NYI");
53 return false;
54}
55
56unsigned int Projection::getBaseDimension() const
57{
58 if (baseSpace_)
59 {
60 return baseSpace_->getDimension();
61 }
62 else
63 {
64 return 0;
65 }
66}
67
68unsigned int Projection::getDimension() const
69{
70 return bundleSpace_->getDimension();
71}
72
73unsigned int Projection::getCoDimension() const
74{
75 return getDimension() - getBaseDimension();
76}
77
78ompl::base::StateSpacePtr Projection::getBundle() const
79{
80 return bundleSpace_;
81}
82
83ompl::base::StateSpacePtr Projection::getBase() const
84{
85 return baseSpace_;
86}
87
88bool Projection::isFibered() const
89{
90 return false;
91}
92
93ProjectionType Projection::getType() const
94{
95 return type_;
96}
97
98void Projection::setType(const ProjectionType type)
99{
100 type_ = type;
101}
102
104{
105 std::string tstr;
106 int type = space->getType();
108 {
109 int N = space->getDimension();
110 tstr = "R";
111 tstr += std::to_string(N);
112 }
113 else if (type == base::STATE_SPACE_SE2)
114 {
115 tstr = "SE2";
116 }
117 else if (type == base::STATE_SPACE_SE3)
118 {
119 tstr = "SE3";
120 }
121 else if (type == base::STATE_SPACE_SO2)
122 {
123 tstr = "SO2";
124 }
125 else if (type == base::STATE_SPACE_SO3)
126 {
127 tstr = "SO3";
128 }
129 else if (type == base::STATE_SPACE_TIME)
130 {
131 tstr = "T";
132 }
133 else if (space->isCompound())
134 {
135 base::CompoundStateSpace *space_compound = space->as<base::CompoundStateSpace>();
136 const std::vector<base::StateSpacePtr> space_decomposed = space_compound->getSubspaces();
137
138 for (unsigned int k = 0; k < space_decomposed.size(); k++)
139 {
140 base::StateSpacePtr s0 = space_decomposed.at(k);
141 tstr = tstr + stateTypeToString(s0);
142 if (k < space_decomposed.size() - 1)
143 tstr += "x";
144 }
145 }
146 else
147 {
148 throw Exception("Unknown State Space");
149 }
150 return tstr;
151}
152
153std::string Projection::getTypeAsString() const
154{
155 if (baseSpace_)
156 {
157 std::string tstr = getBundleTypeAsString() + " -> " + getBaseTypeAsString();
159 {
160 tstr += " (relax)";
161 }
162 else if (type_ == PROJECTION_IDENTITY)
163 {
164 tstr += " (id)";
165 }
166 return tstr;
167 }
168 else
169 {
170 return getBundleTypeAsString();
171 }
172}
173
174std::string Projection::getBaseTypeAsString() const
175{
176 if (baseSpace_)
177 return stateTypeToString(baseSpace_);
178 else
179 return "None";
180}
181
182std::string Projection::getBundleTypeAsString() const
183{
184 return stateTypeToString(bundleSpace_);
185}
186
187void Projection::print(std::ostream &out) const
188{
189 out << getTypeAsString() << std::endl;
190}
191
192namespace ompl
193{
194 namespace multilevel
195 {
196 std::ostream &operator<<(std::ostream &out, const Projection &projection)
197 {
198 projection.print(out);
199 return out;
200 }
201 }
202}
203
204CompoundProjection::CompoundProjection(const StateSpacePtr &bundleSpace, const StateSpacePtr &baseSpace,
205 const std::vector<ProjectionPtr> &components)
206 : Projection(bundleSpace, baseSpace), components_(components)
207{
208 setType(PROJECTION_COMPOUND);
209}
210
211void CompoundProjection::lift(const State *xBase, State *xBundle) const
212{
213 unsigned int M = components_.size();
214
215 if (M > 1)
216 {
217 for (unsigned int m = 0; m < M; m++)
218 {
219 const State *xmBase = xBase->as<CompoundState>()->as<State>(m);
220 State *xmBundle = xBundle->as<CompoundState>()->as<State>(m);
221 components_.at(m)->lift(xmBase, xmBundle);
222 }
223 }
224 else
225 {
226 components_.front()->lift(xBase, xBundle);
227 }
228}
229
230void CompoundProjection::project(const State *xBundle, State *xBase) const
231{
232 unsigned int M = components_.size();
233
234 if (M > 1)
235 {
236 for (unsigned int m = 0; m < M; m++)
237 {
238 if (components_.at(m)->getBaseDimension() > 0)
239 {
240 const State *xmBundle = xBundle->as<CompoundState>()->as<State>(m);
241 State *xmBase = xBase->as<CompoundState>()->as<State>(m);
242 components_.at(m)->project(xmBundle, xmBase);
243 }
244 }
245 }
246 else
247 {
248 components_.front()->project(xBundle, xBase);
249 }
250}
251
253{
254 if (components_.size() > 0)
255 {
256 return components_.front()->getDimension();
257 }
258 else
259 {
260 return 0;
261 }
262}
263
265{
266 if (components_.size() > 0)
267 {
268 return components_.front()->getCoDimension();
269 }
270 else
271 {
272 return 0;
273 }
274}
276{
277 if (components_.size() > 0)
278 {
279 return components_.front()->getBaseDimension();
280 }
281 else
282 {
283 return 0;
284 }
285}
286
288{
289 for (unsigned int k = 0; k < components_.size(); k++)
290 {
291 if (!components_.at(k)->isFibered())
292 return false;
293 }
294 return true;
295}
296
297void CompoundProjection::print(std::ostream &out) const
298{
299 for (unsigned int k = 0; k < components_.size(); k++)
300 {
301 out << components_.at(k) << "|";
302 }
303 out << std::endl;
304}
The exception type for ompl.
Definition Exception.h:47
A space to allow the composition of state spaces.
Definition StateSpace.h:574
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition of a compound state.
Definition State.h:87
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
unsigned int getCoDimension() const
Dimension of Bundle - Dimension of Base.
virtual void print(std::ostream &out) const override
Print to stream.
bool isFibered() const override
Check that every compound has an explicit fiber representation.
unsigned int getBaseDimension() const
Dimension of Base Space.
unsigned int getDimension() const
Dimension of Bundle Space.
std::string stateTypeToString(base::StateSpacePtr) const
Return string representing type of ompl::base::StateSpace.
virtual void print(std::ostream &out) const
Print to stream.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ STATE_SPACE_TIME
ompl::base::TimeStateSpace
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition Cost.cpp:39
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_IDENTITY
ompl::multilevel::Projection_Identity
@ PROJECTION_COMPOUND
Compound projection.
@ PROJECTION_CONSTRAINED_RELAXATION
ompl::multilevel::Projection_Relaxation
Main namespace. Contains everything in this library.