00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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);
00053 components_.back()->setName(components_.back()->getName() + body + ":position");
00054
00055 addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight);
00056 components_.back()->setName(components_.back()->getName() + body + ":linvel");
00057
00058 addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight);
00059 components_.back()->setName(components_.back()->getName() + body + ":angvel");
00060
00061 addSubSpace(base::StateSpacePtr(new base::SO3StateSpace()), orientationWeight);
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
00071 base::RealVectorBounds bounds1(3);
00072 bounds1.setLow(-1);
00073 bounds1.setHigh(1);
00074 setLinearVelocityBounds(bounds1);
00075 setAngularVelocityBounds(bounds1);
00076
00077
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
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
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
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];
00174 int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
00175
00176
00177 if (numc)
00178 {
00179
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 }