FCLMethodWrapper.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2011, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #ifndef OMPLAPP_GEOMETRY_DETAIL_FCL_METHOD_WRAPPER_
14 #define OMPLAPP_GEOMETRY_DETAIL_FCL_METHOD_WRAPPER_
15 
16 #include "omplapp/config.h"
17 
18 // OMPL and OMPL.app headers
19 #include "omplapp/geometry/GeometrySpecification.h"
20 #include "omplapp/geometry/detail/assimpUtil.h"
21 
22 // FCL Headers
23 #include <fcl/config.h>
24 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
25 #include <fcl/collision.h>
26 #include <fcl/distance.h>
27 #include <fcl/collision_node.h>
28 #include <fcl/traversal/traversal_node_setup.h>
29 #include <fcl/continuous_collision.h>
30 #else
31 #include <fcl/narrowphase/collision.h>
32 #include <fcl/narrowphase/distance.h>
33 #include <fcl/narrowphase/continuous_collision.h>
34 #endif
35 
36 // STL headers
37 #include <memory>
38 #include <utility>
39 #include <vector>
40 #include <limits>
41 #include <cmath>
42 
43 namespace ob = ompl::base;
44 
45 namespace ompl
46 {
47  namespace app
48  {
49  OMPL_CLASS_FORWARD(FCLMethodWrapper);
50 
52  class FCLMethodWrapper
53  {
54  public:
55 
56 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
57  using Vector3 = fcl::Vec3f;
58  using Quaternion = fcl::Quaternion3f;
59  using Transform = fcl::Transform3f;
60  using BVType = fcl::OBBRSS;
61  using Model = fcl::BVHModel<BVType>;
62  using MeshDistanceTraversalNodeOBBRSS = fcl::MeshDistanceTraversalNodeOBBRSS;
63  using CollisionRequest = fcl::CollisionRequest;
64  using CollisionResult = fcl::CollisionResult;
65  using ContinuousCollisionRequest = fcl::ContinuousCollisionRequest;
66  using ContinuousCollisionResult = fcl::ContinuousCollisionResult;
67  using DistanceRequest = fcl::DistanceRequest;
68  using DistanceResult = fcl::DistanceResult;
69 #else
70  using Vector3 = fcl::Vector3d;
71  using Quaternion = fcl::Quaterniond;
72  using Transform = fcl::Transform3d;
73  using BVType = fcl::OBBRSS<double>;
74  using Model = fcl::BVHModel<BVType>;
75  using MeshDistanceTraversalNodeOBBRSS = fcl::detail::MeshDistanceTraversalNodeOBBRSS<double>;
76  using CollisionRequest = fcl::CollisionRequest<double>;
77  using CollisionResult = fcl::CollisionResult<double>;
78  using ContinuousCollisionRequest = fcl::ContinuousCollisionRequest<double>;
79  using ContinuousCollisionResult = fcl::ContinuousCollisionResult<double>;
80  using DistanceRequest = fcl::DistanceRequest<double>;
81  using DistanceResult = fcl::DistanceResult<double>;
82 #endif
83 
84  using FCLPoseFromStateCallback = std::function<void(Transform &, const base::State *)>;
85 
86  FCLMethodWrapper(const GeometrySpecification &geom,
87  GeometricStateExtractor se,
88  bool selfCollision,
89  FCLPoseFromStateCallback poseCallback)
90  : extractState_(std::move(se)), selfCollision_(selfCollision),
91  poseFromStateCallback_(std::move(poseCallback))
92  {
93  configure(geom);
94  }
95 
96  virtual ~FCLMethodWrapper()
97  {
98  for (auto & robotPart : robotParts_)
99  delete robotPart;
100  }
101 
104  virtual bool isValid(const base::State *state) const
105  {
106 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
107  static Transform identity;
108 #else
109  static Transform identity(Transform::Identity());
110 #endif
111  CollisionRequest collisionRequest;
112  CollisionResult collisionResult;
113  Transform transform;
114 
115  if (environment_.num_tris > 0)
116  {
117  // Performing collision checking with environment.
118  for (std::size_t i = 0; i < robotParts_.size(); ++i)
119  {
120  poseFromStateCallback_(transform, extractState_(state, i));
121  if (fcl::collide(robotParts_[i], transform, &environment_,
122  identity, collisionRequest, collisionResult) > 0)
123  return false;
124  }
125  }
126 
127  // Checking for self collision
128  if (selfCollision_)
129  {
130  Transform trans_i, trans_j;
131  for (std::size_t i = 0 ; i < robotParts_.size(); ++i)
132  {
133  poseFromStateCallback_(trans_i, extractState_(state, i));
134 
135  for (std::size_t j = i + 1 ; j < robotParts_.size(); ++j)
136  {
137  poseFromStateCallback_(trans_j, extractState_(state, j));
138  if (fcl::collide(robotParts_[i], trans_i, robotParts_[j], trans_j,
139  collisionRequest, collisionResult) > 0)
140  return false;
141  }
142  }
143  }
144 
145  return true;
146  }
147 
150  virtual bool isValid(const base::State *s1, const base::State *s2, double &collisionTime) const
151  {
152  Transform transi_beg, transi_end, trans;
153  ContinuousCollisionRequest collisionRequest(10, 0.0001, fcl::CCDM_SCREW,
154  fcl::GST_LIBCCD, fcl::CCDC_CONSERVATIVE_ADVANCEMENT);
155  ContinuousCollisionResult collisionResult;
156 
157  // Checking for collision with environment
158  if (environment_.num_tris > 0)
159  {
160  for (size_t i = 0; i < robotParts_.size(); ++i)
161  {
162  // Getting the translation and rotation from s1 and s2
163  poseFromStateCallback_(transi_beg, extractState_(s1, i));
164  poseFromStateCallback_(transi_end, extractState_(s2, i));
165 
166  // Checking for collision
167  fcl::continuousCollide(robotParts_[i], transi_beg, transi_end,
168  &environment_, trans, trans,
169  collisionRequest, collisionResult);
170  if (collisionResult.is_collide)
171  {
172  collisionTime = collisionResult.time_of_contact;
173  return false;
174  }
175  }
176  }
177 
178  // Checking for self collision
179  if (selfCollision_)
180  {
181  Transform transj_beg, transj_end;
182  for (std::size_t i = 0 ; i < robotParts_.size(); ++i)
183  {
184  poseFromStateCallback_(transi_beg, extractState_(s1, i));
185  poseFromStateCallback_(transi_end, extractState_(s2, i));
186 
187  for (std::size_t j = i+1; j < robotParts_.size(); ++j)
188  {
189  poseFromStateCallback_(transj_beg, extractState_(s1, j));
190  poseFromStateCallback_(transj_end, extractState_(s2, j));
191 
192  // Checking for collision
193  fcl::continuousCollide(robotParts_[i], transi_beg, transi_end,
194  robotParts_[j], transj_beg, transj_end,
195  collisionRequest, collisionResult);
196  if (collisionResult.is_collide)
197  {
198  collisionTime = collisionResult.time_of_contact;
199  return false;
200  }
201  }
202  }
203  }
204  return true;
205  }
206 
208  virtual double clearance(const base::State *state) const
209  {
210 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
211  static Transform identity;
212 #else
213  static Transform identity(Transform::Identity());
214 #endif
215  double minDist = std::numeric_limits<double>::infinity ();
216  if (environment_.num_tris > 0)
217  {
218  DistanceRequest distanceRequest(true);
219  DistanceResult distanceResult;
220  Transform trans;
221  for (size_t i = 0; i < robotParts_.size (); ++i)
222  {
223  poseFromStateCallback_(trans, extractState_(state, i));
224  fcl::distance(robotParts_[i], trans, &environment_, identity, distanceRequest, distanceResult);
225  if (distanceResult.min_distance < minDist)
226  minDist = distanceResult.min_distance;
227  }
228  }
229 
230  return minDist;
231  }
232 
233  protected:
234 
237  void configure(const GeometrySpecification &geom)
238  {
239  // Configuring the model of the environment
240  environment_.beginModel ();
241  std::pair<std::vector <Vector3>, std::vector<fcl::Triangle>> tri_model;
242  tri_model = getFCLModelFromScene(geom.obstacles, geom.obstaclesShift);
243  environment_.addSubModel(tri_model.first, tri_model.second);
244 
245  environment_.endModel ();
246  environment_.computeLocalAABB();
247 
248  if (environment_.num_tris == 0)
249  OMPL_INFORM("Empty environment loaded");
250  else
251  OMPL_INFORM("Loaded environment model with %d triangles.", environment_.num_tris);
252 
253  // Configuring the model of the robot, composed of one or more pieces
254  for (size_t rbt = 0; rbt < geom.robot.size(); ++rbt)
255  {
256  auto* model = new Model();
257  model->beginModel();
258  aiVector3D shift(0.0, 0.0, 0.0);
259  if (geom.robotShift.size() > rbt)
260  shift = geom.robotShift[rbt];
261 
262  tri_model = getFCLModelFromScene(geom.robot[rbt], shift);
263  model->addSubModel(tri_model.first, tri_model.second);
264 
265  model->endModel();
266  model->computeLocalAABB();
267 
268  OMPL_INFORM("Robot piece with %d triangles loaded", model->num_tris);
269  robotParts_.push_back(model);
270  }
271  }
272 
274  std::pair<std::vector <Vector3>, std::vector<fcl::Triangle>> getFCLModelFromScene(const aiScene *scene, const aiVector3D &center) const
275  {
276  std::vector<const aiScene*> scenes(1, scene);
277  std::vector<aiVector3D> centers(1, center);
278  return getFCLModelFromScene(scenes, centers);
279  }
280 
282  std::pair<std::vector<Vector3>, std::vector<fcl::Triangle>> getFCLModelFromScene(const std::vector<const aiScene*> &scenes, const std::vector<aiVector3D> &center) const
283  {
284  // Model consists of a set of points, and a set of triangles
285  // that connect those points
286  std::vector<fcl::Triangle> triangles;
287  std::vector<Vector3> pts;
288 
289  for (unsigned int i = 0; i < scenes.size(); ++i)
290  {
291  if (scenes[i] != nullptr)
292  {
293  std::vector<aiVector3D> t;
294  // extractTriangles is a misleading name. this extracts the set of points,
295  // where each set of three contiguous points consists of a triangle
296  scene::extractTriangles(scenes[i], t);
297 
298  if (center.size() > i)
299  for (auto & j : t)
300  j -= center[i];
301 
302  assert(t.size() % 3 == 0);
303 
304  for (auto & p : t)
305  {
306  pts.emplace_back(p[0], p[1], p[2]);
307  }
308  }
309  }
310 
311  for (unsigned int i = 0; i < pts.size(); i+=3)
312  triangles.emplace_back(i, i+1, i+2);
313 
314  return std::make_pair(pts, triangles);
315  }
316 
318  Model environment_;
319 
321  mutable std::vector<Model*> robotParts_;
322 
324  GeometricStateExtractor extractState_;
325 
327  bool selfCollision_;
328 
330  FCLPoseFromStateCallback poseFromStateCallback_;
331  };
332  }
333 }
334 
335 #endif
ompl::app::FCLMethodWrapper::robotParts_
std::vector< Model * > robotParts_
List of components for the geometric model of the robot.
Definition: FCLMethodWrapper.h:337
ompl::app::FCLMethodWrapper::environment_
Model environment_
Geometric model used for the environment.
Definition: FCLMethodWrapper.h:334
ompl::app::FCLMethodWrapper::poseFromStateCallback_
FCLPoseFromStateCallback poseFromStateCallback_
Callback to extract translation and rotation from a state.
Definition: FCLMethodWrapper.h:346
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:86
ompl::app::FCLMethodWrapper::isValid
virtual bool isValid(const base::State *state) const
Checks whether the given robot state collides with the environment or itself.
Definition: FCLMethodWrapper.h:120
ompl::app::FCLMethodWrapper::extractState_
GeometricStateExtractor extractState_
Callback to get the geometric portion of a specific state.
Definition: FCLMethodWrapper.h:340
OMPL_INFORM
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
ompl::app::FCLMethodWrapper::getFCLModelFromScene
std::pair< std::vector< Vector3 >, std::vector< fcl::Triangle > > getFCLModelFromScene(const aiScene *scene, const aiVector3D &center) const
Convert a mesh to a FCL BVH model.
Definition: FCLMethodWrapper.h:290
ompl::app::FCLMethodWrapper::selfCollision_
bool selfCollision_
Flag indicating whether the geometry is checked for self collisions.
Definition: FCLMethodWrapper.h:343
ompl::app::FCLMethodWrapper::configure
void configure(const GeometrySpecification &geom)
Configures the geometry of the robot and the environment to setup validity checking.
Definition: FCLMethodWrapper.h:253
ompl::app::FCLMethodWrapper
Wrapper for FCL discrete and continuous collision checking and distance queries.
Definition: FCLMethodWrapper.h:68
ompl::app::FCLMethodWrapper::clearance
virtual double clearance(const base::State *state) const
Returns the minimum distance from the given robot state and the environment.
Definition: FCLMethodWrapper.h:224
ompl
Main namespace. Contains everything in this library.
Definition: AppBase.h:21