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 logDebug("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
00090 cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
00091 }
00092 }
00093 }
00094
00095 delete[] contact;
00096 }
00097 }
00099
00100 void ompl::control::OpenDEStatePropagator::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00101 {
00102 env_->mutex_.lock();
00103
00104
00105 si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
00106
00107
00108 env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);
00109
00110
00111 CallbackParam cp = { env_.get(), false };
00112 for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
00113 dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
00114
00115
00116 dWorldQuickStep(env_->world_, (const dReal)duration);
00117
00118
00119 dJointGroupEmpty(env_->contactGroup_);
00120
00121
00122 si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result);
00123
00124 env_->mutex_.unlock();
00125
00126
00127 if (!(state->as<OpenDEStateSpace::StateType>()->collision & (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT)))
00128 {
00129 if (cp.collision)
00130 state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_VALUE_BIT);
00131 state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT);
00132 }
00133 }
00134
00135 bool ompl::control::OpenDEStatePropagator::canPropagateBackward(void) const
00136 {
00137 return false;
00138 }