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 }