All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RealVectorStateSpace.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Ioan Sucan */
36 
37 #ifndef OMPL_BASE_SPACES_REAL_VECTOR_STATE_SPACE_
38 #define OMPL_BASE_SPACES_REAL_VECTOR_STATE_SPACE_
39 
40 #include "ompl/base/StateSpace.h"
41 #include "ompl/base/spaces/RealVectorBounds.h"
42 #include <vector>
43 #include <string>
44 #include <map>
45 
46 namespace ompl
47 {
48  namespace base
49  {
50 
53  {
54  public:
55 
58  {
59  }
60 
61  virtual void sampleUniform(State *state);
66  virtual void sampleUniformNear(State *state, const State *near, const double distance);
71  virtual void sampleGaussian(State *state, const State *mean, const double stdDev);
72  };
73 
76  {
77  public:
78 
80  class StateType : public State
81  {
82  public:
83  StateType(void) : State()
84  {
85  }
86 
89  double operator[](unsigned int i) const
90  {
91  return values[i];
92  }
93 
96  double& operator[](unsigned int i)
97  {
98  return values[i];
99  }
100 
102  double *values;
103  };
104 
107  RealVectorStateSpace(unsigned int dim = 0) : StateSpace(), dimension_(dim), bounds_(dim), stateBytes_(dim * sizeof(double))
108  {
110  setName("RealVector" + getName());
111  dimensionNames_.resize(dim, "");
112  }
113 
114  virtual ~RealVectorStateSpace(void)
115  {
116  }
117 
119  void addDimension(double minBound = 0.0, double maxBound = 0.0);
120 
122  void addDimension(const std::string &name, double minBound = 0.0, double maxBound = 0.0);
123 
126  void setBounds(const RealVectorBounds &bounds);
127 
130  void setBounds(double low, double high);
131 
133  const RealVectorBounds& getBounds(void) const
134  {
135  return bounds_;
136  }
137 
138  virtual unsigned int getDimension(void) const;
139 
142  const std::string& getDimensionName(unsigned int index) const;
143 
145  int getDimensionIndex(const std::string &name) const;
146 
148  void setDimensionName(unsigned int index, const std::string &name);
149 
150  virtual double getMaximumExtent(void) const;
151 
152  virtual void enforceBounds(State *state) const;
153 
154  virtual bool satisfiesBounds(const State *state) const;
155 
156  virtual void copyState(State *destination, const State *source) const;
157 
158  virtual unsigned int getSerializationLength(void) const;
159 
160  virtual void serialize(void *serialization, const State *state) const;
161 
162  virtual void deserialize(State *state, const void *serialization) const;
163 
164  virtual double distance(const State *state1, const State *state2) const;
165 
166  virtual bool equalStates(const State *state1, const State *state2) const;
167 
168  virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
169 
170  virtual StateSamplerPtr allocDefaultStateSampler(void) const;
171 
172  virtual State* allocState(void) const;
173 
174  virtual void freeState(State *state) const;
175 
176  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
177 
178  virtual void printState(const State *state, std::ostream &out) const;
179 
180  virtual void printSettings(std::ostream &out) const;
181 
182  virtual void registerProjections(void);
183 
184  virtual void setup(void);
185 
186  protected:
187 
189  unsigned int dimension_;
190 
193 
195  std::vector<std::string> dimensionNames_;
196 
198  std::map<std::string, unsigned int> dimensionIndex_;
199 
200  private:
201 
203  std::size_t stateBytes_;
204 
205  };
206  }
207 }
208 
209 #endif
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:192
double operator[](unsigned int i) const
Access element i of values. This does not check whether the index is within bounds.
int type_
A type assigned for this state space.
Definition: StateSpace.h:497
State sampler for the Rn state space.
virtual void setup(void)
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 bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
std::vector< std::string > dimensionNames_
Optional names for individual dimensions.
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
A boost shared pointer wrapper for ompl::base::StateSampler.
const std::string & getDimensionName(unsigned int index) const
Each dimension can optionally have a name associated to it. If it does, this function returns that na...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state such that each component state[i] is uniformly sampled from [near[i]-distance, near[i]+distance]. If this interval exceeds the state space bounds, the interval is truncated.
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual void freeState(State *state) const
Free the memory of the allocated state.
unsigned int dimension_
The dimension of the space.
double & operator[](unsigned int i)
Access element i of values. This does not check whether the index is within bounds.
RealVectorStateSampler(const StateSpace *space)
Constructor.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
const std::string & getName(void) const
Get the name of the state space.
Definition: StateSpace.cpp:187
std::map< std::string, unsigned int > dimensionIndex_
Map from names to index values for dimensions.
RealVectorBounds bounds_
The bounds of the space (used for sampling)
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
Definition of an abstract state.
Definition: State.h:50
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
The lower and upper bounds for an Rn space.
ompl::base::RealVectorStateSpace
double * values
The value of the actual vector in Rn
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
RealVectorStateSpace(unsigned int dim=0)
Constructor. The dimension of of the space needs to be specified. A space representing Rdim will be i...
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
void addDimension(double minBound=0.0, double maxBound=0.0)
Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...
Abstract definition of a state space sampler.
Definition: StateSampler.h:66
const RealVectorBounds & getBounds(void) const
Get the bounds for this state space.
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual void sampleUniform(State *state)
Sample a state.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...