ompl/extensions/opende/src/OpenDEStatePropagator.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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 OMPL_DEBUG("%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 // place the OpenDE world at the start state 00105 si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state); 00106 00107 // apply the controls 00108 env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values); 00109 00110 // created contacts as needed 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 // propagate one step forward 00116 dWorldQuickStep(env_->world_, (const dReal)duration); 00117 00118 // remove created contacts 00119 dJointGroupEmpty(env_->contactGroup_); 00120 00121 // read the final state from the OpenDE world 00122 si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result); 00123 00124 env_->mutex_.unlock(); 00125 00126 // update the collision flag for the start state, if needed 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() const 00136 { 00137 return false; 00138 }