37 #include "ompl/extensions/opende/OpenDEStateSpace.h"
39 #include <boost/lexical_cast.hpp>
44 double positionWeight,
double linVelWeight,
double angVelWeight,
double orientationWeight) :
45 base::CompoundStateSpace(), env_(env)
49 for (
unsigned int i = 0 ; i <
env_->stateBodies_.size() ; ++i)
51 std::string body =
":B" + boost::lexical_cast<std::string>(i);
75 setLinearVelocityBounds(bounds1);
76 setAngularVelocityBounds(bounds1);
79 double mX, mY, mZ, MX, MY, MZ;
80 mX = mY = mZ = std::numeric_limits<double>::infinity();
81 MX = MY = MZ = -std::numeric_limits<double>::infinity();
84 std::queue<dSpaceID> spaces;
85 for (
unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
86 spaces.push(env_->collisionSpaces_[i]);
88 while (!spaces.empty())
90 dSpaceID space = spaces.front();
93 int n = dSpaceGetNumGeoms(space);
95 for (
int j = 0 ; j < n ; ++j)
97 dGeomID geom = dSpaceGetGeom(space, j);
98 if (dGeomIsSpace(geom))
99 spaces.push((dSpaceID)geom);
104 dGeomGetAABB(geom, aabb);
107 for (
int k = 0 ; k < 6 ; ++k)
108 if (fabs(aabb[k]) >= std::numeric_limits<dReal>::max())
116 if (aabb[0] < mX) mX = aabb[0];
117 if (aabb[1] > MX) MX = aabb[1];
118 if (aabb[2] < mY) mY = aabb[2];
119 if (aabb[3] > MY) MY = aabb[3];
120 if (aabb[4] < mZ) mZ = aabb[4];
121 if (aabb[5] > MZ) MZ = aabb[5];
132 double dM = std::max(dx, std::max(dy, dz));
135 dx = dx / 10.0 + dM / 100.0;
136 dy = dy / 10.0 + dM / 100.0;
137 dz = dz / 10.0 + dM / 100.0;
139 bounds1.
low[0] = mX - dx;
140 bounds1.
high[0] = MX + dx;
141 bounds1.
low[1] = mY - dy;
142 bounds1.
high[1] = MY + dy;
143 bounds1.
low[2] = mZ - dz;
144 bounds1.
high[2] = MZ + dz;
146 setVolumeBounds(bounds1);
152 CompoundStateSpace::copyState(destination, source);
165 static void nearCallback(
void *data, dGeomID o1, dGeomID o2)
168 if (reinterpret_cast<CallbackParam*>(data)->collision ==
false)
170 dBodyID b1 = dGeomGetBody(o1);
171 dBodyID b2 = dGeomGetBody(o2);
172 if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
175 int numc = dCollide(o1, o2, 1, &contact[0].geom,
sizeof(dContact));
181 bool valid =
reinterpret_cast<CallbackParam*
>(data)->env->isValidCollision(o1, o2, contact[0]);
182 reinterpret_cast<CallbackParam*
>(data)->collision = !valid;
183 if (reinterpret_cast<CallbackParam*>(data)->env->verboseContacts_)
185 logDebug(
"%s contact between %s and %s", (valid ?
"Valid" :
"Invalid"),
186 reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o1).c_str(),
187 reinterpret_cast<CallbackParam*
>(data)->env->getGeomName(o2).c_str());
198 return state->
as<
StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT);
201 CallbackParam cp = { env_.get(),
false };
202 for (
unsigned int i = 0 ; cp.collision ==
false && i < env_->collisionSpaces_.size() ; ++i)
203 dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
204 env_->mutex_.unlock();
206 state->
as<
StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
207 state->
as<
StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
213 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
215 if (!components_[i]->satisfiesBounds(state->
components[i]))
222 for (
unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
223 components_[i * 4]->as<base::RealVectorStateSpace>()->setBounds(bounds);
228 for (
unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
229 components_[i * 4 + 1]->as<base::RealVectorStateSpace>()->setBounds(bounds);
234 for (
unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
235 components_[i * 4 + 2]->as<base::RealVectorStateSpace>()->setBounds(bounds);
241 allocStateComponents(state);
247 CompoundStateSpace::freeState(state);
254 CompoundStateSpace::interpolate(from, to, t, state);
273 wrapped_->sampleUniform(state);
277 virtual void sampleUniformNear(base::State *state,
const base::State *near,
const double distance)
279 wrapped_->sampleUniformNear(state, near, distance);
283 virtual void sampleGaussian(base::State *state,
const base::State *mean,
const double stdDev)
285 wrapped_->sampleGaussian(state, mean, stdDev);
289 base::StateSamplerPtr wrapped_;
304 if (dynamic_cast<WrapperForOpenDESampler*>(sampler.get()))
313 for (
int i = (
int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
315 unsigned int _i4 = i * 4;
317 const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
318 const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
319 const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
324 for (
int j = 0; j < 3; ++j)
331 const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
345 for (
int i = (
int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
347 unsigned int _i4 = i * 4;
350 dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
353 dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
356 dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]);
364 dBodySetQuaternion(env_->stateBodies_[i], q);