bullet_utils.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD-2-Clause)
3  *
4  * Copyright (c) 2017, Southwest Research Institute
5  * Copyright (c) 2013, John Schulman
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *********************************************************************/
32 
33 /* Authors: John Schulman, Levi Armstrong */
34 
35 #pragma once
36 
37 #include <btBulletCollisionCommon.h>
39 #include <ros/console.h>
40 
46 
48 {
49 #define METERS
50 
51 const btScalar BULLET_MARGIN = 0.0f;
52 const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS;
53 const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS;
54 const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit
55 const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported
56 const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true;
57 
58 MOVEIT_CLASS_FORWARD(CollisionObjectWrapper);
59 
61 inline bool acmCheck(const std::string& body_1, const std::string& body_2,
63 {
65 
66  if (acm != nullptr)
67  {
68  if (acm->getAllowedCollision(body_1, body_2, allowed_type))
69  {
71  {
72  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between "
73  << body_1 << " and " << body_2);
74  return false;
75  }
76  else
77  {
78  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed "
79  << body_1 << " and " << body_2);
80  return true;
81  }
82  }
83  else
84  {
85  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
86  "No entry in ACM found, collision check between " << body_1 << " and " << body_2);
87  return false;
88  }
89  }
90  else
91  {
92  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
93  "No ACM, collision check between " << body_1 << " and " << body_2);
94  return false;
95  }
96 }
97 
99 inline btVector3 convertEigenToBt(const Eigen::Vector3d& v)
100 {
101  return btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2]));
102 }
103 
105 inline Eigen::Vector3d convertBtToEigen(const btVector3& v)
106 {
107  return Eigen::Vector3d(static_cast<double>(v.x()), static_cast<double>(v.y()), static_cast<double>(v.z()));
108 }
109 
111 inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q)
112 {
113  return btQuaternion(static_cast<btScalar>(q.x()), static_cast<btScalar>(q.y()), static_cast<btScalar>(q.z()),
114  static_cast<btScalar>(q.w()));
115 }
116 
118 inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r)
119 {
120  return btMatrix3x3(static_cast<btScalar>(r(0, 0)), static_cast<btScalar>(r(0, 1)), static_cast<btScalar>(r(0, 2)),
121  static_cast<btScalar>(r(1, 0)), static_cast<btScalar>(r(1, 1)), static_cast<btScalar>(r(1, 2)),
122  static_cast<btScalar>(r(2, 0)), static_cast<btScalar>(r(2, 1)), static_cast<btScalar>(r(2, 2)));
123 }
124 
126 inline btTransform convertEigenToBt(const Eigen::Isometry3d& t)
127 {
128  const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0);
129  const Eigen::Vector3d& tran = t.translation();
130 
131  btMatrix3x3 mat = convertEigenToBt(rot);
132  btVector3 translation = convertEigenToBt(tran);
133 
134  return btTransform(mat, translation);
135 }
136 
143 class CollisionObjectWrapper : public btCollisionObject
144 {
145 public:
147 
151  CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id,
152  const std::vector<shapes::ShapeConstPtr>& shapes,
153  const AlignedVector<Eigen::Isometry3d>& shape_poses,
154  const std::vector<CollisionObjectType>& collision_object_types, bool active = true);
155 
159  CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id,
160  const std::vector<shapes::ShapeConstPtr>& shapes,
161  const AlignedVector<Eigen::Isometry3d>& shape_poses,
162  const std::vector<CollisionObjectType>& collision_object_types,
163  const std::set<std::string>& touch_links);
164 
166  short int m_collisionFilterGroup;
167 
169  short int m_collisionFilterMask;
170 
172  bool m_enabled{ true };
173 
175  std::set<std::string> m_touch_links;
176 
178  const std::string& getName() const
179  {
180  return m_name;
181  }
182 
185  {
186  return m_type_id;
187  }
188 
191  bool sameObject(const CollisionObjectWrapper& other) const
192  {
193  return m_name == other.m_name && m_type_id == other.m_type_id && m_shapes.size() == other.m_shapes.size() &&
194  m_shape_poses.size() == other.m_shape_poses.size() &&
195  std::equal(m_shapes.begin(), m_shapes.end(), other.m_shapes.begin()) &&
196  std::equal(m_shape_poses.begin(), m_shape_poses.end(), other.m_shape_poses.begin(),
197  [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); });
198  }
199 
203  void getAABB(btVector3& aabb_min, btVector3& aabb_max) const
204  {
205  getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max);
206  const btScalar& distance = getContactProcessingThreshold();
207  // note that bullet expands each AABB by 4 cm
208  btVector3 contact_threshold(distance, distance, distance);
209  aabb_min -= contact_threshold;
210  aabb_max += contact_threshold;
211  }
212 
215  std::shared_ptr<CollisionObjectWrapper> clone()
216  {
217  std::shared_ptr<CollisionObjectWrapper> clone_cow(
219  clone_cow->setCollisionShape(getCollisionShape());
220  clone_cow->setWorldTransform(getWorldTransform());
221  clone_cow->m_collisionFilterGroup = m_collisionFilterGroup;
222  clone_cow->m_collisionFilterMask = m_collisionFilterMask;
223  clone_cow->m_enabled = m_enabled;
224  clone_cow->setBroadphaseHandle(nullptr);
225  clone_cow->m_touch_links = m_touch_links;
226  clone_cow->setContactProcessingThreshold(this->getContactProcessingThreshold());
227  return clone_cow;
228  }
229 
231  template <class T>
232  void manage(T* t)
233  {
234  m_data.push_back(std::shared_ptr<T>(t));
235  }
236 
238  template <class T>
239  void manage(std::shared_ptr<T> t)
240  {
241  m_data.push_back(t);
242  }
243 
244 protected:
246  CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id,
247  const std::vector<shapes::ShapeConstPtr>& shapes,
248  const AlignedVector<Eigen::Isometry3d>& shape_poses,
249  const std::vector<CollisionObjectType>& collision_object_types,
250  const std::vector<std::shared_ptr<void>>& data);
251 
253  std::string m_name;
254 
256 
258  std::vector<shapes::ShapeConstPtr> m_shapes;
259 
261  AlignedVector<Eigen::Isometry3d> m_shape_poses;
262 
264  std::vector<CollisionObjectType> m_collision_object_types;
265 
267  std::vector<std::shared_ptr<void>> m_data;
268 };
269 
274 struct CastHullShape : public btConvexShape
275 {
276 public:
278  btConvexShape* m_shape;
279 
281  btTransform m_shape_transform;
282 
283  CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), m_shape_transform(t01)
284  {
285  m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
286  }
287 
288  void updateCastTransform(const btTransform& cast_transform)
289  {
290  m_shape_transform = cast_transform;
291  }
292 
294  btVector3 localGetSupportingVertex(const btVector3& vec) const override
295  {
296  btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec);
297  btVector3 support_vector_1 =
298  m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis());
299  return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1;
300  }
301 
302  void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/,
303  btVector3* /*supportVerticesOut*/,
304  int /*numVectors*/) const override
305  {
306  throw std::runtime_error("not implemented");
307  }
308 
312  void getAabb(const btTransform& transform_world, btVector3& aabbMin, btVector3& aabbMax) const override
313  {
314  m_shape->getAabb(transform_world, aabbMin, aabbMax);
315  btVector3 min1, max1;
316  m_shape->getAabb(transform_world * m_shape_transform, min1, max1);
317  aabbMin.setMin(min1);
318  aabbMax.setMax(max1);
319  }
320 
321  void getAabbSlow(const btTransform& /*t*/, btVector3& /*aabbMin*/, btVector3& /*aabbMax*/) const override
322  {
323  throw std::runtime_error("shouldn't happen");
324  }
325 
326  void setLocalScaling(const btVector3& /*scaling*/) override
327  {
328  }
329 
330  const btVector3& getLocalScaling() const override
331  {
332  static btVector3 out(1, 1, 1);
333  return out;
334  }
335 
336  void setMargin(btScalar /*margin*/) override
337  {
338  }
339 
340  btScalar getMargin() const override
341  {
342  return 0;
343  }
344 
345  int getNumPreferredPenetrationDirections() const override
346  {
347  return 0;
348  }
349 
350  void getPreferredPenetrationDirection(int /*index*/, btVector3& /*penetrationVector*/) const override
351  {
352  throw std::runtime_error("not implemented");
353  }
354 
355  void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const override
356  {
357  throw std::runtime_error("not implemented");
358  }
359 
360  const char* getName() const override
361  {
362  return "CastHull";
363  }
364 
365  btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override
366  {
367  return localGetSupportingVertex(v);
368  }
369 };
370 
379 inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport,
380  btVector3& outpt)
381 {
382  btVector3 pt_sum(0, 0, 0);
383  float pt_count = 0;
384  float max_support = -1000;
385 
386  const btPolyhedralConvexShape* pshape = dynamic_cast<const btPolyhedralConvexShape*>(shape);
387  if (pshape)
388  {
389  int n_pts = pshape->getNumVertices();
390 
391  for (int i = 0; i < n_pts; ++i)
392  {
393  btVector3 pt;
394  pshape->getVertex(i, pt);
395 
396  float sup = pt.dot(localNormal);
397  if (sup > max_support + BULLET_EPSILON)
398  {
399  pt_count = 1;
400  pt_sum = pt;
401  max_support = sup;
402  }
403  else if (sup < max_support - BULLET_EPSILON)
404  {
405  }
406  else
407  {
408  pt_count += 1;
409  pt_sum += pt;
410  }
411  }
412  outsupport = max_support;
413  outpt = pt_sum / pt_count;
414  }
415  else
416  {
417  outpt = shape->localGetSupportingVertexWithoutMargin(localNormal);
418  outsupport = localNormal.dot(outpt);
419  }
420 }
421 
423 inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,
424  const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions)
425 {
426  assert(dynamic_cast<const CollisionObjectWrapper*>(colObj0Wrap->getCollisionObject()) != nullptr);
427  assert(dynamic_cast<const CollisionObjectWrapper*>(colObj1Wrap->getCollisionObject()) != nullptr);
428  const CollisionObjectWrapper* cd0 = static_cast<const CollisionObjectWrapper*>(colObj0Wrap->getCollisionObject());
429  const CollisionObjectWrapper* cd1 = static_cast<const CollisionObjectWrapper*>(colObj1Wrap->getCollisionObject());
430 
431  std::pair<std::string, std::string> pc = getObjectPairKey(cd0->getName(), cd1->getName());
432 
433  const auto& it = collisions.res.contacts.find(pc);
434  bool found = (it != collisions.res.contacts.end());
435 
437  contact.body_name_1 = cd0->getName();
438  contact.body_name_2 = cd1->getName();
439  contact.depth = static_cast<double>(cp.m_distance1);
440  contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB);
441  contact.pos = convertBtToEigen(cp.m_positionWorldOnA);
442  contact.nearest_points[0] = contact.pos;
443  contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB);
444 
445  contact.body_type_1 = cd0->getTypeID();
446  contact.body_type_2 = cd1->getTypeID();
447 
448  if (!processResult(collisions, contact, pc, found))
449  {
450  return 0;
451  }
452 
453  return 1;
454 }
455 
456 inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*index0*/,
457  const btCollisionObjectWrapper* colObj1Wrap, int /*index1*/,
458  ContactTestData& collisions)
459 {
460  assert(dynamic_cast<const CollisionObjectWrapper*>(colObj0Wrap->getCollisionObject()) != nullptr);
461  assert(dynamic_cast<const CollisionObjectWrapper*>(colObj1Wrap->getCollisionObject()) != nullptr);
462 
463  const CollisionObjectWrapper* cd0 = static_cast<const CollisionObjectWrapper*>(colObj0Wrap->getCollisionObject());
464  const CollisionObjectWrapper* cd1 = static_cast<const CollisionObjectWrapper*>(colObj1Wrap->getCollisionObject());
465 
466  std::pair<std::string, std::string> pc = getObjectPairKey(cd0->getName(), cd1->getName());
467 
468  const auto& it = collisions.res.contacts.find(pc);
469  bool found = (it != collisions.res.contacts.end());
470 
472  contact.body_name_1 = cd0->getName();
473  contact.body_name_2 = cd1->getName();
474  contact.depth = static_cast<double>(cp.m_distance1);
475  contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB);
476  contact.pos = convertBtToEigen(cp.m_positionWorldOnA);
477 
478  contact.body_type_1 = cd0->getTypeID();
479  contact.body_type_2 = cd1->getTypeID();
480 
481  collision_detection::Contact* col = processResult(collisions, contact, pc, found);
482 
483  if (!col)
484  {
485  return 0;
486  }
487 
488  assert(!((cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) &&
489  (cd1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)));
490 
491  bool cast_shape_is_first = cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter;
492 
493  btVector3 normal_world_from_cast = (cast_shape_is_first ? -1 : 1) * cp.m_normalWorldOnB;
494  const btCollisionObjectWrapper* first_col_obj_wrap = (cast_shape_is_first ? colObj0Wrap : colObj1Wrap);
495 
496  // we want the contact information of the non-casted object come first, therefore swap values
497  if (cast_shape_is_first)
498  {
499  std::swap(col->nearest_points[0], col->nearest_points[1]);
500  contact.pos = convertBtToEigen(cp.m_positionWorldOnB);
501  std::swap(col->body_name_1, col->body_name_2);
502  std::swap(col->body_type_1, col->body_type_2);
503  col->normal *= -1;
504  }
505 
506  btTransform tf_world0, tf_world1;
507  const CastHullShape* shape = static_cast<const CastHullShape*>(first_col_obj_wrap->getCollisionShape());
508 
509  tf_world0 = first_col_obj_wrap->getWorldTransform();
510  tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->m_shape_transform;
511 
512  // transform normals into pose local coordinate systems
513  btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis();
514  btVector3 normal_local1 = normal_world_from_cast * tf_world1.getBasis();
515 
516  btVector3 pt_local0;
517  float localsup0;
518  getAverageSupport(shape->m_shape, normal_local0, localsup0, pt_local0);
519  btVector3 pt_world0 = tf_world0 * pt_local0;
520  btVector3 pt_local1;
521  float localsup1;
522  getAverageSupport(shape->m_shape, normal_local1, localsup1, pt_local1);
523  btVector3 pt_world1 = tf_world1 * pt_local1;
524 
525  float sup0 = normal_world_from_cast.dot(pt_world0);
526  float sup1 = normal_world_from_cast.dot(pt_world1);
527 
528  // TODO: this section is potentially problematic. think hard about the math
529  if (sup0 - sup1 > BULLET_SUPPORT_FUNC_TOLERANCE)
530  {
531  col->percent_interpolation = 0;
532  }
533  else if (sup1 - sup0 > BULLET_SUPPORT_FUNC_TOLERANCE)
534  {
535  col->percent_interpolation = 1;
536  }
537  else
538  {
539  const btVector3& pt_on_cast = cast_shape_is_first ? cp.m_positionWorldOnA : cp.m_positionWorldOnB;
540  float l0c = (pt_on_cast - pt_world0).length();
541  float l1c = (pt_on_cast - pt_world1).length();
542 
543  if (l0c + l1c < BULLET_LENGTH_TOLERANCE)
544  {
545  col->percent_interpolation = .5;
546  }
547  else
548  {
549  col->percent_interpolation = static_cast<double>(l0c / (l0c + l1c));
550  }
551  }
552 
553  return 1;
554 }
555 
557 inline bool isOnlyKinematic(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1)
558 {
559  return cow0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter &&
560  cow1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter;
561 }
562 
567 struct BroadphaseContactResultCallback
568 {
569  ContactTestData& collisions_;
570  double contact_distance_;
572 
574  bool self_;
575 
577  bool cast_{ false };
578 
579  BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance,
580  const collision_detection::AllowedCollisionMatrix* acm, bool self, bool cast = false)
581  : collisions_(collisions), contact_distance_(contact_distance), acm_(acm), self_(self), cast_(cast)
582  {
583  }
584 
586 
589  // TODO: Add check for two objects attached to the same link
590  bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const
591  {
592  if (cast_)
593  {
594  return !collisions_.done && !isOnlyKinematic(cow0, cow1) && !acmCheck(cow0->getName(), cow1->getName(), acm_);
595  }
596  else
597  {
598  return !collisions_.done && (self_ ? isOnlyKinematic(cow0, cow1) : !isOnlyKinematic(cow0, cow1)) &&
599  !acmCheck(cow0->getName(), cow1->getName(), acm_);
600  }
601  }
602 
604  btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/,
605  int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, int index1)
606  {
607  if (cp.m_distance1 > static_cast<btScalar>(contact_distance_))
608  {
609  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1);
610  return 0;
611  }
612 
613  if (cast_)
614  {
615  return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_);
616  }
617  else
618  {
619  return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_);
620  }
621  }
622 };
623 
624 struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult
625 {
626  BroadphaseContactResultCallback& result_callback_;
627 
628  TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap,
629  const btCollisionObjectWrapper* obj1Wrap,
630  BroadphaseContactResultCallback& result_callback)
631  : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback)
632  {
633  }
634 
635  void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override
636  {
638  depth > static_cast<btScalar>(result_callback_.contact_distance_))
639  {
640  return;
641  }
642 
643  bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
644  btVector3 point_a = pointInWorld + normalOnBInWorld * depth;
645  btVector3 local_a;
646  btVector3 local_b;
647  if (is_swapped)
648  {
649  local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a);
650  local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
651  }
652  else
653  {
654  local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a);
655  local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
656  }
657 
658  btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth);
659  new_pt.m_positionWorldOnA = point_a;
660  new_pt.m_positionWorldOnB = pointInWorld;
661 
662  // BP mod, store contact triangles.
663  if (is_swapped)
664  {
665  new_pt.m_partId0 = m_partId1;
666  new_pt.m_partId1 = m_partId0;
667  new_pt.m_index0 = m_index1;
668  new_pt.m_index1 = m_index0;
669  }
670  else
671  {
672  new_pt.m_partId0 = m_partId0;
673  new_pt.m_partId1 = m_partId1;
674  new_pt.m_index0 = m_index0;
675  new_pt.m_index1 = m_index1;
676  }
677 
678  // experimental feature info, for per-triangle material etc.
679  const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap;
680  const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap;
681  result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1,
682  new_pt.m_index1);
683  }
684 };
685 
690 class TesseractCollisionPairCallback : public btOverlapCallback
691 {
692  const btDispatcherInfo& dispatch_info_;
693  btCollisionDispatcher* dispatcher_;
694 
696  BroadphaseContactResultCallback& results_callback_;
697 
698 public:
699  TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher,
700  BroadphaseContactResultCallback& results_callback)
701  : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback)
702  {
703  }
704 
705  ~TesseractCollisionPairCallback() override = default;
706 
707  bool processOverlap(btBroadphasePair& pair) override
708  {
710 
712  {
713  return false;
714  }
715 
716  const CollisionObjectWrapper* cow0 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy0->m_clientObject);
717  const CollisionObjectWrapper* cow1 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy1->m_clientObject);
718 
719  std::pair<std::string, std::string> pair_names{ cow0->getName(), cow1->getName() };
721  {
722  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
723  "Processing " << cow0->getName() << " vs " << cow1->getName());
724  btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1);
725  btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1);
726 
727  // dispatcher will keep algorithms persistent in the collision pair
728  if (!pair.m_algorithm)
729  {
730  pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS);
731  }
732 
733  if (pair.m_algorithm)
734  {
735  TesseractBroadphaseBridgedManifoldResult contact_point_result(&obj0_wrap, &obj1_wrap, results_callback_);
736  contact_point_result.m_closestPointDistanceThreshold =
737  static_cast<btScalar>(results_callback_.contact_distance_);
738 
739  // discrete collision detection query
740  pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result);
741  }
742  }
743  else
744  {
745  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
746  "Not processing " << cow0->getName() << " vs " << cow1->getName());
747  }
748  return false;
749  }
750 };
751 
753 btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom,
754  const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow);
755 
764 inline void updateCollisionObjectFilters(const std::vector<std::string>& active, CollisionObjectWrapper& cow)
765 {
766  // if not active make cow part of static
767  if (!isLinkActive(active, cow.getName()))
768  {
769  cow.m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
770  cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter;
771  }
772  else
773  {
774  cow.m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter;
775  cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter;
776  }
777 
778  if (cow.getBroadphaseHandle())
779  {
780  cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collisionFilterGroup;
781  cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collisionFilterMask;
782  }
783  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "COW " << cow.getName() << " group "
784  << cow.m_collisionFilterGroup << " mask "
785  << cow.m_collisionFilterMask);
786 }
787 
788 inline CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow)
789 {
790  CollisionObjectWrapperPtr new_cow = cow->clone();
791 
792  btTransform tf;
793  tf.setIdentity();
794 
795  if (btBroadphaseProxy::isConvex(new_cow->getCollisionShape()->getShapeType()))
796  {
797  assert(dynamic_cast<btConvexShape*>(new_cow->getCollisionShape()) != nullptr);
798  btConvexShape* convex = static_cast<btConvexShape*>(new_cow->getCollisionShape());
799 
800  // This checks if the collision object is already a cast collision object
801  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
802 
803  CastHullShape* shape = new CastHullShape(convex, tf);
804 
805  new_cow->manage(shape);
806  new_cow->setCollisionShape(shape);
807  }
808  else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType()))
809  {
810  btCompoundShape* compound = static_cast<btCompoundShape*>(new_cow->getCollisionShape());
811  btCompoundShape* new_compound =
812  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes());
813 
814  for (int i = 0; i < compound->getNumChildShapes(); ++i)
815  {
816  if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType()))
817  {
818  btConvexShape* convex = static_cast<btConvexShape*>(compound->getChildShape(i));
819  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object
820 
821  btTransform geom_trans = compound->getChildTransform(i);
822 
823  btCollisionShape* subshape = new CastHullShape(convex, tf);
824 
825  new_cow->manage(subshape);
826  subshape->setMargin(BULLET_MARGIN);
827  new_compound->addChildShape(geom_trans, subshape);
828  }
829  else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
830  {
831  btCompoundShape* second_compound = static_cast<btCompoundShape*>(compound->getChildShape(i));
832  btCompoundShape* new_second_compound =
833  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes());
834  for (int j = 0; j < second_compound->getNumChildShapes(); ++j)
835  {
836  assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType()));
837 
838  btConvexShape* convex = static_cast<btConvexShape*>(second_compound->getChildShape(j));
839  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object
840 
841  btTransform geom_trans = second_compound->getChildTransform(j);
842 
843  btCollisionShape* subshape = new CastHullShape(convex, tf);
844 
845  new_cow->manage(subshape);
846  subshape->setMargin(BULLET_MARGIN);
847  new_second_compound->addChildShape(geom_trans, subshape);
848  }
849 
850  btTransform geom_trans = compound->getChildTransform(i);
851 
852  new_cow->manage(new_second_compound);
853 
854  // margin on compound seems to have no effect when positive but has an effect when negative
855  new_second_compound->setMargin(BULLET_MARGIN);
856  new_compound->addChildShape(geom_trans, new_second_compound);
857  }
858  else
859  {
860  ROS_ERROR_NAMED("collision_detection.bullet",
861  "I can only collision check convex shapes and compound shapes made of convex shapes");
862  throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes");
863  }
864  }
865 
866  // margin on compound seems to have no effect when positive but has an effect when negative
867  new_compound->setMargin(BULLET_MARGIN);
868  new_cow->manage(new_compound);
869  new_cow->setCollisionShape(new_compound);
870  new_cow->setWorldTransform(cow->getWorldTransform());
871  }
872  else
873  {
874  ROS_ERROR_NAMED("collision_detection.bullet",
875  "I can only collision check convex shapes and compound shapes made of convex shapes");
876  throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes");
877  }
878 
879  return new_cow;
880 }
881 
886 inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow,
887  const std::unique_ptr<btBroadphaseInterface>& broadphase,
888  const std::unique_ptr<btCollisionDispatcher>& dispatcher)
889 {
890  btVector3 aabb_min, aabb_max;
891  cow->getAABB(aabb_min, aabb_max);
892 
893  assert(cow->getBroadphaseHandle() != nullptr);
894  broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get());
895 }
896 
901 inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow,
902  const std::unique_ptr<btBroadphaseInterface>& broadphase,
903  const std::unique_ptr<btCollisionDispatcher>& dispatcher)
904 {
905  btBroadphaseProxy* bp = cow->getBroadphaseHandle();
906  if (bp)
907  {
908  // only clear the cached algorithms
909  broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get());
910  broadphase->destroyProxy(bp, dispatcher.get());
911  cow->setBroadphaseHandle(nullptr);
912  }
913 }
914 
919 inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow,
920  const std::unique_ptr<btBroadphaseInterface>& broadphase,
921  const std::unique_ptr<btCollisionDispatcher>& dispatcher)
922 {
923  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Added " << cow->getName() << " to broadphase");
924  btVector3 aabb_min, aabb_max;
925  cow->getAABB(aabb_min, aabb_max);
926 
927  int type = cow->getCollisionShape()->getShapeType();
928  cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collisionFilterGroup,
929  cow->m_collisionFilterMask, dispatcher.get()));
930 }
931 
932 struct BroadphaseFilterCallback : public btOverlapFilterCallback
933 {
934  bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override
935  {
936  bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup);
937  cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup);
938 
939  if (cull)
940  return false;
941 
942  const CollisionObjectWrapper* cow0 = static_cast<const CollisionObjectWrapper*>(proxy0->m_clientObject);
943  const CollisionObjectWrapper* cow1 = static_cast<const CollisionObjectWrapper*>(proxy1->m_clientObject);
944 
945  if (!cow0->m_enabled)
946  return false;
947 
948  if (!cow1->m_enabled)
949  return false;
950 
951  if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
952  cow1->getTypeID() == collision_detection::BodyType::ROBOT_LINK)
953  if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end())
954  return false;
955 
956  if (cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
957  cow0->getTypeID() == collision_detection::BodyType::ROBOT_LINK)
958  if (cow1->m_touch_links.find(cow0->getName()) != cow1->m_touch_links.end())
959  return false;
960 
961  if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
963  if (cow0->m_touch_links == cow1->m_touch_links)
964  return false;
965 
966  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
967  "Broadphase pass " << cow0->getName() << " vs " << cow1->getName());
968  return true;
969  }
970 };
971 
972 } // namespace collision_detection_bullet
collision_detection_bullet::CastHullShape
Casted collision shape used for checking if an object is collision free between two discrete poses.
Definition: bullet_utils.h:304
collision_detection_bullet::TesseractCollisionPairCallback::results_callback_
BroadphaseContactResultCallback & results_callback_
Callback executed for each broadphase pair to check if needs collision.
Definition: bullet_utils.h:726
collision_detection_bullet::TesseractCollisionPairCallback::TesseractCollisionPairCallback
TesseractCollisionPairCallback(const btDispatcherInfo &dispatchInfo, btCollisionDispatcher *dispatcher, BroadphaseContactResultCallback &results_callback)
Definition: bullet_utils.h:729
collision_detection_bullet::ContactTestData::pair_done
bool pair_done
Indicates if search between a single pair is finished.
Definition: basic_types.h:96
collision_detection_bullet::CastHullShape::localGetSupportingVertex
btVector3 localGetSupportingVertex(const btVector3 &vec) const override
From both shape poses computes the support vertex and returns the larger one.
Definition: bullet_utils.h:324
collision_detection_bullet::CollisionObjectWrapper::getAABB
void getAABB(btVector3 &aabb_min, btVector3 &aabb_max) const
Get the collision objects axis aligned bounding box.
Definition: bullet_utils.h:233
collision_detection_bullet
Definition: basic_types.h:34
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
collision_detection_bullet::CastHullShape::m_shape_transform
btTransform m_shape_transform
Transformation from the first pose to the second pose.
Definition: bullet_utils.h:311
shapes
collision_detection_bullet::CastHullShape::CastHullShape
CastHullShape(btConvexShape *shape, const btTransform &t01)
Definition: bullet_utils.h:313
collision_detection::Contact::depth
double depth
depth (penetration between bodies)
Definition: include/moveit/collision_detection/collision_common.h:116
collision_detection_bullet::CastHullShape::getNumPreferredPenetrationDirections
int getNumPreferredPenetrationDirections() const override
Definition: bullet_utils.h:375
collision_detection_bullet::CollisionObjectWrapper::m_collisionFilterMask
short int m_collisionFilterMask
Bitfield specifies against which other groups the object is collision checked.
Definition: bullet_utils.h:199
collision_detection_bullet::CollisionObjectWrapper::clone
std::shared_ptr< CollisionObjectWrapper > clone()
Clones the collision objects but not the collision shape wich is const.
Definition: bullet_utils.h:245
collision_detection_bullet::CastHullShape::getPreferredPenetrationDirection
void getPreferredPenetrationDirection(int, btVector3 &) const override
Definition: bullet_utils.h:380
basic_types.h
collision_detection_bullet::BroadphaseContactResultCallback::addSingleResult
btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int, int index0, const btCollisionObjectWrapper *colObj1Wrap, int, int index1)
This callback is used after btManifoldResult processed a collision result.
Definition: bullet_utils.h:634
collision_detection_bullet::convertBtToEigen
Eigen::Vector3d convertBtToEigen(const btVector3 &v)
Converts bullet vector to eigen vector.
Definition: bullet_utils.h:135
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
collision_detection_bullet::TesseractBroadphaseBridgedManifoldResult::result_callback_
BroadphaseContactResultCallback & result_callback_
Definition: bullet_utils.h:656
collision_detection_bullet::CastHullShape::getAabbSlow
void getAabbSlow(const btTransform &, btVector3 &, btVector3 &) const override
Definition: bullet_utils.h:351
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
mesh_operations.h
collision_detection_bullet::BroadphaseFilterCallback
Definition: bullet_utils.h:962
collision_detection_bullet::CollisionObjectType
CollisionObjectType
Definition: basic_types.h:62
collision_detection_bullet::getObjectPairKey
std::pair< std::string, std::string > getObjectPairKey(const std::string &obj1, const std::string &obj2)
Get a key for two object to search the collision matrix.
Definition: contact_checker_common.h:56
collision_detection_bullet::BroadphaseContactResultCallback::contact_distance_
double contact_distance_
Definition: bullet_utils.h:600
collision_detection_bullet::BULLET_SUPPORT_FUNC_TOLERANCE
const btScalar BULLET_SUPPORT_FUNC_TOLERANCE
Definition: bullet_utils.h:82
collision_detection_bullet::CollisionObjectWrapper::manage
void manage(T *t)
Manage memory of a raw pointer shape.
Definition: bullet_utils.h:262
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
collision_detection_bullet::isOnlyKinematic
bool isOnlyKinematic(const CollisionObjectWrapper *cow0, const CollisionObjectWrapper *cow1)
Checks if the collision pair is kinematic vs kinematic objects.
Definition: bullet_utils.h:587
collision_detection_bullet::addCastSingleResult
btScalar addCastSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int, const btCollisionObjectWrapper *colObj1Wrap, int, ContactTestData &collisions)
Definition: bullet_utils.h:486
class_forward.h
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
contact_checker_common.h
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_detection_bullet::BroadphaseContactResultCallback::self_
bool self_
Indicates if the callback is used for only self-collision checking.
Definition: bullet_utils.h:604
collision_detection_bullet::CastHullShape::updateCastTransform
void updateCastTransform(const btTransform &cast_transform)
Definition: bullet_utils.h:318
collision_detection_bullet::addCollisionObjectToBroadphase
void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Add the collision object to broadphase.
Definition: bullet_utils.h:949
collision_detection_bullet::BroadphaseContactResultCallback::collisions_
ContactTestData & collisions_
Definition: bullet_utils.h:599
f
f
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
collision_detection_bullet::CollisionObjectWrapper::CollisionObjectWrapper
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionObjectWrapper(const std::string &name, const collision_detection::BodyType &type_id, const std::vector< shapes::ShapeConstPtr > &shapes, const AlignedVector< Eigen::Isometry3d > &shape_poses, const std::vector< CollisionObjectType > &collision_object_types, bool active=true)
Standard constructor.
Definition: bullet_utils.cpp:298
collision_detection_bullet::BULLET_LENGTH_TOLERANCE
const btScalar BULLET_LENGTH_TOLERANCE
Definition: bullet_utils.h:83
collision_detection_bullet::TesseractBroadphaseBridgedManifoldResult
Definition: bullet_utils.h:654
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
collision_detection_bullet::CollisionObjectWrapper::getTypeID
const collision_detection::BodyType & getTypeID() const
Get a user defined type.
Definition: bullet_utils.h:214
collision_common.h
console.h
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::Contact::nearest_points
Eigen::Vector3d nearest_points[2]
The two nearest points connecting the two bodies.
Definition: include/moveit/collision_detection/collision_common.h:137
collision_detection_bullet::TesseractCollisionPairCallback::~TesseractCollisionPairCallback
~TesseractCollisionPairCallback() override=default
collision_detection_bullet::BroadphaseContactResultCallback::~BroadphaseContactResultCallback
~BroadphaseContactResultCallback()=default
declare_ptr.h
collision_detection_bullet::acmCheck
bool acmCheck(const std::string &body_1, const std::string &body_2, const collision_detection::AllowedCollisionMatrix *acm)
Allowed = true.
Definition: bullet_utils.h:91
name
std::string name
collision_detection_bullet::getAverageSupport
void getAverageSupport(const btConvexShape *shape, const btVector3 &localNormal, float &outsupport, btVector3 &outpt)
Computes the local supporting vertex of a convex shape.
Definition: bullet_utils.h:409
collision_detection_bullet::TesseractCollisionPairCallback::processOverlap
bool processOverlap(btBroadphasePair &pair) override
Definition: bullet_utils.h:737
collision_detection_bullet::CollisionObjectWrapper::m_enabled
bool m_enabled
Indicates if the collision object is used for a collision check.
Definition: bullet_utils.h:202
collision_detection_bullet::BroadphaseFilterCallback::needBroadphaseCollision
bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const override
Definition: bullet_utils.h:964
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
collision_detection_bullet::CollisionObjectWrapper::m_shape_poses
AlignedVector< Eigen::Isometry3d > m_shape_poses
The poses of the shapes, must be same length as m_shapes.
Definition: bullet_utils.h:291
collision_detection_bullet::BroadphaseContactResultCallback::needsCollision
bool needsCollision(const CollisionObjectWrapper *cow0, const CollisionObjectWrapper *cow1) const
This callback is used for each overlapping pair in a pair cache of the broadphase interface to check ...
Definition: bullet_utils.h:620
collision_detection_bullet::convertEigenToBt
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
Definition: bullet_utils.h:129
collision_detection_bullet::BULLET_EPSILON
const btScalar BULLET_EPSILON
Definition: bullet_utils.h:84
collision_detection_bullet::CastHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin
void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *, btVector3 *, int) const override
Definition: bullet_utils.h:332
collision_detection_bullet::CollisionObjectWrapper
Tesseract bullet collision object.
Definition: bullet_utils.h:173
collision_detection_bullet::BULLET_COMPOUND_USE_DYNAMIC_AABB
const bool BULLET_COMPOUND_USE_DYNAMIC_AABB
Definition: bullet_utils.h:86
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
collision_detection_bullet::TesseractCollisionPairCallback::dispatch_info_
const btDispatcherInfo & dispatch_info_
Definition: bullet_utils.h:722
collision_detection_bullet::isLinkActive
bool isLinkActive(const std::vector< std::string > &active, const std::string &name)
This will check if a link is active provided a list. If the list is empty the link is considered acti...
Definition: contact_checker_common.h:66
collision_detection_bullet::CollisionObjectWrapper::m_touch_links
std::set< std::string > m_touch_links
The robot links the collision objects is allowed to touch.
Definition: bullet_utils.h:205
collision_detection_bullet::CastHullShape::setMargin
void setMargin(btScalar) override
Definition: bullet_utils.h:366
collision_detection_bullet::makeCastCollisionObject
CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr &cow)
Definition: bullet_utils.h:818
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
collision_detection_bullet::updateBroadphaseAABB
void updateBroadphaseAABB(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Update the Broadphase AABB for the input collision object.
Definition: bullet_utils.h:916
collision_detection_bullet::CollisionObjectWrapper::m_collision_object_types
std::vector< CollisionObjectType > m_collision_object_types
The shape collision object type to be used.
Definition: bullet_utils.h:294
collision_detection_bullet::TesseractBroadphaseBridgedManifoldResult::TesseractBroadphaseBridgedManifoldResult
TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper *obj0Wrap, const btCollisionObjectWrapper *obj1Wrap, BroadphaseContactResultCallback &result_callback)
Definition: bullet_utils.h:658
r
S r
collision_detection_bullet::createShapePrimitive
btCollisionShape * createShapePrimitive(const shapes::ShapeConstPtr &geom, const CollisionObjectType &collision_object_type, CollisionObjectWrapper *cow)
Casts a geometric shape into a btCollisionShape.
Definition: bullet_utils.cpp:261
collision_detection_bullet::CollisionObjectWrapper::m_type_id
collision_detection::BodyType m_type_id
Definition: bullet_utils.h:285
collision_detection_bullet::CollisionObjectWrapper::m_shapes
std::vector< shapes::ShapeConstPtr > m_shapes
The shapes that define the collison object.
Definition: bullet_utils.h:288
collision_detection_bullet::CollisionObjectWrapper::m_collisionFilterGroup
short int m_collisionFilterGroup
Bitfield specifies to which group the object belongs.
Definition: bullet_utils.h:196
collision_detection::Contact::percent_interpolation
double percent_interpolation
The distance percentage between casted poses until collision.
Definition: include/moveit/collision_detection/collision_common.h:134
collision_detection_bullet::removeCollisionObjectFromBroadphase
void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Remove the collision object from broadphase.
Definition: bullet_utils.h:931
collision_detection_bullet::updateCollisionObjectFilters
void updateCollisionObjectFilters(const std::vector< std::string > &active, CollisionObjectWrapper &cow)
Update a collision objects filters.
Definition: bullet_utils.h:794
collision_detection_bullet::addDiscreteSingleResult
btScalar addDiscreteSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, const btCollisionObjectWrapper *colObj1Wrap, ContactTestData &collisions)
Converts a bullet contact result to MoveIt format and adds it to the result data structure.
Definition: bullet_utils.h:453
collision_detection::BodyTypes::Type
Type
The types of bodies that are considered for collision.
Definition: include/moveit/collision_detection/collision_common.h:88
collision_detection_bullet::CollisionObjectWrapper::m_name
std::string m_name
The name of the object, must be unique.
Definition: bullet_utils.h:283
collision_detection_bullet::ContactTestData::done
bool done
Indicates if search is finished.
Definition: basic_types.h:93
collision_detection::Contact::pos
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
Definition: include/moveit/collision_detection/collision_common.h:110
collision_detection_bullet::processResult
collision_detection::Contact * processResult(ContactTestData &cdata, collision_detection::Contact &contact, const std::pair< std::string, std::string > &key, bool found)
Stores a single contact result in the requested way.
Definition: contact_checker_common.h:74
collision_detection_bullet::BroadphaseContactResultCallback::cast_
bool cast_
Indicates if the callback is used for casted collisions.
Definition: bullet_utils.h:607
collision_detection::Contact::normal
Eigen::Vector3d normal
normal unit vector at contact
Definition: include/moveit/collision_detection/collision_common.h:113
collision_detection_bullet::CollisionObjectWrapper::sameObject
bool sameObject(const CollisionObjectWrapper &other) const
Check if two CollisionObjectWrapper objects point to the same source object.
Definition: bullet_utils.h:221
collision_detection_bullet::CastHullShape::getLocalScaling
const btVector3 & getLocalScaling() const override
Definition: bullet_utils.h:360
collision_detection_bullet::CollisionObjectWrapper::getName
const std::string & getName() const
Get the collision object name.
Definition: bullet_utils.h:208
collision_detection_bullet::CastHullShape::m_shape
btConvexShape * m_shape
The casted shape.
Definition: bullet_utils.h:308
collision_detection_bullet::CastHullShape::getAabb
void getAabb(const btTransform &transform_world, btVector3 &aabbMin, btVector3 &aabbMax) const override
Shape specific fast recalculation of the AABB at a certain pose.
Definition: bullet_utils.h:342
collision_detection_bullet::BroadphaseContactResultCallback::acm_
const collision_detection::AllowedCollisionMatrix * acm_
Definition: bullet_utils.h:601
METERS
#define METERS
Definition: bullet_utils.h:79
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection_bullet::TesseractBroadphaseBridgedManifoldResult::addContactPoint
void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth) override
Definition: bullet_utils.h:665
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
collision_detection_bullet::CastHullShape::setLocalScaling
void setLocalScaling(const btVector3 &) override
Definition: bullet_utils.h:356
collision_detection_bullet::CastHullShape::getMargin
btScalar getMargin() const override
Definition: bullet_utils.h:370
collision_detection_bullet::CollisionObjectWrapper::m_data
std::vector< std::shared_ptr< void > > m_data
Manages the collision shape pointer so they get destroyed.
Definition: bullet_utils.h:297
collision_detection_bullet::CastHullShape::getName
const char * getName() const override
Definition: bullet_utils.h:390
collision_detection_bullet::TesseractCollisionPairCallback::dispatcher_
btCollisionDispatcher * dispatcher_
Definition: bullet_utils.h:723
collision_detection_bullet::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(BulletBVHManager)
collision_detection_bullet::BroadphaseContactResultCallback::BroadphaseContactResultCallback
BroadphaseContactResultCallback(ContactTestData &collisions, double contact_distance, const collision_detection::AllowedCollisionMatrix *acm, bool self, bool cast=false)
Definition: bullet_utils.h:609
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
collision_detection_bullet::CastHullShape::localGetSupportingVertexWithoutMargin
btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &v) const override
Definition: bullet_utils.h:395
collision_detection_bullet::CastHullShape::calculateLocalInertia
void calculateLocalInertia(btScalar, btVector3 &) const override
Definition: bullet_utils.h:385
t
geometry_msgs::TransformStamped t
collision_detection::AllowedCollision::NEVER
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Definition: collision_matrix.h:153
collision_detection_bullet::BULLET_DEFAULT_CONTACT_DISTANCE
const btScalar BULLET_DEFAULT_CONTACT_DISTANCE
Definition: bullet_utils.h:85
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
collision_detection_bullet::BULLET_MARGIN
const btScalar BULLET_MARGIN
Definition: bullet_utils.h:81


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56