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 }