fcl/src/collision_common.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Jia Pan */
36 
40 
41 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
44 #else
45 #include <fcl/BVH/BVH_model.h>
46 #include <fcl/shape/geometric_shapes.h>
47 #include <fcl/octree.h>
48 #endif
49 
50 #include <boost/thread/mutex.hpp>
51 #include <memory>
52 #include <type_traits>
53 
54 namespace collision_detection
55 {
57 {
58  CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
59  if (cdata->done_)
60  return true;
61  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
62  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
63 
64  // do not collision check geoms part of the same object / link / attached body
65  if (cd1->sameObject(*cd2))
66  return false;
67 
68  // If active components are specified
69  if (cdata->active_components_only_)
70  {
71  const moveit::core::LinkModel* l1 =
72  cd1->type == BodyTypes::ROBOT_LINK ?
73  cd1->ptr.link :
74  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
75  const moveit::core::LinkModel* l2 =
76  cd2->type == BodyTypes::ROBOT_LINK ?
77  cd2->ptr.link :
78  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
79 
80  // If neither of the involved components is active
81  if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
82  (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
83  return false;
84  }
85 
86  // use the collision matrix (if any) to avoid certain collision checks
87  DecideContactFn dcf;
88  bool always_allow_collision = false;
89  if (cdata->acm_)
90  {
92  bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
93  if (found)
94  {
95  // if we have an entry in the collision matrix, we read it
96  if (type == AllowedCollision::ALWAYS)
97  {
98  always_allow_collision = true;
99  if (cdata->req_->verbose)
100  ROS_DEBUG_NAMED("collision_detection.fcl",
101  "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "
102  "No contacts are computed.",
103  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
104  cd2->getTypeString().c_str());
105  }
106  else if (type == AllowedCollision::CONDITIONAL)
107  {
108  cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
109  if (cdata->req_->verbose)
110  ROS_DEBUG_NAMED("collision_detection.fcl", "Collision between '%s' and '%s' is conditionally allowed",
111  cd1->getID().c_str(), cd2->getID().c_str());
112  }
113  }
114  }
115 
116  // check if a link is touching an attached object
117  if (cd1->type == BodyTypes::ROBOT_LINK && cd2->type == BodyTypes::ROBOT_ATTACHED)
118  {
119  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
120  if (tl.find(cd1->getID()) != tl.end())
121  {
122  always_allow_collision = true;
123  if (cdata->req_->verbose)
124  ROS_DEBUG_NAMED("collision_detection.fcl",
125  "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
126  cd1->getID().c_str(), cd2->getID().c_str());
127  }
128  }
129  else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
130  {
131  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
132  if (tl.find(cd2->getID()) != tl.end())
133  {
134  always_allow_collision = true;
135  if (cdata->req_->verbose)
136  ROS_DEBUG_NAMED("collision_detection.fcl",
137  "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
138  cd2->getID().c_str(), cd1->getID().c_str());
139  }
140  }
141  // bodies attached to the same link should not collide
142  if (cd1->type == BodyTypes::ROBOT_ATTACHED && cd2->type == BodyTypes::ROBOT_ATTACHED)
143  {
144  if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
145  always_allow_collision = true;
146  }
147 
148  // if collisions are always allowed, we are done
149  if (always_allow_collision)
150  return false;
151 
152  if (cdata->req_->verbose)
153  ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
154  cd2->getID().c_str());
155 
156  // see if we need to compute a contact
157  std::size_t want_contact_count = 0;
158  if (cdata->req_->contacts)
159  if (cdata->res_->contact_count < cdata->req_->max_contacts)
160  {
161  std::size_t have;
162  if (cd1->getID() < cd2->getID())
163  {
164  std::pair<std::string, std::string> cp(cd1->getID(), cd2->getID());
165  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
166  }
167  else
168  {
169  std::pair<std::string, std::string> cp(cd2->getID(), cd1->getID());
170  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
171  }
172  if (have < cdata->req_->max_contacts_per_pair)
173  want_contact_count =
174  std::min(cdata->req_->max_contacts_per_pair - have, cdata->req_->max_contacts - cdata->res_->contact_count);
175  }
176 
177  if (dcf)
178  {
179  // if we have a decider for allowed contacts, we need to look at all the contacts
180  bool enable_cost = cdata->req_->cost;
181  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
182  bool enable_contact = true;
183  fcl::CollisionResultd col_result;
184  int num_contacts = fcl::collide(o1, o2,
185  fcl::CollisionRequestd(std::numeric_limits<size_t>::max(), enable_contact,
186  num_max_cost_sources, enable_cost),
187  col_result);
188  if (num_contacts > 0)
189  {
190  if (cdata->req_->verbose)
191  ROS_INFO_NAMED("collision_detection.fcl",
192  "Found %d contacts between '%s' and '%s'. "
193  "These contacts will be evaluated to check if they are accepted or not",
194  num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
195  Contact c;
196  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
197  std::make_pair(cd1->getID(), cd2->getID()) :
198  std::make_pair(cd2->getID(), cd1->getID());
199  for (int i = 0; i < num_contacts; ++i)
200  {
201  fcl2contact(col_result.getContact(i), c);
202  // if the contact is not allowed, we have a collision
203  if (!dcf(c))
204  {
205  // store the contact, if it is needed
206  if (want_contact_count > 0)
207  {
208  --want_contact_count;
209  cdata->res_->contacts[pc].push_back(c);
210  cdata->res_->contact_count++;
211  if (cdata->req_->verbose)
212  ROS_INFO_NAMED("collision_detection.fcl",
213  "Found unacceptable contact between '%s' and '%s'. Contact was stored.",
214  cd1->getID().c_str(), cd2->getID().c_str());
215  }
216  else if (cdata->req_->verbose)
217  ROS_INFO_NAMED("collision_detection.fcl",
218  "Found unacceptable contact between '%s' (type '%s') and '%s' "
219  "(type '%s'). Contact was stored.",
220  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
221  cd2->getTypeString().c_str());
222  cdata->res_->collision = true;
223  if (want_contact_count == 0)
224  break;
225  }
226  }
227  }
228 
229  if (enable_cost)
230  {
231  std::vector<fcl::CostSourced> cost_sources;
232  col_result.getCostSources(cost_sources);
233 
234  CostSource cs;
235  for (auto& cost_source : cost_sources)
236  {
237  fcl2costsource(cost_source, cs);
238  cdata->res_->cost_sources.insert(cs);
239  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
240  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
241  }
242  }
243  }
244  else
245  {
246  if (want_contact_count > 0)
247  {
248  // otherwise, we need to compute more things
249  bool enable_cost = cdata->req_->cost;
250  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
251  bool enable_contact = true;
252 
253  fcl::CollisionResultd col_result;
254  int num_contacts =
255  fcl::collide(o1, o2,
256  fcl::CollisionRequestd(want_contact_count, enable_contact, num_max_cost_sources, enable_cost),
257  col_result);
258  if (num_contacts > 0)
259  {
260  int num_contacts_initial = num_contacts;
261 
262  // make sure we don't get more contacts than we want
263  if (want_contact_count >= (std::size_t)num_contacts)
264  want_contact_count -= num_contacts;
265  else
266  {
267  num_contacts = want_contact_count;
268  want_contact_count = 0;
269  }
270 
271  if (cdata->req_->verbose)
272  ROS_INFO_NAMED("collision_detection.fcl",
273  "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), "
274  "which constitute a collision. %d contacts will be stored",
275  num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
276  cd2->getTypeString().c_str(), num_contacts);
277 
278  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
279  std::make_pair(cd1->getID(), cd2->getID()) :
280  std::make_pair(cd2->getID(), cd1->getID());
281  cdata->res_->collision = true;
282  for (int i = 0; i < num_contacts; ++i)
283  {
284  Contact c;
285  fcl2contact(col_result.getContact(i), c);
286  cdata->res_->contacts[pc].push_back(c);
287  cdata->res_->contact_count++;
288  }
289  }
290 
291  if (enable_cost)
292  {
293  std::vector<fcl::CostSourced> cost_sources;
294  col_result.getCostSources(cost_sources);
295 
296  CostSource cs;
297  for (auto& cost_source : cost_sources)
298  {
299  fcl2costsource(cost_source, cs);
300  cdata->res_->cost_sources.insert(cs);
301  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
302  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
303  }
304  }
305  }
306  else
307  {
308  bool enable_cost = cdata->req_->cost;
309  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
310  bool enable_contact = false;
311  fcl::CollisionResultd col_result;
312  int num_contacts = fcl::collide(
313  o1, o2, fcl::CollisionRequestd(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
314  if (num_contacts > 0)
315  {
316  cdata->res_->collision = true;
317  if (cdata->req_->verbose)
318  ROS_INFO_NAMED("collision_detection.fcl",
319  "Found a contact between '%s' (type '%s') and '%s' (type '%s'), "
320  "which constitutes a collision. "
321  "Contact information is not stored.",
322  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
323  cd2->getTypeString().c_str());
324  }
325 
326  if (enable_cost)
327  {
328  std::vector<fcl::CostSourced> cost_sources;
329  col_result.getCostSources(cost_sources);
330 
331  CostSource cs;
332  for (auto& cost_source : cost_sources)
333  {
334  fcl2costsource(cost_source, cs);
335  cdata->res_->cost_sources.insert(cs);
336  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
337  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
338  }
339  }
340  }
341  }
342 
343  if (cdata->res_->collision)
344  if (!cdata->req_->contacts || cdata->res_->contact_count >= cdata->req_->max_contacts)
345  {
346  if (!cdata->req_->cost)
347  cdata->done_ = true;
348  if (cdata->req_->verbose)
349  ROS_INFO_NAMED("collision_detection.fcl",
350  "Collision checking is considered complete (collision was found and %u contacts are stored)",
351  (unsigned int)cdata->res_->contact_count);
352  }
353 
354  if (!cdata->done_ && cdata->req_->is_done)
355  {
356  cdata->done_ = cdata->req_->is_done(*cdata->res_);
357  if (cdata->done_ && cdata->req_->verbose)
358  ROS_INFO_NAMED("collision_detection.fcl",
359  "Collision checking is considered complete due to external callback. "
360  "%s was found. %u contacts are stored.",
361  cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
362  }
363 
364  return cdata->done_;
365 }
366 
370 struct FCLShapeCache
371 {
372  using ShapeKey = shapes::ShapeConstWeakPtr;
373  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
374 
376  {
377  }
378 
379  void bumpUseCount(bool force = false)
380  {
381  clean_count_++;
382 
383  // clean-up for cache (we don't want to keep infinitely large number of weak ptrs stored)
384  if (clean_count_ > MAX_CLEAN_COUNT || force)
385  {
386  clean_count_ = 0;
387  for (auto it = map_.begin(); it != map_.end();)
388  {
389  auto nit = it;
390  ++nit;
391  if (it->first.expired())
392  map_.erase(it);
393  it = nit;
394  }
395  // ROS_DEBUG_NAMED("collision_detection.fcl", "Cleaning up cache for FCL objects that correspond to static
396  // shapes. Cache size
397  // reduced from %u
398  // to %u", from, (unsigned int)map_.size());
399  }
400  }
401 
402  static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is
403  // executed (this is only removal of expired entries)
406 
408  unsigned int clean_count_;
409 };
410 
411 bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& /*min_dist*/)
412 {
413  DistanceData* cdata = reinterpret_cast<DistanceData*>(data);
414 
415  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
416  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
417 
418  // do not distance check for geoms part of the same object / link / attached body
419  if (cd1->sameObject(*cd2))
420  return false;
421 
422  // If active components are specified
423  if (cdata->req->active_components_only)
424  {
425  const moveit::core::LinkModel* l1 =
426  cd1->type == BodyTypes::ROBOT_LINK ?
427  cd1->ptr.link :
428  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
429  const moveit::core::LinkModel* l2 =
430  cd2->type == BodyTypes::ROBOT_LINK ?
431  cd2->ptr.link :
432  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
433 
434  // If neither of the involved components is active
435  if ((!l1 || cdata->req->active_components_only->find(l1) == cdata->req->active_components_only->end()) &&
436  (!l2 || cdata->req->active_components_only->find(l2) == cdata->req->active_components_only->end()))
437  {
438  return false;
439  }
440  }
441 
442  // use the collision matrix (if any) to avoid certain distance checks
443  bool always_allow_collision = false;
444  if (cdata->req->acm)
445  {
447  bool found = cdata->req->acm->getAllowedCollision(cd1->getID(), cd2->getID(), type);
448  if (found)
449  {
450  // if we have an entry in the collision matrix, we read it
451  if (type == AllowedCollision::ALWAYS)
452  {
453  always_allow_collision = true;
454  if (cdata->req->verbose)
455  ROS_DEBUG_NAMED("collision_detection.fcl",
456  "Collision between '%s' and '%s' is always allowed. No distances are computed.",
457  cd1->getID().c_str(), cd2->getID().c_str());
458  }
459  }
460  }
461 
462  // check if a link is touching an attached object
464  {
465  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
466  if (tl.find(cd1->getID()) != tl.end())
467  {
468  always_allow_collision = true;
469  if (cdata->req->verbose)
470  ROS_DEBUG_NAMED("collision_detection.fcl",
471  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
472  cd1->getID().c_str(), cd2->getID().c_str());
473  }
474  }
475  else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
476  {
477  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
478  if (tl.find(cd2->getID()) != tl.end())
479  {
480  always_allow_collision = true;
481  if (cdata->req->verbose)
482  ROS_DEBUG_NAMED("collision_detection.fcl",
483  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
484  cd2->getID().c_str(), cd1->getID().c_str());
485  }
486  }
487 
488  if (always_allow_collision)
489  {
490  return false;
491  }
492  if (cdata->req->verbose)
493  ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
494  cd2->getID().c_str());
495 
496  double dist_threshold = cdata->req->distance_threshold;
497 
498  const std::pair<const std::string&, const std::string&> pc =
499  cd1->getID() < cd2->getID() ? std::make_pair(std::cref(cd1->getID()), std::cref(cd2->getID())) :
500  std::make_pair(std::cref(cd2->getID()), std::cref(cd1->getID()));
501 
502  DistanceMap::iterator it = cdata->res->distances.find(pc);
503 
504  // GLOBAL search: for efficiency, distance_threshold starts at the smallest distance between any pairs found so far
505  if (cdata->req->type == DistanceRequestType::GLOBAL)
506  {
507  dist_threshold = cdata->res->minimum_distance.distance;
508  }
509  // Check if a distance between this pair has been found yet. Decrease threshold_distance if so, to narrow the search
510  else if (it != cdata->res->distances.end())
511  {
512  if (cdata->req->type == DistanceRequestType::LIMITED)
513  {
514  // If at the limit for a given pair just return
515  if (it->second.size() >= cdata->req->max_contacts_per_body)
516  {
517  return cdata->done;
518  }
519  }
520  else if (cdata->req->type == DistanceRequestType::SINGLE)
521  {
522  dist_threshold = it->second[0].distance;
523  }
524  }
525 
526  fcl::DistanceResultd fcl_result;
527  fcl_result.min_distance = dist_threshold;
528  // fcl::distance segfaults when given an octree with a null root pointer (using FCL 0.6.1)
529  if ((o1->getObjectType() == fcl::OT_OCTREE &&
530  !std::static_pointer_cast<const fcl::OcTreed>(o1->collisionGeometry())->getRoot()) ||
531  (o2->getObjectType() == fcl::OT_OCTREE &&
532  !std::static_pointer_cast<const fcl::OcTreed>(o2->collisionGeometry())->getRoot()))
533  {
534  return false;
535  }
536  double distance = fcl::distance(o1, o2, fcl::DistanceRequestd(cdata->req->enable_nearest_points), fcl_result);
537 
538  // Check if either object is already in the map. If not add it or if present
539  // check to see if the new distance is closer. If closer remove the existing
540  // one and add the new distance information.
541  if (distance < dist_threshold)
542  {
543  // thread_local storage makes this variable persistent. We do not clear it at every iteration because all members
544  // get overwritten.
545  thread_local DistanceResultsData dist_result;
546  dist_result.distance = fcl_result.min_distance;
547 
548  // Careful here: Get the collision geometry data again, since FCL might
549  // swap o1 and o2 in the result.
550  const CollisionGeometryData* res_cd1 = static_cast<const CollisionGeometryData*>(fcl_result.o1->getUserData());
551  const CollisionGeometryData* res_cd2 = static_cast<const CollisionGeometryData*>(fcl_result.o2->getUserData());
552 
553 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
554  dist_result.nearest_points[0] = fcl_result.nearest_points[0];
555  dist_result.nearest_points[1] = fcl_result.nearest_points[1];
556 #else
557  dist_result.nearest_points[0] = Eigen::Map<const Eigen::Vector3d>(fcl_result.nearest_points[0].data.vs);
558  dist_result.nearest_points[1] = Eigen::Map<const Eigen::Vector3d>(fcl_result.nearest_points[1].data.vs);
559 #endif
560  dist_result.link_names[0] = res_cd1->getID();
561  dist_result.link_names[1] = res_cd2->getID();
562  dist_result.body_types[0] = res_cd1->type;
563  dist_result.body_types[1] = res_cd2->type;
564  if (cdata->req->enable_nearest_points)
565  {
566  dist_result.normal = (dist_result.nearest_points[1] - dist_result.nearest_points[0]).normalized();
567  }
568 
569  if (distance <= 0 && cdata->req->enable_signed_distance)
570  {
571  dist_result.nearest_points[0].setZero();
572  dist_result.nearest_points[1].setZero();
573  dist_result.normal.setZero();
574 
575  fcl::CollisionRequestd coll_req;
576  thread_local fcl::CollisionResultd coll_res;
577  coll_res.clear(); // thread_local storage makes the variable persistent. Ensure that it is cleared!
578  coll_req.enable_contact = true;
579  coll_req.num_max_contacts = 200;
580  std::size_t contacts = fcl::collide(o1, o2, coll_req, coll_res);
581  if (contacts > 0)
582  {
583  double max_dist = 0;
584  int max_index = 0;
585  for (std::size_t i = 0; i < contacts; ++i)
586  {
587  const fcl::Contactd& contact = coll_res.getContact(i);
588  if (contact.penetration_depth > max_dist)
589  {
590  max_dist = contact.penetration_depth;
591  max_index = i;
592  }
593  }
594 
595  const fcl::Contactd& contact = coll_res.getContact(max_index);
596  dist_result.distance = -contact.penetration_depth;
597 
598 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
599  dist_result.nearest_points[0] = contact.pos;
600  dist_result.nearest_points[1] = contact.pos;
601 #else
602  dist_result.nearest_points[0] = Eigen::Map<const Eigen::Vector3d>(contact.pos.data.vs);
603  dist_result.nearest_points[1] = Eigen::Map<const Eigen::Vector3d>(contact.pos.data.vs);
604 #endif
605 
606  if (cdata->req->enable_nearest_points)
607  {
608 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
609  Eigen::Vector3d normal(contact.normal);
610 #else
611  Eigen::Vector3d normal(contact.normal.data.vs);
612 #endif
613 
614  // Check order of o1/o2 again, we might need to flip the normal
615  if (contact.o1 == o1->collisionGeometry().get())
616  dist_result.normal = normal;
617  else
618  dist_result.normal = -normal;
619  }
620  }
621  }
622 
623  if (dist_result.distance < cdata->res->minimum_distance.distance)
624  {
625  cdata->res->minimum_distance = dist_result;
626  }
627 
628  if (dist_result.distance <= 0)
629  {
630  cdata->res->collision = true;
631  }
632 
633  if (cdata->req->type != DistanceRequestType::GLOBAL)
634  {
635  if (it == cdata->res->distances.end())
636  {
637  std::vector<DistanceResultsData> data;
638  data.reserve(cdata->req->type == DistanceRequestType::SINGLE ? 1 : cdata->req->max_contacts_per_body);
639  data.push_back(dist_result);
640  cdata->res->distances.insert(std::make_pair(pc, data));
641  }
642  else
643  {
644  if (cdata->req->type == DistanceRequestType::ALL)
645  {
646  it->second.push_back(dist_result);
647  }
648  else if (cdata->req->type == DistanceRequestType::SINGLE)
649  {
650  if (dist_result.distance < it->second[0].distance)
651  it->second[0] = dist_result;
652  }
653  else if (cdata->req->type == DistanceRequestType::LIMITED)
654  {
655  assert(it->second.size() < cdata->req->max_contacts_per_body);
656  it->second.push_back(dist_result);
657  }
658  }
659  }
660 
661  if (!cdata->req->enable_signed_distance && cdata->res->collision)
662  {
663  cdata->done = true;
664  }
665  }
666 
667  return cdata->done;
668 }
669 
670 /* Templated function to get a different cache for each of the template arguments combinations.
671  *
672  * The returned cache is a quasi-singleton for each thread as it is created \e thread_local. */
673 template <typename BV, typename T>
674 FCLShapeCache& GetShapeCache()
675 {
676  /* The cache is created thread_local, that is each thread calling
677  * this quasi-singleton function will get its own instance. Once
678  * the thread joins/exits, the cache gets deleted.
679  * Reasoning is that multi-threaded planners (eg OMPL) or user-code
680  * will often need to do collision checks with the same object
681  * simultaneously (especially true for attached objects). Having only
682  * one global cache leads to many cache misses. Also as the cache can
683  * only be accessed by one thread we don't need any locking.
684  */
685  static thread_local FCLShapeCache cache;
686  return cache;
687 }
688 
694 template <typename BV, typename T>
695 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
696 {
697  using ShapeKey = shapes::ShapeConstWeakPtr;
698  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
699 
700  FCLShapeCache& cache = GetShapeCache<BV, T>();
701 
702  shapes::ShapeConstWeakPtr wptr(shape);
703  {
704  ShapeMap::const_iterator cache_it = cache.map_.find(wptr);
705  if (cache_it != cache.map_.end())
706  {
707  if (cache_it->second->collision_geometry_data_->ptr.raw == data)
708  {
709  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
710  // cache.",
711  // cache_it->second->collision_geometry_data_->getID().c_str());
712  return cache_it->second;
713  }
714  else if (cache_it->second.unique())
715  {
716  const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
717  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
718  // cache after updating
719  // the source
720  // object.", cache_it->second->collision_geometry_data_->getID().c_str());
721  return cache_it->second;
722  }
723  }
724  }
725 
726  // attached objects could have previously been World::Object; we try to move them
727  // from their old cache to the new one, if possible. the code is not pretty, but should help
728  // when we attach/detach objects that are in the world
729  if (std::is_same<T, moveit::core::AttachedBody>::value)
730  {
731  // get the cache that corresponds to objects; maybe this attached object used to be a world object
732  FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
733 
734  // attached bodies could be just moved from the environment.
735  auto cache_it = othercache.map_.find(wptr);
736  if (cache_it != othercache.map_.end())
737  {
738  if (cache_it->second.unique())
739  {
740  // remove from old cache
741  FCLGeometryConstPtr obj_cache = cache_it->second;
742  othercache.map_.erase(cache_it);
743  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
744  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
745 
746  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for attached body %s retrieved
747  // from the cache for
748  // world objects.",
749  // obj_cache->collision_geometry_data_->getID().c_str());
750 
751  // add to the new cache
752  cache.map_[wptr] = obj_cache;
753  cache.bumpUseCount();
754  return obj_cache;
755  }
756  }
757  }
758  else
759  // world objects could have previously been attached objects; we try to move them
760  // from their old cache to the new one, if possible. the code is not pretty, but should help
761  // when we attach/detach objects that are in the world
762  if (std::is_same<T, World::Object>::value)
763  {
764  // get the cache that corresponds to objects; maybe this attached object used to be a world object
765  FCLShapeCache& othercache = GetShapeCache<BV, moveit::core::AttachedBody>();
766 
767  // attached bodies could be just moved from the environment.
768  auto cache_it = othercache.map_.find(wptr);
769  if (cache_it != othercache.map_.end())
770  {
771  if (cache_it->second.unique())
772  {
773  // remove from old cache
774  FCLGeometryConstPtr obj_cache = cache_it->second;
775  othercache.map_.erase(cache_it);
776 
777  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
778  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
779 
780  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for world object %s retrieved
781  // from the cache for
782  // attached
783  // bodies.",
784  // obj_cache->collision_geometry_data_->getID().c_str());
785 
786  // add to the new cache
787  cache.map_[wptr] = obj_cache;
788  cache.bumpUseCount();
789  return obj_cache;
790  }
791  }
792  }
793 
794  fcl::CollisionGeometryd* cg_g = nullptr;
795  // handle cases individually
796  switch (shape->type)
797  {
798  case shapes::PLANE:
799  {
800  const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
801  cg_g = new fcl::Planed(p->a, p->b, p->c, p->d);
802  }
803  break;
804  case shapes::SPHERE:
805  {
806  const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
807  cg_g = new fcl::Sphered(s->radius);
808  }
809  break;
810  case shapes::BOX:
811  {
812  const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
813  const double* size = s->size;
814  cg_g = new fcl::Boxd(size[0], size[1], size[2]);
815  }
816  break;
817  case shapes::CYLINDER:
818  {
819  const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
820  cg_g = new fcl::Cylinderd(s->radius, s->length);
821  }
822  break;
823  case shapes::CONE:
824  {
825  const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
826  cg_g = new fcl::Coned(s->radius, s->length);
827  }
828  break;
829  case shapes::MESH:
830  {
831  auto g = new fcl::BVHModel<BV>();
832  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape.get());
833  if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
834  {
835  std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
836  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
837  tri_indices[i] =
838  fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
839 
840  std::vector<fcl::Vector3d> points(mesh->vertex_count);
841  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
842  points[i] = fcl::Vector3d(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
843 
844  g->beginModel();
845  g->addSubModel(points, tri_indices);
846  g->endModel();
847  }
848  cg_g = g;
849  }
850  break;
851  case shapes::OCTREE:
852  {
853  const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
854  cg_g = new fcl::OcTreed(g->octree);
855  }
856  break;
857  default:
858  ROS_ERROR_NAMED("collision_detection.fcl", "This shape type (%d) is not supported using FCL yet",
859  (int)shape->type);
860  cg_g = nullptr;
861  }
862 
863  if (cg_g)
864  {
865  cg_g->computeLocalAABB();
866  FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
867  cache.map_[wptr] = res;
868  cache.bumpUseCount();
869  return res;
870  }
871  return FCLGeometryConstPtr();
872 }
873 
874 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link,
875  int shape_index)
876 {
877  return createCollisionGeometry<fcl::OBBRSSd, moveit::core::LinkModel>(shape, link, shape_index);
878 }
879 
880 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab,
881  int shape_index)
882 {
883  return createCollisionGeometry<fcl::OBBRSSd, moveit::core::AttachedBody>(shape, ab, shape_index);
884 }
885 
886 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj)
887 {
888  return createCollisionGeometry<fcl::OBBRSSd, World::Object>(shape, obj, 0);
889 }
890 
893 template <typename BV, typename T>
894 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
895  const T* data, int shape_index)
896 {
897  if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
898  fabs(padding) <= std::numeric_limits<double>::epsilon())
899  return createCollisionGeometry<BV, T>(shape, data, shape_index);
900  else
901  {
902  shapes::ShapePtr scaled_shape(shape->clone());
903  scaled_shape->scaleAndPadd(scale, padding);
904  return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
905  }
906 }
907 
908 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
909  const moveit::core::LinkModel* link, int shape_index)
910 {
911  return createCollisionGeometry<fcl::OBBRSSd, moveit::core::LinkModel>(shape, scale, padding, link, shape_index);
912 }
913 
914 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
915  const moveit::core::AttachedBody* ab, int shape_index)
916 {
917  return createCollisionGeometry<fcl::OBBRSSd, moveit::core::AttachedBody>(shape, scale, padding, ab, shape_index);
918 }
919 
920 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
921  const World::Object* obj)
922 {
923  return createCollisionGeometry<fcl::OBBRSSd, World::Object>(shape, scale, padding, obj, 0);
924 }
925 
927 {
928  FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSSd, World::Object>();
929  {
930  cache1.bumpUseCount(true);
931  }
932  FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSSd, moveit::core::AttachedBody>();
933  {
934  cache2.bumpUseCount(true);
935  }
936 }
937 
938 void CollisionData::enableGroup(const moveit::core::RobotModelConstPtr& robot_model)
939 {
940  if (robot_model->hasJointModelGroup(req_->group_name))
941  active_components_only_ = &robot_model->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
942  else
943  active_components_only_ = nullptr;
944 }
945 
947 {
948  std::vector<fcl::CollisionObjectd*> collision_objects(collision_objects_.size());
949  for (std::size_t i = 0; i < collision_objects_.size(); ++i)
950  collision_objects[i] = collision_objects_[i].get();
951  if (!collision_objects.empty())
952  manager->registerObjects(collision_objects);
953 }
954 
956 {
957  for (auto& collision_object : collision_objects_)
958  manager->unregisterObject(collision_object.get());
959 }
960 
961 void FCLObject::clear()
962 {
963  collision_objects_.clear();
964  collision_geometry_.clear();
965 }
966 } // namespace collision_detection
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
fcl::DistanceRequest
collision_common.h
fcl::Planed
Plane< double > Planed
shapes::OcTree::octree
std::shared_ptr< const octomap::OcTree > octree
fcl::DistanceResult::o1
const CollisionGeometry< S > * o1
collision_detection::CollisionData::active_components_only_
const std::set< const moveit::core::LinkModel * > * active_components_only_
If the collision request includes a group name, this set contains the pointers to the link models tha...
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:198
shapes::Mesh::triangles
unsigned int * triangles
fcl::CollisionRequest::num_max_contacts
size_t num_max_contacts
collision_detection::DistanceData::res
DistanceResult * res
Distance query results information.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:224
shapes::PLANE
PLANE
get
ROSCPP_DECL bool get(const std::string &key, bool &b)
collision_detection::CollisionData::req_
const CollisionRequest * req_
The collision request passed by the user.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:192
fcl::Sphered
Sphere< double > Sphered
s
XmlRpcServer s
collision_detection::DistanceData::req
const DistanceRequest * req
Distance query request information.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:221
collision_detection::AllowedCollisionMatrix::getAllowedCollision
bool getAllowedCollision(const std::string &name1, const std::string &name2, DecideContactFn &fn) const
Get the allowed collision predicate between two elements. Return true if a predicate for entry is inc...
Definition: collision_matrix.cpp:287
collision_detection::DistanceRequest::max_contacts_per_body
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
Definition: include/moveit/collision_detection/collision_common.h:310
collision_detection::DistanceRequestTypes::LIMITED
@ LIMITED
Find a limited(max_contacts_per_body) set of contacts for a given pair.
Definition: include/moveit/collision_detection/collision_common.h:267
fcl::BroadPhaseCollisionManager::registerObjects
virtual void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
collision_detection::FCLShapeCache::FCLShapeCache
FCLShapeCache()
Definition: fcl/src/collision_common.cpp:407
fcl::Contact
collision_detection::FCLShapeCache::ShapeKey
shapes::ShapeConstWeakPtr ShapeKey
Definition: fcl/src/collision_common.cpp:404
collision_detection::FCLGeometry
Bundles the CollisionGeometryData and FCL collision geometry representation into a single class.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:233
fcl::OcTreed
fcl::OcTree OcTreed
Definition: fcl_compat.h:93
shapes::Cylinder
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
moveit::core::AttachedBody::getTouchLinks
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:181
collision_detection::AllowedCollision::ALWAYS
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
Definition: collision_matrix.h:157
fcl::Contact::normal
Vector3< S > normal
collision_detection::CollisionGeometryData::type
BodyType type
Indicates the body type of the object.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:154
shapes::Mesh::triangle_count
unsigned int triangle_count
collision_detection::collisionCallback
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
Definition: fcl/src/collision_common.cpp:88
shapes::SPHERE
SPHERE
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
fcl::BroadPhaseCollisionManager::unregisterObject
virtual void unregisterObject(CollisionObject< S > *obj)=0
collision_detection::FCLShapeCache::MAX_CLEAN_COUNT
static const unsigned int MAX_CLEAN_COUNT
Definition: fcl/src/collision_common.cpp:434
collision_detection::fcl2costsource
void fcl2costsource(const fcl::CostSourced &fcs, CostSource &cs)
Transforms the FCL internal representation to the MoveIt CostSource data structure.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:385
collision_detection::CollisionGeometryData::getID
const std::string & getID() const
Returns the name which is saved in the member pointed to in ptr.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:118
collision_detection::DistanceRequest::type
DistanceRequestType type
Definition: include/moveit/collision_detection/collision_common.h:307
shapes::Mesh
fcl::CollisionResult::clear
void clear()
collision_detection::DistanceRequestTypes::GLOBAL
@ GLOBAL
Find the global minimum.
Definition: include/moveit/collision_detection/collision_common.h:265
collision_detection::DistanceRequest::verbose
bool verbose
Log debug information.
Definition: include/moveit/collision_detection/collision_common.h:326
obj
CollisionObject< S > * obj
fcl::CollisionResult
collision_detection::DistanceRequest::distance_threshold
double distance_threshold
Definition: include/moveit/collision_detection/collision_common.h:323
shapes::MESH
MESH
fcl::CollisionObject::getObjectType
OBJECT_TYPE getObjectType() const
collision_detection::GetShapeCache
FCLShapeCache & GetShapeCache()
Definition: fcl/src/collision_common.cpp:706
collision_detection::FCLObject::collision_objects_
std::vector< FCLCollisionObjectPtr > collision_objects_
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:290
fcl::CollisionResult::getCostSources
void getCostSources(std::vector< CostSource< S >> &cost_sources_)
fcl::Triangle
collision_detection::cleanCollisionGeometryCache
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them.
Definition: fcl/src/collision_common.cpp:958
fcl::Vector3d
Vector3< double > Vector3d
collision_detection::DistanceRequestTypes::ALL
@ ALL
Find all the contacts for a given pair.
Definition: include/moveit/collision_detection/collision_common.h:268
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
shapes::CONE
CONE
fcl::DistanceResult
collision_detection::CollisionGeometryData::ab
const moveit::core::AttachedBody * ab
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:166
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
collision_detection::distanceCallback
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
Definition: fcl/src/collision_common.cpp:443
shapes::Plane
shapes::Box
fcl::CollisionGeometry
collision_detection::CollisionGeometryData::ptr
union collision_detection::CollisionGeometryData::@0 ptr
Points to the type of body which contains the geometry.
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
shapes::Sphere
fcl::distance
template double distance(const CollisionGeometry< double > *o1, const Transform3< double > &tf1, const CollisionGeometry< double > *o2, const Transform3< double > &tf2, const DistanceRequest< double > &request, DistanceResult< double > &result)
BVH_model.h
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
shapes::Mesh::vertex_count
unsigned int vertex_count
collision_detection::CollisionRequest::group_name
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)
Definition: include/moveit/collision_detection/collision_common.h:233
collision_detection::FCLObject::clear
void clear()
Definition: fcl/src/collision_common.cpp:993
shapes::Plane::a
double a
fcl::Contact::pos
Vector3< S > pos
collision_detection::CollisionGeometryData
Wrapper around world, link and attached objects' geometry data.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:95
collision_detection::DistanceRequest::acm
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Definition: include/moveit/collision_detection/collision_common.h:319
collision_detection::DistanceResult::collision
bool collision
Indicates if two objects were in collision.
Definition: include/moveit/collision_detection/collision_common.h:397
shapes.h
fcl::CollisionRequest
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
shapes::Plane::d
double d
collision_detection::AllowedCollision::CONDITIONAL
@ CONDITIONAL
The collision is allowed depending on a predicate evaluated on the produced contact....
Definition: collision_matrix.h:162
fcl::CollisionObject::collisionGeometry
const std::shared_ptr< const CollisionGeometry< S > > & collisionGeometry() const
fcl::Contact::o1
const CollisionGeometry< S > * o1
shapes::Mesh::vertices
double * vertices
moveit::core::AttachedBody::getAttachedLink
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:162
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionGeometry< double > *o1, const Transform3< double > &tf1, const CollisionGeometry< double > *o2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::CollisionGeometry::computeLocalAABB
virtual void computeLocalAABB()=0
fcl::Coned
Cone< double > Coned
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
fcl::DistanceResult::min_distance
S min_distance
shapes::OcTree
collision_detection::FCLObject::unregisterFrom
void unregisterFrom(fcl::BroadPhaseCollisionManagerd *manager)
Definition: fcl/src/collision_common.cpp:987
shapes::OCTREE
OCTREE
fcl::BVHModel< BV >
collision_detection::FCLShapeCache::clean_count_
unsigned int clean_count_
Counts cache usage and triggers clearing of cache when \m MAX_CLEAN_COUNT is exceeded.
Definition: fcl/src/collision_common.cpp:440
collision_detection::CollisionData::enableGroup
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on the joint group specified in req_.
Definition: fcl/src/collision_common.cpp:970
collision_detection::CollisionGeometryData::link
const moveit::core::LinkModel * link
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:165
collision_detection::DistanceResult::minimum_distance
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Definition: include/moveit/collision_detection/collision_common.h:400
collision_detection::DistanceResult::distances
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
Definition: include/moveit/collision_detection/collision_common.h:403
collision_detection::createCollisionGeometry
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const moveit::core::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
Definition: fcl/src/collision_common.cpp:906
collision_detection::DistanceRequestTypes::SINGLE
@ SINGLE
Find the global minimum for each pair.
Definition: include/moveit/collision_detection/collision_common.h:266
fcl::Cylinderd
Cylinder< double > Cylinderd
fcl::OT_OCTREE
OT_OCTREE
std
octree.h
fcl_compat.h
fcl::Contact::penetration_depth
S penetration_depth
fcl::CollisionResult::getContact
const Contact< S > & getContact(size_t i) const
shapes::Plane::c
double c
shapes::CYLINDER
CYLINDER
fcl::Boxd
Box< double > Boxd
collision_detection::FCLShapeCache::bumpUseCount
void bumpUseCount(bool force=false)
Definition: fcl/src/collision_common.cpp:411
collision_detection::FCLShapeCache
Cache for an arbitrary type of shape. It is assigned during the execution of createCollisionGeometry(...
Definition: fcl/src/collision_common.cpp:402
collision_detection::CollisionGeometryData::sameObject
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:148
enable_contact
bool enable_contact
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::FCLObject::registerTo
void registerTo(fcl::BroadPhaseCollisionManagerd *manager)
Definition: fcl/src/collision_common.cpp:978
collision_detection::DistanceData
Data structure which is passed to the distance callback function of the collision manager.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:211
collision_detection::DistanceRequest::enable_nearest_points
bool enable_nearest_points
Indicate if nearest point information should be calculated.
Definition: include/moveit/collision_detection/collision_common.h:299
collision_detection::DistanceResultsData::distance
double distance
The distance between two objects. If two objects are in collision, distance <= 0.
Definition: include/moveit/collision_detection/collision_common.h:342
collision_detection::FCLObject::collision_geometry_
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:293
shapes::Cone
fcl::DistanceResult::o2
const CollisionGeometry< S > * o2
fcl::CollisionGeometry::getUserData
void * getUserData() const
collision_detection::fcl2contact
void fcl2contact(const fcl::Contactd &fc, Contact &c)
Transforms an FCL contact into a MoveIt contact point.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:371
collision_detection::FCLShapeCache::map_
ShapeMap map_
Map of weak pointers to the FCLGeometry.
Definition: fcl/src/collision_common.cpp:437
shapes::Plane::b
double b
collision_detection::DecideContactFn
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
Definition: collision_matrix.h:104
collision_detection::DistanceRequest::enable_signed_distance
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
Definition: include/moveit/collision_detection/collision_common.h:302
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
fcl::CollisionRequest::enable_contact
bool enable_contact
shapes::BOX
BOX
fcl::CollisionObject
collision_detection::FCLShapeCache::ShapeMap
std::map< ShapeKey, FCLGeometryConstPtr, std::owner_less< ShapeKey > > ShapeMap
Definition: fcl/src/collision_common.cpp:405
collision_detection::DistanceRequest::active_components_only
const std::set< const moveit::core::LinkModel * > * active_components_only
The set of active components to check.
Definition: include/moveit/collision_detection/collision_common.h:316
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
fcl::BroadPhaseCollisionManager
collision_detection
Definition: collision_detector_allocator_allvalid.h:42
collision_detection::DistanceData::done
bool done
Indicates if distance query is finished.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:227


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21