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, const double distance) override;
69  void sampleGaussian(State *state, const State *mean, const double stdDev) override;
70  };
71 
74  {
75  public:
77  class StateType : public State
78  {
79  public:
80  StateType() : State()
81  {
82  }
83 
86  double operator[](unsigned int i) const
87  {
88  return values[i];
89  }
90 
93  double &operator[](unsigned int i)
94  {
95  return values[i];
96  }
97 
99  double *values;
100  };
101 
104  RealVectorStateSpace(unsigned int dim = 0)
105  : StateSpace(), dimension_(dim), bounds_(dim), stateBytes_(dim * sizeof(double))
106  {
107  type_ = STATE_SPACE_REAL_VECTOR;
108  setName("RealVector" + getName());
109  dimensionNames_.resize(dim, "");
110  }
111 
112  ~RealVectorStateSpace() override = default;
113 
116  void addDimension(double minBound = 0.0, double maxBound = 0.0);
117 
121  void addDimension(const std::string &name, double minBound = 0.0, double maxBound = 0.0);
122 
125  void setBounds(const RealVectorBounds &bounds);
126 
129  void setBounds(double low, double high);
130 
133  {
134  return bounds_;
135  }
136 
137  unsigned int getDimension() const override;
138 
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  double getMaximumExtent() const override;
151 
152  double getMeasure() const override;
153 
154  void enforceBounds(State *state) const override;
155 
156  bool satisfiesBounds(const State *state) const override;
157 
158  void copyState(State *destination, const State *source) const override;
159 
160  unsigned int getSerializationLength() const override;
161 
162  void serialize(void *serialization, const State *state) const override;
163 
164  void deserialize(State *state, const void *serialization) const override;
165 
166  double distance(const State *state1, const State *state2) const override;
167 
168  bool equalStates(const State *state1, const State *state2) const override;
169 
170  void interpolate(const State *from, const State *to, const double t, State *state) const override;
171 
172  StateSamplerPtr allocDefaultStateSampler() const override;
173 
174  State *allocState() const override;
175 
176  void freeState(State *state) const override;
177 
178  double *getValueAddressAtIndex(State *state, const unsigned int index) const override;
179 
180  void printState(const State *state, std::ostream &out) const override;
181 
182  void printSettings(std::ostream &out) const override;
183 
184  void registerProjections() override;
185 
186  void setup() override;
187 
188  protected:
190  unsigned int dimension_;
191 
194 
196  std::vector<std::string> dimensionNames_;
197 
199  std::map<std::string, unsigned int> dimensionIndex_;
200 
201  private:
203  std::size_t stateBytes_;
204  };
205  }
206 }
207 
208 #endif
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
State sampler for the Rn state space.
void sampleGaussian(State *state, const State *mean, const double stdDev) override
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
std::vector< std::string > dimensionNames_
Optional names for individual dimensions.
A shared pointer wrapper for ompl::base::StateSampler.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int dimension_
The dimension of the space.
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.
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: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.
The lower and upper bounds for an Rn space.
ompl::base::RealVectorStateSpace
double * values
The value of the actual vector in Rn
void sampleUniformNear(State *state, const State *near, const 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.
RealVectorStateSpace(unsigned int dim=0)
Constructor. The dimension of of the space needs to be specified. A space representing Rdim will be i...
Abstract definition of a state space sampler.
Definition: StateSampler.h:64