OpenDEStateSpace.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/OpenDEStateSpace.h"
38 #include "ompl/util/Console.h"
39 #include <limits>
40 #include <queue>
41 #include <utility>
42 
43 ompl::control::OpenDEStateSpace::OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight, double linVelWeight,
44  double angVelWeight, double orientationWeight)
45  : env_(std::move(env))
46 {
47  setName("OpenDE" + getName());
49  for (unsigned int i = 0; i < env_->stateBodies_.size(); ++i)
50  {
51  std::string body = ":B" + std::to_string(i);
52 
53  addSubspace(std::make_shared<base::RealVectorStateSpace>(3), positionWeight); // position
54  components_.back()->setName(components_.back()->getName() + body + ":position");
55 
56  addSubspace(std::make_shared<base::RealVectorStateSpace>(3), linVelWeight); // linear velocity
57  components_.back()->setName(components_.back()->getName() + body + ":linvel");
58 
59  addSubspace(std::make_shared<base::RealVectorStateSpace>(3), angVelWeight); // angular velocity
60  components_.back()->setName(components_.back()->getName() + body + ":angvel");
61 
62  addSubspace(std::make_shared<base::SO3StateSpace>(), orientationWeight); // orientation
63  components_.back()->setName(components_.back()->getName() + body + ":orientation");
64  }
65  lock();
67 }
68 
70 {
71  // limit all velocities to 1 m/s, 1 rad/s, respectively
72  base::RealVectorBounds bounds1(3);
73  bounds1.setLow(-1);
74  bounds1.setHigh(1);
75  setLinearVelocityBounds(bounds1);
76  setAngularVelocityBounds(bounds1);
77 
78  // find the bounding box that contains all geoms included in the collision spaces
79  double mX, mY, mZ, MX, MY, MZ;
80  mX = mY = mZ = std::numeric_limits<double>::infinity();
81  MX = MY = MZ = -std::numeric_limits<double>::infinity();
82  bool found = false;
83 
84  std::queue<dSpaceID> spaces;
85  for (auto &collisionSpace : env_->collisionSpaces_)
86  spaces.push(collisionSpace);
87 
88  while (!spaces.empty())
89  {
90  dSpaceID space = spaces.front();
91  spaces.pop();
92 
93  int n = dSpaceGetNumGeoms(space);
94 
95  for (int j = 0; j < n; ++j)
96  {
97  dGeomID geom = dSpaceGetGeom(space, j);
98  if (dGeomIsSpace(geom) != 0)
99  spaces.push((dSpaceID)geom);
100  else
101  {
102  bool valid = true;
103  dReal aabb[6];
104  dGeomGetAABB(geom, aabb);
105 
106  // things like planes are infinite; we want to ignore those
107  for (double k : aabb)
108  if (fabs(k) >= std::numeric_limits<dReal>::max())
109  {
110  valid = false;
111  break;
112  }
113  if (valid)
114  {
115  found = true;
116  if (aabb[0] < mX)
117  mX = aabb[0];
118  if (aabb[1] > MX)
119  MX = aabb[1];
120  if (aabb[2] < mY)
121  mY = aabb[2];
122  if (aabb[3] > MY)
123  MY = aabb[3];
124  if (aabb[4] < mZ)
125  mZ = aabb[4];
126  if (aabb[5] > MZ)
127  MZ = aabb[5];
128  }
129  }
130  }
131  }
132 
133  if (found)
134  {
135  double dx = MX - mX;
136  double dy = MY - mY;
137  double dz = MZ - mZ;
138  double dM = std::max(dx, std::max(dy, dz));
139 
140  // add 10% in each dimension + 1% of the max dimension
141  dx = dx / 10.0 + dM / 100.0;
142  dy = dy / 10.0 + dM / 100.0;
143  dz = dz / 10.0 + dM / 100.0;
144 
145  bounds1.low[0] = mX - dx;
146  bounds1.high[0] = MX + dx;
147  bounds1.low[1] = mY - dy;
148  bounds1.high[1] = MY + dy;
149  bounds1.low[2] = mZ - dz;
150  bounds1.high[2] = MZ + dz;
151 
152  setVolumeBounds(bounds1);
153  }
154 }
155 
157 {
158  CompoundStateSpace::copyState(destination, source);
159  destination->as<StateType>()->collision = source->as<StateType>()->collision;
160 }
161 
162 namespace ompl
163 {
165  struct CallbackParam
166  {
167  const control::OpenDEEnvironment *env;
168  bool collision;
169  };
170 
171  static void nearCallback(void *data, dGeomID o1, dGeomID o2)
172  {
173  // if a collision has not already been detected
174  if (!reinterpret_cast<CallbackParam *>(data)->collision)
175  {
176  dBodyID b1 = dGeomGetBody(o1);
177  dBodyID b2 = dGeomGetBody(o2);
178  if ((b1 != nullptr) && (b2 != nullptr) && (dAreConnectedExcluding(b1, b2, dJointTypeContact) != 0))
179  return;
180 
181  dContact contact[1]; // one contact is sufficient
182  int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
183 
184  // check if there is really a collision
185  if (numc != 0)
186  {
187  // check if the collision is allowed
188  bool valid = reinterpret_cast<CallbackParam *>(data)->env->isValidCollision(o1, o2, contact[0]);
189  reinterpret_cast<CallbackParam *>(data)->collision = !valid;
190  if (reinterpret_cast<CallbackParam *>(data)->env->verboseContacts_)
191  {
192  OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
193  reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o1).c_str(),
194  reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o2).c_str());
195  }
196  }
197  }
198  }
200 }
201 
203 {
204  if ((state->as<StateType>()->collision & (1 << STATE_COLLISION_KNOWN_BIT)) != 0)
205  return (state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT)) != 0;
206  env_->mutex_.lock();
207  writeState(state);
208  CallbackParam cp = {env_.get(), false};
209  for (unsigned int i = 0; !cp.collision && i < env_->collisionSpaces_.size(); ++i)
210  dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
211  env_->mutex_.unlock();
212  if (cp.collision)
213  state->as<StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
214  state->as<StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
215  return cp.collision;
216 }
217 
219 {
220  for (unsigned int i = 0; i < componentCount_; ++i)
221  if (i % 4 != 3)
222  if (!components_[i]->satisfiesBounds(state->components[i]))
223  return false;
224  return true;
225 }
226 
228 {
229  for (unsigned int i = 0; i < env_->stateBodies_.size(); ++i)
230  components_[i * 4]->as<base::RealVectorStateSpace>()->setBounds(bounds);
231 }
232 
234 {
235  for (unsigned int i = 0; i < env_->stateBodies_.size(); ++i)
236  components_[i * 4 + 1]->as<base::RealVectorStateSpace>()->setBounds(bounds);
237 }
238 
240 {
241  for (unsigned int i = 0; i < env_->stateBodies_.size(); ++i)
242  components_[i * 4 + 2]->as<base::RealVectorStateSpace>()->setBounds(bounds);
243 }
244 
246 {
247  auto *state = new StateType();
248  allocStateComponents(state);
249  return state;
250 }
251 
253 {
254  CompoundStateSpace::freeState(state);
255 }
256 
257 // this function should most likely not be used with OpenDE propagations, but just in case it is called, we need to make
258 // sure the collision information
259 // is cleared from the resulting state
260 void ompl::control::OpenDEStateSpace::interpolate(const base::State *from, const base::State *to, const double t,
261  base::State *state) const
262 {
263  CompoundStateSpace::interpolate(from, to, t, state);
264  state->as<StateType>()->collision = 0;
265 }
266 
268 namespace ompl
269 {
270  namespace control
271  {
272  // we need to make sure any collision information is cleared when states are sampled (just in case this ever
273  // happens)
274  class WrapperForOpenDESampler : public ompl::base::StateSampler
275  {
276  public:
277  WrapperForOpenDESampler(const base::StateSpace *space, base::StateSamplerPtr wrapped)
278  : base::StateSampler(space), wrapped_(std::move(wrapped))
279  {
280  }
281 
282  void sampleUniform(ompl::base::State *state) override
283  {
284  wrapped_->sampleUniform(state);
285  state->as<OpenDEStateSpace::StateType>()->collision = 0;
286  }
287 
288  void sampleUniformNear(base::State *state, const base::State *near, const double distance) override
289  {
290  wrapped_->sampleUniformNear(state, near, distance);
291  state->as<OpenDEStateSpace::StateType>()->collision = 0;
292  }
293 
294  void sampleGaussian(base::State *state, const base::State *mean, const double stdDev) override
295  {
296  wrapped_->sampleGaussian(state, mean, stdDev);
297  state->as<OpenDEStateSpace::StateType>()->collision = 0;
298  }
299 
300  private:
301  base::StateSamplerPtr wrapped_;
302  };
303  }
304 }
306 
308 {
309  base::StateSamplerPtr sampler = base::CompoundStateSpace::allocDefaultStateSampler();
310  return std::make_shared<WrapperForOpenDESampler>(this, sampler);
311 }
312 
314 {
315  base::StateSamplerPtr sampler = base::CompoundStateSpace::allocStateSampler();
316  if (dynamic_cast<WrapperForOpenDESampler *>(sampler.get()) != nullptr)
317  return sampler;
318  return std::make_shared<WrapperForOpenDESampler>(this, sampler);
319 }
320 
322 {
323  auto *s = state->as<StateType>();
324  for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
325  {
326  unsigned int _i4 = i * 4;
327 
328  const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
329  const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
330  const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
331  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
332  ++_i4;
333  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
334  ++_i4;
335  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
336  ++_i4;
337 
338  for (int j = 0; j < 3; ++j)
339  {
340  s_pos[j] = pos[j];
341  s_vel[j] = vel[j];
342  s_ang[j] = ang[j];
343  }
344 
345  const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
347 
348  s_rot.w = rot[0];
349  s_rot.x = rot[1];
350  s_rot.y = rot[2];
351  s_rot.z = rot[3];
352  }
353  s->collision = 0;
354 }
355 
357 {
358  const auto *s = state->as<StateType>();
359  for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
360  {
361  unsigned int _i4 = i * 4;
362 
363  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
364  ++_i4;
365  dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
366 
367  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
368  ++_i4;
369  dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
370 
371  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
372  ++_i4;
373  dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]);
374 
376  dQuaternion q;
377  q[0] = s_rot.w;
378  q[1] = s_rot.x;
379  q[2] = s_rot.y;
380  q[3] = s_rot.z;
381  dBodySetQuaternion(env_->stateBodies_[i], q);
382  }
383 }
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.
void copyState(base::State *destination, const base::State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
Construct a state space representing OpenDE states.
@ STATE_SPACE_TYPE_COUNT
Number of state space types; To add new types, use values that are larger than the count.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:113
void freeState(base::State *state) const override
Free the memory of the allocated state.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
std::vector< double > low
Lower bound.
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:800
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
base::State * allocState() const override
Allocate a state that can store a point in the described space.
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
void setDefaultBounds()
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
virtual void writeState(const base::State *state) const
Set the parameters of the OpenDE bodies to be the ones read from state. The code will technically wor...
The definition of a state in SO(3) represented as a unit quaternion.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
void interpolate(const base::State *from, const base::State *to, double t, base::State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
base::StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
int collision
Flag containing information about state validity.
This class contains the OpenDE constructs OMPL needs to know about when planning.
std::vector< double > high
Upper bound.
double z
Z component of quaternion vector.
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
base::StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
virtual bool evaluateCollision(const base::State *state) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state,...
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
int type_
A type assigned for this state space.
Definition: StateSpace.h:595
State ** components
The components that make up a compound state.
Definition: State.h:192
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
Abstract definition of a state space sampler.
Definition: StateSampler.h:128
double x
X component of quaternion vector.
A shared pointer wrapper for ompl::base::StateSampler.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
Definition: StateSpace.cpp:871
double w
scalar component of quaternion
double y
Y component of quaternion vector.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:799
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
The lower and upper bounds for an Rn space.