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
108Configuration *Head::getConfiguration() const
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
138Configuration *Head::getTargetConfiguration() const
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
friend std::ostream & operator<<(std::ostream &, const Head &)
Pipe head to stream.
Definition Head.cpp:275
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.
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 sampling based planning routines shared by both planning under geometric cons...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.