CForestStateSpaceWrapper.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, 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 /* Authors: Mark Moll */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_CFOREST_CFORESTSTATESPACEWRAPPER_
38 #define OMPL_GEOMETRIC_PLANNERS_CFOREST_CFORESTSTATESPACEWRAPPER_
39 
40 #include "ompl/geometric/planners/cforest/CForestStateSampler.h"
41 #include "ompl/base/StateSpace.h"
42 #include "ompl/base/Planner.h"
43 
44 namespace ompl
45 {
46  namespace geometric
47  {
48  class CForest;
49  }
50 
51  namespace base
52  {
56  {
57  public:
59  : cforest_(cforest), space_(space), planner_(nullptr)
60  {
61  setName(space->getName() + "CForestWrapper");
62  }
63 
64  ~CForestStateSpaceWrapper() override = default;
65 
66  void setPlanner(base::Planner *planner)
67  {
68  planner_ = planner;
69  }
70 
71  const base::Planner *getPlanner() const
72  {
73  return planner_;
74  }
75 
76  geometric::CForest *getCForestInstance() const
77  {
78  return cforest_;
79  }
80 
81  StateSamplerPtr allocDefaultStateSampler() const override;
82 
83  StateSamplerPtr allocStateSampler() const override;
84 
85  void setup() override;
86 
87  bool isCompound() const override
88  {
89  return space_->isCompound();
90  }
91  bool isDiscrete() const override
92  {
93  return space_->isDiscrete();
94  }
95  bool isHybrid() const override
96  {
97  return space_->isHybrid();
98  }
99  bool isMetricSpace() const override
100  {
101  return space_->isMetricSpace();
102  }
103  bool hasSymmetricDistance() const override
104  {
105  return space_->hasSymmetricDistance();
106  }
107  bool hasSymmetricInterpolate() const override
108  {
109  return space_->hasSymmetricInterpolate();
110  }
111  double getLongestValidSegmentFraction() const override
112  {
113  return space_->getLongestValidSegmentFraction();
114  }
115  void setLongestValidSegmentFraction(double segmentFraction) override
116  {
117  space_->setLongestValidSegmentFraction(segmentFraction);
118  }
119  unsigned int validSegmentCount(const State *state1, const State *state2) const override
120  {
121  return space_->validSegmentCount(state1, state2);
122  }
123  unsigned int getDimension() const override
124  {
125  return space_->getDimension();
126  }
127  double getMaximumExtent() const override
128  {
129  return space_->getMaximumExtent();
130  }
131  double getMeasure() const override
132  {
133  return space_->getMeasure();
134  }
135  void enforceBounds(State *state) const override
136  {
137  space_->enforceBounds(state);
138  }
139  bool satisfiesBounds(const State *state) const override
140  {
141  return space_->satisfiesBounds(state);
142  }
143  void copyState(State *destination, const State *source) const override
144  {
145  space_->copyState(destination, source);
146  }
147  double distance(const State *state1, const State *state2) const override
148  {
149  return space_->distance(state1, state2);
150  }
151  unsigned int getSerializationLength() const override
152  {
153  return space_->getSerializationLength();
154  }
155  void serialize(void *serialization, const State *state) const override
156  {
157  space_->serialize(serialization, state);
158  }
159  void deserialize(State *state, const void *serialization) const override
160  {
161  space_->deserialize(state, serialization);
162  }
163  bool equalStates(const State *state1, const State *state2) const override
164  {
165  return space_->equalStates(state1, state2);
166  }
167  void interpolate(const State *from, const State *to, const double t, State *state) const override
168  {
169  space_->interpolate(from, to, t, state);
170  }
171  State *allocState() const override
172  {
173  return space_->allocState();
174  }
175  void freeState(State *state) const override
176  {
177  space_->freeState(state);
178  }
179  double *getValueAddressAtIndex(State *state, const unsigned int index) const override
180  {
181  return space_->getValueAddressAtIndex(state, index);
182  }
183  void registerProjections() override
184  {
185  space_->registerProjections();
186  }
187  void printState(const State *state, std::ostream &out) const override
188  {
189  space_->printState(state, out);
190  }
191  void printSettings(std::ostream &out) const override
192  {
193  space_->printSettings(out);
194  }
195  void printProjections(std::ostream &out) const override
196  {
197  space_->printProjections(out);
198  }
199  void sanityChecks(double zero, double eps, unsigned int flags) const override
200  {
201  space_->sanityChecks(zero, eps, flags);
202  }
203  void sanityChecks() const override
204  {
205  space_->sanityChecks();
206  }
207  StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
208  {
209  return space_->allocSubspaceStateSampler(subspace);
210  }
211  void computeLocations() override
212  {
213  space_->computeLocations();
214  }
215 
216  protected:
217  geometric::CForest *cforest_;
218  StateSpace *space_;
219  Planner *planner_;
220  };
221  }
222 }
223 
224 #endif
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
State * allocState() const override
Allocate a state that can store a point in the described space.
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 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 getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
bool isDiscrete() const override
Check if the set of states is discrete.
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
void interpolate(const State *from, const State *to, const 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...
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...
A shared pointer wrapper for ompl::base::StateSampler.
void freeState(State *state) const override
Free the memory of the allocated state.
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
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...
double * getValueAddressAtIndex(State *state, const unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
State space wrapper to use together with CForest. It adds some functionalities to the regular state s...
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings() ...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:209
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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-...
Base class for a planner.
Definition: Planner.h:232
bool isCompound() const override
Check if the state space is compound.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
Definition of an abstract state.
Definition: State.h:49
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
Coupled Forest of Random Engrafting Search Trees.
Definition: CForest.h:75
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 getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
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.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...