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  {
51  class RealVectorStateSampler : public StateSampler
52  {
53  public:
55  RealVectorStateSampler(const StateSpace *space) : StateSampler(space)
56  {
57  }
58 
59  void sampleUniform(State *state) override;
64  void sampleUniformNear(State *state, const State *near, double distance) override;
69  void sampleGaussian(State *state, const State *mean, double stdDev) override;
70  };
71 
73  class RealVectorStateSpace : public StateSpace
74  {
75  public:
77  class StateType : public State
78  {
79  public:
80  StateType() = default;
81 
84  double operator[](unsigned int i) const
85  {
86  return values[i];
87  }
88 
91  double &operator[](unsigned int i)
92  {
93  return values[i];
94  }
95 
97  double *values;
98  };
99 
102  RealVectorStateSpace(unsigned int dim = 0)
103  : dimension_(dim), bounds_(dim), stateBytes_(dim * sizeof(double))
104  {
106  setName("RealVector" + getName());
107  dimensionNames_.resize(dim, "");
108  }
109 
110  ~RealVectorStateSpace() override = default;
111 
114  void addDimension(double minBound = 0.0, double maxBound = 0.0);
115 
119  void addDimension(const std::string &name, double minBound = 0.0, double maxBound = 0.0);
120 
123  void setBounds(const RealVectorBounds &bounds);
124 
127  void setBounds(double low, double high);
128 
130  const RealVectorBounds &getBounds() const
131  {
132  return bounds_;
133  }
134 
135  unsigned int getDimension() const override;
136 
140  const std::string &getDimensionName(unsigned int index) const;
141 
143  int getDimensionIndex(const std::string &name) const;
144 
146  void setDimensionName(unsigned int index, const std::string &name);
147 
148  double getMaximumExtent() const override;
149 
150  double getMeasure() const override;
151 
152  void enforceBounds(State *state) const override;
153 
154  bool satisfiesBounds(const State *state) const override;
155 
156  void copyState(State *destination, const State *source) const override;
157 
158  unsigned int getSerializationLength() const override;
159 
160  void serialize(void *serialization, const State *state) const override;
161 
162  void deserialize(State *state, const void *serialization) const override;
163 
164  double distance(const State *state1, const State *state2) const override;
165 
166  bool equalStates(const State *state1, const State *state2) const override;
167 
168  void interpolate(const State *from, const State *to, double t, State *state) const override;
169 
170  StateSamplerPtr allocDefaultStateSampler() const override;
171 
172  State *allocState() const override;
173 
174  void freeState(State *state) const override;
175 
176  double *getValueAddressAtIndex(State *state, unsigned int index) const override;
177 
178  void printState(const State *state, std::ostream &out) const override;
179 
180  void printSettings(std::ostream &out) const override;
181 
182  void registerProjections() override;
183 
184  void setup() override;
185 
186  protected:
188  unsigned int dimension_;
189 
192 
194  std::vector<std::string> dimensionNames_;
195 
197  std::map<std::string, unsigned int> dimensionIndex_;
198 
199  private:
201  std::size_t stateBytes_;
202  };
203  }
204 }
205 
206 #endif
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
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-...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
std::vector< std::string > dimensionNames_
Optional names for individual dimensions.
Definition of an abstract state.
Definition: State.h:113
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.
double operator[](unsigned int i) const
Access element i of values. This does not check whether the index is within bounds.
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
double * values
The value of the actual vector in Rn
RealVectorStateSpace(unsigned int dim=0)
Constructor. The dimension of of the space needs to be specified. A space representing Rdim will be i...
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...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
RealVectorBounds bounds_
The bounds of the space (used for sampling)
State * allocState() const override
Allocate a state that can store a point in the described space.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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...
const RealVectorBounds & getBounds() const
Get the bounds for this state 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 sampleUniform(State *state) override
Sample a state.
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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...
void freeState(State *state) const override
Free the memory of the allocated state.
unsigned int dimension_
The dimension of the space.
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....
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 serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
std::map< std::string, unsigned int > dimensionIndex_
Map from names to index values for dimensions.
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
int type_
A type assigned for this state space.
Definition: StateSpace.h:595
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state such that each component state[i] is uniformly sampled from [near[i]-distance,...
RealVectorStateSampler(const StateSpace *space)
Constructor.
A 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...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
Main namespace. Contains everything in this library.
The lower and upper bounds for an Rn space.