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 
42 #include <boost/lexical_cast.hpp>
43 
44 ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight, double linVelWeight,
45  double angVelWeight, double orientationWeight) :
46  CompoundStateSpace(), env_(env)
47 {
48  setName("Morse" + getName());
50  for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
51  {
52  std::string body = ":B" + boost::lexical_cast<std::string>(i);
53 
54  addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), positionWeight); // position
55  components_.back()->setName(components_.back()->getName() + body + ":position");
56 
57  addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), linVelWeight); // linear velocity
58  components_.back()->setName(components_.back()->getName() + body + ":linvel");
59 
60  addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), angVelWeight); // angular velocity
61  components_.back()->setName(components_.back()->getName() + body + ":angvel");
62 
63  addSubspace(StateSpacePtr(new SO3StateSpace()), orientationWeight); // orientation
64  components_.back()->setName(components_.back()->getName() + body + ":orientation");
65  }
66  // Add the goal region satisfaction flag as a subspace.
68  components_.back()->setName(components_.back()->getName() + ":goalRegionSat");
69 
70  lock();
71  setBounds();
72 }
73 
75 {
76  RealVectorBounds pbounds(3), lbounds(3), abounds(3);
77  for (unsigned int i = 0; i < 3; i++)
78  {
79  pbounds.low[i] = env_->positionBounds_[2*i];
80  pbounds.high[i] = env_->positionBounds_[2*i+1];
81  lbounds.low[i] = env_->linvelBounds_[2*i];
82  lbounds.high[i] = env_->linvelBounds_[2*i+1];
83  abounds.low[i] = env_->angvelBounds_[2*i];
84  abounds.high[i] = env_->angvelBounds_[2*i+1];
85  }
86  setPositionBounds(pbounds);
87  setLinearVelocityBounds(lbounds);
88  setAngularVelocityBounds(abounds);
89 }
90 
91 void ompl::base::MorseStateSpace::copyState(State *destination, const State *source) const
92 {
93  CompoundStateSpace::copyState(destination, source);
94 }
95 
97 {
98  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
99  {
100  // for each body, check all bounds except the rotation
101  if (i % 4 != 3)
102  {
103  if (!components_[i]->satisfiesBounds(state->as<CompoundStateSpace::StateType>()->components[i]))
104  return false;
105  }
106  }
107  return true;
108 }
109 
111 {
112  for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
113  components_[i * 4]->as<RealVectorStateSpace>()->setBounds(bounds);
114 }
115 
117 {
118  for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
119  components_[i * 4 + 1]->as<RealVectorStateSpace>()->setBounds(bounds);
120 }
121 
123 {
124  for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
125  components_[i * 4 + 2]->as<RealVectorStateSpace>()->setBounds(bounds);
126 }
127 
129 {
130  StateType *state = new StateType();
131  allocStateComponents(state);
132  return static_cast<State*>(state);
133 }
134 
136 {
138 }
139 
140 // this function should most likely not be used with MORSE propagations
141 void ompl::base::MorseStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
142 {
143  CompoundStateSpace::interpolate(from, to, t, state);
144 }
145 
147 {
149 }
150 
152 {
154 }
155 
157 {
158  env_->readState(state);
159  for (unsigned int i = 0; i < env_->rigidBodies_*4; i+=4)
160  {
161  // for each body, ensure its rotation is normalized
162  SO3StateSpace::StateType *quat = state->as<StateType>()->as<SO3StateSpace::StateType>(i+3);
163  getSubspace(i+3)->as<SO3StateSpace>()->enforceBounds(quat);
164  }
165 }
166 
168 {
169  env_->writeState(state);
170 }
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:203
int type_
A type assigned for this state space.
Definition: StateSpace.h:503
MORSE State. This is a compound state that allows accessing the properties of the bodies the state sp...
Definition of a compound state.
Definition: State.h:95
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
std::vector< double > low
Lower bound.
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
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, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
void readState(State *state) const
Read the parameters of the MORSE bodies and store them in state.
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:84
StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
State * allocState() const
Allocate a state that can store a point in the described space.
void freeState(State *state) const
Free the memory of the allocated state.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:94
std::vector< double > high
Upper bound.
A state space representing Rn. The distance function is the L2 norm.
StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
virtual void freeState(State *state) const
Free the memory of the allocated state.
void setBounds()
Set the bounds given by the MorseEnvironment.
Definition of an abstract state.
Definition: State.h:50
void writeState(const State *state) const
Set the parameters of the MORSE bodies to be the ones read from state.
MorseEnvironmentPtr env_
Representation of the MORSE parameters OMPL needs to plan.
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:855
bool satisfiesBounds(const State *state) const
This function checks whether a state satisfies its bounds.
The lower and upper bounds for an Rn space.
State ** components
The components that make up a compound state.
Definition: State.h:142
void setLinearVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
Number of state space types; To add new types, use values that are larger than the count...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:198
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
void setPositionBounds(const RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
A boost shared pointer wrapper for ompl::base::MorseEnvironment.
void copyState(State *destination, const State *source) const
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:707
void setAngularVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
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.