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  {
55  class CForestStateSpaceWrapper : public StateSpace
56  {
57  public:
58  CForestStateSpaceWrapper(geometric::CForest *cforest, base::StateSpace *space)
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
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
Base class for a planner.
Definition: Planner.h:279
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:375
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:841
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings()
Definition of an abstract state.
Definition: State.h:113
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:384
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....
bool hasSymmetricDistance() const override
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap.
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-...
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in 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.
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: StateSpace.cpp:775
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
virtual bool isMetricSpace() const
Return true if the distance function associated with the space is a metric.
Definition: StateSpace.h:247
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
Definition: StateSpace.cpp:808
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 printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
virtual bool isDiscrete() const
Check if the set of states is discrete.
Definition: StateSpace.cpp:770
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
Definition: StateSpace.cpp:233
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:828
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
bool isDiscrete() const override
Check if the set of states is discrete.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:393
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.
Definition: StateSpace.cpp:609
Coupled Forest of Random Engrafting Search Trees.
Definition: CForest.h:139
State * allocState() const override
Allocate a state that can store a point in the described space.
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:214
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 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...
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
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 copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
virtual bool isCompound() const
Check if the state space is compound.
Definition: StateSpace.cpp:765
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
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...
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...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
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...
virtual double getMaximumExtent() const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
bool hasSymmetricInterpolate() const override
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
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:306
virtual double getMeasure() const =0
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....
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
Definition: StateSpace.cpp:780
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
A shared pointer wrapper for ompl::base::StateSampler.
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings()
Definition: StateSpace.cpp:399
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...
virtual bool hasSymmetricInterpolate() const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
Definition: StateSpace.cpp:785
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.
Definition: StateSpace.cpp:851
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:380
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
bool isCompound() const override
Check if the state space is compound.
Main namespace. Contains everything in this library.
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:388