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 <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  : base::CompoundStateSpace(), 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))
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 == false)
175  {
176  dBodyID b1 = dGeomGetBody(o1);
177  dBodyID b2 = dGeomGetBody(o2);
178  if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
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)
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))
205  return state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT);
206  env_->mutex_.lock();
207  writeState(state);
208  CallbackParam cp = {env_.get(), false};
209  for (unsigned int i = 0; cp.collision == false && 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 {
310  return std::make_shared<WrapperForOpenDESampler>(this, sampler);
311 }
312 
314 {
316  if (dynamic_cast<WrapperForOpenDESampler *>(sampler.get()))
317  return sampler;
318  else
319  return std::make_shared<WrapperForOpenDESampler>(this, sampler);
320 }
321 
323 {
324  StateType *s = state->as<StateType>();
325  for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
326  {
327  unsigned int _i4 = i * 4;
328 
329  const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
330  const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
331  const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
332  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
333  ++_i4;
334  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
335  ++_i4;
336  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
337  ++_i4;
338 
339  for (int j = 0; j < 3; ++j)
340  {
341  s_pos[j] = pos[j];
342  s_vel[j] = vel[j];
343  s_ang[j] = ang[j];
344  }
345 
346  const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
348 
349  s_rot.w = rot[0];
350  s_rot.x = rot[1];
351  s_rot.y = rot[2];
352  s_rot.z = rot[3];
353  }
354  s->collision = 0;
355 }
356 
358 {
359  const StateType *s = state->as<StateType>();
360  for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
361  {
362  unsigned int _i4 = i * 4;
363 
364  double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
365  ++_i4;
366  dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
367 
368  double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
369  ++_i4;
370  dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
371 
372  double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
373  ++_i4;
374  dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]);
375 
377  dQuaternion q;
378  q[0] = s_rot.w;
379  q[1] = s_rot.x;
380  q[2] = s_rot.y;
381  q[3] = s_rot.z;
382  dBodySetQuaternion(env_->stateBodies_[i], q);
383  }
384 }
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:214
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
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...
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
base::State * allocState() const override
Allocate a state that can store a point in the described space.
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
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 shared pointer wrapper for ompl::base::StateSampler.
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.
void interpolate(const base::State *from, const base::State *to, const 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...
STL namespace.
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:97
unsigned int componentCount_
The number of components.
Definition: StateSpace.h:738
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:209
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: AppBase.h:21
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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:90
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
std::vector< double > high
Upper bound.
void freeState(base::State *state) const override
Free the memory of the allocated state.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
base::StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:49
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...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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.
The lower and upper bounds for an Rn space.
State ** components
The components that make up a compound state.
Definition: State.h:130
base::StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
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. ...
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:64
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:735
double x
X component of quaternion vector.
A 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.