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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Sep 8 2020 04:12:44