PQPStateValidityChecker.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
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>
116  class PQPStateValidityChecker : public base::StateValidityChecker
117  {
118  public:
119 
120  PQPStateValidityChecker(const base::SpaceInformationPtr &si, const GeometrySpecification &geom,
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_)
219  OMPL_INFORM("Empty environment loaded");
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 
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
263 #ifdef OMPLAPP_ADD_PADDING_FOR_DIMENSION
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();
329  model->AddTri(dV0, dV1, dV2, id++);
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.
bool isValid(const base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
@ EXACT
Exact clearance computation is available.
ClearanceComputationType clearanceComputationType
Value indicating the kind of clearance computation this StateValidityChecker can compute (if any).
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
StateValidityCheckerSpecs specs_
The specifications of the state validity checker (its capabilities)
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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...
StateValidityChecker(SpaceInformation *si)
Constructor.
double distanceTol_
Tolerance passed to PQP for distance calculations.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
PQPModelPtr environment_
Model of the environment.
std::vector< PQPModelPtr > robotParts_
Model of the robot.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
double avgEnvSide_
The average length of a side in the environment.
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...
The exception type for ompl.
Definition: Exception.h:78
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
Definition: AppBase.h:21