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);
00017 components_.back()->setName(components_.back()->getName() + body + ":position");
00018
00019 addSubManifold(base::StateManifoldPtr(new base::RealVectorStateManifold(3)), linVelWeight);
00020 components_.back()->setName(components_.back()->getName() + body + ":linvel");
00021
00022 addSubManifold(base::StateManifoldPtr(new base::RealVectorStateManifold(3)), angVelWeight);
00023 components_.back()->setName(components_.back()->getName() + body + ":angvel");
00024
00025 addSubManifold(base::StateManifoldPtr(new base::SO3StateManifold()), orientationWeight);
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
00035 base::RealVectorBounds bounds1(3);
00036 bounds1.setLow(-1);
00037 bounds1.setHigh(1);
00038 setLinearVelocityBounds(bounds1);
00039 setAngularVelocityBounds(bounds1);
00040
00041
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
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
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
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];
00140 int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
00141
00142
00143 if (numc)
00144 {
00145
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 }