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  {
52  {
53  public:
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 
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 
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 setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:200
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...
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
State sampler for the Rn state space.
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-...
std::vector< std::string > dimensionNames_
Optional names for individual dimensions.
A shared pointer wrapper for ompl::base::StateSampler.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
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...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
State * allocState() const override
Allocate a state that can store a point in the described space.
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...
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...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:195
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
unsigned int dimension_
The dimension of the space.
void freeState(State *state) const override
Free the memory of the allocated state.
void sampleUniform(State *state) override
Sample a state.
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.
std::map< std::string, unsigned int > dimensionIndex_
Map from names to index values for dimensions.
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...
RealVectorBounds bounds_
The bounds of the space (used for sampling)
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
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:70
Definition of an abstract state.
Definition: State.h:49
double operator[](unsigned int i) const
Access element i of values. This does not check whether the index is within bounds.
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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...
The lower and upper bounds for an Rn space.
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 deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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:64
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
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, near[i]+distance]. If this interval exceeds the state space bounds, the interval is truncated.
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.
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...