All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
OpenDEStatePropagator.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/extensions/opende/OpenDEStatePropagator.h"
38 #include "ompl/extensions/opende/OpenDEStateSpace.h"
39 #include "ompl/extensions/opende/OpenDEControlSpace.h"
40 #include "ompl/util/Exception.h"
41 #include "ompl/util/Console.h"
42 
44 {
45  if (OpenDEStateSpace *oss = dynamic_cast<OpenDEStateSpace*>(si->getStateSpace().get()))
46  env_ = oss->getEnvironment();
47  else
48  throw Exception("OpenDE State Space needed for OpenDEStatePropagator");
49 }
50 
52 namespace ompl
53 {
54 
55  struct CallbackParam
56  {
57  const control::OpenDEEnvironment *env;
58  bool collision;
59  };
60 
61  void nearCallback(void *data, dGeomID o1, dGeomID o2)
62  {
63  dBodyID b1 = dGeomGetBody(o1);
64  dBodyID b2 = dGeomGetBody(o2);
65 
66  if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
67 
68  CallbackParam *cp = reinterpret_cast<CallbackParam*>(data);
69 
70  const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
71  if (maxContacts <= 0) return;
72 
73  dContact *contact = new dContact[maxContacts];
74 
75  for (unsigned int i = 0; i < maxContacts; ++i)
76  cp->env->setupContact(o1, o2, contact[i]);
77 
78  if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)))
79  {
80  for (int i = 0; i < numc; ++i)
81  {
82  dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
83  dJointAttach(c, b1, b2);
84  bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
85  if (!valid)
86  cp->collision = true;
87  if (cp->env->verboseContacts_)
88  {
89  logDebug("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
90  cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
91  }
92  }
93  }
94 
95  delete[] contact;
96  }
97 }
99 
100 void ompl::control::OpenDEStatePropagator::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
101 {
102  env_->mutex_.lock();
103 
104  // place the OpenDE world at the start state
105  si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
106 
107  // apply the controls
108  env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);
109 
110  // created contacts as needed
111  CallbackParam cp = { env_.get(), false };
112  for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
113  dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
114 
115  // propagate one step forward
116  dWorldQuickStep(env_->world_, (const dReal)duration);
117 
118  // remove created contacts
119  dJointGroupEmpty(env_->contactGroup_);
120 
121  // read the final state from the OpenDE world
122  si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result);
123 
124  env_->mutex_.unlock();
125 
126  // update the collision flag for the start state, if needed
128  {
129  if (cp.collision)
132  }
133 }
134 
136 {
137  return false;
138 }