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/collision.h>
24 #include <fcl/collision_node.h>
25 #include <fcl/traversal/traversal_node_setup.h>
26 #include <fcl/continuous_collision.h>
27 
28 // STL headers
29 #include <memory>
30 #include <utility>
31 #include <vector>
32 #include <limits>
33 #include <cmath>
34 
35 namespace ob = ompl::base;
36 
37 namespace ompl
38 {
39  namespace app
40  {
41  OMPL_CLASS_FORWARD (FCLMethodWrapper);
42 
45  {
46  public:
47 
48  using FCLPoseFromStateCallback = std::function<void (fcl::Vec3f &, fcl::Quaternion3f &, const base::State *)>;
49 
51  GeometricStateExtractor se,
52  bool selfCollision,
53  FCLPoseFromStateCallback poseCallback) : extractState_(std::move(se)), selfCollision_(selfCollision),
54  poseFromStateCallback_ (std::move(poseCallback))
55  {
56  configure (geom);
57  }
58 
59  virtual ~FCLMethodWrapper ()
60  {
61  for (auto & robotPart : robotParts_)
62  delete robotPart;
63  }
64 
67  virtual bool isValid (const base::State *state) const
68  {
69  fcl::CollisionRequest collisionRequest;
70  fcl::CollisionResult collisionResult;
71  fcl::Quaternion3f rot;
72  fcl::Vec3f pos;
73  fcl::Transform3f transform;
74 
75  if (environment_.num_tris > 0)
76  {
77  // Performing collision checking with environment.
78  for (std::size_t i = 0; i < robotParts_.size(); ++i)
79  {
80  poseFromStateCallback_(pos, rot, extractState_(state, i));
81  transform.setTransform(rot, pos);
82  if (fcl::collide(robotParts_[i], transform, &environment_,
83  fcl::Transform3f(), collisionRequest, collisionResult) > 0)
84  return false;
85  }
86  }
87 
88  // Checking for self collision
89  if (selfCollision_)
90  {
91  fcl::Transform3f trans_i, trans_j;
92  for (std::size_t i = 0 ; i < robotParts_.size(); ++i)
93  {
94  poseFromStateCallback_(pos, rot, extractState_(state, i));
95  trans_i.setTransform(rot, pos);
96 
97  for (std::size_t j = i + 1 ; j < robotParts_.size (); ++j)
98  {
99  poseFromStateCallback_(pos, rot, extractState_(state, j));
100  trans_j.setTransform(rot, pos);
101  if (fcl::collide(robotParts_[i], trans_i, robotParts_[j], trans_j,
102  collisionRequest, collisionResult) > 0)
103  return false;
104  }
105  }
106  }
107 
108  return true;
109  }
110 
113  virtual bool isValid (const base::State *s1, const base::State *s2, double &collisionTime) const
114  {
115  fcl::Transform3f transi_beg, transi_end, trans;
116  fcl::Quaternion3f rot;
117  fcl::Vec3f pos;
118  fcl::ContinuousCollisionRequest collisionRequest(10, 0.0001, fcl::CCDM_SCREW,
119  fcl::GST_LIBCCD, fcl::CCDC_CONSERVATIVE_ADVANCEMENT);
120  fcl::ContinuousCollisionResult collisionResult;
121 
122  // Checking for collision with environment
123  if (environment_.num_tris > 0)
124  {
125  for (size_t i = 0; i < robotParts_.size(); ++i)
126  {
127  // Getting the translation and rotation from s1 and s2
128  poseFromStateCallback_(pos, rot, extractState_(s1, i));
129  transi_beg.setTransform(rot, pos);
130  poseFromStateCallback_(pos, rot, extractState_(s2, i));
131  transi_end.setTransform(rot, pos);
132 
133  // Checking for collision
134  fcl::continuousCollide(robotParts_[i], transi_beg, transi_end,
135  &environment_, trans, trans,
136  collisionRequest, collisionResult);
137  if (collisionResult.is_collide)
138  {
139  collisionTime = collisionResult.time_of_contact;
140  return false;
141  }
142  }
143  }
144 
145  // Checking for self collision
146  if (selfCollision_)
147  {
148  fcl::Transform3f transj_beg, transj_end;
149  for (std::size_t i = 0 ; i < robotParts_.size(); ++i)
150  {
151  poseFromStateCallback_(pos, rot, extractState_(s1, i));
152  transi_beg.setTransform(rot, pos);
153  poseFromStateCallback_(pos, rot, extractState_(s2, i));
154  transi_end.setTransform(rot, pos);
155 
156  for (std::size_t j = i+1; j < robotParts_.size(); ++j)
157  {
158  poseFromStateCallback_(pos, rot, extractState_(s1, j));
159  transj_beg.setTransform(rot, pos);
160  poseFromStateCallback_(pos, rot, extractState_(s2, j));
161  transj_end.setTransform(rot, pos);
162 
163  // Checking for collision
164  fcl::continuousCollide(robotParts_[i], transi_beg, transi_end,
165  robotParts_[j], transj_beg, transj_end,
166  collisionRequest, collisionResult);
167  if (collisionResult.is_collide)
168  {
169  collisionTime = collisionResult.time_of_contact;
170  return false;
171  }
172  }
173  }
174  }
175  return true;
176  }
177 
179  virtual double clearance (const base::State *state) const
180  {
181  double minDist = std::numeric_limits<double>::infinity ();
182  if (environment_.num_tris > 0)
183  {
184  fcl::DistanceRequest distanceRequest(true);
185  fcl::DistanceResult distanceResult;
186  fcl::Transform3f trans, trans_env;
187  fcl::Quaternion3f rot;
188  fcl::Vec3f pos;
189  fcl::MeshDistanceTraversalNodeOBBRSS distanceNode;
190  for (size_t i = 0; i < robotParts_.size (); ++i)
191  {
192  poseFromStateCallback_(pos, rot, extractState_(state, i));
193  trans.setTransform(rot, pos);
194  fcl::initialize(distanceNode, *robotParts_[i], trans, environment_, trans_env, distanceRequest, distanceResult);
195  fcl::distance (&distanceNode);
196  if (distanceResult.min_distance < minDist)
197  minDist = distanceResult.min_distance;
198  }
199  }
200 
201  return minDist;
202  }
203 
204  protected:
205 
208  void configure (const GeometrySpecification &geom)
209  {
210  // Configuring the model of the environment
211  environment_.beginModel ();
212  std::pair <std::vector <fcl::Vec3f>, std::vector<fcl::Triangle> > tri_model;
213  tri_model = getFCLModelFromScene (geom.obstacles, geom.obstaclesShift);
214  environment_.addSubModel (tri_model.first, tri_model.second);
215 
216  environment_.endModel ();
217  environment_.computeLocalAABB ();
218 
219  if (environment_.num_tris == 0)
220  OMPL_INFORM("Empty environment loaded");
221  else
222  OMPL_INFORM("Loaded environment model with %d triangles.", environment_.num_tris);
223 
224  // Configuring the model of the robot, composed of one or more pieces
225  for (size_t rbt = 0; rbt < geom.robot.size (); ++rbt)
226  {
227  auto* model = new Model();
228  model->beginModel ();
229  aiVector3D shift(0.0, 0.0, 0.0);
230  if (geom.robotShift.size () > rbt)
231  shift = geom.robotShift[rbt];
232 
233  tri_model = getFCLModelFromScene (geom.robot[rbt], shift);
234  model->addSubModel (tri_model.first, tri_model.second);
235 
236  model->endModel ();
237  model->computeLocalAABB ();
238 
239  OMPL_INFORM("Robot piece with %d triangles loaded", model->num_tris);
240  robotParts_.push_back (model);
241  }
242  }
243 
245  std::pair <std::vector <fcl::Vec3f>, std::vector<fcl::Triangle> > getFCLModelFromScene (const aiScene *scene, const aiVector3D &center) const
246  {
247  std::vector<const aiScene*> scenes(1, scene);
248  std::vector<aiVector3D> centers(1, center);
249  return getFCLModelFromScene(scenes, centers);
250  }
251 
253  std::pair <std::vector <fcl::Vec3f>, std::vector<fcl::Triangle> >getFCLModelFromScene (const std::vector<const aiScene*> &scenes, const std::vector<aiVector3D> &center) const
254  {
255  // Model consists of a set of points, and a set of triangles
256  // that connect those points
257  std::vector<fcl::Triangle> triangles;
258  std::vector <fcl::Vec3f> pts;
259 
260  for (unsigned int i = 0; i < scenes.size (); ++i)
261  {
262  if (scenes[i] != nullptr)
263  {
264  std::vector<aiVector3D> t;
265  // extractTriangles is a misleading name. this extracts the set of points,
266  // where each set of three contiguous points consists of a triangle
267  scene::extractTriangles (scenes[i], t);
268 
269  if (center.size () > i)
270  for (auto & j : t)
271  j -= center[i];
272 
273  assert (t.size () % 3 == 0);
274 
275  for (auto & j : t)
276  {
277  pts.emplace_back(j[0], j[1], j[2]);
278  }
279 
280  for (unsigned int j = 0; j < t.size (); j+=3)
281  triangles.emplace_back(j, j+1, j+2);
282  }
283  }
284  return std::make_pair (pts, triangles);
285  }
286 
288  using BVType = fcl::OBBRSS;
290  using Model = fcl::BVHModel<BVType>;
291 
294 
296  mutable std::vector <Model*> robotParts_;
297 
299  GeometricStateExtractor extractState_;
300 
303 
305  FCLPoseFromStateCallback poseFromStateCallback_;
306  };
307  }
308 }
309 
310 #endif
void configure(const GeometrySpecification &geom)
Configures the geometry of the robot and the environment to setup validity checking.
Model environment_
Geometric model used for the environment.
std::pair< std::vector< fcl::Vec3f >, std::vector< fcl::Triangle > > getFCLModelFromScene(const aiScene *scene, const aiVector3D &center) const
Convert a mesh to a FCL BVH model.
fcl::BVHModel< BVType > Model
The type geometric model used for the meshes.
FCLPoseFromStateCallback poseFromStateCallback_
Callback to extract translation and rotation from a state.
std::vector< Model * > robotParts_
List of components for the geometric model of the robot.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual bool isValid(const base::State *s1, const base::State *s2, double &collisionTime) const
Check the continuous motion between s1 and s2. If there is a collision collisionTime will contain the...
virtual bool isValid(const base::State *state) const
Checks whether the given robot state collides with the environment or itself.
bool selfCollision_
Flag indicating whether the geometry is checked for self collisions.
std::pair< std::vector< fcl::Vec3f >, std::vector< fcl::Triangle > > getFCLModelFromScene(const std::vector< const aiScene *> &scenes, const std::vector< aiVector3D > &center) const
Convert a mesh to a FCL BVH model.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
fcl::OBBRSS BVType
The type of geometric bounding done for the robot and environment.
GeometricStateExtractor extractState_
Callback to get the geometric portion of a specific state.
virtual double clearance(const base::State *state) const
Returns the minimum distance from the given robot state and the environment.
Wrapper for FCL discrete and continuous collision checking and distance queries.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68