WrapperStateSpace.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2017, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Zachary Kingston */
36
37#ifndef OMPL_BASE_SPACES_WRAPPER_STATE_SPACE_
38#define OMPL_BASE_SPACES_WRAPPER_STATE_SPACE_
39
40#include <utility>
41
42#include "ompl/base/StateSpace.h"
43
44namespace ompl
45{
46 namespace base
47 {
49
50 OMPL_CLASS_FORWARD(WrapperStateSpace);
52
55 {
56 public:
60 : StateSampler(space), sampler_(std::move(sampler))
61 {
62 }
63
65 void sampleUniform(State *state) override;
66
68 void sampleUniformNear(State *state, const State *near, double distance) override;
69
71 void sampleGaussian(State *state, const State *mean, double stdDev) override;
72
73 protected:
76 };
77
81 {
82 public:
84
85 void setup() override;
86
87 unsigned int getDimension() const override;
88
89 void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
90
91 private:
93 ProjectionEvaluatorPtr projection_;
94 };
95
100 {
101 public:
103 class StateType : public State
104 {
105 public:
107 StateType(State *state) : state_(state)
108 {
109 }
110
112 const State *getState() const
113 {
114 return state_;
115 }
116
119 {
120 return state_;
121 }
122
123 protected:
126 };
127
128 WrapperStateSpace(const StateSpacePtr &space) : space_(space)
129 {
130 }
131
132 bool isCompound() const override
133 {
134 return space_->isCompound();
135 }
136
137 bool isDiscrete() const override
138 {
139 return space_->isDiscrete();
140 }
141
142 bool isHybrid() const override
143 {
144 return space_->isHybrid();
145 }
146
147 bool isMetricSpace() const override
148 {
149 return space_->isMetricSpace();
150 }
151
152 bool hasSymmetricDistance() const override
153 {
154 return space_->hasSymmetricDistance();
155 }
156
157 bool hasSymmetricInterpolate() const override
158 {
159 return space_->hasSymmetricInterpolate();
160 }
161
162 const std::string &getName() const
163 {
164 return space_->getName();
165 }
166
167 void setName(const std::string &name)
168 {
169 space_->setName(name);
170 }
171
172 int getType() const
173 {
174 return space_->getType();
175 }
176
177 bool includes(const StateSpacePtr &other) const
178 {
179 return space_->includes(other);
180 }
181
182 bool includes(const StateSpace *other) const
183 {
184 return space_->includes(other);
185 }
186
187 bool covers(const StateSpacePtr &other) const
188 {
189 return space_->covers(other);
190 }
191
192 bool covers(const StateSpace *other) const
193 {
194 return space_->covers(other);
195 }
196
197 ParamSet &params()
198 {
199 return space_->params();
200 }
201
203 const ParamSet &params() const
204 {
205 return space_->params();
206 }
207
208 double getLongestValidSegmentFraction() const override
209 {
210 return space_->getLongestValidSegmentFraction();
211 }
212
213 void setLongestValidSegmentFraction(double segmentFraction) override
214 {
215 space_->setLongestValidSegmentFraction(segmentFraction);
216 }
217
218 unsigned int validSegmentCount(const State *state1, const State *state2) const override
219 {
220 return space_->validSegmentCount(state1->as<StateType>()->getState(),
221 state2->as<StateType>()->getState());
222 }
223
224 void setValidSegmentCountFactor(unsigned int factor) override
225 {
226 space_->setValidSegmentCountFactor(factor);
227 }
228
229 unsigned int getValidSegmentCountFactor() const override
230 {
231 return space_->getValidSegmentCountFactor();
232 }
233
234 double getLongestValidSegmentLength() const override
235 {
236 return space_->getLongestValidSegmentLength();
237 }
238
239 void computeSignature(std::vector<int> &signature) const override
240 {
241 space_->computeSignature(signature);
242 }
243
244 unsigned int getDimension() const override
245 {
246 return space_->getDimension();
247 }
248
249 double getMaximumExtent() const override
250 {
251 return space_->getMaximumExtent();
252 }
253
254 double getMeasure() const override
255 {
256 return space_->getMeasure();
257 }
258
259 void enforceBounds(State *state) const override
260 {
261 space_->enforceBounds(state->as<StateType>()->getState());
262 }
263
264 bool satisfiesBounds(const State *state) const override
265 {
266 return space_->satisfiesBounds(state->as<StateType>()->getState());
267 }
268
269 void copyState(State *destination, const State *source) const override
270 {
271 space_->copyState(destination->as<StateType>()->getState(), source->as<StateType>()->getState());
272 }
273
274 double distance(const State *state1, const State *state2) const override
275 {
276 return space_->distance(state1->as<StateType>()->getState(), state2->as<StateType>()->getState());
277 }
278
279 unsigned int getSerializationLength() const override
280 {
281 return space_->getSerializationLength();
282 }
283
284 void serialize(void *serialization, const State *state) const override
285 {
286 space_->serialize(serialization, state->as<StateType>()->getState());
287 }
288
289 void deserialize(State *state, const void *serialization) const override
290 {
291 space_->deserialize(state->as<StateType>()->getState(), serialization);
292 }
293
294 bool equalStates(const State *state1, const State *state2) const override
295 {
296 return space_->equalStates(state1->as<StateType>()->getState(), state2->as<StateType>()->getState());
297 }
298
299 void interpolate(const State *from, const State *to, double t, State *state) const override
300 {
301 return space_->interpolate(from->as<StateType>()->getState(), to->as<StateType>()->getState(), t,
302 state->as<StateType>()->getState());
303 }
304
306 {
307 return std::make_shared<WrapperStateSampler>(this, space_->allocDefaultStateSampler());
308 }
309
310 State *allocState() const override
311 {
312 return new StateType(space_->allocState());
313 }
314
315 void freeState(State *state) const override
316 {
317 auto *wstate = state->as<StateType>();
318 space_->freeState(wstate->getState());
319 delete wstate;
320 }
321
322 double *getValueAddressAtIndex(State *state, unsigned int index) const override
323 {
324 return space_->getValueAddressAtIndex(state->as<StateType>()->getState(), index);
325 }
326
327 const double *getValueAddressAtIndex(const State *state, unsigned int index) const
328 {
329 return space_->getValueAddressAtIndex(state->as<StateType>()->getState(), index);
330 }
331
332 const std::vector<ValueLocation> &getValueLocations() const
333 {
334 return space_->getValueLocations();
335 }
336
337 const std::map<std::string, ValueLocation> &getValueLocationsByName() const
338 {
339 return space_->getValueLocationsByName();
340 }
341
342 double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const
343 {
344 return space_->getValueAddressAtLocation(state->as<StateType>()->getState(), loc);
345 }
346
347 const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
348 {
349 return space_->getValueAddressAtLocation(state->as<StateType>()->getState(), loc);
350 }
351
352 double *getValueAddressAtName(State *state, const std::string &name) const
353 {
354 return space_->getValueAddressAtName(state->as<StateType>()->getState(), name);
355 }
356
357 const double *getValueAddressAtName(const State *state, const std::string &name) const
358 {
359 return space_->getValueAddressAtName(state->as<StateType>()->getState(), name);
360 }
361
362 void copyToReals(std::vector<double> &reals, const State *source) const override
363 {
364 space_->copyToReals(reals, source->as<StateType>()->getState());
365 }
366
367 void copyFromReals(State *destination, const std::vector<double> &reals) const override
368 {
369 space_->copyFromReals(destination->as<StateType>()->getState(), reals);
370 }
371
372 void registerProjections() override
373 {
374 space_->registerProjections();
375 }
376
377 void printState(const State *state, std::ostream &out = std::cout) const override
378 {
379 space_->printState(state->as<StateType>()->getState(), out);
380 }
381
382 void printSettings(std::ostream &out) const override
383 {
384 space_->printSettings(out);
385 }
386
387 void printProjections(std::ostream &out) const override
388 {
389 space_->printProjections(out);
390 }
391
392 void sanityChecks(double zero, double eps, unsigned int flags) const override
393 {
394 space_->sanityChecks(zero, eps, flags);
395 }
396
397 void sanityChecks() const override
398 {
399 space_->sanityChecks();
400 }
401
403 {
404 return space_->allocSubspaceStateSampler(subspace);
405 }
406
407 void computeLocations() override
408 {
409 space_->computeLocations();
410 }
411
412 void setup() override;
413
414 const StateSpacePtr &getSpace() const
415 {
416 return space_;
417 }
418
419 protected:
420 const StateSpacePtr space_;
421 };
422 }
423}
424
425#endif
Maintain a set of parameters.
Definition: GenericParam.h:226
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
Definition: StateSampler.h:65
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
StateSpace()
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:84
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A projection evaluator that wraps around another projection evaluator.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void setup() override
Perform configuration steps, if needed.
A state sampler that wraps around another state sampler.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
StateSamplerPtr sampler_
Underlying state sampler.
void sampleUniform(State *state) override
Sample a state using underlying sampler.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
Constructor. Requires the wrapper state space space and the underlying sampler sampler.
Wrapper state type. Contains a reference to an underlying state.
State * getState()
Get a pointer to the underlying state.
StateType(State *state)
Constructor. Takes a reference state to the underlying state.
const State * getState() const
Get a const pointer to the underlying state.
State space wrapper that transparently passes state space operations through to the underlying space....
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void copyToReals(std::vector< double > &reals, const State *source) const override
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
double getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
bool hasSymmetricInterpolate() const override
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
void copyFromReals(State *destination, const std::vector< double > &reals) const override
Copy the values from reals to the state destination using getValueAddressAtLocation()
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
void setValidSegmentCountFactor(unsigned int factor) override
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
const ParamSet & params() const
Get the parameters for this space.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
void sanityChecks(double zero, double eps, unsigned int flags) const override
Perform sanity checks for this state space. Throws an exception if failures are found.
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
unsigned int validSegmentCount(const State *state1, const State *state2) const override
Count how many segments of the "longest valid length" fit on the motion from state1 to state2.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void freeState(State *state) const override
Free the memory of the allocated state.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
bool isDiscrete() const override
Check if the set of states is discrete.
unsigned int getValidSegmentCountFactor() const override
Get the value used to multiply the return value of validSegmentCount().
void printState(const State *state, std::ostream &out=std::cout) const override
Print a state to a stream.
void computeSignature(std::vector< int > &signature) const override
Compute an array of ints that uniquely identifies the structure of the state space....
double getLongestValidSegmentLength() const override
Get the longest valid segment at the time setup() was called.
State * allocState() const override
Allocate a state that can store a point in the described space.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
bool isCompound() const override
Check if the state space is compound.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
bool hasSymmetricDistance() const override
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings()
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Main namespace. Contains everything in this library.
STL namespace.