Loading...
Searching...
No Matches
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
46namespace 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
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
The lower and upper bounds for an Rn space.
State sampler for the Rn state 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...
RealVectorStateSampler(const StateSpace *space)
Constructor.
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,...
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.
double operator[](unsigned int i) const
Access element i of values. This does not check whether the index is within bounds.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
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-...
RealVectorBounds bounds_
The bounds of the space (used for sampling)
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
unsigned int dimension_
The dimension of the space.
void serialize(void *serialization, const State *state) const override
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...
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
RealVectorStateSpace(unsigned int dim=0)
Constructor. The dimension of of the space needs to be specified. A space representing Rdim will be i...
void printSettings(std::ostream &out) const override
Print the settings for this state space 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...
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...
State * allocState() const override
Allocate a state that can store a point in the described 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....
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
std::vector< std::string > dimensionNames_
Optional names for individual dimensions.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient 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...
void freeState(State *state) const override
Free the memory of the allocated state.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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 RealVectorBounds & getBounds() const
Get the bounds for this state space.
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
int type_
A type assigned for this state space.
Definition StateSpace.h:531
void setName(const std::string &name)
Set the name of the state space.
const std::string & getName() const
Get the name of the state space.
Definition of an abstract state.
Definition State.h:50
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
Main namespace. Contains everything in this library.