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 OMPL_DEBUG(
"%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);
void setName(const std::string &name)
Set the name of the state space.
int type_
A type assigned for this state space.
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
std::vector< double > low
Lower bound.
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
A boost shared pointer wrapper for ompl::base::StateSampler.
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
A state space representing SO(3). The internal representation is done with quaternions. The distance between states is the angle between quaternions and interpolation is done with slerp.
CompoundState StateType
Define the type of state allocated by this state space.
virtual void writeState(const base::State *state) const
Set the parameters of the OpenDE bodies to be the ones read from state. The code will technically wor...
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
const T * as(void) const
Cast this instance to a desired type.
This class contains the OpenDE constructs OMPL needs to know about when planning. ...
virtual bool evaluateCollision(const base::State *source) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state...
double w
scalar component of quaternion
void setLow(double value)
Set the lower bound in each dimension to a specific value.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
virtual base::State * allocState(void) const
Allocate a state that can store a point in the described space.
The definition of a state in SO(3) represented as a unit quaternion.
OpenDEStateSpace(const OpenDEEnvironmentPtr &env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
Construct a state space representing OpenDE states.
const std::string & getName(void) const
Get the name of the state space.
std::vector< double > high
Upper bound.
virtual void copyState(base::State *destination, const base::State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
virtual void freeState(base::State *state) const
Free the memory of the allocated state.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
virtual StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual void interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
The lower and upper bounds for an Rn space.
The definition of a state in Rn
virtual base::StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
State ** components
The components that make up a compound state.
void setDefaultBounds(void)
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
Number of state space types; To add new types, use values that are larger than the count...
double z
Z component of quaternion vector.
Abstract definition of a state space sampler.
double y
Y component of quaternion vector.
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
virtual base::StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
int collision
Flag containing information about state validity.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
double x
X component of quaternion vector.
A boost shared pointer wrapper for ompl::control::OpenDEEnvironment.
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.