All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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/opende/OpenDEStateSpace.h"
38 #include "ompl/util/Console.h"
39 #include <boost/lexical_cast.hpp>
40 #include <limits>
41 #include <queue>
42 
44  double positionWeight, double linVelWeight, double angVelWeight, double orientationWeight) :
45  base::CompoundStateSpace(), env_(env)
46 {
47  setName("OpenDE" + getName());
49  for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
50  {
51  std::string body = ":B" + boost::lexical_cast<std::string>(i);
52 
53  addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), positionWeight); // position
54  components_.back()->setName(components_.back()->getName() + body + ":position");
55 
56  addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight); // linear velocity
57  components_.back()->setName(components_.back()->getName() + body + ":linvel");
58 
59  addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight); // angular velocity
60  components_.back()->setName(components_.back()->getName() + body + ":angvel");
61 
62  addSubspace(base::StateSpacePtr(new 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 (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
86  spaces.push(env_->collisionSpaces_[i]);
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))
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 (int k = 0 ; k < 6 ; ++k)
108  if (fabs(aabb[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) mX = aabb[0];
117  if (aabb[1] > MX) MX = aabb[1];
118  if (aabb[2] < mY) mY = aabb[2];
119  if (aabb[3] > MY) MY = aabb[3];
120  if (aabb[4] < mZ) mZ = aabb[4];
121  if (aabb[5] > MZ) MZ = aabb[5];
122  }
123  }
124  }
125  }
126 
127  if (found)
128  {
129  double dx = MX - mX;
130  double dy = MY - mY;
131  double dz = MZ - mZ;
132  double dM = std::max(dx, std::max(dy, dz));
133 
134  // add 10% in each dimension + 1% of the max dimension
135  dx = dx / 10.0 + dM / 100.0;
136  dy = dy / 10.0 + dM / 100.0;
137  dz = dz / 10.0 + dM / 100.0;
138 
139  bounds1.low[0] = mX - dx;
140  bounds1.high[0] = MX + dx;
141  bounds1.low[1] = mY - dy;
142  bounds1.high[1] = MY + dy;
143  bounds1.low[2] = mZ - dz;
144  bounds1.high[2] = MZ + dz;
145 
146  setVolumeBounds(bounds1);
147  }
148 }
149 
151 {
152  CompoundStateSpace::copyState(destination, source);
153  destination->as<StateType>()->collision = source->as<StateType>()->collision;
154 }
155 
156 namespace ompl
157 {
159  struct CallbackParam
160  {
161  const control::OpenDEEnvironment *env;
162  bool collision;
163  };
164 
165  static void nearCallback(void *data, dGeomID o1, dGeomID o2)
166  {
167  // if a collision has not already been detected
168  if (reinterpret_cast<CallbackParam*>(data)->collision == false)
169  {
170  dBodyID b1 = dGeomGetBody(o1);
171  dBodyID b2 = dGeomGetBody(o2);
172  if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
173 
174  dContact contact[1]; // one contact is sufficient
175  int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
176 
177  // check if there is really a collision
178  if (numc)
179  {
180  // check if the collision is allowed
181  bool valid = reinterpret_cast<CallbackParam*>(data)->env->isValidCollision(o1, o2, contact[0]);
182  reinterpret_cast<CallbackParam*>(data)->collision = !valid;
183  if (reinterpret_cast<CallbackParam*>(data)->env->verboseContacts_)
184  {
185  logDebug("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
186  reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o1).c_str(),
187  reinterpret_cast<CallbackParam*>(data)->env->getGeomName(o2).c_str());
188  }
189  }
190  }
191  }
193 }
194 
196 {
197  if (state->as<StateType>()->collision & (1 << STATE_COLLISION_KNOWN_BIT))
198  return state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT);
199  env_->mutex_.lock();
200  writeState(state);
201  CallbackParam cp = { env_.get(), false };
202  for (unsigned int i = 0 ; cp.collision == false && i < env_->collisionSpaces_.size() ; ++i)
203  dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
204  env_->mutex_.unlock();
205  if (cp.collision)
206  state->as<StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
207  state->as<StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
208  return cp.collision;
209 }
210 
212 {
213  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
214  if (i % 4 != 3)
215  if (!components_[i]->satisfiesBounds(state->components[i]))
216  return false;
217  return true;
218 }
219 
221 {
222  for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
223  components_[i * 4]->as<base::RealVectorStateSpace>()->setBounds(bounds);
224 }
225 
227 {
228  for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
229  components_[i * 4 + 1]->as<base::RealVectorStateSpace>()->setBounds(bounds);
230 }
231 
233 {
234  for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
235  components_[i * 4 + 2]->as<base::RealVectorStateSpace>()->setBounds(bounds);
236 }
237 
239 {
240  StateType *state = new StateType();
241  allocStateComponents(state);
242  return state;
243 }
244 
246 {
247  CompoundStateSpace::freeState(state);
248 }
249 
250 // this function should most likely not be used with OpenDE propagations, but just in case it is called, we need to make sure the collision information
251 // is cleared from the resulting state
252 void ompl::control::OpenDEStateSpace::interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
253 {
254  CompoundStateSpace::interpolate(from, to, t, state);
255  state->as<StateType>()->collision = 0;
256 }
257 
259 namespace ompl
260 {
261  namespace control
262  {
263  // we need to make sure any collision information is cleared when states are sampled (just in case this ever happens)
264  class WrapperForOpenDESampler : public ompl::base::StateSampler
265  {
266  public:
267  WrapperForOpenDESampler(const base::StateSpace *space, const base::StateSamplerPtr &wrapped) : base::StateSampler(space), wrapped_(wrapped)
268  {
269  }
270 
271  virtual void sampleUniform(ompl::base::State *state)
272  {
273  wrapped_->sampleUniform(state);
274  state->as<OpenDEStateSpace::StateType>()->collision = 0;
275  }
276 
277  virtual void sampleUniformNear(base::State *state, const base::State *near, const double distance)
278  {
279  wrapped_->sampleUniformNear(state, near, distance);
280  state->as<OpenDEStateSpace::StateType>()->collision = 0;
281  }
282 
283  virtual void sampleGaussian(base::State *state, const base::State *mean, const double stdDev)
284  {
285  wrapped_->sampleGaussian(state, mean, stdDev);
286  state->as<OpenDEStateSpace::StateType>()->collision = 0;
287  }
288  private:
289  base::StateSamplerPtr wrapped_;
290  };
291  }
292 }
294 
296 {
298  return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
299 }
300 
302 {
304  if (dynamic_cast<WrapperForOpenDESampler*>(sampler.get()))
305  return sampler;
306  else
307  return base::StateSamplerPtr(new WrapperForOpenDESampler(this, sampler));
308 }
309 
311 {
312  StateType *s = state->as<StateType>();
313  for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
314  {
315  unsigned int _i4 = i * 4;
316 
317  const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
318  const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
319  const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
320  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
321  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
322  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
323 
324  for (int j = 0; j < 3; ++j)
325  {
326  s_pos[j] = pos[j];
327  s_vel[j] = vel[j];
328  s_ang[j] = ang[j];
329  }
330 
331  const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
333 
334  s_rot.w = rot[0];
335  s_rot.x = rot[1];
336  s_rot.y = rot[2];
337  s_rot.z = rot[3];
338  }
339  s->collision = 0;
340 }
341 
343 {
344  const StateType *s = state->as<StateType>();
345  for (int i = (int)env_->stateBodies_.size() - 1 ; i >= 0 ; --i)
346  {
347  unsigned int _i4 = i * 4;
348 
349  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
350  dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
351 
352  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
353  dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
354 
355  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4;
356  dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]);
357 
359  dQuaternion q;
360  q[0] = s_rot.w;
361  q[1] = s_rot.x;
362  q[2] = s_rot.y;
363  q[3] = s_rot.z;
364  dBodySetQuaternion(env_->stateBodies_[i], q);
365  }
366 }