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  OMPL_DEBUG("%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 }
Index of bit in StateType::collision indicating whether it is known if a state is in collision or not...
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:203
int type_
A type assigned for this state space.
Definition: StateSpace.h:503
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...
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
virtual bool evaluateCollision(const base::State *source) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state...
std::vector< double > low
Lower bound.
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual void interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
Index of bit in StateType::collision indicating whether a state is in collision or not...
virtual void sampleUniform(State *state)=0
Sample a state.
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
A state space representing SO(3). The internal representation is done with quaternions. The distance between states is the angle between quaternions and interpolation is done with slerp.
Definition: SO3StateSpace.h:84
virtual base::State * allocState() const
Allocate a state that can store a point in the described space.
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
virtual void copyState(base::State *destination, const base::State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109
virtual base::StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
unsigned int componentCount_
The number of components.
Definition: StateSpace.h:710
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:198
This class contains the OpenDE constructs OMPL needs to know about when planning. ...
double w
scalar component of quaternion
void setLow(double value)
Set the lower bound in each dimension to a specific value.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
void setDefaultBounds()
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:94
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:785
OpenDEStateSpace(const 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.
std::vector< double > high
Upper bound.
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
Definition of an abstract state.
Definition: State.h:50
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:855
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual base::StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
The lower and upper bounds for an Rn space.
State ** components
The components that make up a compound state.
Definition: State.h:142
virtual void freeState(base::State *state) const
Free the memory of the allocated state.
Number of state space types; To add new types, use values that are larger than the count...
double z
Z component of quaternion vector.
Abstract definition of a state space sampler.
Definition: StateSampler.h:66
double y
Y component of quaternion vector.
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
int collision
Flag containing information about state validity.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:707
double x
X component of quaternion vector.
A boost shared pointer wrapper for ompl::control::OpenDEEnvironment.
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.