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