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
43ompl::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
162namespace ompl
163{
165 struct CallbackParam
166 {
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
260void 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
268namespace 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}
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:735
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
State ** components
The components that make up a compound state.
Definition: State.h:128
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
double x
X component of quaternion vector.
double w
scalar component of quaternion
double z
Z component of quaternion vector.
double y
Y component of quaternion vector.
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
Definition: StateSampler.h:65
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
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
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
This class contains the OpenDE constructs OMPL needs to know about when planning.
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
int collision
Flag containing information about state validity.
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....
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 setDefaultBounds()
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
base::State * allocState() const override
Allocate a state that can store a point in the described space.
base::StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
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.
void freeState(base::State *state) const override
Free the memory of the allocated state.
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.
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.
base::StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual bool evaluateCollision(const base::State *state) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state,...
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
@ STATE_SPACE_TYPE_COUNT
Number of state space types; To add new types, use values that are larger than the count.
Main namespace. Contains everything in this library.
STL namespace.