PQPStateValidityChecker.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10
11 /* Author: Ioan Sucan */
12
13 #ifndef OMPLAPP_GEOMETRY_DETAIL_PQP_COLLISION_CHECKER_
14 #define OMPLAPP_GEOMETRY_DETAIL_PQP_COLLISION_CHECKER_
15
16 #include <ompl/base/SpaceInformation.h>
17 #include <ompl/base/spaces/SE2StateSpace.h>
18 #include <ompl/base/spaces/SE3StateSpace.h>
19
20 #include "omplapp/geometry/GeometrySpecification.h"
21 #include "omplapp/geometry/detail/assimpUtil.h"
22
23 #include <PQP.h>
24 #include <memory>
25 #include <utility>
26 #include <vector>
27 #include <limits>
28 #include <cmath>
29 #include <mutex>
30
31 namespace ompl
32 {
33  namespace app
34  {
35
37
38  template<MotionModel T>
39  struct OMPL_StateType
40  {
41  using type = base::SE3StateSpace::StateType;
42
45  void quaternionToMatrix(PQP_REAL m[3][3], const base::SO3StateSpace::StateType &q) const
46  {
47  double sqw = q.w*q.w;
48  double sqx = q.x*q.x;
49  double sqy = q.y*q.y;
50  double sqz = q.z*q.z;
51
52  m[0][0] = sqx - sqy - sqz + sqw;
53  m[1][1] = -sqx + sqy - sqz + sqw;
54  m[2][2] = -sqx - sqy + sqz + sqw;
55
56  double tmp1 = q.x*q.y;
57  double tmp2 = q.z*q.w;
58  m[1][0] = 2.0 * (tmp1 + tmp2);
59  m[0][1] = 2.0 * (tmp1 - tmp2);
60  tmp1 = q.x*q.z;
61  tmp2 = q.y*q.w;
62  m[2][0] = 2.0 * (tmp1 - tmp2);
63  m[0][2] = 2.0 * (tmp1 + tmp2);
64  tmp1 = q.y*q.z;
65  tmp2 = q.x*q.w;
66  m[2][1] = 2.0 * (tmp1 + tmp2);
67  m[1][2] = 2.0 * (tmp1 - tmp2);
68  }
69
70  void PQP_pose_from_state(PQP_REAL robTrans[3], PQP_REAL robRot[3][3], const type &s) const
71  {
72  robTrans[0] = s.getX();
73  robTrans[1] = s.getY();
74  robTrans[2] = s.getZ();
75  quaternionToMatrix(robRot, s.rotation());
76  }
77  };
78
79  template<>
80  struct OMPL_StateType<Motion_2D>
81  {
82  using type = base::SE2StateSpace::StateType;
83
84  void PQP_pose_from_state(PQP_REAL robTrans[3], PQP_REAL robRot[3][3], const type &s) const
85  {
86  robTrans[0] = s.getX();
87  robTrans[1] = s.getY();
88  robTrans[2] = 0.0;
89
90  const double ca = cos(s.getYaw());
91  const double sa = sin(s.getYaw());
92
93  robRot[0][0] = ca;
94  robRot[0][1] = -sa;
95  robRot[0][2] = 0.0;
96
97  robRot[1][0] = sa;
98  robRot[1][1] = ca;
99  robRot[1][2] = 0.0;
100
101  robRot[2][0] = 0.0;
102  robRot[2][1] = 0.0;
103  robRot[2][2] = 1.0;
104  }
105  };
106
108
109
115  template<MotionModel T>
117  {
118  public:
119
121  GeometricStateExtractor se, bool selfCollision) : base::StateValidityChecker(si), extractState_(std::move(se)),
122  selfCollision_(selfCollision)
123  {
124  configure(geom);
126  }
127
128  bool isValid(const base::State *state) const override
129  {
130  using StateType = typename OMPL_StateType<T>::type;
131
132  if (!si_->satisfiesBounds(state))
133  return false;
134
135  if (!environment_)
136  return true;
137
138  static PQP_REAL identityTranslation[3] = { 0.0, 0.0, 0.0 };
139  static PQP_REAL identityRotation[3][3] = { { 1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} };
140
141  std::lock_guard<std::mutex> slock(mutex_);
142
143  PQP_REAL robTrans[3];
144  PQP_REAL robRot[3][3];
145
146  for (std::size_t i = 0 ; i < robotParts_.size() ; ++i)
147  {
148  stateConvertor_.PQP_pose_from_state(robTrans, robRot, *static_cast<const StateType*>(extractState_(state, i)));
149  PQP_CollideResult cr;
150  PQP_Collide(&cr, robRot, robTrans, robotParts_[i].get(),
151  identityRotation, identityTranslation, environment_.get(), PQP_FIRST_CONTACT);
152  if (cr.Colliding() != 0)
153  return false;
154  }
155
156  if (selfCollision_)
157  {
158  PQP_REAL robTrans2[3];
159  PQP_REAL robRot2[3][3];
160  for (std::size_t i = 0 ; i < robotParts_.size() ; ++i)
161  {
162  stateConvertor_.PQP_pose_from_state(robTrans, robRot, *static_cast<const StateType*>(extractState_(state, i)));
163  for (std::size_t j = i + 1 ; j < robotParts_.size() ; ++j)
164  {
165  stateConvertor_.PQP_pose_from_state(robTrans2, robRot2, *static_cast<const StateType*>(extractState_(state, j)));
166  PQP_CollideResult cr;
167  PQP_Collide(&cr, robRot, robTrans, robotParts_[i].get(),
168  robRot2, robTrans2, robotParts_[j].get(), PQP_FIRST_CONTACT);
169  if (cr.Colliding() != 0)
170  return false;
171  }
172  }
173  }
174
175  return true;
176  }
177
178  double clearance(const base::State *state) const override
179  {
180  using StateType = typename OMPL_StateType<T>::type;
181
182  double dist = std::numeric_limits<double>::infinity();
183  if (environment_)
184  {
185  static PQP_REAL identityTranslation[3] = { 0.0, 0.0, 0.0 };
186  static PQP_REAL identityRotation[3][3] = { { 1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} };
187
188  std::lock_guard<std::mutex> slock(mutex_);
189
190  PQP_REAL robTrans[3];
191  PQP_REAL robRot[3][3];
192
193  for (std::size_t i = 0 ; i < robotParts_.size() ; ++i)
194  {
195  stateConvertor_.PQP_pose_from_state(robTrans, robRot, *static_cast<const StateType*>(extractState_(state, i)));
196  PQP_DistanceResult dr;
197  PQP_Distance(&dr, robRot, robTrans, robotParts_[i].get(),
198  identityRotation, identityTranslation, environment_.get(), 1e-2, distanceTol_);
199  if (dist > dr.Distance())
200  dist = dr.Distance();
201  }
202  }
203  return dist;
204  }
205
206  protected:
207
209  using PQPModelPtr = std::shared_ptr<PQP_Model>;
210
211  void configure(const GeometrySpecification &geom)
212  {
213  std::pair<PQPModelPtr, double> p = getPQPModelFromScene(geom.obstacles, geom.obstaclesShift);
214  environment_ = p.first;
215  avgEnvSide_ = p.second;
216  distanceTol_ = avgEnvSide_ / 100.0;
217
218  if (!environment_)
220  else
221  OMPL_INFORM("Loaded environment model with %d triangles. Average side length is %lf.", environment_->num_tris, avgEnvSide_);
222
223  for (unsigned int i = 0 ; i < geom.robot.size() ; ++i)
224  {
225  aiVector3D shift(0.0, 0.0, 0.0);
226  if (geom.robotShift.size() > i)
227  shift = geom.robotShift[i];
228  PQPModelPtr m = getPQPModelFromScene(geom.robot[i], shift).first;
229  if (!m)
230  throw Exception("Invalid robot mesh");
231  else
232  OMPL_INFORM("Loaded robot model with %d triangles", m->num_tris);
233  robotParts_.push_back(m);
234  }
235  }
236
237  std::pair<PQPModelPtr, double> getPQPModelFromScene(const aiScene *scene, const aiVector3D &center) const
238  {
239  std::vector<const aiScene*> scenes(1, scene);
240  std::vector<aiVector3D> centers(1, center);
241  return getPQPModelFromScene(scenes, centers);
242  }
243
245  std::pair<PQPModelPtr, double> getPQPModelFromScene(const std::vector<const aiScene*> &scenes, const std::vector<aiVector3D> &center) const
246  {
247  std::vector<aiVector3D> triangles;
248  for (unsigned int i = 0 ; i < scenes.size() ; ++i)
249  if (scenes[i] != nullptr)
250  {
251  std::vector<aiVector3D> t;
252  scene::extractTriangles(scenes[i], t);
253  if (center.size() > i)
254  for (auto & j : t)
255  j -= center[i];
256  triangles.insert(triangles.end(), t.begin(), t.end());
257  }
258  return getPQPModelFromTris(triangles);
259  }
260
262  std::pair<PQPModelPtr, double> getPQPModelFromTris(const std::vector<aiVector3D> &triangles) const
264  {
265  base::RealVectorBounds bounds(3);
266  scene::inferBounds(bounds, triangles, 1.0, 0.0);
267  const std::vector<double> &b = bounds.getDifference();
268  unsigned int d = b[0] > b[1] ? 1 : 0;
269  if (b[d] > b[2])
270  d = 2;
271  // the dimension with minimum extents is in d
272  double other = (b[(d + 1) % 3] + b[(d + 2) % 3]) / 2.0;
273  if (b[d] * 1000.0 < other && b[d] < 0.1)
274  {
275  OMPL_DEBUG("Adding padding for dimension %u so that collision checking is more accurate", d);
276  std::vector<aiVector3D> extraTri;
277  extraTri.reserve(triangles.size() * 8);
278  const int N = triangles.size() / 3;
279  double padd = other / 10.0;
280
281  for (int j = 0 ; j < N ; ++j)
282  {
283  const aiVector3D &v0 = triangles[j * 3];
284  const aiVector3D &v1 = triangles[j * 3 + 1];
285  const aiVector3D &v2 = triangles[j * 3 + 2];
286  aiVector3D x0 = v0; x0[d] += padd;
287  aiVector3D x1 = v1; x1[d] += padd;
288  aiVector3D x2 = v2; x2[d] += padd;
289  extraTri.push_back(v0); extraTri.push_back(v1); extraTri.push_back(v2);
290  extraTri.push_back(x0); extraTri.push_back(x2); extraTri.push_back(x1);
291  extraTri.push_back(x1); extraTri.push_back(v1); extraTri.push_back(v2);
292  extraTri.push_back(x1); extraTri.push_back(v2); extraTri.push_back(x2);
293  extraTri.push_back(x1); extraTri.push_back(v0); extraTri.push_back(v1);
294  extraTri.push_back(x1); extraTri.push_back(x0); extraTri.push_back(v0);
295  extraTri.push_back(v0); extraTri.push_back(v2); extraTri.push_back(x2);
296  extraTri.push_back(v0); extraTri.push_back(x2); extraTri.push_back(x0);
297  }
298
299  return getPQPModelFromTrisHelper(extraTri);
300  }
301  else
302  return getPQPModelFromTrisHelper(triangles);
303  }
304
306  std::pair<PQPModelPtr, double> getPQPModelFromTrisHelper(const std::vector<aiVector3D> &triangles) const
307 #endif
308  {
309  PQPModelPtr model;
310
311  if (triangles.empty())
312  return std::make_pair(model, 0.0);
313
314  // create the PQP model
315  model = std::make_shared<PQP_Model>();
316  model->BeginModel();
317  int id = 0;
318  const int N = triangles.size() / 3;
319  double avgSide = 0.0;
320  for (int j = 0 ; j < N ; ++j)
321  {
322  const aiVector3D &v0 = triangles[j * 3];
323  const aiVector3D &v1 = triangles[j * 3 + 1];
324  const aiVector3D &v2 = triangles[j * 3 + 2];
325  PQP_REAL dV0[3] = {v0.x, v0.y, v0.z};
326  PQP_REAL dV1[3] = {v1.x, v1.y, v1.z};
327  PQP_REAL dV2[3] = {v2.x, v2.y, v2.z};
328  avgSide += (v1 - v0).Length() + (v1 - v2).Length() + (v2 - v0).Length();
330  }
331
332  model->EndModel();
333
334  return std::make_pair(model, avgSide / (double)triangles.size());
335  }
336
337  OMPL_StateType<T> stateConvertor_;
338
339  GeometricStateExtractor extractState_;
340
341  bool selfCollision_;
342
344  std::vector<PQPModelPtr> robotParts_;
345
348
350  double avgEnvSide_;
351
353  double distanceTol_;
354
355  mutable std::mutex mutex_;
356
357  };
358
359  }
360 }
361
362 #endif
std::shared_ptr< PQP_Model > PQPModelPtr
Shared pointer wrapper for PQP_Model.
double avgEnvSide_
The average length of a side in the environment.
StateValidityCheckerSpecs specs_
The specifications of the state validity checker (its capabilities)
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Exact clearance computation is available.
std::pair< PQPModelPtr, double > getPQPModelFromTris(const std::vector< aiVector3D > &triangles) const
Convert a set of triangles to a PQP model, but add extra padding if a particular dimension is disprop...
std::vector< PQPModelPtr > robotParts_
Model of the robot.
A shared pointer wrapper for ompl::base::SpaceInformation.
double distanceTol_
Tolerance passed to PQP for distance calculations.
Definition of an abstract state.
Definition: State.h:49
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
The lower and upper bounds for an Rn space.
PQPModelPtr environment_
Model of the environment.
bool isValid(const base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
SpaceInformation * si_
The instance of space information this state validity checker operates on.
ClearanceComputationType clearanceComputationType
Value indicating the kind of clearance computation this StateValidityChecker can compute (if any)...
Define an ompl::base::StateValidityChecker that can construct PQP models internally. The instance is still abstract however, as the isValid() function is not implemented (knowledge of the state space is needed for this function to be implemented)
std::pair< PQPModelPtr, double > getPQPModelFromScene(const std::vector< const aiScene *> &scenes, const std::vector< aiVector3D > &center) const
Convert a mesh to a PQP model.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
double clearance(const base::State *state) const override
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...