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>
117  {
118  public:
119 
121  GeometricStateExtractor se, bool selfCollision) : base::StateValidityChecker(si), extractState_(std::move(se)),
122  selfCollision_(selfCollision)
123  {
124  configure(geom);
125  specs_.clearanceComputationType = base::StateValidityCheckerSpecs::EXACT;
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())
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())
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  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])
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.
double avgEnvSide_
The average length of a side in the environment.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
std::vector< double > getDifference() const
Get the difference between the high and low bounds for each dimension: result[i] = high[i] - low[i]...
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().
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...