37 #include "ompl/extensions/opende/OpenDEStatePropagator.h"
38 #include "ompl/extensions/opende/OpenDEStateSpace.h"
39 #include "ompl/extensions/opende/OpenDEControlSpace.h"
40 #include "ompl/util/Exception.h"
45 if (
OpenDEStateSpace *oss = dynamic_cast<OpenDEStateSpace*>(si->getStateSpace().get()))
46 env_ = oss->getEnvironment();
48 throw Exception(
"OpenDE State Space needed for OpenDEStatePropagator");
61 void nearCallback(
void *data, dGeomID o1, dGeomID o2)
63 dBodyID b1 = dGeomGetBody(o1);
64 dBodyID b2 = dGeomGetBody(o2);
66 if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
68 CallbackParam *cp =
reinterpret_cast<CallbackParam*
>(data);
70 const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
71 if (maxContacts <= 0)
return;
73 dContact *contact =
new dContact[maxContacts];
75 for (
unsigned int i = 0; i < maxContacts; ++i)
76 cp->env->setupContact(o1, o2, contact[i]);
78 if (
int numc = dCollide(o1, o2, maxContacts, &contact[0].geom,
sizeof(dContact)))
80 for (
int i = 0; i < numc; ++i)
82 dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
83 dJointAttach(c, b1, b2);
84 bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
87 if (cp->env->verboseContacts_)
89 OMPL_DEBUG(
"%s contact between %s and %s", (valid ?
"Valid" :
"Invalid"),
90 cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
111 CallbackParam cp = { env_.get(),
false };
112 for (
unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
113 dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
116 dWorldQuickStep(env_->world_, (
const dReal)duration);
119 dJointGroupEmpty(env_->contactGroup_);
124 env_->mutex_.unlock();
Index of bit in StateType::collision indicating whether it is known if a state is in collision or not...
Definition of an abstract control.
Index of bit in StateType::collision indicating whether a state is in collision or not...
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
Model the effect of controls on system states.
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. ...
State space representing OpenDE states.
virtual void propagate(const base::State *state, const Control *control, const double duration, base::State *result) const
Propagate from a state, given a control, for some specified amount of time (the amount of time can al...
The definition of a control in Rn
Definition of an abstract state.
The exception type for ompl.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
OpenDEStatePropagator(const SpaceInformationPtr &si)
Construct a representation of OpenDE state propagator. If si->getStateSpace() does not cast to an Ope...
const T * as(void) const
Cast this instance to a desired type.
virtual bool canPropagateBackward(void) const
Some systems can only propagate forward in time (i.e., the duration argument for the propagate() func...
OpenDEEnvironmentPtr env_
The OpenDE environment this state propagator operates on.
int collision
Flag containing information about state validity.
double * values
An array of length n, representing the value of the control.