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 
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 setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:200
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.
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
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...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
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...
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:379
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.
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:213
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...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:232
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:195
virtual bool hasSymmetricInterpolate() const
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.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
Definition: StateSpace.cpp:383
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-...
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:392
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:387
virtual void interpolate(const State *from, const State *to, double t, State *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
Base class for a planner.
Definition: Planner.h:223
virtual bool isCompound() const
Check if the state space is compound.
virtual bool isDiscrete() const
Check if the set of states is discrete.
bool isCompound() const override
Check if the state space is compound.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:374
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
virtual bool satisfiesBounds(const State *state) const =0
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
virtual double * getValueAddressAtIndex(State *state, unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: StateSpace.cpp:305
virtual void sanityChecks(double zero, double eps, unsigned int flags) const
Perform sanity checks for this state space. Throws an exception if failures are found.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
virtual double getMeasure() const =0
Get a measure of the space (this can be thought of as a generalization of volume) ...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for 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.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
virtual void enforceBounds(State *state) const =0
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
virtual bool isMetricSpace() const
Return true if the distance function associated with the space is a metric.
Definition: StateSpace.h:183
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
Definition: StateSpace.cpp:398
virtual double distance(const State *state1, const State *state2) const =0
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Coupled Forest of Random Engrafting Search Trees.
Definition: CForest.h:75
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
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...
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
double getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
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.
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2...
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual double getMaximumExtent() const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...