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 logDebug("%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(void) 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
00251
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
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(void) 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(void) 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 }