37 #include <BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h>
38 #include <BulletCollision/CollisionShapes/btShapeHull.h>
39 #include <BulletCollision/Gimpact/btGImpactShape.h>
40 #include <geometric_shapes/shapes.h>
42 #include <octomap/octomap.h>
43 #include <rclcpp/logger.hpp>
44 #include <rclcpp/logging.hpp>
46 static const rclcpp::Logger
BULLET_LOGGER = rclcpp::get_logger(
"collision_detection.bullet");
50 bool acmCheck(
const std::string& body_1,
const std::string& body_2,
62 "Not allowed entry in ACM found, collision check between " << body_1 <<
" and " << body_2);
68 "Entry in ACM found, skipping collision check as allowed " << body_1 <<
" and " << body_2);
75 "No entry in ACM found, collision check between " << body_1 <<
" and " << body_2);
81 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"No ACM, collision check between " << body_1 <<
" and " << body_2);
88 (void)(collision_object_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);
95 return (
new btBoxShape(btVector3(
a, b,
c)));
100 (void)(collision_object_type);
102 return (
new btSphereShape(
static_cast<btScalar
>(geom->radius)));
107 (void)(collision_object_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)));
116 (void)(collision_object_type);
118 btScalar
r =
static_cast<btScalar
>(geom->radius);
119 btScalar l =
static_cast<btScalar
>(geom->length);
120 return (
new btConeShapeZ(
r, l));
130 if (geom->vertex_count > 0 && geom->triangle_count > 0)
133 switch (collision_object_type)
140 std::vector<int> faces;
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]));
149 btConvexHullShape* subshape =
new btConvexHullShape();
152 btVector3(
static_cast<btScalar
>(v[0]),
static_cast<btScalar
>(v[1]),
static_cast<btScalar
>(v[2])));
158 btCompoundShape* compound =
163 for (
unsigned i = 0; i < geom->triangle_count; ++i)
166 for (
unsigned x = 0;
x < 3; ++
x)
168 unsigned idx = geom->triangles[3 * i +
x];
169 for (
unsigned y = 0;
y < 3; ++
y)
171 v[
x][
y] =
static_cast<btScalar
>(geom->vertices[3 * idx +
y]);
175 btCollisionShape* subshape =
new btTriangleShapeEx(v[0], v[1], v[2]);
176 if (subshape !=
nullptr)
180 btTransform geom_trans;
181 geom_trans.setIdentity();
182 compound->addChildShape(geom_trans, subshape);
190 RCLCPP_ERROR(
BULLET_LOGGER,
"This bullet shape type (%d) is not supported for geometry meshs",
191 static_cast<int>(collision_object_type));
208 btCompoundShape* subshape =
210 double occupancy_threshold = geom->octree->getOccupancyThres();
213 switch (collision_object_type)
217 for (
auto it = geom->octree->begin(
static_cast<unsigned char>(geom->octree->getTreeDepth())),
218 end = geom->octree->end();
221 if (it->getOccupancy() >= occupancy_threshold)
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));
233 subshape->addChildShape(geom_trans, childshape);
240 for (
auto it = geom->octree->begin(
static_cast<unsigned char>(geom->octree->getTreeDepth())),
241 end = geom->octree->end();
244 if (it->getOccupancy() >= occupancy_threshold)
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)))));
256 subshape->addChildShape(geom_trans, childshape);
263 RCLCPP_ERROR(
BULLET_LOGGER,
"This bullet shape type (%d) is not supported for geometry octree",
264 static_cast<int>(collision_object_type));
284 if (cow.getBroadphaseHandle())
295 CollisionObjectWrapperPtr new_cow = cow->clone();
300 if (btBroadphaseProxy::isConvex(new_cow->getCollisionShape()->getShapeType()))
302 assert(
dynamic_cast<btConvexShape*
>(new_cow->getCollisionShape()) !=
nullptr);
303 btConvexShape* convex =
static_cast<btConvexShape*
>(new_cow->getCollisionShape());
306 assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
310 new_cow->manage(shape);
311 new_cow->setCollisionShape(shape);
313 else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType()))
315 btCompoundShape* compound =
static_cast<btCompoundShape*
>(new_cow->getCollisionShape());
316 btCompoundShape* new_compound =
319 for (
int i = 0; i < compound->getNumChildShapes(); ++i)
321 if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType()))
323 btConvexShape* convex =
static_cast<btConvexShape*
>(compound->getChildShape(i));
324 assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
326 btTransform geom_trans = compound->getChildTransform(i);
330 new_cow->manage(subshape);
332 new_compound->addChildShape(geom_trans, subshape);
334 else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
336 btCompoundShape* second_compound =
static_cast<btCompoundShape*
>(compound->getChildShape(i));
337 btCompoundShape* new_second_compound =
339 for (
int j = 0; j < second_compound->getNumChildShapes(); ++j)
341 assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType()));
343 btConvexShape* convex =
static_cast<btConvexShape*
>(second_compound->getChildShape(j));
344 assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
346 btTransform geom_trans = second_compound->getChildTransform(j);
350 new_cow->manage(subshape);
352 new_second_compound->addChildShape(geom_trans, subshape);
355 btTransform geom_trans = compound->getChildTransform(i);
357 new_cow->manage(new_second_compound);
361 new_compound->addChildShape(geom_trans, new_second_compound);
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");
373 new_cow->manage(new_compound);
374 new_cow->setCollisionShape(new_compound);
375 new_cow->setWorldTransform(cow->getWorldTransform());
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");
388 const std::unique_ptr<btBroadphaseInterface>& broadphase,
389 const std::unique_ptr<btCollisionDispatcher>& dispatcher)
391 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"Added " << cow->getName() <<
" to broadphase");
392 btVector3 aabb_min, aabb_max;
393 cow->getAABB(aabb_min, aabb_max);
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()));
407 return createShapePrimitive(
static_cast<const shapes::Box*
>(geom.get()), collision_object_type);
411 return createShapePrimitive(
static_cast<const shapes::Sphere*
>(geom.get()), collision_object_type);
413 case shapes::CYLINDER:
415 return createShapePrimitive(
static_cast<const shapes::Cylinder*
>(geom.get()), collision_object_type);
419 return createShapePrimitive(
static_cast<const shapes::Cone*
>(geom.get()), collision_object_type);
423 return createShapePrimitive(
static_cast<const shapes::Mesh*
>(geom.get()), collision_object_type, cow);
427 return createShapePrimitive(
static_cast<const shapes::OcTree*
>(geom.get()), collision_object_type, cow);
431 RCLCPP_ERROR(
BULLET_LOGGER,
"This geometric shape type (%d) is not supported using BULLET yet",
432 static_cast<int>(geom->type));
440 bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup);
441 cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup);
475 const btCollisionObjectWrapper* colObj0Wrap,
int ,
476 int index0,
const btCollisionObjectWrapper* colObj1Wrap,
481 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"Not close enough for collision with " << cp.m_distance1);
507 std::pair<std::string, std::string> pair_names{ cow0->
getName(), 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);
515 if (!pair.m_algorithm)
517 pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap,
nullptr, BT_CLOSEST_POINT_ALGORITHMS);
520 if (pair.m_algorithm)
523 contact_point_result.m_closestPointDistanceThreshold =
static_cast<btScalar
>(results_callback_.
contact_distance_);
526 pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result);
537 const std::vector<shapes::ShapeConstPtr>&
shapes,
539 const std::vector<CollisionObjectType>& collision_object_types,
544 , m_shape_poses(shape_poses)
545 , m_collision_object_types(collision_object_types)
547 if (
shapes.empty() || shape_poses.empty() ||
548 (
shapes.size() != shape_poses.size() || collision_object_types.empty() ||
549 shapes.size() != collision_object_types.size()))
551 throw std::exception();
555 assert(!
name.empty());
573 setCollisionShape(shape);
578 btCompoundShape* compound =
583 setCollisionShape(compound);
588 for (std::size_t j = 0; j <
m_shapes.size(); ++j)
591 if (subshape !=
nullptr)
596 compound->addChildShape(geom_trans, subshape);
603 const std::vector<shapes::ShapeConstPtr>&
shapes,
605 const std::vector<CollisionObjectType>& collision_object_types,
606 const std::set<std::string>& touch_links)
613 const std::vector<shapes::ShapeConstPtr>&
shapes,
615 const std::vector<CollisionObjectType>& collision_object_types,
616 const std::vector<std::shared_ptr<void>>& data)
620 , m_shape_poses(shape_poses)
621 , m_collision_object_types(collision_object_types)
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.
short int m_collisionFilterGroup
Bitfield specifies to which group the object belongs.
bool m_enabled
Indicates if the collision object is used for a collision check.
AlignedVector< Eigen::Isometry3d > m_shape_poses
The poses of the shapes, must be same length as m_shapes.
void manage(T *t)
Manage memory of a raw pointer shape.
const std::string & getName() const
Get the collision object name.
std::set< std::string > m_touch_links
The robot links the collision objects is allowed to touch.
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.
short int m_collisionFilterMask
Bitfield specifies against which other groups the object is collision checked.
const collision_detection::BodyType & getTypeID() const
Get a user defined type.
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.
const btScalar BULLET_MARGIN
CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr &cow)
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
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
btScalar addCastSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int, const btCollisionObjectWrapper *colObj1Wrap, int, ContactTestData &collisions)
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
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.
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
const rclcpp::Logger BULLET_LOGGER
bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const override
Casted collision shape used for checking if an object is collision free between two discrete poses.