MorseStateSpace.cpp
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/* Authors: Caleb Voss */
36
37#include "ompl/extensions/morse/MorseStateSpace.h"
38#include "ompl/base/spaces/RealVectorStateSpace.h"
39#include "ompl/base/spaces/SO3StateSpace.h"
40#include "ompl/base/spaces/DiscreteStateSpace.h"
41
42ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight, double linVelWeight,
43 double angVelWeight, double orientationWeight)
44 : CompoundStateSpace(), env_(env)
45{
46 setName("Morse" + getName());
48 for (unsigned int i = 0; i < env_->rigidBodies_; ++i)
49 {
50 std::string body = ":B" + std::to_string(i);
51
52 addSubspace(std::make_shared<RealVectorStateSpace>(3), positionWeight); // position
53 components_.back()->setName(components_.back()->getName() + body + ":position");
54
55 addSubspace(std::make_shared<RealVectorStateSpace>(3), linVelWeight); // linear velocity
56 components_.back()->setName(components_.back()->getName() + body + ":linvel");
57
58 addSubspace(std::make_shared<RealVectorStateSpace>(3), angVelWeight); // angular velocity
59 components_.back()->setName(components_.back()->getName() + body + ":angvel");
60
61 addSubspace(std::make_shared<SO3StateSpace>(), orientationWeight); // orientation
62 components_.back()->setName(components_.back()->getName() + body + ":orientation");
63 }
64 // Add the goal region satisfaction flag as a subspace.
65 addSubspace(std::make_shared<DiscreteStateSpace>(0, 1), 0.01);
66 components_.back()->setName(components_.back()->getName() + ":goalRegionSat");
67
68 lock();
69 setBounds();
70}
71
73{
74 RealVectorBounds pbounds(3), lbounds(3), abounds(3);
75 for (unsigned int i = 0; i < 3; i++)
76 {
77 pbounds.low[i] = env_->positionBounds_[2 * i];
78 pbounds.high[i] = env_->positionBounds_[2 * i + 1];
79 lbounds.low[i] = env_->linvelBounds_[2 * i];
80 lbounds.high[i] = env_->linvelBounds_[2 * i + 1];
81 abounds.low[i] = env_->angvelBounds_[2 * i];
82 abounds.high[i] = env_->angvelBounds_[2 * i + 1];
83 }
84 setPositionBounds(pbounds);
85 setLinearVelocityBounds(lbounds);
86 setAngularVelocityBounds(abounds);
87}
88
89void ompl::base::MorseStateSpace::copyState(State *destination, const State *source) const
90{
91 CompoundStateSpace::copyState(destination, source);
92}
93
95{
96 for (unsigned int i = 0; i < componentCount_; ++i)
97 {
98 // for each body, check all bounds except the rotation
99 if (i % 4 != 3)
100 {
101 if (!components_[i]->satisfiesBounds(state->as<CompoundStateSpace::StateType>()->components[i]))
102 return false;
103 }
104 }
105 return true;
106}
107
109{
110 for (unsigned int i = 0; i < env_->rigidBodies_; ++i)
111 components_[i * 4]->as<RealVectorStateSpace>()->setBounds(bounds);
112}
113
115{
116 for (unsigned int i = 0; i < env_->rigidBodies_; ++i)
117 components_[i * 4 + 1]->as<RealVectorStateSpace>()->setBounds(bounds);
118}
119
121{
122 for (unsigned int i = 0; i < env_->rigidBodies_; ++i)
123 components_[i * 4 + 2]->as<RealVectorStateSpace>()->setBounds(bounds);
124}
125
127{
128 StateType *state = new StateType();
129 allocStateComponents(state);
130 return static_cast<State *>(state);
131}
132
134{
136}
137
138// this function should most likely not be used with MORSE propagations
139void ompl::base::MorseStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
140{
141 CompoundStateSpace::interpolate(from, to, t, state);
142}
143
145{
147}
148
150{
152}
153
155{
156 env_->readState(state);
157 for (unsigned int i = 0; i < env_->rigidBodies_ * 4; i += 4)
158 {
159 // for each body, ensure its rotation is normalized
160 SO3StateSpace::StateType *quat = state->as<StateType>()->as<SO3StateSpace::StateType>(i + 3);
161 getSubspace(i + 3)->as<SO3StateSpace>()->enforceBounds(quat);
162 }
163}
164
166{
167 env_->writeState(state);
168}
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
Definition: StateSpace.cpp:871
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
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....
void freeState(State *state) const override
Free the memory of the allocated state.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:735
Definition of a compound state.
Definition: State.h:87
State ** components
The components that make up a compound state.
Definition: State.h:128
A shared pointer wrapper for ompl::base::MorseEnvironment.
MORSE State. This is a compound state that allows accessing the properties of the bodies the state sp...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
bool satisfiesBounds(const State *state) const override
This function checks whether a state satisfies its bounds.
StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
void readState(State *state) const
Read the parameters of the MORSE bodies and store them in state.
void setLinearVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
void writeState(const State *state) const
Set the parameters of the MORSE bodies to be the ones read from state.
void setBounds()
Set the bounds given by the MorseEnvironment.
MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
Construct a state space representing MORSE states.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
MorseEnvironmentPtr env_
Representation of the MORSE parameters OMPL needs to plan.
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....
void freeState(State *state) const override
Free the memory of the allocated state.
void setPositionBounds(const RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
void setAngularVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
State * allocState() const override
Allocate a state that can store a point in the described space.
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
A state space representing SO(3). The internal representation is done with quaternions....
Definition: SO3StateSpace.h:83
A shared pointer wrapper for ompl::base::StateSampler.
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.
Definition: StateSpace.cpp:201
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
@ STATE_SPACE_TYPE_COUNT
Number of state space types; To add new types, use values that are larger than the count.