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 
54  class WrapperStateSampler : public StateSampler
55  {
56  public:
59  WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
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 
80  class WrapperProjectionEvaluator : public ProjectionEvaluator
81  {
82  public:
83  WrapperProjectionEvaluator(const WrapperStateSpace *space);
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 
99  class WrapperStateSpace : public StateSpace
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:
125  State *state_;
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 
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
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.
bool isDiscrete() const override
Check if the set of states is discrete.
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()
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
Maintain a set of parameters.
Definition: GenericParam.h:289
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
Wrapper state type. Contains a reference to an underlying state.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
StateType(State *state)
Constructor. Takes a reference state to the underlying state.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
Definition of an abstract state.
Definition: State.h:113
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
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...
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...
StateSpace()
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:84
unsigned int getValidSegmentCountFactor() const override
Get the value used to multiply the return value of validSegmentCount().
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void setValidSegmentCountFactor(unsigned int factor) override
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
State * allocState() const override
Allocate a state that can store a point in the described space.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
void copyFromReals(State *destination, const std::vector< double > &reals) const override
Copy the values from reals to the state destination using getValueAddressAtLocation()
bool hasSymmetricInterpolate() const override
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
void setup() override
Perform configuration steps, if needed.
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void computeSignature(std::vector< int > &signature) const override
Compute an array of ints that uniquely identifies the structure of the state space....
WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
Constructor. Requires the wrapper state space space and the underlying sampler sampler.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this 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 sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
void freeState(State *state) const override
Free the memory of the allocated state.
double getLongestValidSegmentLength() const override
Get the longest valid segment at the time setup() was called.
const State * getState() const
Get a const pointer to the underlying state.
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
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...
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings()
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
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....
StateSamplerPtr sampler_
Underlying state sampler.
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 getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
A shared pointer wrapper for ompl::base::StateSpace.
double getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
A shared pointer wrapper for ompl::base::StateSampler.
void printState(const State *state, std::ostream &out=std::cout) const override
Print a state to a stream.
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
bool isCompound() const override
Check if the state space is compound.
void sampleUniform(State *state) override
Sample a state using underlying sampler.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool hasSymmetricDistance() const override
Check if the distance function on this state space is symmetric, i.e. distance(s1,...