Loading...
Searching...
No Matches
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
43using namespace ompl::multilevel;
44
45Head::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
70Head::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
85Head::~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
153void 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
218void 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
255void 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
271namespace 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}
The exception type for ompl.
Definition Exception.h:47
Definition of an abstract state.
Definition State.h:50
A graph on a Bundle-space.
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.
A pointer to a specific location on the base path of the path restriction.
Definition Head.h:64
void setLocationOnBasePath(double d)
Set location of head along base path.
Definition Head.cpp:213
Configuration * getTargetConfiguration() const
Get target configuration.
Definition Head.cpp:138
void setCurrent(Configuration *, double)
Setter for current configuration/state.
Definition Head.cpp:153
int getNextValidBasePathIndex() const
Get next base path index (after head)
Definition Head.cpp:180
int getLastValidBasePathIndex() const
Get last base path index (before head)
Definition Head.cpp:175
const base::State * getState() const
Get state to which head points.
Definition Head.cpp:113
double getLocationOnBasePath() const
Get location in [0,1] on base path to which head points.
Definition Head.cpp:193
base::State * getStateFiberNonConst() const
Get projection of state onto fiber space (non const)
Definition Head.cpp:128
const base::State * getBaseStateAt(int k) const
Get target configuration projected onto fiber (non const)
Definition Head.cpp:223
const base::State * getStateFiber() const
Get projection of state onto fiber space.
Definition Head.cpp:118
int getNumberOfRemainingStates()
Remaining discrete states starting at head (including head) and relative to the head.
Definition Head.cpp:198
base::State * getStateBaseNonConst() const
Get projection of state onto base space (non const)
Definition Head.cpp:133
Configuration * getConfiguration() const
Get state as configuration.
Definition Head.cpp:108
PathRestriction * getRestriction() const
Get underlying path restriction.
Definition Head.cpp:103
int getBaseStateIndexAt(int k) const
Get base state at base path index.
Definition Head.cpp:238
const base::State * getStateTargetFiber() const
Get target configuration projected onto fiber.
Definition Head.cpp:143
base::State * getStateTargetFiberNonConst() const
Get target configuration projected onto fiber (non const)
Definition Head.cpp:148
const base::State * getStateBase() const
Get projection of state onto base space.
Definition Head.cpp:123
Representation of path restriction (union of fibers over a base path).
double getLengthBasePath() const
Length of base path.
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
int getBasePathLastIndexFromLocation(double d)
Given a position d in [0, lengthbasepath_], return the index of the nearest state on base path before...
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.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.