ompl/extensions/opende/src/OpenDEStateSpace.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/opende/OpenDEStateSpace.h"
00038 #include "ompl/util/Console.h"
00039 #include <boost/lexical_cast.hpp>
00040 #include <limits>
00041 #include <queue>
00042 
00043 ompl::control::OpenDEStateSpace::OpenDEStateSpace(const OpenDEEnvironmentPtr &env,
00044                                                   double positionWeight, double linVelWeight, double angVelWeight, double orientationWeight) :
00045     base::CompoundStateSpace(), env_(env)
00046 {
00047     setName("OpenDE" + getName());
00048     type_ = base::STATE_SPACE_TYPE_COUNT + 1;
00049     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00050     {
00051         std::string body = ":B" + boost::lexical_cast<std::string>(i);
00052 
00053         addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), positionWeight); // position
00054         components_.back()->setName(components_.back()->getName() + body + ":position");
00055 
00056         addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight);   // linear velocity
00057         components_.back()->setName(components_.back()->getName() + body + ":linvel");
00058 
00059         addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight);   // angular velocity
00060         components_.back()->setName(components_.back()->getName() + body + ":angvel");
00061 
00062         addSubspace(base::StateSpacePtr(new base::SO3StateSpace()), orientationWeight);      // orientation
00063         components_.back()->setName(components_.back()->getName() + body + ":orientation");
00064     }
00065     lock();
00066     setDefaultBounds();
00067 }
00068 
00069 void ompl::control::OpenDEStateSpace::setDefaultBounds()
00070 {
00071     // limit all velocities to 1 m/s, 1 rad/s, respectively
00072     base::RealVectorBounds bounds1(3);
00073     bounds1.setLow(-1);
00074     bounds1.setHigh(1);
00075     setLinearVelocityBounds(bounds1);
00076     setAngularVelocityBounds(bounds1);
00077 
00078     // find the bounding box that contains all geoms included in the collision spaces
00079     double mX, mY, mZ, MX, MY, MZ;
00080     mX = mY = mZ = std::numeric_limits<double>::infinity();
00081     MX = MY = MZ = -std::numeric_limits<double>::infinity();
00082     bool found = false;
00083 
00084     std::queue<dSpaceID> spaces;
00085     for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
00086         spaces.push(env_->collisionSpaces_[i]);
00087 
00088     while (!spaces.empty())
00089     {
00090         dSpaceID space = spaces.front();
00091         spaces.pop();
00092 
00093         int n = dSpaceGetNumGeoms(space);
00094 
00095         for (int j = 0 ; j < n ; ++j)
00096         {
00097             dGeomID geom = dSpaceGetGeom(space, j);
00098             if (dGeomIsSpace(geom))
00099                 spaces.push((dSpaceID)geom);
00100             else
00101             {
00102                 bool valid = true;
00103                 dReal aabb[6];
00104                 dGeomGetAABB(geom, aabb);
00105 
00106                 // things like planes are infinite; we want to ignore those
00107                 for (int k = 0 ; k < 6 ; ++k)
00108                     if (fabs(aabb[k]) >= std::numeric_limits<dReal>::max())
00109                     {
00110                         valid = false;
00111                         break;
00112                     }
00113                 if (valid)
00114                 {
00115                     found = true;
00116                     if (aabb[0] < mX) mX = aabb[0];
00117                     if (aabb[1] > MX) MX = aabb[1];
00118                     if (aabb[2] < mY) mY = aabb[2];
00119                     if (aabb[3] > MY) MY = aabb[3];
00120                     if (aabb[4] < mZ) mZ = aabb[4];
00121                     if (aabb[5] > MZ) MZ = aabb[5];
00122                 }
00123             }
00124         }
00125     }
00126 
00127     if (found)
00128     {
00129         double dx = MX - mX;
00130         double dy = MY - mY;
00131         double dz = MZ - mZ;
00132         double dM = std::max(dx, std::max(dy, dz));
00133 
00134         // add 10% in each dimension + 1% of the max dimension
00135         dx = dx / 10.0 + dM / 100.0;
00136         dy = dy / 10.0 + dM / 100.0;
00137         dz = dz / 10.0 + dM / 100.0;
00138 
00139         bounds1.low[0] = mX - dx;
00140         bounds1.high[0] = MX + dx;
00141         bounds1.low[1] = mY - dy;
00142         bounds1.high[1] = MY + dy;
00143         bounds1.low[2] = mZ - dz;
00144         bounds1.high[2] = MZ + dz;
00145 
00146         setVolumeBounds(bounds1);
00147     }
00148 }
00149 
00150 void ompl::control::OpenDEStateSpace::copyState(base::State *destination, const base::State *source) const
00151 {
00152     CompoundStateSpace::copyState(destination, source);
00153     destination->as<StateType>()->collision = source->as<StateType>()->collision;
00154 }
00155 
00156 namespace ompl
00157 {
00159     struct CallbackParam
00160     {
00161         const control::OpenDEEnvironment *env;
00162         bool                           collision;
00163     };
00164 
00165     static void nearCallback(void *data, dGeomID o1, dGeomID o2)
00166     {
00167         // if a collision has not already been detected
00168         if (reinterpret_cast<CallbackParam*>(data)->collision == false)
00169         {
00170             dBodyID b1 = dGeomGetBody(o1);
00171             dBodyID b2 = dGeomGetBody(o2);
00172             if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
00173 
00174             dContact contact[1];  // one contact is sufficient
00175             int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
00176 
00177             // check if there is really a collision
00178             if (numc)
00179             {
00180                 // check if the collision is allowed
00181                 bool valid = reinterpret_cast<CallbackParam*>(data)->env->isValidCollision(o1, o2, contact[0]);
00182                 reinterpret_cast<CallbackParam*>(data)->collision = !valid;
00183                 if (reinterpret_cast<CallbackParam*>(data)->env->verboseContacts_)
00184                 {
00185                     OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
00186                              reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o1).c_str(),
00187                              reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o2).c_str());
00188                 }
00189             }
00190         }
00191     }
00193 }
00194 
00195 bool ompl::control::OpenDEStateSpace::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::OpenDEStateSpace::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::OpenDEStateSpace::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::OpenDEStateSpace::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::OpenDEStateSpace::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::OpenDEStateSpace::allocState() const
00239 {
00240     StateType *state = new StateType();
00241     allocStateComponents(state);
00242     return state;
00243 }
00244 
00245 void ompl::control::OpenDEStateSpace::freeState(base::State *state) const
00246 {
00247     CompoundStateSpace::freeState(state);
00248 }
00249 
00250 // this function should most likely not be used with OpenDE propagations, but just in case it is called, we need to make sure the collision information
00251 // is cleared from the resulting state
00252 void ompl::control::OpenDEStateSpace::interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
00253 {
00254     CompoundStateSpace::interpolate(from, to, t, state);
00255     state->as<StateType>()->collision = 0;
00256 }
00257 
00259 namespace ompl
00260 {
00261     namespace control
00262     {
00263         // we need to make sure any collision information is cleared when states are sampled (just in case this ever happens)
00264         class WrapperForOpenDESampler : public ompl::base::StateSampler
00265         {
00266         public:
00267             WrapperForOpenDESampler(const base::StateSpace *space, const base::StateSamplerPtr &wrapped) : base::StateSampler(space), wrapped_(wrapped)
00268             {
00269             }
00270 
00271             virtual void sampleUniform(ompl::base::State *state)
00272             {
00273                 wrapped_->sampleUniform(state);
00274                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00275             }
00276 
00277             virtual void sampleUniformNear(base::State *state, const base::State *near, const double distance)
00278             {
00279                 wrapped_->sampleUniformNear(state, near, distance);
00280                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00281             }
00282 
00283             virtual void sampleGaussian(base::State *state, const base::State *mean, const double stdDev)
00284             {
00285                 wrapped_->sampleGaussian(state, mean, stdDev);
00286                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00287             }
00288         private:
00289             base::StateSamplerPtr wrapped_;
00290         };
00291     }
00292 }
00294 
00295 ompl::base::StateSamplerPtr ompl::control::OpenDEStateSpace::allocDefaultStateSampler() const
00296 {
00297     base::StateSamplerPtr sampler = base::CompoundStateSpace::allocDefaultStateSampler();
00298     return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
00299 }
00300 
00301 ompl::base::StateSamplerPtr ompl::control::OpenDEStateSpace::allocStateSampler() const
00302 {
00303     base::StateSamplerPtr sampler = base::CompoundStateSpace::allocStateSampler();
00304     if (dynamic_cast<WrapperForOpenDESampler*>(sampler.get()))
00305         return sampler;
00306     else
00307         return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
00308 }
00309 
00310 void ompl::control::OpenDEStateSpace::readState(base::State *state) const
00311 {
00312     StateType *s = state->as<StateType>();
00313     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00314     {
00315         unsigned int _i4 = i * 4;
00316 
00317         const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
00318         const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
00319         const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
00320         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00321         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00322         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00323 
00324         for (int j = 0; j < 3; ++j)
00325         {
00326             s_pos[j] = pos[j];
00327             s_vel[j] = vel[j];
00328             s_ang[j] = ang[j];
00329         }
00330 
00331         const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
00332             base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00333 
00334         s_rot.w = rot[0];
00335         s_rot.x = rot[1];
00336         s_rot.y = rot[2];
00337         s_rot.z = rot[3];
00338     }
00339     s->collision = 0;
00340 }
00341 
00342 void ompl::control::OpenDEStateSpace::writeState(const base::State *state) const
00343 {
00344     const StateType *s = state->as<StateType>();
00345     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00346     {
00347         unsigned int _i4 = i * 4;
00348 
00349         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00350         dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
00351 
00352         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00353         dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
00354 
00355         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00356         dBodySetAngularVel(env_->stateBodies_[i],  s_ang[0], s_ang[1], s_ang[2]);
00357 
00358             const base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00359         dQuaternion q;
00360         q[0] = s_rot.w;
00361         q[1] = s_rot.x;
00362         q[2] = s_rot.y;
00363         q[3] = s_rot.z;
00364         dBodySetQuaternion(env_->stateBodies_[i], q);
00365     }
00366 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines