PathSection.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/PathSection.h>
40 #include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>
41 #include <ompl/multilevel/datastructures/pathrestriction/Head.h>
42 #include <ompl/multilevel/datastructures/projections/FiberedProjection.h>
43 
44 using namespace ompl::multilevel;
45 
46 PathSection::PathSection(PathRestriction *restriction) : restriction_(restriction)
47 {
48  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
49  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
50  if (graph->getCoDimension() > 0)
51  {
52  base::StateSpacePtr fiber = projection->getFiberSpace();
53  xFiberStart_ = fiber->allocState();
54  xFiberGoal_ = fiber->allocState();
55  xFiberTmp_ = fiber->allocState();
56  }
57  if (graph->getBaseDimension() > 0)
58  {
59  base::SpaceInformationPtr base = graph->getBase();
60  xBaseTmp_ = base->allocState();
61  }
62  base::SpaceInformationPtr bundle = graph->getBundle();
63  xBundleTmp_ = bundle->allocState();
64  lastValid_.first = bundle->allocState();
65 }
66 
67 PathSection::~PathSection()
68 {
69  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
70  base::SpaceInformationPtr bundle = graph->getBundle();
71 
72  if (graph->getCoDimension() > 0)
73  {
74  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
75  base::StateSpacePtr fiber = projection->getFiberSpace();
76  fiber->freeState(xFiberStart_);
77  fiber->freeState(xFiberGoal_);
78  fiber->freeState(xFiberTmp_);
79  }
80  if (graph->getBaseDimension() > 0)
81  {
82  base::SpaceInformationPtr base = graph->getBase();
83  base->freeState(xBaseTmp_);
84  }
85 
86  bundle->freeStates(section_);
87  bundle->freeState(lastValid_.first);
88  bundle->freeState(xBundleTmp_);
89 }
90 
91 bool PathSection::checkMotion(HeadPtr &head)
92 {
93  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
94 
95  base::SpaceInformationPtr bundle = graph->getBundle();
96  base::SpaceInformationPtr base = graph->getBase();
97 
98  for (unsigned int k = 1; k < section_.size(); k++)
99  {
100  if (bundle->checkMotion(head->getState(), section_.at(k), lastValid_))
101  {
102  if (k < section_.size() - 1)
103  {
104  Configuration *xLast = addFeasibleSegment(head->getConfiguration(), section_.at(k));
105 
106  double locationOnBasePath = restriction_->getLengthBasePathUntil(sectionBaseStateIndices_.at(k));
107 
108  head->setCurrent(xLast, locationOnBasePath);
109  }
110  else
111  {
112  addFeasibleGoalSegment(head->getConfiguration(), head->getTargetConfiguration());
113  return true;
114  }
115  }
116  else
117  {
118  lastValidIndexOnBasePath_ = sectionBaseStateIndices_.at(k - 1);
119 
120  base::State *lastValidBaseState = restriction_->getBasePath().at(lastValidIndexOnBasePath_);
121 
122  graph->project(lastValid_.first, xBaseTmp_);
123 
124  double distBaseSegment = base->distance(lastValidBaseState, xBaseTmp_);
125 
126  double locationOnBasePath =
127  restriction_->getLengthBasePathUntil(lastValidIndexOnBasePath_) + distBaseSegment;
128 
129  //############################################################################
130  // Get Last valid
131  //############################################################################
132  if (lastValid_.second > 0)
133  {
134  // add last valid into the bundle graph
135  Configuration *xBundleLastValid = new Configuration(bundle, lastValid_.first);
136  graph->addConfiguration(xBundleLastValid);
137  graph->addBundleEdge(head->getConfiguration(), xBundleLastValid);
138 
139  head->setCurrent(xBundleLastValid, locationOnBasePath);
140  }
141  return false;
142  }
143  }
144  return false;
145 }
146 
148 {
149  return section_.at(k);
150 }
151 
152 const ompl::base::State *PathSection::back() const
153 {
154  return section_.back();
155 }
156 
157 const ompl::base::State *PathSection::front() const
158 {
159  return section_.front();
160 }
161 
163 {
164  section_.clear();
165  sectionBaseStateIndices_.clear();
166 
167  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
168  base::SpaceInformationPtr base = graph->getBase();
169  base::SpaceInformationPtr bundle = graph->getBundle();
170 
171  int size = head->getNumberOfRemainingStates() + 1;
172 
173  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
174 
175  if (graph->getCoDimension() > 0)
176  {
177  const base::State *xFiberStart = head->getStateFiber();
178  const base::State *xFiberGoal = head->getStateTargetFiber();
179 
180  section_.resize(size + 1);
181 
182  bundle->allocStates(section_);
183 
184  projection->lift(head->getBaseStateAt(0), xFiberStart, section_.front());
185 
186  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(0));
187 
188  for (unsigned int k = 1; k < section_.size(); k++)
189  {
190  projection->lift(head->getBaseStateAt(k - 1), xFiberGoal, section_.at(k));
191  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k - 1));
192  }
193  }
194  else
195  {
196  section_.resize(size);
197 
198  bundle->allocStates(section_);
199 
200  for (int k = 0; k < size; k++)
201  {
202  bundle->copyState(section_.at(k), head->getBaseStateAt(k));
203  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
204  }
205  }
206 }
207 
209 {
210  section_.clear();
211  sectionBaseStateIndices_.clear();
212 
213  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
214  const base::SpaceInformationPtr bundle = graph->getBundle();
215  const base::SpaceInformationPtr base = graph->getBase();
216 
217  int size = head->getNumberOfRemainingStates() + 1;
218 
219  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
220  if (graph->getCoDimension() > 0)
221  {
222  const base::State *xFiberStart = head->getStateFiber();
223  const base::State *xFiberGoal = head->getStateTargetFiber();
224 
225  section_.resize(size + 1);
226 
227  bundle->allocStates(section_);
228 
229  for (int k = 0; k < size; k++)
230  {
231  projection->lift(head->getBaseStateAt(k), xFiberStart, section_.at(k));
232  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
233  }
234  projection->lift(head->getBaseStateAt(size - 1), xFiberGoal, section_.back());
235  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(size - 1));
236  }
237  else
238  {
239  section_.resize(size);
240  bundle->allocStates(section_);
241  for (int k = 0; k < size; k++)
242  {
243  bundle->copyState(section_.at(k), head->getBaseStateAt(k));
244  sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
245  }
246  }
247  sanityCheck(head);
248 }
249 
250 void PathSection::interpolateL2(HeadPtr &head)
251 {
252  section_.clear();
253  sectionBaseStateIndices_.clear();
254 
255  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
256  base::SpaceInformationPtr bundle = graph->getBundle();
257  const std::vector<base::State *> basePath = restriction_->getBasePath();
258 
259  int size = head->getNumberOfRemainingStates() + 1;
260 
261  section_.resize(size);
262  bundle->allocStates(section_);
263 
264  if (graph->getCoDimension() > 0)
265  {
266  const base::State *xFiberStart = head->getStateFiber();
267  const base::State *xFiberGoal = head->getStateTargetFiber();
268 
269  double totalLengthBasePath = restriction_->getLengthBasePath();
270 
271  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
272  base::StateSpacePtr fiber = projection->getFiberSpace();
273 
274  for (unsigned int k = 0; k < restriction_->size(); k++)
275  {
276  double lengthCurrent = restriction_->getLengthBasePathUntil(k);
277  double step = lengthCurrent / totalLengthBasePath;
278 
279  fiber->interpolate(xFiberStart, xFiberGoal, step, xFiberTmp_);
280 
281  projection->lift(restriction_->getBaseStateAt(k), xFiberTmp_, section_.at(k));
282 
283  sectionBaseStateIndices_.push_back(k);
284  }
285  }
286  else
287  {
288  for (unsigned int k = 0; k < basePath.size(); k++)
289  {
290  bundle->copyState(section_.at(k), basePath.at(k));
291  sectionBaseStateIndices_.push_back(k);
292  }
293  }
294 }
295 
297 {
298  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
299 
300  base::SpaceInformationPtr bundle = graph->getBundle();
301 
302  Configuration *x = new Configuration(bundle, sNext);
303  graph->addConfiguration(x);
304  graph->addBundleEdge(xLast, x);
305 
306  x->parent = xLast;
307  return x;
308 }
309 
310 void PathSection::addFeasibleGoalSegment(Configuration *xLast, Configuration *xGoal)
311 {
312  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
313  if (xGoal->index < 0) // graph->getGoalIndex())
314  {
315  graph->addConfiguration(xGoal);
316  graph->addGoalConfiguration(xGoal);
317  }
318  graph->addBundleEdge(xLast, xGoal);
319 
320  xGoal->parent = xLast;
321 }
322 
323 void PathSection::sanityCheck(HeadPtr &head)
324 {
325  if (section_.size() > 0)
326  {
327  base::State *xi = head->getConfiguration()->state;
328  base::State *xg = head->getTargetConfiguration()->state;
329 
330  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
331  base::SpaceInformationPtr bundle = graph->getBundle();
332  base::SpaceInformationPtr base = graph->getBase();
333 
334  double d1 = bundle->distance(section_.front(), xi);
335  double d2 = bundle->distance(section_.back(), xg);
336 
337  if (d1 > 1e-5 || d2 > 1e-5)
338  {
339  std::stringstream buffer;
340  buffer << "START STATE" << std::endl;
341  bundle->printState(xi, buffer);
342  bundle->printState(section_.front(), buffer);
343  buffer << "Distance: " << d1 << std::endl;
344  buffer << "GOAL STATE" << std::endl;
345  bundle->printState(xg, buffer);
346  buffer << "GOAL STATE (SECTION)" << std::endl;
347  bundle->printState(section_.back(), buffer);
348  buffer << "Dist:" << d2 << std::endl;
349  int size = head->getNumberOfRemainingStates();
350  buffer << "Section size: " << section_.size() << std::endl;
351  buffer << "Remaining states: " << size << std::endl;
352  buffer << "Restriction size:" << restriction_->size() << std::endl;
353  buffer << "Base states:" << std::endl;
354  buffer << *restriction_ << std::endl;
355  OMPL_ERROR("Invalid Section: %s", buffer.str().c_str());
356  throw Exception("Invalid Section");
357  }
358  }
359 }
360 
362 {
363  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
364  base::SpaceInformationPtr bundle = graph->getBundle();
365  bool feasible = true;
366  for (unsigned int k = 1; k < section_.size(); k++)
367  {
368  base::State *sk1 = section_.at(k - 1);
369  base::State *sk2 = section_.at(k);
370  if (!bundle->checkMotion(sk1, sk2))
371  {
372  feasible = false;
373  OMPL_ERROR("Error between states %d and %d.", k - 1, k);
374  bundle->printState(sk1);
375  bundle->printState(sk2);
376  }
377  }
378 
379  if (!feasible)
380  {
381  throw Exception("Reported feasible path section, \
382  but path section is infeasible.");
383  }
384 }
385 
386 unsigned int PathSection::size() const
387 {
388  return section_.size();
389 }
390 
391 void PathSection::print(std::ostream &out) const
392 {
393  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
394  base::SpaceInformationPtr bundle = graph->getBundle();
395  base::SpaceInformationPtr base = graph->getBase();
396 
397  out << std::string(80, '-') << std::endl;
398  out << "PATH SECTION" << std::endl;
399  out << std::string(80, '-') << std::endl;
400 
401  out << section_.size() << " states over " << restriction_->size() << " base states." << std::endl;
402 
403  int maxDisplay = 5; // display first and last N elements
404  for (int k = 0; k < (int)section_.size(); k++)
405  {
406  if (k > maxDisplay && k < std::max(0, (int)section_.size() - maxDisplay))
407  continue;
408  int idx = sectionBaseStateIndices_.at(k);
409  out << "State " << k << ": ";
410  bundle->printState(section_.at(k));
411  out << "Over Base state (idx " << idx << ") ";
412  base->printState(restriction_->getBasePath().at(idx));
413  out << std::endl;
414  }
415 
416  out << std::string(80, '-') << std::endl;
417 }
418 
419 namespace ompl
420 {
421  namespace multilevel
422  {
423  std::ostream &operator<<(std::ostream &out, const PathSection &s)
424  {
425  s.print(out);
426  return out;
427  }
428  }
429 }
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition: Cost.cpp:39
base::State * at(int k) const
Methods to access sections like std::vector.
Representation of path restriction (union of fibers over a base path).
void interpolateL1FiberLast(HeadPtr &)
Definition of an abstract state.
Definition: State.h:113
bool checkMotion(HeadPtr &)
Checks if section is feasible.
Definition: PathSection.cpp:91
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
Configuration * addFeasibleSegment(Configuration *xLast, base::State *sNext)
Add vertex for sNext and edge to xLast by assuming motion is valid
This namespace contains datastructures and planners to exploit multilevel abstractions,...
std::pair< base::State *, double > lastValid_
Last valid state on feasible segment.
Definition: PathSection.h:235
void interpolateL1FiberFirst(HeadPtr &)
Interpolate along restriction using L1 metric.
void sanityCheck()
checks if section is feasible
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
A graph on a Bundle-space.
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.
std::vector< base::State * > section_
Interpolated section along restriction.
Definition: PathSection.h:230
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.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
double getLengthBasePathUntil(int k)
Cumulative length until base state index k.
Configuration * parent
parent index for {qrrt*}
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
unsigned int size() const
Return number of discrete states in base path.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
const std::vector< base::State * > & getBasePath() const
Return discrete states representation of base path.
The exception type for ompl.
Definition: Exception.h:78
const base::State * getBaseStateAt(int k) const
Return State at index k on base path.
Representation of a path section (not necessarily feasible).
Definition: PathSection.h:128
Main namespace. Contains everything in this library.