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 
42 using namespace ompl::base;
43 using namespace ompl::multilevel;
44 
45 Projection::Projection(ompl::base::StateSpacePtr bundleSpace, ompl::base::StateSpacePtr baseSpace)
46  : bundleSpace_(bundleSpace), baseSpace_(baseSpace)
47 {
48 }
49 
50 bool Projection::isAdmissible() const
51 {
52  OMPL_WARN("NYI");
53  return false;
54 }
55 
56 unsigned int Projection::getBaseDimension() const
57 {
58  if (baseSpace_)
59  {
60  return baseSpace_->getDimension();
61  }
62  else
63  {
64  return 0;
65  }
66 }
67 
68 unsigned int Projection::getDimension() const
69 {
70  return bundleSpace_->getDimension();
71 }
72 
73 unsigned int Projection::getCoDimension() const
74 {
75  return getDimension() - getBaseDimension();
76 }
77 
78 ompl::base::StateSpacePtr Projection::getBundle() const
79 {
80  return bundleSpace_;
81 }
82 
83 ompl::base::StateSpacePtr Projection::getBase() const
84 {
85  return baseSpace_;
86 }
87 
88 bool Projection::isFibered() const
89 {
90  return false;
91 }
92 
93 ProjectionType Projection::getType() const
94 {
95  return type_;
96 }
97 
98 void Projection::setType(const ProjectionType type)
99 {
100  type_ = type;
101 }
102 
104 {
105  std::string tstr;
106  int type = space->getType();
107  if (type == base::STATE_SPACE_REAL_VECTOR)
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 
153 std::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 
174 std::string Projection::getBaseTypeAsString() const
175 {
176  if (baseSpace_)
177  return stateTypeToString(baseSpace_);
178  else
179  return "None";
180 }
181 
182 std::string Projection::getBundleTypeAsString() const
183 {
184  return stateTypeToString(bundleSpace_);
185 }
186 
187 void Projection::print(std::ostream &out) const
188 {
189  out << getTypeAsString() << std::endl;
190 }
191 
192 namespace 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 
204 CompoundProjection::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 
211 void 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 
230 void 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 
297 void 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 }
Definition of a compound state.
Definition: State.h:150
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition: Cost.cpp:39
@ PROJECTION_COMPOUND
Compound projection.
A space to allow the composition of state spaces.
Definition: StateSpace.h:637
bool isFibered() const override
Check that every compound has an explicit fiber representation.
Definition: Projection.cpp:287
@ PROJECTION_CONSTRAINED_RELAXATION
ompl::multilevel::Projection_Relaxation
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
virtual void print(std::ostream &out) const
Print to stream.
Definition: Projection.cpp:187
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int getDimension() const
Dimension of Bundle Space.
Definition: Projection.cpp:252
virtual void print(std::ostream &out) const override
Print to stream.
Definition: Projection.cpp:297
@ PROJECTION_IDENTITY
ompl::multilevel::Projection_Identity
unsigned int getCoDimension() const
Dimension of Bundle - Dimension of Base.
Definition: Projection.cpp:264
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ STATE_SPACE_TIME
ompl::base::TimeStateSpace
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
unsigned int getBaseDimension() const
Dimension of Base Space.
Definition: Projection.cpp:275
The exception type for ompl.
Definition: Exception.h:78
std::string stateTypeToString(base::StateSpacePtr) const
Return string representing type of ompl::base::StateSpace.
Definition: Projection.cpp:103
Main namespace. Contains everything in this library.