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/ode/OpenDEStatePropagator.h"
38 #include "ompl/extensions/ode/OpenDEStateSpace.h"
39 #include "ompl/extensions/ode/OpenDEControlSpace.h"
40 #include "ompl/util/Exception.h"
41 #include "ompl/util/Console.h"
42 
44 {
45  if (auto *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  struct CallbackParam
55  {
56  const control::OpenDEEnvironment *env;
57  bool collision;
58  };
59 
60  void nearCallback(void *data, dGeomID o1, dGeomID o2)
61  {
62  dBodyID b1 = dGeomGetBody(o1);
63  dBodyID b2 = dGeomGetBody(o2);
64 
65  if ((b1 != nullptr) && (b2 != nullptr) && (dAreConnectedExcluding(b1, b2, dJointTypeContact) != 0))
66  return;
67 
68  auto *cp = reinterpret_cast<CallbackParam *>(data);
69 
70  const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
71  if (maxContacts <= 0)
72  return;
73 
74  auto *contact = new dContact[maxContacts];
75 
76  for (unsigned int i = 0; i < maxContacts; ++i)
77  cp->env->setupContact(o1, o2, contact[i]);
78 
79  if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)))
80  {
81  for (int i = 0; i < numc; ++i)
82  {
83  dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
84  dJointAttach(c, b1, b2);
85  bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
86  if (!valid)
87  cp->collision = true;
88  if (cp->env->verboseContacts_)
89  {
90  OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
91  cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
92  }
93  }
94  }
95 
96  delete[] contact;
97  }
98 }
100 
102  const double duration, base::State *result) const
103 {
104  env_->mutex_.lock();
105 
106  // place the OpenDE world at the start state
107  si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
108 
109  // apply the controls
110  env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);
111 
112  // created contacts as needed
113  CallbackParam cp = {env_.get(), false};
114  for (auto &collisionSpace : env_->collisionSpaces_)
115  dSpaceCollide(collisionSpace, &cp, &nearCallback);
116 
117  // propagate one step forward
118  dWorldQuickStep(env_->world_, (dReal)duration);
119 
120  // remove created contacts
121  dJointGroupEmpty(env_->contactGroup_);
122 
123  // read the final state from the OpenDE world
124  si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result);
125 
126  env_->mutex_.unlock();
127 
128  // update the collision flag for the start state, if needed
130  {
131  if (cp.collision)
134  }
135 }
136 
138 {
139  return false;
140 }
Definition of an abstract control.
Definition: Control.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
OpenDEEnvironmentPtr env_
The OpenDE environment this state propagator operates on.
@ STATE_COLLISION_VALUE_BIT
Index of bit in StateType::collision indicating whether a state is in collision or not....
Definition of an abstract state.
Definition: State.h:50
OpenDEStatePropagator(const SpaceInformationPtr &si)
Construct a representation of OpenDE state propagator. If si->getStateSpace() does not cast to an Ope...
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
Model the effect of controls on system states.
@ STATE_COLLISION_KNOWN_BIT
Index of bit in StateType::collision indicating whether it is known if a state is in collision or not...
bool canPropagateBackward() const override
Some systems can only propagate forward in time (i.e., the duration argument for the propagate() func...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
int collision
Flag containing information about state validity.
This class contains the OpenDE constructs OMPL needs to know about when planning.
void propagate(const base::State *state, const Control *control, double duration, base::State *result) const override
Propagate from a state, given a control, for some specified amount of time (the amount of time can al...
State space representing OpenDE states.
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
double * values
An array of length n, representing the value of the control.
The exception type for ompl.
Definition: Exception.h:47
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
Definition: AppBase.h:22