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