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/ode/ODEStatePropagator.h"
00038 #include "ompl/extensions/ode/ODEStateSpace.h"
00039 #include "ompl/extensions/ode/ODEControlSpace.h"
00040 #include "ompl/util/Exception.h"
00041 #include "ompl/util/Console.h"
00042
00043 ompl::control::ODEStatePropagator::ODEStatePropagator(const SpaceInformationPtr &si) : StatePropagator(si)
00044 {
00045 if (ODEStateSpace *oss = dynamic_cast<ODEStateSpace*>(si->getStateSpace().get()))
00046 env_ = oss->getEnvironment();
00047 else
00048 throw Exception("ODE State Space needed for ODEStatePropagator");
00049 }
00050
00052 namespace ompl
00053 {
00054
00055 struct CallbackParam
00056 {
00057 const control::ODEEnvironment *env;
00058 bool collision;
00059 };
00060
00061 void nearCallback(void *data, dGeomID o1, dGeomID o2)
00062 {
00063 dBodyID b1 = dGeomGetBody(o1);
00064 dBodyID b2 = dGeomGetBody(o2);
00065
00066 if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
00067
00068 CallbackParam *cp = reinterpret_cast<CallbackParam*>(data);
00069
00070 const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
00071
00072 dContact *contact = (dContact*)alloca(maxContacts * sizeof(dContact));
00073
00074 for (unsigned int i = 0; i < maxContacts; ++i)
00075 cp->env->setupContact(o1, o2, contact[i]);
00076
00077 if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)))
00078 {
00079 for (int i = 0; i < numc; ++i)
00080 {
00081 dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
00082 dJointAttach(c, b1, b2);
00083 bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
00084 if (!valid)
00085 cp->collision = true;
00086 if (cp->env->verboseContacts_)
00087 {
00088 static msg::Interface msg;
00089 msg.debug((valid ? "Valid" : "Invalid") + std::string(" contact between ") +
00090 cp->env->getGeomName(o1) + " and " + cp->env->getGeomName(o2));
00091 }
00092 }
00093 }
00094 }
00095 }
00097
00098 void ompl::control::ODEStatePropagator::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00099 {
00100 env_->mutex_.lock();
00101
00102
00103 si_->getStateSpace()->as<ODEStateSpace>()->writeState(state);
00104
00105
00106 env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);
00107
00108
00109 CallbackParam cp = { env_.get(), false };
00110 for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
00111 dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
00112
00113
00114 dWorldQuickStep(env_->world_, (const dReal)duration);
00115
00116
00117 dJointGroupEmpty(env_->contactGroup_);
00118
00119
00120 si_->getStateSpace()->as<ODEStateSpace>()->readState(result);
00121
00122 env_->mutex_.unlock();
00123
00124
00125 if (!(state->as<ODEStateSpace::StateType>()->collision & (1 << ODEStateSpace::STATE_COLLISION_KNOWN_BIT)))
00126 {
00127 if (cp.collision)
00128 state->as<ODEStateSpace::StateType>()->collision &= (1 << ODEStateSpace::STATE_COLLISION_VALUE_BIT);
00129 state->as<ODEStateSpace::StateType>()->collision &= (1 << ODEStateSpace::STATE_COLLISION_KNOWN_BIT);
00130 }
00131 }
00132
00133 bool ompl::control::ODEStatePropagator::canPropagateBackward(void) const
00134 {
00135 return false;
00136 }