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 
39 #include <fcl/BVH/BVH_model.h>
40 #include <fcl/shape/geometric_shapes.h>
41 #include <fcl/octree.h>
42 #include <boost/thread/mutex.hpp>
43 #include <memory>
44 
45 namespace collision_detection
46 {
47 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data)
48 {
49  CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
50  if (cdata->done_)
51  return true;
52  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
53  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
54 
55  // do not collision check geoms part of the same object / link / attached body
56  if (cd1->sameObject(*cd2))
57  return false;
58 
59  // If active components are specified
60  if (cdata->active_components_only_)
61  {
62  const robot_model::LinkModel* l1 =
63  cd1->type == BodyTypes::ROBOT_LINK ?
64  cd1->ptr.link :
65  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
66  const robot_model::LinkModel* l2 =
67  cd2->type == BodyTypes::ROBOT_LINK ?
68  cd2->ptr.link :
69  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
70 
71  // If neither of the involved components is active
72  if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
73  (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
74  return false;
75  }
76 
77  // use the collision matrix (if any) to avoid certain collision checks
78  DecideContactFn dcf;
79  bool always_allow_collision = false;
80  if (cdata->acm_)
81  {
83  bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
84  if (found)
85  {
86  // if we have an entry in the collision matrix, we read it
87  if (type == AllowedCollision::ALWAYS)
88  {
89  always_allow_collision = true;
90  if (cdata->req_->verbose)
92  "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. No contacts are computed.",
93  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
94  }
95  else if (type == AllowedCollision::CONDITIONAL)
96  {
97  cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
98  if (cdata->req_->verbose)
99  CONSOLE_BRIDGE_logDebug("Collision between '%s' and '%s' is conditionally allowed", cd1->getID().c_str(),
100  cd2->getID().c_str());
101  }
102  }
103  }
104 
105  // check if a link is touching an attached object
107  {
108  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
109  if (tl.find(cd1->getID()) != tl.end())
110  {
111  always_allow_collision = true;
112  if (cdata->req_->verbose)
113  CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
114  cd1->getID().c_str(), cd2->getID().c_str());
115  }
116  }
117  else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
118  {
119  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
120  if (tl.find(cd2->getID()) != tl.end())
121  {
122  always_allow_collision = true;
123  if (cdata->req_->verbose)
124  CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
125  cd2->getID().c_str(), cd1->getID().c_str());
126  }
127  }
128  // bodies attached to the same link should not collide
130  {
131  if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
132  always_allow_collision = true;
133  }
134 
135  // if collisions are always allowed, we are done
136  if (always_allow_collision)
137  return false;
138 
139  if (cdata->req_->verbose)
140  CONSOLE_BRIDGE_logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(),
141  cd2->getID().c_str());
142 
143  // see if we need to compute a contact
144  std::size_t want_contact_count = 0;
145  if (cdata->req_->contacts)
146  if (cdata->res_->contact_count < cdata->req_->max_contacts)
147  {
148  std::size_t have;
149  if (cd1->getID() < cd2->getID())
150  {
151  std::pair<std::string, std::string> cp(cd1->getID(), cd2->getID());
152  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
153  }
154  else
155  {
156  std::pair<std::string, std::string> cp(cd2->getID(), cd1->getID());
157  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
158  }
159  if (have < cdata->req_->max_contacts_per_pair)
160  want_contact_count =
161  std::min(cdata->req_->max_contacts_per_pair - have, cdata->req_->max_contacts - cdata->res_->contact_count);
162  }
163 
164  if (dcf)
165  {
166  // if we have a decider for allowed contacts, we need to look at all the contacts
167  bool enable_cost = cdata->req_->cost;
168  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
169  bool enable_contact = true;
170  fcl::CollisionResult col_result;
171  int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(std::numeric_limits<size_t>::max(), enable_contact,
172  num_max_cost_sources, enable_cost),
173  col_result);
174  if (num_contacts > 0)
175  {
176  if (cdata->req_->verbose)
177  CONSOLE_BRIDGE_logInform("Found %d contacts between '%s' and '%s'. These contacts will be evaluated to check "
178  "if they are accepted or not",
179  num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
180  Contact c;
181  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
182  std::make_pair(cd1->getID(), cd2->getID()) :
183  std::make_pair(cd2->getID(), cd1->getID());
184  for (int i = 0; i < num_contacts; ++i)
185  {
186  fcl2contact(col_result.getContact(i), c);
187  // if the contact is not allowed, we have a collision
188  if (dcf(c) == false)
189  {
190  // store the contact, if it is needed
191  if (want_contact_count > 0)
192  {
193  --want_contact_count;
194  cdata->res_->contacts[pc].push_back(c);
195  cdata->res_->contact_count++;
196  if (cdata->req_->verbose)
197  CONSOLE_BRIDGE_logInform("Found unacceptable contact between '%s' and '%s'. Contact was stored.",
198  cd1->getID().c_str(), cd2->getID().c_str());
199  }
200  else if (cdata->req_->verbose)
202  "Found unacceptable contact between '%s' (type '%s') and '%s' (type '%s'). Contact was stored.",
203  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
204  cdata->res_->collision = true;
205  if (want_contact_count == 0)
206  break;
207  }
208  }
209  }
210 
211  if (enable_cost)
212  {
213  std::vector<fcl::CostSource> cost_sources;
214  col_result.getCostSources(cost_sources);
215 
216  CostSource cs;
217  for (auto& cost_source : cost_sources)
218  {
219  fcl2costsource(cost_source, cs);
220  cdata->res_->cost_sources.insert(cs);
221  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
222  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
223  }
224  }
225  }
226  else
227  {
228  if (want_contact_count > 0)
229  {
230  // otherwise, we need to compute more things
231  bool enable_cost = cdata->req_->cost;
232  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
233  bool enable_contact = true;
234 
235  fcl::CollisionResult col_result;
236  int num_contacts = fcl::collide(
237  o1, o2, fcl::CollisionRequest(want_contact_count, enable_contact, num_max_cost_sources, enable_cost),
238  col_result);
239  if (num_contacts > 0)
240  {
241  int num_contacts_initial = num_contacts;
242 
243  // make sure we don't get more contacts than we want
244  if (want_contact_count >= (std::size_t)num_contacts)
245  want_contact_count -= num_contacts;
246  else
247  {
248  num_contacts = want_contact_count;
249  want_contact_count = 0;
250  }
251 
252  if (cdata->req_->verbose)
253  CONSOLE_BRIDGE_logInform("Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), which constitute "
254  "a collision. %d contacts will be stored",
255  num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(),
256  cd2->getID().c_str(), cd2->getTypeString().c_str(), num_contacts);
257 
258  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
259  std::make_pair(cd1->getID(), cd2->getID()) :
260  std::make_pair(cd2->getID(), cd1->getID());
261  cdata->res_->collision = true;
262  for (int i = 0; i < num_contacts; ++i)
263  {
264  Contact c;
265  fcl2contact(col_result.getContact(i), c);
266  cdata->res_->contacts[pc].push_back(c);
267  cdata->res_->contact_count++;
268  }
269  }
270 
271  if (enable_cost)
272  {
273  std::vector<fcl::CostSource> cost_sources;
274  col_result.getCostSources(cost_sources);
275 
276  CostSource cs;
277  for (auto& cost_source : cost_sources)
278  {
279  fcl2costsource(cost_source, cs);
280  cdata->res_->cost_sources.insert(cs);
281  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
282  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
283  }
284  }
285  }
286  else
287  {
288  bool enable_cost = cdata->req_->cost;
289  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
290  bool enable_contact = false;
291  fcl::CollisionResult col_result;
292  int num_contacts =
293  fcl::collide(o1, o2, fcl::CollisionRequest(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
294  if (num_contacts > 0)
295  {
296  cdata->res_->collision = true;
297  if (cdata->req_->verbose)
299  "Found a contact between '%s' (type '%s') and '%s' (type '%s'), which constitutes a collision. "
300  "Contact information is not stored.",
301  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
302  }
303 
304  if (enable_cost)
305  {
306  std::vector<fcl::CostSource> cost_sources;
307  col_result.getCostSources(cost_sources);
308 
309  CostSource cs;
310  for (auto& cost_source : cost_sources)
311  {
312  fcl2costsource(cost_source, cs);
313  cdata->res_->cost_sources.insert(cs);
314  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
315  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
316  }
317  }
318  }
319  }
320 
321  if (cdata->res_->collision)
322  if (!cdata->req_->contacts || cdata->res_->contact_count >= cdata->req_->max_contacts)
323  {
324  if (!cdata->req_->cost)
325  cdata->done_ = true;
326  if (cdata->req_->verbose)
327  CONSOLE_BRIDGE_logInform("Collision checking is considered complete (collision was found and %u contacts are "
328  "stored)",
329  (unsigned int)cdata->res_->contact_count);
330  }
331 
332  if (!cdata->done_ && cdata->req_->is_done)
333  {
334  cdata->done_ = cdata->req_->is_done(*cdata->res_);
335  if (cdata->done_ && cdata->req_->verbose)
337  "Collision checking is considered complete due to external callback. %s was found. %u contacts are "
338  "stored.",
339  cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
340  }
341 
342  return cdata->done_;
343 }
344 
346 {
347  using ShapeKey = std::weak_ptr<const shapes::Shape>;
348  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
349 
351  {
352  }
353 
354  void bumpUseCount(bool force = false)
355  {
356  clean_count_++;
357 
358  // clean-up for cache (we don't want to keep infinitely large number of weak ptrs stored)
359  if (clean_count_ > MAX_CLEAN_COUNT || force)
360  {
361  clean_count_ = 0;
362  unsigned int from = map_.size();
363  for (auto it = map_.begin(); it != map_.end();)
364  {
365  auto nit = it;
366  ++nit;
367  if (it->first.expired())
368  map_.erase(it);
369  it = nit;
370  }
371  // CONSOLE_BRIDGE_logDebug("Cleaning up cache for FCL objects that correspond to static shapes. Cache size
372  // reduced from %u
373  // to %u", from, (unsigned int)map_.size());
374  }
375  }
376 
377  static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is
378  // executed (this is only removal of expired entries)
380  unsigned int clean_count_;
381  boost::mutex lock_;
382 };
383 
384 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist)
385 {
386  CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
387 
388  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
389  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
390 
391  // do not perform distance calculation for geoms part of the same object / link / attached body
392  if (cd1->sameObject(*cd2))
393  return false;
394 
395  // If active components are specified
396  if (cdata->active_components_only_)
397  {
398  const robot_model::LinkModel* l1 =
399  cd1->type == BodyTypes::ROBOT_LINK ?
400  cd1->ptr.link :
401  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
402  const robot_model::LinkModel* l2 =
403  cd2->type == BodyTypes::ROBOT_LINK ?
404  cd2->ptr.link :
405  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
406 
407  // If neither of the involved components is active
408  if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
409  (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
410  {
411  min_dist = cdata->res_->distance;
412  return cdata->done_;
413  }
414  }
415 
416  // use the collision matrix (if any) to avoid certain distance checks
417  bool always_allow_collision = false;
418  if (cdata->acm_)
419  {
421 
422  bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
423  if (found)
424  {
425  // if we have an entry in the collision matrix, we read it
426  if (type == AllowedCollision::ALWAYS)
427  {
428  always_allow_collision = true;
429  if (cdata->req_->verbose)
430  CONSOLE_BRIDGE_logDebug("Collision between '%s' and '%s' is always allowed. No distances are computed.",
431  cd1->getID().c_str(), cd2->getID().c_str());
432  }
433  }
434  }
435 
436  // check if a link is touching an attached object
438  {
439  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
440  if (tl.find(cd1->getID()) != tl.end())
441  {
442  always_allow_collision = true;
443  if (cdata->req_->verbose)
444  CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
445  cd1->getID().c_str(), cd2->getID().c_str());
446  }
447  }
448  else
449  {
451  {
452  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
453  if (tl.find(cd2->getID()) != tl.end())
454  {
455  always_allow_collision = true;
456  if (cdata->req_->verbose)
457  CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No distances are "
458  "computed.",
459  cd2->getID().c_str(), cd1->getID().c_str());
460  }
461  }
462  }
463 
464  if (always_allow_collision)
465  {
466  min_dist = cdata->res_->distance;
467  return cdata->done_;
468  }
469 
470  fcl::DistanceResult dist_result;
471  dist_result.update(cdata->res_->distance, nullptr, nullptr, fcl::DistanceResult::NONE,
472  fcl::DistanceResult::NONE); // can be faster
473  const double d = fcl::distance(o1, o2, fcl::DistanceRequest(), dist_result);
474 
475  if (cdata->req_->verbose)
476  CONSOLE_BRIDGE_logDebug("Distance between %s and %s: %f", cd1->getID().c_str(), cd2->getID().c_str(), d);
477 
478  if (d < 0) // a penetration was found, no further distance calculations are necessary
479  {
480  cdata->done_ = true;
481  cdata->res_->distance = -1;
482  }
483  else
484  {
485  if (cdata->res_->distance > d)
486  {
487  if (cdata->req_->verbose)
488  CONSOLE_BRIDGE_logWarn("Distance between %s and %s: %f decreased", cd1->getID().c_str(), cd2->getID().c_str(),
489  d);
490  cdata->res_->distance = d;
491  }
492  }
493 
494  min_dist = cdata->res_->distance;
495 
496  return cdata->done_;
497 }
498 
499 /* We template the function so we get a different cache for each of the template arguments combinations */
500 template <typename BV, typename T>
502 {
503  static FCLShapeCache cache;
504  return cache;
505 }
506 
507 template <typename T1, typename T2>
509 {
510  enum
511  {
512  value = 0
513  };
514 };
515 
516 template <typename T>
517 struct IfSameType<T, T>
518 {
519  enum
520  {
521  value = 1
522  };
523 };
524 
525 template <typename BV, typename T>
526 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
527 {
528  using ShapeKey = std::weak_ptr<const shapes::Shape>;
529  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
530 
531  FCLShapeCache& cache = GetShapeCache<BV, T>();
532 
533  std::weak_ptr<const shapes::Shape> wptr(shape);
534  {
535  boost::mutex::scoped_lock slock(cache.lock_);
536  ShapeMap::const_iterator cache_it = cache.map_.find(wptr);
537  if (cache_it != cache.map_.end())
538  {
539  if (cache_it->second->collision_geometry_data_->ptr.raw == (void*)data)
540  {
541  // CONSOLE_BRIDGE_logDebug("Collision data structures for object %s retrieved from cache.",
542  // cache_it->second->collision_geometry_data_->getID().c_str());
543  return cache_it->second;
544  }
545  else if (cache_it->second.unique())
546  {
547  const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
548  // CONSOLE_BRIDGE_logDebug("Collision data structures for object %s retrieved from cache after updating
549  // the source
550  // object.", cache_it->second->collision_geometry_data_->getID().c_str());
551  return cache_it->second;
552  }
553  }
554  }
555 
556  // attached objects could have previously been World::Object; we try to move them
557  // from their old cache to the new one, if possible. the code is not pretty, but should help
558  // when we attach/detach objects that are in the world
560  {
561  // get the cache that corresponds to objects; maybe this attached object used to be a world object
562  FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
563 
564  // attached bodies could be just moved from the environment.
565  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
566  auto cache_it = othercache.map_.find(wptr);
567  if (cache_it != othercache.map_.end())
568  {
569  if (cache_it->second.unique())
570  {
571  // remove from old cache
572  FCLGeometryConstPtr obj_cache = cache_it->second;
573  othercache.map_.erase(cache_it);
574  othercache.lock_.unlock();
575 
576  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
577  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
578 
579  // CONSOLE_BRIDGE_logDebug("Collision data structures for attached body %s retrieved from the cache for
580  // world objects.",
581  // obj_cache->collision_geometry_data_->getID().c_str());
582 
583  // add to the new cache
584  boost::mutex::scoped_lock slock(cache.lock_);
585  cache.map_[wptr] = obj_cache;
586  cache.bumpUseCount();
587  return obj_cache;
588  }
589  }
590  othercache.lock_.unlock();
591  }
592  else
593  // world objects could have previously been attached objects; we try to move them
594  // from their old cache to the new one, if possible. the code is not pretty, but should help
595  // when we attach/detach objects that are in the world
597  {
598  // get the cache that corresponds to objects; maybe this attached object used to be a world object
599  FCLShapeCache& othercache = GetShapeCache<BV, robot_state::AttachedBody>();
600 
601  // attached bodies could be just moved from the environment.
602  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
603  auto cache_it = othercache.map_.find(wptr);
604  if (cache_it != othercache.map_.end())
605  {
606  if (cache_it->second.unique())
607  {
608  // remove from old cache
609  FCLGeometryConstPtr obj_cache = cache_it->second;
610  othercache.map_.erase(cache_it);
611  othercache.lock_.unlock();
612 
613  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
614  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
615 
616  // CONSOLE_BRIDGE_logDebug("Collision data structures for world object %s retrieved from the cache for
617  // attached
618  // bodies.",
619  // obj_cache->collision_geometry_data_->getID().c_str());
620 
621  // add to the new cache
622  boost::mutex::scoped_lock slock(cache.lock_);
623  cache.map_[wptr] = obj_cache;
624  cache.bumpUseCount();
625  return obj_cache;
626  }
627  }
628  othercache.lock_.unlock();
629  }
630 
631  fcl::CollisionGeometry* cg_g = nullptr;
632  if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry
633  {
634  // handle cases individually
635  switch (shape->type)
636  {
637  case shapes::PLANE:
638  {
639  const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
640  cg_g = new fcl::Plane(p->a, p->b, p->c, p->d);
641  }
642  break;
643  default:
644  break;
645  }
646  }
647  else
648  {
649  switch (shape->type)
650  {
651  case shapes::SPHERE:
652  {
653  const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
654  cg_g = new fcl::Sphere(s->radius);
655  }
656  break;
657  case shapes::BOX:
658  {
659  const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
660  const double* size = s->size;
661  cg_g = new fcl::Box(size[0], size[1], size[2]);
662  }
663  break;
664  case shapes::CYLINDER:
665  {
666  const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
667  cg_g = new fcl::Cylinder(s->radius, s->length);
668  }
669  break;
670  case shapes::CONE:
671  {
672  const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
673  cg_g = new fcl::Cone(s->radius, s->length);
674  }
675  break;
676  case shapes::MESH:
677  {
678  auto g = new fcl::BVHModel<BV>();
679  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape.get());
680  if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
681  {
682  std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
683  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
684  tri_indices[i] =
685  fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
686 
687  std::vector<fcl::Vec3f> points(mesh->vertex_count);
688  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
689  points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
690 
691  g->beginModel();
692  g->addSubModel(points, tri_indices);
693  g->endModel();
694  }
695  cg_g = g;
696  }
697  break;
698  case shapes::OCTREE:
699  {
700  const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
701  cg_g = new fcl::OcTree(g->octree);
702  }
703  break;
704  default:
705  CONSOLE_BRIDGE_logError("This shape type (%d) is not supported using FCL yet", (int)shape->type);
706  cg_g = nullptr;
707  }
708  }
709  if (cg_g)
710  {
711  cg_g->computeLocalAABB();
712  FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
713  boost::mutex::scoped_lock slock(cache.lock_);
714  cache.map_[wptr] = res;
715  cache.bumpUseCount();
716  return res;
717  }
718  return FCLGeometryConstPtr();
719 }
720 
722 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
723  int shape_index)
724 {
725  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link, shape_index);
726 }
727 
728 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
729  int shape_index)
730 {
731  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab, shape_index);
732 }
733 
734 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj)
735 {
736  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj, 0);
737 }
738 
739 template <typename BV, typename T>
740 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
741  const T* data, int shape_index)
742 {
743  if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
744  fabs(padding) <= std::numeric_limits<double>::epsilon())
745  return createCollisionGeometry<BV, T>(shape, data, shape_index);
746  else
747  {
748  shapes::ShapePtr scaled_shape(shape->clone());
749  scaled_shape->scaleAndPadd(scale, padding);
750  return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
751  }
752 }
753 
754 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
755  const robot_model::LinkModel* link, int shape_index)
756 {
757  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link, shape_index);
758 }
759 
760 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
761  const robot_state::AttachedBody* ab, int shape_index)
762 {
763  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab, shape_index);
764 }
765 
766 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
767  const World::Object* obj)
768 {
769  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj, 0);
770 }
771 
773 {
774  FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
775  {
776  boost::mutex::scoped_lock slock(cache1.lock_);
777  cache1.bumpUseCount(true);
778  }
779  FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
780  {
781  boost::mutex::scoped_lock slock(cache2.lock_);
782  cache2.bumpUseCount(true);
783  }
784 }
785 }
786 
787 void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& kmodel)
788 {
789  if (kmodel->hasJointModelGroup(req_->group_name))
790  active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
791  else
792  active_components_only_ = nullptr;
793 }
794 
795 void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager* manager)
796 {
797  std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
798  for (std::size_t i = 0; i < collision_objects_.size(); ++i)
799  collision_objects[i] = collision_objects_[i].get();
800  if (!collision_objects.empty())
801  manager->registerObjects(collision_objects);
802 }
803 
804 void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManager* manager)
805 {
806  for (auto& collision_object : collision_objects_)
807  manager->unregisterObject(collision_object.get());
808 }
809 
811 {
812  collision_objects_.clear();
813  collision_geometry_.clear();
814 }
d
#define CONSOLE_BRIDGE_logDebug(fmt,...)
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
const CollisionRequest * req_
The collision request passed by the user.
CollisionResult * res_
The user specified response location.
bool done_
Flag indicating whether collision checking is complete.
FCLShapeCache & GetShapeCache()
std::map< ShapeKey, FCLGeometryConstPtr, std::owner_less< ShapeKey >> ShapeMap
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
union collision_detection::CollisionGeometryData::@0 ptr
#define CONSOLE_BRIDGE_logError(fmt,...)
double size[3]
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
XmlRpcServer s
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
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...
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
std::shared_ptr< Shape > ShapePtr
Generic interface to collision detection.
void unregisterFrom(fcl::BroadPhaseCollisionManager *manager)
bool collision
True if collision was found, false otherwise.
std::set< CostSource > cost_sources
When costs are computed, the individual cost sources are.
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
std::weak_ptr< const shapes::Shape > ShapeKey
std::shared_ptr< const octomap::OcTree > octree
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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:80
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
#define CONSOLE_BRIDGE_logInform(fmt,...)
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:94
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:82
const AllowedCollisionMatrix * acm_
The user specified collision matrix (may be NULL)
void fcl2costsource(const fcl::CostSource &fcs, CostSource &cs)
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...
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
When collision costs are computed, this structure contains information about the partial cost incurre...
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
void registerTo(fcl::BroadPhaseCollisionManager *manager)
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
static const unsigned int MAX_CLEAN_COUNT
const std::set< const robot_model::LinkModel * > * active_components_only_
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44