Head.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020,
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/pathrestriction/Head.h>
40 #include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>
41 #include <ompl/multilevel/datastructures/projections/FiberedProjection.h>
42 
43 using namespace ompl::multilevel;
44 
45 Head::Head(PathRestriction *restriction, Configuration *xCurrent, Configuration *xTarget)
46 {
47  xCurrent_ = xCurrent;
48  xTarget_ = xTarget;
49 
50  restriction_ = restriction;
51  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
52  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
53 
54  if (graph->getBaseDimension() > 0)
55  {
56  base::SpaceInformationPtr base = graph->getBase();
57  xBaseCurrent_ = base->allocState();
58  graph->project(xCurrent->state, xBaseCurrent_);
59  }
60  if (graph->getCoDimension() > 0)
61  {
62  base::StateSpacePtr fiber = projection->getFiberSpace();
63  xFiberCurrent_ = fiber->allocState();
64  xFiberTarget_ = fiber->allocState();
65  projection->projectFiber(xCurrent->state, xFiberCurrent_);
66  projection->projectFiber(xTarget->state, xFiberTarget_);
67  }
68 }
69 
70 Head::Head(const Head &rhs)
71 {
72  xTarget_ = rhs.getTargetConfiguration();
73  restriction_ = rhs.getRestriction();
74 
75  locationOnBasePath_ = rhs.getLocationOnBasePath();
76  lastValidIndexOnBasePath_ = rhs.getLastValidBasePathIndex();
77 
78  xCurrent_ = rhs.getConfiguration();
79  xFiberCurrent_ = rhs.getStateFiberNonConst();
80  xBaseCurrent_ = rhs.getStateBaseNonConst();
81  xFiberTarget_ = rhs.getStateTargetFiberNonConst();
82  xTarget_ = rhs.getTargetConfiguration();
83 }
84 
85 Head::~Head()
86 {
87  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
88  if (graph->getCoDimension() > 0)
89  {
90  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
91  base::StateSpacePtr fiber = projection->getFiberSpace();
92  fiber->freeState(xFiberCurrent_);
93  fiber->freeState(xFiberTarget_);
94  }
95 
96  if (graph->getBaseDimension() > 0)
97  {
98  base::SpaceInformationPtr base = graph->getBase();
99  base->freeState(xBaseCurrent_);
100  }
101 }
102 
104 {
105  return restriction_;
106 }
107 
109 {
110  return xCurrent_;
111 }
112 
114 {
115  return xCurrent_->state;
116 }
117 
119 {
120  return xFiberCurrent_;
121 }
122 
124 {
125  return xBaseCurrent_;
126 }
127 
129 {
130  return xFiberCurrent_;
131 }
132 
134 {
135  return xBaseCurrent_;
136 }
137 
139 {
140  return xTarget_;
141 }
142 
144 {
145  return xFiberTarget_;
146 }
147 
149 {
150  return xFiberTarget_;
151 }
152 
153 void Head::setCurrent(Configuration *newCurrent, double location)
154 {
155  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
156 
157  xCurrent_ = newCurrent;
158 
159  locationOnBasePath_ = location;
160 
161  lastValidIndexOnBasePath_ = restriction_->getBasePathLastIndexFromLocation(location);
162 
163  if (graph->getBaseDimension() > 0)
164  {
165  base::SpaceInformationPtr base = graph->getBase();
166  graph->project(xCurrent_->state, xBaseCurrent_);
167  }
168  if (graph->getCoDimension() > 0)
169  {
170  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
171  projection->projectFiber(xCurrent_->state, xFiberCurrent_);
172  }
173 }
174 
176 {
177  return lastValidIndexOnBasePath_;
178 }
179 
181 {
182  int Nlast = getRestriction()->size() - 1;
183  if (lastValidIndexOnBasePath_ < Nlast)
184  {
185  return lastValidIndexOnBasePath_ + 1;
186  }
187  else
188  {
189  return Nlast;
190  }
191 }
192 
194 {
195  return locationOnBasePath_;
196 }
197 
199 {
200  //----- | ---------------X-------|---------|
201  // lastValid xCurrent
202  // would result in three (including current head)
203 
204  if (getLocationOnBasePath() >= restriction_->getLengthBasePath())
205  {
206  return 1;
207  }
208 
209  int Nstates = restriction_->getBasePath().size();
210  return std::max(1, Nstates - (lastValidIndexOnBasePath_ + 1));
211 }
212 
214 {
215  locationOnBasePath_ = d;
216 }
217 
218 void Head::setLastValidBasePathIndex(int k)
219 {
220  lastValidIndexOnBasePath_ = k;
221 }
222 
224 {
225  //----- | ---------------X-------|---------
226  // lastValid xCurrent basePath(lastValid + 1)
227  if (k <= 0)
228  {
229  return xBaseCurrent_;
230  }
231  else
232  {
233  int idx = std::min(restriction_->size() - 1, (unsigned int)lastValidIndexOnBasePath_ + k);
234  return restriction_->getBasePath().at(idx);
235  }
236 }
237 
239 {
240  //----- | ---------------X-------|---------
241  // lastValid xCurrent basePath(lastValid + 1)
242 
243  unsigned int idx = lastValidIndexOnBasePath_ + k;
244  if (restriction_->size() < 1)
245  {
246  throw Exception("EmptyRestriction");
247  }
248  if (idx > restriction_->size() - 1)
249  {
250  idx = restriction_->size() - 1;
251  }
252  return idx;
253 }
254 
255 void Head::print(std::ostream &out) const
256 {
257  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
258  base::SpaceInformationPtr bundle = graph->getBundle();
259  base::SpaceInformationPtr base = graph->getBase();
260 
261  out << std::endl << "[ Head at:";
262  int idx = getLastValidBasePathIndex();
263  bundle->printState(xCurrent_->state, out);
264  out << "base location " << getLocationOnBasePath() << "/" << restriction_->getLengthBasePath() << " idx " << idx
265  << "/" << restriction_->size() << std::endl;
266  out << "last base state idx ";
267  base->printState(restriction_->getBasePath().at(idx), out);
268  out << "]";
269 }
270 
271 namespace ompl
272 {
273  namespace multilevel
274  {
275  std::ostream &operator<<(std::ostream &out, const Head &h)
276  {
277  h.print(out);
278  return out;
279  }
280  }
281 }
Representation of path restriction (union of fibers over a base path).
double getLocationOnBasePath() const
Get location in [0,1] on base path to which head points.
Definition: Head.cpp:193
Definition of an abstract state.
Definition: State.h:113
const base::State * getStateBase() const
Get projection of state onto base space.
Definition: Head.cpp:123
base::State * getStateFiberNonConst() const
Get projection of state onto fiber space (non const)
Definition: Head.cpp:128
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
int getLastValidBasePathIndex() const
Get last base path index (before head)
Definition: Head.cpp:175
This namespace contains datastructures and planners to exploit multilevel abstractions,...
A pointer to a specific location on the base path of the path restriction.
Definition: Head.h:131
int getBasePathLastIndexFromLocation(double d)
Given a position d in [0, lengthbasepath_], return the index of the nearest state on base path before...
int getNextValidBasePathIndex() const
Get next base path index (after head)
Definition: Head.cpp:180
Configuration * getTargetConfiguration() const
Get target configuration.
Definition: Head.cpp:138
const base::State * getStateTargetFiber() const
Get target configuration projected onto fiber.
Definition: Head.cpp:143
const base::State * getBaseStateAt(int k) const
Get target configuration projected onto fiber (non const)
Definition: Head.cpp:223
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
A graph on a Bundle-space.
void setCurrent(Configuration *, double)
Setter for current configuration/state.
Definition: Head.cpp:153
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
base::State * getStateBaseNonConst() const
Get projection of state onto base space (non const)
Definition: Head.cpp:133
const base::State * getStateFiber() const
Get projection of state onto fiber space.
Definition: Head.cpp:118
base::State * getStateTargetFiberNonConst() const
Get target configuration projected onto fiber (non const)
Definition: Head.cpp:148
double getLengthBasePath() const
Length of base path.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
unsigned int getBaseDimension() const
Dimension of Base Space.
const base::State * getState() const
Get state to which head points.
Definition: Head.cpp:113
void setLocationOnBasePath(double d)
Set location of head along base path.
Definition: Head.cpp:213
int getNumberOfRemainingStates()
Remaining discrete states starting at head (including head) and relative to the head.
Definition: Head.cpp:198
Configuration * getConfiguration() const
Get state as configuration.
Definition: Head.cpp:108
int getBaseStateIndexAt(int k) const
Get base state at base path index.
Definition: Head.cpp:238
unsigned int size() const
Return number of discrete states in base path.
const std::vector< base::State * > & getBasePath() const
Return discrete states representation of base path.
The exception type for ompl.
Definition: Exception.h:78
PathRestriction * getRestriction() const
Get underlying path restriction.
Definition: Head.cpp:103
Main namespace. Contains everything in this library.