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/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);
00054 components_.back()->setName(components_.back()->getName() + body + ":position");
00055
00056 addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight);
00057 components_.back()->setName(components_.back()->getName() + body + ":linvel");
00058
00059 addSubSpace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight);
00060 components_.back()->setName(components_.back()->getName() + body + ":angvel");
00061
00062 addSubSpace(base::StateSpacePtr(new base::SO3StateSpace()), orientationWeight);
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
00072 base::RealVectorBounds bounds1(3);
00073 bounds1.setLow(-1);
00074 bounds1.setHigh(1);
00075 setLinearVelocityBounds(bounds1);
00076 setAngularVelocityBounds(bounds1);
00077
00078
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
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
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
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];
00175 int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
00176
00177
00178 if (numc)
00179 {
00180
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
00252
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
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 }