Loading...
Searching...
No Matches
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
44using namespace ompl::multilevel;
45
46PathSection::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
67PathSection::~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
91bool 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
152const ompl::base::State *PathSection::back() const
153{
154 return section_.back();
155}
156
157const 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
250void 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
310void 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
323void 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
386unsigned int PathSection::size() const
387{
388 return section_.size();
389}
390
391void 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
419namespace 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}
The exception type for ompl.
Definition Exception.h:47
Definition of an abstract state.
Definition State.h:50
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Configuration * parent
parent index for {qrrt*}
A graph on a Bundle-space.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
unsigned int getBaseDimension() const
Dimension of Base Space.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
Representation of path restriction (union of fibers over a base path).
double getLengthBasePath() const
Length of base path.
const base::State * getBaseStateAt(int k) const
Return State at index k on base path.
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
double getLengthBasePathUntil(int k)
Cumulative length until base state index k.
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.
Representation of a path section (not necessarily feasible).
Definition PathSection.h:61
bool checkMotion(HeadPtr &)
Checks if section is feasible.
void interpolateL1FiberFirst(HeadPtr &)
Interpolate along restriction using L1 metric.
void interpolateL1FiberLast(HeadPtr &)
Interpolate along restriction using L1 metric (Fiber Last)
base::State * at(int k) const
Methods to access sections like std::vector.
void interpolateL2(HeadPtr &)
Interpolate along restriction using L2 metric.
std::vector< base::State * > section_
Interpolated section along restriction.
void sanityCheck()
checks if section is feasible
std::pair< base::State *, double > lastValid_
Last valid state on feasible segment.
Configuration * addFeasibleSegment(Configuration *xLast, base::State *sNext)
Add vertex for sNext and edge to xLast by assuming motion is valid
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
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,...
Main namespace. Contains everything in this library.