ompl/extensions/morse/src/MorseStateSpace.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Authors: Caleb Voss */
00036 
00037 #include "ompl/extensions/morse/MorseStateSpace.h"
00038 #include "ompl/base/spaces/RealVectorStateSpace.h"
00039 #include "ompl/base/spaces/SO3StateSpace.h"
00040 #include "ompl/base/spaces/DiscreteStateSpace.h"
00041 
00042 #include <boost/lexical_cast.hpp>
00043 
00044 ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight, double linVelWeight,
00045                                              double angVelWeight, double orientationWeight) :
00046     CompoundStateSpace(), env_(env)
00047 {
00048     setName("Morse" + getName());
00049     type_ = STATE_SPACE_TYPE_COUNT + 1;
00050     for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
00051     {
00052         std::string body = ":B" + boost::lexical_cast<std::string>(i);
00053 
00054         addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), positionWeight); // position
00055         components_.back()->setName(components_.back()->getName() + body + ":position");
00056 
00057         addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), linVelWeight);   // linear velocity
00058         components_.back()->setName(components_.back()->getName() + body + ":linvel");
00059 
00060         addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), angVelWeight);   // angular velocity
00061         components_.back()->setName(components_.back()->getName() + body + ":angvel");
00062 
00063         addSubspace(StateSpacePtr(new SO3StateSpace()), orientationWeight);      // orientation
00064         components_.back()->setName(components_.back()->getName() + body + ":orientation");
00065     }
00066     // Add the goal region satisfaction flag as a subspace.
00067     addSubspace(StateSpacePtr(new DiscreteStateSpace(0, 1)), 0.01);
00068     components_.back()->setName(components_.back()->getName() + ":goalRegionSat");
00069     
00070     lock();
00071     setBounds();
00072 }
00073 
00074 void ompl::base::MorseStateSpace::setBounds()
00075 {
00076     RealVectorBounds pbounds(3), lbounds(3), abounds(3);
00077     for (unsigned int i = 0; i < 3; i++)
00078     {
00079         pbounds.low[i] = env_->positionBounds_[2*i];
00080         pbounds.high[i] = env_->positionBounds_[2*i+1];
00081         lbounds.low[i] = env_->linvelBounds_[2*i];
00082         lbounds.high[i] = env_->linvelBounds_[2*i+1];
00083         abounds.low[i] = env_->angvelBounds_[2*i];
00084         abounds.high[i] = env_->angvelBounds_[2*i+1];
00085     }
00086     setPositionBounds(pbounds);
00087     setLinearVelocityBounds(lbounds);
00088     setAngularVelocityBounds(abounds);
00089 }
00090 
00091 void ompl::base::MorseStateSpace::copyState(State *destination, const State *source) const
00092 {
00093     CompoundStateSpace::copyState(destination, source);
00094 }
00095 
00096 bool ompl::base::MorseStateSpace::satisfiesBounds(const State *state) const
00097 {
00098     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00099     {
00100         // for each body, check all bounds except the rotation
00101         if (i % 4 != 3)
00102         {
00103             if (!components_[i]->satisfiesBounds(state->as<CompoundStateSpace::StateType>()->components[i]))
00104                 return false;
00105         }
00106     }
00107     return true;
00108 }
00109 
00110 void ompl::base::MorseStateSpace::setPositionBounds(const RealVectorBounds &bounds)
00111 {
00112     for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
00113         components_[i * 4]->as<RealVectorStateSpace>()->setBounds(bounds);
00114 }
00115 
00116 void ompl::base::MorseStateSpace::setLinearVelocityBounds(const RealVectorBounds &bounds)
00117 {
00118     for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
00119         components_[i * 4 + 1]->as<RealVectorStateSpace>()->setBounds(bounds);
00120 }
00121 
00122 void ompl::base::MorseStateSpace::setAngularVelocityBounds(const RealVectorBounds &bounds)
00123 {
00124     for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
00125         components_[i * 4 + 2]->as<RealVectorStateSpace>()->setBounds(bounds);
00126 }
00127 
00128 ompl::base::State* ompl::base::MorseStateSpace::allocState() const
00129 {
00130     StateType *state = new StateType();
00131     allocStateComponents(state);
00132     return static_cast<State*>(state);
00133 }
00134 
00135 void ompl::base::MorseStateSpace::freeState(State *state) const
00136 {
00137     CompoundStateSpace::freeState(state);
00138 }
00139 
00140 // this function should most likely not be used with MORSE propagations
00141 void ompl::base::MorseStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00142 {
00143     CompoundStateSpace::interpolate(from, to, t, state);
00144 }
00145 
00146 ompl::base::StateSamplerPtr ompl::base::MorseStateSpace::allocDefaultStateSampler() const
00147 {
00148     return CompoundStateSpace::allocDefaultStateSampler();
00149 }
00150 
00151 ompl::base::StateSamplerPtr ompl::base::MorseStateSpace::allocStateSampler() const
00152 {
00153     return CompoundStateSpace::allocDefaultStateSampler();
00154 }
00155 
00156 void ompl::base::MorseStateSpace::readState(State *state) const
00157 {
00158     env_->readState(state);
00159     for (unsigned int i = 0; i < env_->rigidBodies_*4; i+=4)
00160     {
00161         // for each body, ensure its rotation is normalized
00162         SO3StateSpace::StateType *quat = state->as<StateType>()->as<SO3StateSpace::StateType>(i+3);
00163         getSubspace(i+3)->as<SO3StateSpace>()->enforceBounds(quat);
00164     }
00165 }
00166 
00167 void ompl::base::MorseStateSpace::writeState(const State *state) const
00168 {
00169     env_->writeState(state);
00170 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines