36 #include <rclcpp/logger.hpp>
37 #include <rclcpp/logging.hpp>
41 static const rclcpp::Logger
BULLET_LOGGER = rclcpp::get_logger(
"collision_detection.bullet");
47 BulletCastBVHManagerPtr manager = std::make_shared<BulletCastBVHManager>();
49 for (
const std::pair<const std::string, collision_detection_bullet::CollisionObjectWrapperPtr>& cow :
link2cow_)
51 CollisionObjectWrapperPtr new_cow = cow.second->clone();
53 assert(new_cow->getCollisionShape());
54 assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
56 new_cow->setWorldTransform(cow.second->getWorldTransform());
58 manager->addCollisionObject(new_cow);
61 manager->setActiveCollisionObjects(
active_);
68 const Eigen::Isometry3d& pose2)
75 CollisionObjectWrapperPtr& cow = it->second;
76 assert(cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter);
81 cow->setWorldTransform(tf1);
87 if (btBroadphaseProxy::isConvex(cow->getCollisionShape()->getShapeType()))
89 static_cast<CastHullShape*
>(cow->getCollisionShape())->updateCastTransform(tf1.inverseTimes(tf2));
91 else if (btBroadphaseProxy::isCompound(cow->getCollisionShape()->getShapeType()))
93 btCompoundShape* compound =
static_cast<btCompoundShape*
>(cow->getCollisionShape());
95 for (
int i = 0; i < compound->getNumChildShapes(); ++i)
97 if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType()))
99 const btTransform& local_tf = compound->getChildTransform(i);
101 btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf);
102 static_cast<CastHullShape*
>(compound->getChildShape(i))->updateCastTransform(delta_tf);
103 compound->updateChildTransform(i, local_tf,
false);
105 else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
107 btCompoundShape* second_compound =
static_cast<btCompoundShape*
>(compound->getChildShape(i));
109 for (
int j = 0; j < second_compound->getNumChildShapes(); ++j)
111 assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType()));
112 const btTransform& local_tf = second_compound->getChildTransform(j);
114 btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf);
115 static_cast<CastHullShape*
>(second_compound->getChildShape(j))->updateCastTransform(delta_tf);
116 second_compound->updateChildTransform(j, local_tf,
false);
118 second_compound->recalculateLocalAabb();
121 compound->recalculateLocalAabb();
125 RCLCPP_ERROR_STREAM(
BULLET_LOGGER,
"I can only continuous collision check convex shapes and "
126 "compound shapes made of convex shapes");
127 throw std::runtime_error(
128 "I can only continuous collision check convex shapes and compound shapes made of convex shapes");
143 btOverlappingPairCache* pair_cache =
broadphase_->getOverlappingPairCache();
145 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"Number overlapping candidates " << pair_cache->getNumOverlappingPairs());
149 pair_cache->processAllOverlappingPairs(&collision_callback,
dispatcher_.get());
154 std::string
name = cow->getName();
155 if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)
165 btVector3 aabb_min, aabb_max;
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
std::vector< std::string > active_
A list of the active collision links.
std::unique_ptr< btBroadphaseInterface > broadphase_
The bullet broadphase interface.
double contact_distance_
The contact distance threshold.
std::map< std::string, CollisionObjectWrapperPtr > link2cow_
A map of collision objects being managed.
btDispatcherInfo dispatch_info_
The bullet collision dispatcher configuration information.
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collision algorithm.
void setCastCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2)
Set a single cast (moving) collision object's tansforms.
void contactTest(collision_detection::CollisionResult &collisions, const collision_detection::CollisionRequest &req, const collision_detection::AllowedCollisionMatrix *acm, bool self) override
Perform a contact test for all objects.
BulletCastBVHManagerPtr clone() const
Clone the manager.
void addCollisionObject(const CollisionObjectWrapperPtr &cow) override
Add a tesseract collision object to the manager.
A callback function that is called as part of the broadphase collision checking.
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr &cow)
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.
const rclcpp::Logger BULLET_LOGGER
Representation of a collision checking request.
Representation of a collision checking result.
Casted collision shape used for checking if an object is collision free between two discrete poses.