SO3StateSpace.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_SO3_STATE_SPACE_
38 #define OMPL_BASE_SPACES_SO3_STATE_SPACE_
39 
40 #include "ompl/base/StateSpace.h"
41 
42 namespace ompl
43 {
44  namespace base
45  {
48  {
49  public:
51  SO3StateSampler(const StateSpace *space) : StateSampler(space)
52  {
53  }
54 
55  void sampleUniform(State *state) override;
64  void sampleUniformNear(State *state, const State *near, double distance) override;
75  void sampleGaussian(State *state, const State *mean, double stdDev) override;
76  };
77 
82  class SO3StateSpace : public StateSpace
83  {
84  public:
90  class StateType : public State
91  {
92  public:
94  void setAxisAngle(double ax, double ay, double az, double angle);
95 
97  void setIdentity();
98 
100  double x;
101 
103  double y;
104 
106  double z;
107 
109  double w;
110  };
111 
112  SO3StateSpace()
113  {
114  setName("SO3" + getName());
116  }
117 
118  ~SO3StateSpace() override = default;
119 
121  double norm(const StateType *state) const;
122 
123  unsigned int getDimension() const override;
124 
125  double getMaximumExtent() const override;
126 
127  double getMeasure() const override;
128 
129  void enforceBounds(State *state) const override;
130 
131  bool satisfiesBounds(const State *state) const override;
132 
133  void copyState(State *destination, const State *source) const override;
134 
135  unsigned int getSerializationLength() const override;
136 
137  void serialize(void *serialization, const State *state) const override;
138 
139  void deserialize(State *state, const void *serialization) const override;
140 
141  double distance(const State *state1, const State *state2) const override;
142 
143  bool equalStates(const State *state1, const State *state2) const override;
144 
145  void interpolate(const State *from, const State *to, double t, State *state) const override;
146 
147  StateSamplerPtr allocDefaultStateSampler() const override;
148 
149  State *allocState() const override;
150 
151  void freeState(State *state) const override;
152 
153  double *getValueAddressAtIndex(State *state, unsigned int index) const override;
154 
155  void printState(const State *state, std::ostream &out) const override;
156 
157  void printSettings(std::ostream &out) const override;
158 
159  void registerProjections() override;
160  };
161  }
162 }
163 
164 #endif
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:200
void freeState(State *state) const override
Free the memory of the allocated state.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
ompl::base::SO3StateSpace
A shared pointer wrapper for ompl::base::StateSampler.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
A state space representing SO(3). The internal representation is done with quaternions. The distance between states is the angle between quaternions and interpolation is done with slerp.
Definition: SO3StateSpace.h:82
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
SO3StateSampler(const StateSpace *space)
Constructor.
Definition: SO3StateSpace.h:51
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 sampleUniform(State *state) override
Sample a state.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:195
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 w
scalar component of quaternion
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:90
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-...
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 setIdentity()
Set the state to identity – no rotation.
double norm(const StateType *state) const
Compute the norm of a state.
State space sampler for SO(3), using quaternion representation.
Definition: SO3StateSpace.h:47
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
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...
Definition of an abstract state.
Definition: State.h:49
void setAxisAngle(double ax, double ay, double az, double angle)
Set the quaternion from axis-angle representation. The angle is given in radians. ...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that the expected distance between mean and state is stdDev.
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.
void sampleUniformNear(State *state, const State *near, double distance) override
To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 ta...
double z
Z component of quaternion vector.
Abstract definition of a state space sampler.
Definition: StateSampler.h:64
double y
Y component of quaternion vector.
double x
X component of quaternion vector.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...