moveit2
The MoveIt Motion Planning Framework for ROS 2.
bullet_utils.cpp
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 
36 
37 #include <BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h>
38 #include <BulletCollision/CollisionShapes/btShapeHull.h>
39 #include <BulletCollision/Gimpact/btGImpactShape.h>
40 #include <geometric_shapes/shapes.h>
41 #include <memory>
42 #include <octomap/octomap.h>
43 #include <rclcpp/logger.hpp>
44 #include <rclcpp/logging.hpp>
45 
46 static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet");
47 
49 {
50 bool acmCheck(const std::string& body_1, const std::string& body_2,
52 {
54 
55  if (acm != nullptr)
56  {
57  if (acm->getAllowedCollision(body_1, body_2, allowed_type))
58  {
60  {
61  RCLCPP_DEBUG_STREAM(BULLET_LOGGER,
62  "Not allowed entry in ACM found, collision check between " << body_1 << " and " << body_2);
63  return false;
64  }
65  else
66  {
67  RCLCPP_DEBUG_STREAM(BULLET_LOGGER,
68  "Entry in ACM found, skipping collision check as allowed " << body_1 << " and " << body_2);
69  return true;
70  }
71  }
72  else
73  {
74  RCLCPP_DEBUG_STREAM(BULLET_LOGGER,
75  "No entry in ACM found, collision check between " << body_1 << " and " << body_2);
76  return false;
77  }
78  }
79  else
80  {
81  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "No ACM, collision check between " << body_1 << " and " << body_2);
82  return false;
83  }
84 }
85 
86 btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type)
87 {
88  (void)(collision_object_type);
89  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
90  const double* size = geom->size;
91  btScalar a = static_cast<btScalar>(size[0] / 2);
92  btScalar b = static_cast<btScalar>(size[1] / 2);
93  btScalar c = static_cast<btScalar>(size[2] / 2);
94 
95  return (new btBoxShape(btVector3(a, b, c)));
96 }
97 
98 btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type)
99 {
100  (void)(collision_object_type);
101  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
102  return (new btSphereShape(static_cast<btScalar>(geom->radius)));
103 }
104 
105 btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type)
106 {
107  (void)(collision_object_type);
108  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
109  btScalar r = static_cast<btScalar>(geom->radius);
110  btScalar l = static_cast<btScalar>(geom->length / 2);
111  return (new btCylinderShapeZ(btVector3(r, r, l)));
112 }
113 
114 btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type)
115 {
116  (void)(collision_object_type);
117  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
118  btScalar r = static_cast<btScalar>(geom->radius);
119  btScalar l = static_cast<btScalar>(geom->length);
120  return (new btConeShapeZ(r, l));
121 }
122 
123 btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const CollisionObjectType& collision_object_type,
125 {
126  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE ||
127  collision_object_type == CollisionObjectType::CONVEX_HULL ||
128  collision_object_type == CollisionObjectType::SDF);
129 
130  if (geom->vertex_count > 0 && geom->triangle_count > 0)
131  {
132  // convert the mesh to the assigned collision object type
133  switch (collision_object_type)
134  {
136  {
137  // Create a convex hull shape to approximate Trimesh
140  std::vector<int> faces;
141 
142  input.reserve(geom->vertex_count);
143  for (unsigned int i = 0; i < geom->vertex_count; ++i)
144  input.push_back(Eigen::Vector3d(geom->vertices[3 * i], geom->vertices[3 * i + 1], geom->vertices[3 * i + 2]));
145 
146  if (collision_detection_bullet::createConvexHull(vertices, faces, input) < 0)
147  return nullptr;
148 
149  btConvexHullShape* subshape = new btConvexHullShape();
150  for (const Eigen::Vector3d& v : vertices)
151  subshape->addPoint(
152  btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2])));
153 
154  return subshape;
155  }
157  {
158  btCompoundShape* compound =
159  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(geom->triangle_count));
160  compound->setMargin(
161  BULLET_MARGIN); // margin: compound seems to have no effect when positive but has an effect when negative
162 
163  for (unsigned i = 0; i < geom->triangle_count; ++i)
164  {
165  btVector3 v[3];
166  for (unsigned x = 0; x < 3; ++x)
167  {
168  unsigned idx = geom->triangles[3 * i + x];
169  for (unsigned y = 0; y < 3; ++y)
170  {
171  v[x][y] = static_cast<btScalar>(geom->vertices[3 * idx + y]);
172  }
173  }
174 
175  btCollisionShape* subshape = new btTriangleShapeEx(v[0], v[1], v[2]);
176  if (subshape != nullptr)
177  {
178  cow->manage(subshape);
179  subshape->setMargin(BULLET_MARGIN);
180  btTransform geom_trans;
181  geom_trans.setIdentity();
182  compound->addChildShape(geom_trans, subshape);
183  }
184  }
185 
186  return compound;
187  }
188  default:
189  {
190  RCLCPP_ERROR(BULLET_LOGGER, "This bullet shape type (%d) is not supported for geometry meshs",
191  static_cast<int>(collision_object_type));
192  return nullptr;
193  }
194  }
195  }
196  RCLCPP_ERROR(BULLET_LOGGER, "The mesh is empty!");
197  return nullptr;
198 }
199 
200 btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const CollisionObjectType& collision_object_type,
202 {
203  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE ||
204  collision_object_type == CollisionObjectType::CONVEX_HULL ||
205  collision_object_type == CollisionObjectType::SDF ||
206  collision_object_type == CollisionObjectType::MULTI_SPHERE);
207 
208  btCompoundShape* subshape =
209  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(geom->octree->size()));
210  double occupancy_threshold = geom->octree->getOccupancyThres();
211 
212  // convert the mesh to the assigned collision object type
213  switch (collision_object_type)
214  {
216  {
217  for (auto it = geom->octree->begin(static_cast<unsigned char>(geom->octree->getTreeDepth())),
218  end = geom->octree->end();
219  it != end; ++it)
220  {
221  if (it->getOccupancy() >= occupancy_threshold)
222  {
223  double size = it.getSize();
224  btTransform geom_trans;
225  geom_trans.setIdentity();
226  geom_trans.setOrigin(btVector3(static_cast<btScalar>(it.getX()), static_cast<btScalar>(it.getY()),
227  static_cast<btScalar>(it.getZ())));
228  btScalar l = static_cast<btScalar>(size / 2);
229  btBoxShape* childshape = new btBoxShape(btVector3(l, l, l));
230  childshape->setMargin(BULLET_MARGIN);
231  cow->manage(childshape);
232 
233  subshape->addChildShape(geom_trans, childshape);
234  }
235  }
236  return subshape;
237  }
239  {
240  for (auto it = geom->octree->begin(static_cast<unsigned char>(geom->octree->getTreeDepth())),
241  end = geom->octree->end();
242  it != end; ++it)
243  {
244  if (it->getOccupancy() >= occupancy_threshold)
245  {
246  double size = it.getSize();
247  btTransform geom_trans;
248  geom_trans.setIdentity();
249  geom_trans.setOrigin(btVector3(static_cast<btScalar>(it.getX()), static_cast<btScalar>(it.getY()),
250  static_cast<btScalar>(it.getZ())));
251  btSphereShape* childshape =
252  new btSphereShape(static_cast<btScalar>(std::sqrt(2 * ((size / 2) * (size / 2)))));
253  childshape->setMargin(BULLET_MARGIN);
254  cow->manage(childshape);
255 
256  subshape->addChildShape(geom_trans, childshape);
257  }
258  }
259  return subshape;
260  }
261  default:
262  {
263  RCLCPP_ERROR(BULLET_LOGGER, "This bullet shape type (%d) is not supported for geometry octree",
264  static_cast<int>(collision_object_type));
265  return nullptr;
266  }
267  }
268 }
269 
270 void updateCollisionObjectFilters(const std::vector<std::string>& active, CollisionObjectWrapper& cow)
271 {
272  // if not active make cow part of static
273  if (!isLinkActive(active, cow.getName()))
274  {
275  cow.m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
276  cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter;
277  }
278  else
279  {
280  cow.m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter;
281  cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter;
282  }
283 
284  if (cow.getBroadphaseHandle())
285  {
286  cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collisionFilterGroup;
287  cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collisionFilterMask;
288  }
289  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "COW " << cow.getName() << " group " << cow.m_collisionFilterGroup << " mask "
290  << cow.m_collisionFilterMask);
291 }
292 
293 CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow)
294 {
295  CollisionObjectWrapperPtr new_cow = cow->clone();
296 
297  btTransform tf;
298  tf.setIdentity();
299 
300  if (btBroadphaseProxy::isConvex(new_cow->getCollisionShape()->getShapeType()))
301  {
302  assert(dynamic_cast<btConvexShape*>(new_cow->getCollisionShape()) != nullptr);
303  btConvexShape* convex = static_cast<btConvexShape*>(new_cow->getCollisionShape());
304 
305  // This checks if the collision object is already a cast collision object
306  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
307 
308  CastHullShape* shape = new CastHullShape(convex, tf);
309 
310  new_cow->manage(shape);
311  new_cow->setCollisionShape(shape);
312  }
313  else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType()))
314  {
315  btCompoundShape* compound = static_cast<btCompoundShape*>(new_cow->getCollisionShape());
316  btCompoundShape* new_compound =
317  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes());
318 
319  for (int i = 0; i < compound->getNumChildShapes(); ++i)
320  {
321  if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType()))
322  {
323  btConvexShape* convex = static_cast<btConvexShape*>(compound->getChildShape(i));
324  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object
325 
326  btTransform geom_trans = compound->getChildTransform(i);
327 
328  btCollisionShape* subshape = new CastHullShape(convex, tf);
329 
330  new_cow->manage(subshape);
331  subshape->setMargin(BULLET_MARGIN);
332  new_compound->addChildShape(geom_trans, subshape);
333  }
334  else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
335  {
336  btCompoundShape* second_compound = static_cast<btCompoundShape*>(compound->getChildShape(i));
337  btCompoundShape* new_second_compound =
338  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes());
339  for (int j = 0; j < second_compound->getNumChildShapes(); ++j)
340  {
341  assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType()));
342 
343  btConvexShape* convex = static_cast<btConvexShape*>(second_compound->getChildShape(j));
344  assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object
345 
346  btTransform geom_trans = second_compound->getChildTransform(j);
347 
348  btCollisionShape* subshape = new CastHullShape(convex, tf);
349 
350  new_cow->manage(subshape);
351  subshape->setMargin(BULLET_MARGIN);
352  new_second_compound->addChildShape(geom_trans, subshape);
353  }
354 
355  btTransform geom_trans = compound->getChildTransform(i);
356 
357  new_cow->manage(new_second_compound);
358 
359  // margin on compound seems to have no effect when positive but has an effect when negative
360  new_second_compound->setMargin(BULLET_MARGIN);
361  new_compound->addChildShape(geom_trans, new_second_compound);
362  }
363  else
364  {
365  RCLCPP_ERROR_STREAM(BULLET_LOGGER,
366  "I can only collision check convex shapes and compound shapes made of convex shapes");
367  throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes");
368  }
369  }
370 
371  // margin on compound seems to have no effect when positive but has an effect when negative
372  new_compound->setMargin(BULLET_MARGIN);
373  new_cow->manage(new_compound);
374  new_cow->setCollisionShape(new_compound);
375  new_cow->setWorldTransform(cow->getWorldTransform());
376  }
377  else
378  {
379  RCLCPP_ERROR_STREAM(BULLET_LOGGER,
380  "I can only collision check convex shapes and compound shapes made of convex shapes");
381  throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes");
382  }
383 
384  return new_cow;
385 }
386 
387 void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow,
388  const std::unique_ptr<btBroadphaseInterface>& broadphase,
389  const std::unique_ptr<btCollisionDispatcher>& dispatcher)
390 {
391  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Added " << cow->getName() << " to broadphase");
392  btVector3 aabb_min, aabb_max;
393  cow->getAABB(aabb_min, aabb_max);
394 
395  int type = cow->getCollisionShape()->getShapeType();
396  cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collisionFilterGroup,
397  cow->m_collisionFilterMask, dispatcher.get()));
398 }
399 
400 btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom,
401  const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow)
402 {
403  switch (geom->type)
404  {
405  case shapes::BOX:
406  {
407  return createShapePrimitive(static_cast<const shapes::Box*>(geom.get()), collision_object_type);
408  }
409  case shapes::SPHERE:
410  {
411  return createShapePrimitive(static_cast<const shapes::Sphere*>(geom.get()), collision_object_type);
412  }
413  case shapes::CYLINDER:
414  {
415  return createShapePrimitive(static_cast<const shapes::Cylinder*>(geom.get()), collision_object_type);
416  }
417  case shapes::CONE:
418  {
419  return createShapePrimitive(static_cast<const shapes::Cone*>(geom.get()), collision_object_type);
420  }
421  case shapes::MESH:
422  {
423  return createShapePrimitive(static_cast<const shapes::Mesh*>(geom.get()), collision_object_type, cow);
424  }
425  case shapes::OCTREE:
426  {
427  return createShapePrimitive(static_cast<const shapes::OcTree*>(geom.get()), collision_object_type, cow);
428  }
429  default:
430  {
431  RCLCPP_ERROR(BULLET_LOGGER, "This geometric shape type (%d) is not supported using BULLET yet",
432  static_cast<int>(geom->type));
433  return nullptr;
434  }
435  }
436 }
437 
438 bool BroadphaseFilterCallback::needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
439 {
440  bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup);
441  cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup);
442 
443  if (cull)
444  return false;
445 
446  const CollisionObjectWrapper* cow0 = static_cast<const CollisionObjectWrapper*>(proxy0->m_clientObject);
447  const CollisionObjectWrapper* cow1 = static_cast<const CollisionObjectWrapper*>(proxy1->m_clientObject);
448 
449  if (!cow0->m_enabled)
450  return false;
451 
452  if (!cow1->m_enabled)
453  return false;
454 
457  if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end())
458  return false;
459 
462  if (cow1->m_touch_links.find(cow0->getName()) != cow1->m_touch_links.end())
463  return false;
464 
467  if (cow0->m_touch_links == cow1->m_touch_links)
468  return false;
469 
470  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Broadphase pass " << cow0->getName() << " vs " << cow1->getName());
471  return true;
472 }
473 
475  const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/,
476  int index0, const btCollisionObjectWrapper* colObj1Wrap,
477  int /*partId1*/, int index1)
478 {
479  if (cp.m_distance1 > static_cast<btScalar>(contact_distance_))
480  {
481  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Not close enough for collision with " << cp.m_distance1);
482  return 0;
483  }
484 
485  if (cast_)
486  {
487  return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_);
488  }
489  else
490  {
491  return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_);
492  }
493 }
494 
496 {
497  results_callback_.collisions_.pair_done = false;
498 
499  if (results_callback_.collisions_.done)
500  {
501  return false;
502  }
503 
504  const CollisionObjectWrapper* cow0 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy0->m_clientObject);
505  const CollisionObjectWrapper* cow1 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy1->m_clientObject);
506 
507  std::pair<std::string, std::string> pair_names{ cow0->getName(), cow1->getName() };
508  if (results_callback_.needsCollision(cow0, cow1))
509  {
510  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Processing " << cow0->getName() << " vs " << cow1->getName());
511  btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1);
512  btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1);
513 
514  // dispatcher will keep algorithms persistent in the collision pair
515  if (!pair.m_algorithm)
516  {
517  pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS);
518  }
519 
520  if (pair.m_algorithm)
521  {
522  TesseractBroadphaseBridgedManifoldResult contact_point_result(&obj0_wrap, &obj1_wrap, results_callback_);
523  contact_point_result.m_closestPointDistanceThreshold = static_cast<btScalar>(results_callback_.contact_distance_);
524 
525  // discrete collision detection query
526  pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result);
527  }
528  }
529  else
530  {
531  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Not processing " << cow0->getName() << " vs " << cow1->getName());
532  }
533  return false;
534 }
535 
537  const std::vector<shapes::ShapeConstPtr>& shapes,
538  const AlignedVector<Eigen::Isometry3d>& shape_poses,
539  const std::vector<CollisionObjectType>& collision_object_types,
540  bool active)
541  : m_name(name)
542  , m_type_id(type_id)
543  , m_shapes(shapes)
544  , m_shape_poses(shape_poses)
545  , m_collision_object_types(collision_object_types)
546 {
547  if (shapes.empty() || shape_poses.empty() ||
548  (shapes.size() != shape_poses.size() || collision_object_types.empty() ||
549  shapes.size() != collision_object_types.size()))
550  {
551  throw std::exception();
552  }
553 
554  this->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE);
555  assert(!name.empty());
556 
557  if (active)
558  {
559  m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter;
560  m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter;
561  }
562  else
563  {
564  m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
565  m_collisionFilterMask = btBroadphaseProxy::KinematicFilter;
566  }
567 
568  if (shapes.size() == 1)
569  {
570  btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this);
571  shape->setMargin(BULLET_MARGIN);
572  manage(shape);
573  setCollisionShape(shape);
574  setWorldTransform(convertEigenToBt(m_shape_poses[0]));
575  }
576  else
577  {
578  btCompoundShape* compound =
579  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(m_shapes.size()));
580  manage(compound);
581  // margin on compound seems to have no effect when positive but has an effect when negative
582  compound->setMargin(BULLET_MARGIN);
583  setCollisionShape(compound);
584 
585  setWorldTransform(convertEigenToBt(m_shape_poses[0]));
586  Eigen::Isometry3d inv_world = m_shape_poses[0].inverse();
587 
588  for (std::size_t j = 0; j < m_shapes.size(); ++j)
589  {
590  btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this);
591  if (subshape != nullptr)
592  {
593  manage(subshape);
594  subshape->setMargin(BULLET_MARGIN);
595  btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]);
596  compound->addChildShape(geom_trans, subshape);
597  }
598  }
599  }
600 }
601 
603  const std::vector<shapes::ShapeConstPtr>& shapes,
604  const AlignedVector<Eigen::Isometry3d>& shape_poses,
605  const std::vector<CollisionObjectType>& collision_object_types,
606  const std::set<std::string>& touch_links)
607  : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, true)
608 {
609  m_touch_links = touch_links;
610 }
611 
613  const std::vector<shapes::ShapeConstPtr>& shapes,
614  const AlignedVector<Eigen::Isometry3d>& shape_poses,
615  const std::vector<CollisionObjectType>& collision_object_types,
616  const std::vector<std::shared_ptr<void>>& data)
617  : m_name(name)
618  , m_type_id(type_id)
619  , m_shapes(shapes)
620  , m_shape_poses(shape_poses)
621  , m_collision_object_types(collision_object_types)
622  , m_data(data)
623 {
624 }
625 } // namespace collision_detection_bullet
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
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...
Tesseract bullet collision object.
Definition: bullet_utils.h:110
short int m_collisionFilterGroup
Bitfield specifies to which group the object belongs.
Definition: bullet_utils.h:132
bool m_enabled
Indicates if the collision object is used for a collision check.
Definition: bullet_utils.h:138
AlignedVector< Eigen::Isometry3d > m_shape_poses
The poses of the shapes, must be same length as m_shapes.
Definition: bullet_utils.h:227
void manage(T *t)
Manage memory of a raw pointer shape.
Definition: bullet_utils.h:198
const std::string & getName() const
Get the collision object name.
Definition: bullet_utils.h:144
std::set< std::string > m_touch_links
The robot links the collision objects is allowed to touch.
Definition: bullet_utils.h:141
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.
std::vector< shapes::ShapeConstPtr > m_shapes
The shapes that define the collision object.
Definition: bullet_utils.h:224
short int m_collisionFilterMask
Bitfield specifies against which other groups the object is collision checked.
Definition: bullet_utils.h:135
const collision_detection::BodyType & getTypeID() const
Get a user defined type.
Definition: bullet_utils.h:150
bool processOverlap(btBroadphasePair &pair) override
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Type
The types of bodies that are considered for collision.
@ ROBOT_LINK
A link on the robot.
@ ROBOT_ATTACHED
A body attached to a robot link.
btCollisionShape * createShapePrimitive(const shapes::ShapeConstPtr &geom, const CollisionObjectType &collision_object_type, CollisionObjectWrapper *cow)
Casts a geometric shape into a btCollisionShape.
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
Definition: bullet_utils.h:65
const btScalar BULLET_MARGIN
Definition: bullet_utils.h:51
CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr &cow)
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Definition: basic_types.h:37
void updateCollisionObjectFilters(const std::vector< std::string > &active, CollisionObjectWrapper &cow)
Update a collision objects filters.
bool acmCheck(const std::string &body_1, const std::string &body_2, const collision_detection::AllowedCollisionMatrix *acm)
Allowed = true.
const bool BULLET_COMPOUND_USE_DYNAMIC_AABB
Definition: bullet_utils.h:56
btScalar addCastSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int, const btCollisionObjectWrapper *colObj1Wrap, int, ContactTestData &collisions)
Definition: bullet_utils.h:420
void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Add the collision object to broadphase.
@ MULTI_SPHERE
Use the mesh and represent it by multiple spheres collision object.
@ SDF
Use the mesh and rpresent it by a signed distance fields collision object.
@ USE_SHAPE_TYPE
Infer the type from the type specified in the shapes::Shape class.
@ CONVEX_HULL
Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex it will be c...
const btScalar BULLET_DEFAULT_CONTACT_DISTANCE
Definition: bullet_utils.h:55
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:387
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...
int createConvexHull(AlignedVector< Eigen::Vector3d > &vertices, std::vector< int > &faces, const AlignedVector< Eigen::Vector3d > &input, double shrink=-1, double shrinkClamp=-1)
Create a convex hull from vertices using Bullet Convex Hull Computer.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
x
Definition: pick.py:64
y
Definition: pick.py:65
a
Definition: plan.py:54
r
Definition: plan.py:56
name
Definition: setup.py:7
Definition: world.h:49
const rclcpp::Logger BULLET_LOGGER
btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1)
This callback is used after btManifoldResult processed a collision result.
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:554
bool cast_
Indicates if the callback is used for casted collisions.
Definition: bullet_utils.h:541
bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const override
Casted collision shape used for checking if an object is collision free between two discrete poses.
Definition: bullet_utils.h:241
bool done
Indicates if search is finished.
Definition: basic_types.h:77
bool pair_done
Indicates if search between a single pair is finished.
Definition: basic_types.h:80