All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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(void)
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                     static msg::Interface msg;
00186                     msg.debug((valid ? "Valid" : "Invalid") + std::string(" contact between ") +
00187                               reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o1) + " and " +
00188                               reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o2));
00189                 }
00190             }
00191         }
00192     }
00194 }
00195 
00196 bool ompl::control::OpenDEStateSpace::evaluateCollision(const base::State *state) const
00197 {
00198     if (state->as<StateType>()->collision & (1 << STATE_COLLISION_KNOWN_BIT))
00199         return state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT);
00200     env_->mutex_.lock();
00201     writeState(state);
00202     CallbackParam cp = { env_.get(), false };
00203     for (unsigned int i = 0 ; cp.collision == false && i < env_->collisionSpaces_.size() ; ++i)
00204         dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
00205     env_->mutex_.unlock();
00206     if (cp.collision)
00207         state->as<StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
00208     state->as<StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
00209     return cp.collision;
00210 }
00211 
00212 bool ompl::control::OpenDEStateSpace::satisfiesBoundsExceptRotation(const StateType *state) const
00213 {
00214     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00215         if (i % 4 != 3)
00216             if (!components_[i]->satisfiesBounds(state->components[i]))
00217                 return false;
00218     return true;
00219 }
00220 
00221 void ompl::control::OpenDEStateSpace::setVolumeBounds(const base::RealVectorBounds &bounds)
00222 {
00223     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00224         components_[i * 4]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00225 }
00226 
00227 void ompl::control::OpenDEStateSpace::setLinearVelocityBounds(const base::RealVectorBounds &bounds)
00228 {
00229     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00230         components_[i * 4 + 1]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00231 }
00232 
00233 void ompl::control::OpenDEStateSpace::setAngularVelocityBounds(const base::RealVectorBounds &bounds)
00234 {
00235     for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
00236         components_[i * 4 + 2]->as<base::RealVectorStateSpace>()->setBounds(bounds);
00237 }
00238 
00239 ompl::base::State* ompl::control::OpenDEStateSpace::allocState(void) const
00240 {
00241     StateType *state = new StateType();
00242     allocStateComponents(state);
00243     return state;
00244 }
00245 
00246 void ompl::control::OpenDEStateSpace::freeState(base::State *state) const
00247 {
00248     CompoundStateSpace::freeState(state);
00249 }
00250 
00251 // 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
00252 // is cleared from the resulting state
00253 void ompl::control::OpenDEStateSpace::interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
00254 {
00255     CompoundStateSpace::interpolate(from, to, t, state);
00256     state->as<StateType>()->collision = 0;
00257 }
00258 
00260 namespace ompl
00261 {
00262     namespace control
00263     {
00264         // we need to make sure any collision information is cleared when states are sampled (just in case this ever happens)
00265         class WrapperForOpenDESampler : public ompl::base::StateSampler
00266         {
00267         public:
00268             WrapperForOpenDESampler(const base::StateSpace *space, const base::StateSamplerPtr &wrapped) : base::StateSampler(space), wrapped_(wrapped)
00269             {
00270             }
00271 
00272             virtual void sampleUniform(ompl::base::State *state)
00273             {
00274                 wrapped_->sampleUniform(state);
00275                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00276             }
00277 
00278             virtual void sampleUniformNear(base::State *state, const base::State *near, const double distance)
00279             {
00280                 wrapped_->sampleUniformNear(state, near, distance);
00281                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00282             }
00283 
00284             virtual void sampleGaussian(base::State *state, const base::State *mean, const double stdDev)
00285             {
00286                 wrapped_->sampleGaussian(state, mean, stdDev);
00287                 state->as<OpenDEStateSpace::StateType>()->collision = 0;
00288             }
00289         private:
00290             base::StateSamplerPtr wrapped_;
00291         };
00292     }
00293 }
00295 
00296 ompl::base::StateSamplerPtr ompl::control::OpenDEStateSpace::allocDefaultStateSampler(void) const
00297 {
00298     base::StateSamplerPtr sampler = base::CompoundStateSpace::allocDefaultStateSampler();
00299     return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
00300 }
00301 
00302 ompl::base::StateSamplerPtr ompl::control::OpenDEStateSpace::allocStateSampler(void) const
00303 {
00304     base::StateSamplerPtr sampler = base::CompoundStateSpace::allocStateSampler();
00305     if (dynamic_cast<WrapperForOpenDESampler*>(sampler.get()))
00306         return sampler;
00307     else
00308         return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
00309 }
00310 
00311 void ompl::control::OpenDEStateSpace::readState(base::State *state) const
00312 {
00313     StateType *s = state->as<StateType>();
00314     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00315     {
00316         unsigned int _i4 = i * 4;
00317 
00318         const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
00319         const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
00320         const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
00321         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00322         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00323         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00324 
00325         for (int j = 0; j < 3; ++j)
00326         {
00327             s_pos[j] = pos[j];
00328             s_vel[j] = vel[j];
00329             s_ang[j] = ang[j];
00330         }
00331 
00332         const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
00333             base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00334 
00335         s_rot.w = rot[0];
00336         s_rot.x = rot[1];
00337         s_rot.y = rot[2];
00338         s_rot.z = rot[3];
00339     }
00340     s->collision = 0;
00341 }
00342 
00343 void ompl::control::OpenDEStateSpace::writeState(const base::State *state) const
00344 {
00345     const StateType *s = state->as<StateType>();
00346     for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
00347     {
00348         unsigned int _i4 = i * 4;
00349 
00350         double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00351         dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
00352 
00353         double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00354         dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
00355 
00356         double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
00357         dBodySetAngularVel(env_->stateBodies_[i],  s_ang[0], s_ang[1], s_ang[2]);
00358 
00359             const base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
00360         dQuaternion q;
00361         q[0] = s_rot.w;
00362         q[1] = s_rot.x;
00363         q[2] = s_rot.y;
00364         q[3] = s_rot.z;
00365         dBodySetQuaternion(env_->stateBodies_[i], q);
00366     }
00367 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends