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