From 211da433aa86d989098938eb36c3fa840015cb88 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 15 Dec 2024 11:59:34 -0600 Subject: [PATCH] Fix use of compound mesh in FCL --- .../bullet/src/bullet_utils.cpp | 3 ++- .../bullet/src/create_convex_hull.cpp | 2 -- .../fcl/fcl_collision_object_wrapper.h | 15 +++++++++++++ .../tesseract_collision/fcl/fcl_utils.h | 7 +++---- .../fcl/src/fcl_collision_object_wrapper.cpp | 8 +++++++ tesseract_collision/fcl/src/fcl_utils.cpp | 21 +++++++------------ 6 files changed, 36 insertions(+), 20 deletions(-) diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 21dc4e338ba..c5aaf8a558a 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -472,10 +472,11 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, { if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH) { + int shape_index = shape_id++; const auto& meshes = std::static_pointer_cast(m_shapes[j])->getMeshes(); for (const auto& mesh : meshes) { - std::shared_ptr subshape = createShapePrimitive(mesh, this, shape_id++); + std::shared_ptr subshape = createShapePrimitive(mesh, this, shape_index); if (subshape != nullptr) { manage(subshape); diff --git a/tesseract_collision/bullet/src/create_convex_hull.cpp b/tesseract_collision/bullet/src/create_convex_hull.cpp index 454aa0309ce..48af7be3f63 100644 --- a/tesseract_collision/bullet/src/create_convex_hull.cpp +++ b/tesseract_collision/bullet/src/create_convex_hull.cpp @@ -27,8 +27,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h index 860c03016a3..3d901c9e105 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h @@ -64,8 +64,23 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject */ void updateAABB(); + /** + * @brief Set the shape index. This is the geometries index in the urdf. + * @param index The index + */ + void setShapeIndex(int index); + + /** + * @brief Get the shape index. This is the geometries index in the urdf. + * @return The shape index + */ + int getShapeIndex() const; + protected: double contact_distance_{ 0 }; /**< @brief The contact distance threshold. */ + + /** @brief The shape index, which is the geometries index in the urdf. */ + int shape_index_{ -1 }; }; } // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h index 8803c4a6b94..9ad71682ce2 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h @@ -113,10 +113,9 @@ class CollisionObjectWrapper void setCollisionObjectsTransform(const Eigen::Isometry3d& pose) { world_pose_ = pose; - for (unsigned i = 0; i < collision_objects_.size(); ++i) + for (auto& co : collision_objects_) { - CollisionObjectPtr& co = collision_objects_[i]; - co->setTransform(pose * shape_poses_[i]); + co->setTransform(pose * shape_poses_[static_cast(co->getShapeIndex())]); co->updateAABB(); // This a tesseract function that updates abb to take into account contact distance } } @@ -168,7 +167,7 @@ class CollisionObjectWrapper * @param co fcl collision shape * @return links collision shape index */ - int getShapeIndex(const fcl::CollisionObjectd* co) const; + static int getShapeIndex(const fcl::CollisionObjectd* co); protected: std::string name_; // name of the collision object diff --git a/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp b/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp index b452e488779..c6f823c6c70 100644 --- a/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp +++ b/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp @@ -54,4 +54,12 @@ void FCLCollisionObjectWrapper::updateAABB() } } +void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; } + +int FCLCollisionObjectWrapper::getShapeIndex() const +{ + assert(shape_index_ >= 0); + return shape_index_; +} + } // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index b2cac88505d..b310ef484d4 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -246,8 +246,8 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi ContactResult contact; contact.link_names[0] = cd1->getName(); contact.link_names[1] = cd2->getName(); - contact.shape_id[0] = static_cast(cd1->getShapeIndex(o1)); - contact.shape_id[1] = static_cast(cd2->getShapeIndex(o2)); + contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1); + contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2); contact.subshape_id[0] = static_cast(fcl_contact.b1); contact.subshape_id[1] = static_cast(fcl_contact.b2); contact.nearest_points[0] = fcl_contact.pos; @@ -307,8 +307,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void ContactResult contact; contact.link_names[0] = cd1->getName(); contact.link_names[1] = cd2->getName(); - contact.shape_id[0] = cd1->getShapeIndex(o1); - contact.shape_id[1] = cd2->getShapeIndex(o2); + contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1); + contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2); contact.subshape_id[0] = static_cast(fcl_result.b1); contact.subshape_id[1] = static_cast(fcl_result.b2); contact.nearest_points[0] = fcl_result.nearest_points[0]; @@ -365,6 +365,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, collision_geometries_.push_back(subshape); auto co = std::make_shared(subshape); co->setUserData(this); + co->setShapeIndex(static_cast(i)); co->setTransform(shape_poses_[i]); co->updateAABB(); collision_objects_.push_back(co); @@ -380,6 +381,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, collision_geometries_.push_back(subshape); auto co = std::make_shared(subshape); co->setUserData(this); + co->setShapeIndex(static_cast(i)); co->setTransform(shape_poses_[i]); co->updateAABB(); collision_objects_.push_back(co); @@ -389,16 +391,9 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, } } -int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co) const +int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co) { - auto it = std::find_if(collision_objects_.begin(), collision_objects_.end(), [&co](const CollisionObjectPtr& c) { - return c.get() == co; - }); - - if (it != collision_objects_.end()) - return static_cast(std::distance(collision_objects_.begin(), it)); - - return -1; + return static_cast(co)->getShapeIndex(); } } // namespace tesseract_collision::tesseract_collision_fcl