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 
44 namespace 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) : StateSpace(), space_(std::move(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 
402  StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
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
void setValidSegmentCountFactor(unsigned int factor) override
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
bool isDiscrete() const override
Check if the set of states is discrete.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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...
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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.
A state sampler that wraps around another state sampler.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
StateType(State *state)
Constructor. Takes a reference state to the underlying state.
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...
double getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
StateSpace()
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:83
STL namespace.
void copyFromReals(State *destination, const std::vector< double > &reals) const override
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
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() ...
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...
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void freeState(State *state) const override
Free the memory of the allocated state.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
Maintain a set of parameters.
Definition: GenericParam.h:226
void printState(const State *state, std::ostream &out=std::cout) const override
Print a state to a stream.
void sampleUniform(State *state) override
Sample a state using underlying sampler.
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
const State * getState() const
Get a const pointer to the underlying state.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings() ...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
double getLongestValidSegmentLength() const override
Get the longest valid segment at the time setup() was called.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
const ParamSet & params() const
Get the parameters for this space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
State * allocState() const override
Allocate a state that can store a point in the described space.
State space wrapper that transparently passes state space operations through to the underlying space...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
unsigned int getValidSegmentCountFactor() const override
Get the value used to multiply the return value of validSegmentCount().
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
bool hasSymmetricInterpolate() const override
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to, t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.
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 enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
void setup() override
Perform configuration steps, if needed.
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
bool isCompound() const override
Check if the state space is compound.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
Definition of an abstract state.
Definition: State.h:49
A projection evaluator that wraps around another projection evaluator.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
Constructor. Requires the wrapper state space space and the underlying sampler sampler.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Wrapper state type. Contains a reference to an underlying state.
Abstract definition of a state space sampler.
Definition: StateSampler.h:64
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this 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...
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
void computeSignature(std::vector< int > &signature) const override
Compute an array of ints that uniquely identifies the structure of the state space. The first element of the signature is the number of integers that follow.
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
State * getState()
Get a pointer to the underlying state.
StateSamplerPtr sampler_
Underlying state sampler.
bool hasSymmetricDistance() const override
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.