All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ODEStateSpace.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 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/extensions/ode/ODEStateSpace.h"
00038 #include "ompl/util/Console.h"
00039 #include <boost/lexical_cast.hpp>
00040 #include <limits>
00041 #include <queue>
00042 
00043 ompl::control::ODEStateSpace::ODEStateSpace(const ODEEnvironmentPtr &env,
00044                                                   double positionWeight, double linVelWeight, double angVelWeight, double orientationWeight) :
00045     base::CompoundStateSpace(), env_(env)
00046 {
00047     setName("ODE" + getName());
00048     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00049     {
00050         std::string body = ":B" + boost::lexical_cast<std::string>(i);
00051 
00052         addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), positionWeight); // position
00053         components_.back()->setName(components_.back()->getName() + body + ":position");
00054 
00055         addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight);   // linear velocity
00056         components_.back()->setName(components_.back()->getName() + body + ":linvel");
00057 
00058         addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight);   // angular velocity
00059         components_.back()->setName(components_.back()->getName() + body + ":angvel");
00060 
00061         addSubSpace(base::StateSpacePtr(new base::SO3StateSpace()), orientationWeight);      // orientation
00062         components_.back()->setName(components_.back()->getName() + body + ":orientation");
00063     }
00064     lock();
00065     setDefaultBounds();
00066 }
00067 
00068 void ompl::control::ODEStateSpace::setDefaultBounds(void)
00069 {
00070     // limit all velocities to 1 m/s, 1 rad/s, respectively
00071     base::RealVectorBounds bounds1(3);
00072     bounds1.setLow(-1);
00073     bounds1.setHigh(1);
00074     setLinearVelocityBounds(bounds1);
00075     setAngularVelocityBounds(bounds1);
00076 
00077     // find the bounding box that contains all geoms included in the collision spaces
00078     double mX, mY, mZ, MX, MY, MZ;
00079     mX = mY = mZ = std::numeric_limits<double>::infinity();
00080     MX = MY = MZ = -std::numeric_limits<double>::infinity();
00081     bool found = false;
00082 
00083     std::queue<dSpaceID> spaces;
00084     for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
00085         spaces.push(env_->collisionSpaces_[i]);
00086 
00087     while (!spaces.empty())
00088     {
00089         dSpaceID space = spaces.front();
00090         spaces.pop();
00091 
00092         int n = dSpaceGetNumGeoms(space);
00093 
00094         for (int j = 0 ; j < n ; ++j)
00095         {
00096             dGeomID geom = dSpaceGetGeom(space, j);
00097             if (dGeomIsSpace(geom))
00098                 spaces.push((dSpaceID)geom);
00099             else
00100             {
00101                 bool valid = true;
00102                 dReal aabb[6];
00103                 dGeomGetAABB(geom, aabb);
00104 
00105                 // things like planes are infinite; we want to ignore those
00106                 for (int k = 0 ; k < 6 ; ++k)
00107                     if (fabs(aabb[k]) >= std::numeric_limits<dReal>::max())
00108                     {
00109                         valid = false;
00110                         break;
00111                     }
00112                 if (valid)
00113                 {
00114                     found = true;
00115                     if (aabb[0] < mX) mX = aabb[0];
00116                     if (aabb[1] > MX) MX = aabb[1];
00117                     if (aabb[2] < mY) mY = aabb[2];
00118                     if (aabb[3] > MY) MY = aabb[3];
00119                     if (aabb[4] < mZ) mZ = aabb[4];
00120                     if (aabb[5] > MZ) MZ = aabb[5];
00121                 }
00122             }
00123         }
00124     }
00125 
00126     if (found)
00127     {
00128         double dx = MX - mX;
00129         double dy = MY - mY;
00130         double dz = MZ - mZ;
00131         double dM = std::max(dx, std::max(dy, dz));
00132 
00133         // add 10% in each dimension + 1% of the max dimension
00134         dx = dx / 10.0 + dM / 100.0;
00135         dy = dy / 10.0 + dM / 100.0;
00136         dz = dz / 10.0 + dM / 100.0;
00137 
00138         bounds1.low[0] = mX - dx;
00139         bounds1.high[0] = MX + dx;
00140         bounds1.low[1] = mY - dy;
00141         bounds1.high[1] = MY + dy;
00142         bounds1.low[2] = mZ - dz;
00143         bounds1.high[2] = MZ + dz;
00144 
00145         setVolumeBounds(bounds1);
00146     }
00147 }
00148 
00149 void ompl::control::ODEStateSpace::copyState(base::State *destination, const base::State *source) const
00150 {
00151     CompoundStateSpace::copyState(destination, source);
00152     destination->as<StateType>()->collision = source->as<StateType>()->collision;
00153 }
00154 
00155 namespace ompl
00156 {
00158     struct CallbackParam
00159     {
00160         const control::ODEEnvironment *env;
00161         bool                           collision;
00162     };
00163 
00164     static void nearCallback(void *data, dGeomID o1, dGeomID o2)
00165     {
00166         // if a collision has not already been detected
00167         if (reinterpret_cast<CallbackParam*>(data)->collision == false)
00168         {
00169             dBodyID b1 = dGeomGetBody(o1);
00170             dBodyID b2 = dGeomGetBody(o2);
00171             if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
00172 
00173             dContact contact[1];  // one contact is sufficient
00174             int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
00175 
00176             // check if there is really a collision
00177             if (numc)
00178             {
00179                 // check if the collision is allowed
00180                 bool valid = reinterpret_cast<CallbackParam*>(data)->env->isValidCollision(o1, o2, contact[0]);
00181                 reinterpret_cast<CallbackParam*>(data)->collision = !valid;
00182                 if (reinterpret_cast<CallbackParam*>(data)->env->verboseContacts_)
00183                 {
00184                     static msg::Interface msg;
00185                     msg.debug((valid ? "Valid" : "Invalid") + std::string(" contact between ") +
00186                               reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o1) + " and " +
00187                               reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o2));
00188                 }
00189             }
00190         }
00191     }
00193 }
00194 
00195 bool ompl::control::ODEStateSpace::evaluateCollision(const base::State *state) const
00196 {
00197     if (state->as<StateType>()->collision & (1 << STATE_COLLISION_KNOWN_BIT))
00198         return state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT);
00199     env_->mutex_.lock();
00200     writeState(state);
00201     CallbackParam cp = { env_.get(), false };
00202     for (unsigned int i = 0 ; cp.collision == false && i < env_->collisionSpaces_.size() ; ++i)
00203         dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
00204     env_->mutex_.unlock();
00205     if (cp.collision)
00206         state->as<StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
00207     state->as<StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
00208     return cp.collision;
00209 }
00210 
00211 bool ompl::control::ODEStateSpace::satisfiesBoundsExceptRotation(const StateType *state) const
00212 {
00213     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00214         if (i % 4 != 3)
00215             if (!components_[i]->satisfiesBounds(state->components[i]))
00216                 return false;
00217     return true;
00218 }
00219 
00220 void ompl::control::ODEStateSpace::setVolumeBounds(const base::RealVectorBounds &bounds)
00221 {
00222     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00223         components_[i * 4]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00224 }
00225 
00226 void ompl::control::ODEStateSpace::setLinearVelocityBounds(const base::RealVectorBounds &bounds)
00227 {
00228     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00229         components_[i * 4 + 1]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00230 }
00231 
00232 void ompl::control::ODEStateSpace::setAngularVelocityBounds(const base::RealVectorBounds &bounds)
00233 {
00234     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00235         components_[i * 4 + 2]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00236 }
00237 
00238 ompl::base::State* ompl::control::ODEStateSpace::allocState(void) const
00239 {
00240     StateType *state = new StateType();
00241     allocStateComponents(state);
00242     return state;
00243 }
00244 
00245 void ompl::control::ODEStateSpace::freeState(base::State *state) const
00246 {
00247     CompoundStateSpace::freeState(state);
00248 }
00249 
00250 void ompl::control::ODEStateSpace::readState(base::State *state) const
00251 {
00252     StateType *s = state->as<StateType>();
00253     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00254     {
00255         unsigned int _i4 = i * 4;
00256 
00257         const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
00258         const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
00259         const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
00260         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00261         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00262         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00263 
00264         for (int j = 0; j < 3; ++j)
00265         {
00266             s_pos[j] = pos[j];
00267             s_vel[j] = vel[j];
00268             s_ang[j] = ang[j];
00269         }
00270 
00271         const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
00272             base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00273 
00274         s_rot.w = rot[0];
00275         s_rot.x = rot[1];
00276         s_rot.y = rot[2];
00277         s_rot.z = rot[3];
00278     }
00279 }
00280 
00281 void ompl::control::ODEStateSpace::writeState(const base::State *state) const
00282 {
00283     const StateType *s = state->as<StateType>();
00284     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00285     {
00286         unsigned int _i4 = i * 4;
00287 
00288         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00289         dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
00290 
00291         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00292         dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
00293 
00294         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00295         dBodySetAngularVel(env_->stateBodies_[i],  s_ang[0], s_ang[1], s_ang[2]);
00296 
00297             const base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00298         dQuaternion q;
00299         q[0] = s_rot.w;
00300         q[1] = s_rot.x;
00301         q[2] = s_rot.y;
00302         q[3] = s_rot.z;
00303         dBodySetQuaternion(env_->stateBodies_[i], q);
00304     }
00305 }