From 39dff667d74377125e196e1fea1627d8227c9742 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 6 Feb 2024 12:03:32 +0100 Subject: [PATCH 1/6] Add hpp-fcl as a collision checker --- tesseract_collision/CMakeLists.txt | 17 +- tesseract_collision/hpp_fcl/CMakeLists.txt | 74 ++++ .../hpp_fcl_collision_object_wrapper.h | 74 ++++ .../hpp_fcl/hpp_fcl_discrete_managers.h | 155 +++++++ .../hpp_fcl/hpp_fcl_factories.h | 42 ++ .../hpp_fcl/hpp_fcl_utils.h | 289 +++++++++++++ .../src/hpp_fcl_collision_object_wrapper.cpp | 57 +++ .../hpp_fcl/src/hpp_fcl_discrete_managers.cpp | 402 ++++++++++++++++++ .../hpp_fcl/src/hpp_fcl_factories.cpp | 45 ++ .../hpp_fcl/src/hpp_fcl_utils.cpp | 386 +++++++++++++++++ tesseract_collision/package.xml | 1 + tesseract_collision/test/CMakeLists.txt | 4 +- .../test/benchmarks/CMakeLists.txt | 6 +- .../test/benchmarks/collision_profile.cpp | 10 +- .../hpp_fcl_discrete_bvh_benchmarks.cpp | 314 ++++++++++++++ .../test/collision_box_box_unit.cpp | 12 + .../test/collision_box_capsule_unit.cpp | 6 + .../test/collision_box_cone_unit.cpp | 6 + .../test/collision_box_cylinder_unit.cpp | 21 +- .../test/collision_box_sphere_unit.cpp | 12 + .../test/collision_clone_unit.cpp | 6 + .../test/collision_compound_compound_unit.cpp | 6 + .../test/collision_large_dataset_unit.cpp | 13 + .../test/collision_mesh_mesh_unit.cpp | 6 + .../test/collision_multi_threaded_unit.cpp | 12 + .../test/collision_octomap_mesh_unit.cpp | 14 +- .../test/collision_octomap_sphere_unit.cpp | 13 + .../test/collision_sphere_sphere_unit.cpp | 13 + .../test/contact_manager_plugins.yaml | 3 + .../test/contact_managers_config_unit.cpp | 7 + .../contact_managers_factory_static_unit.cpp | 3 + .../test/contact_managers_factory_unit.cpp | 29 +- 32 files changed, 2029 insertions(+), 29 deletions(-) create mode 100644 tesseract_collision/hpp_fcl/CMakeLists.txt create mode 100644 tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_collision_object_wrapper.h create mode 100644 tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_discrete_managers.h create mode 100644 tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h create mode 100644 tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_utils.h create mode 100644 tesseract_collision/hpp_fcl/src/hpp_fcl_collision_object_wrapper.cpp create mode 100644 tesseract_collision/hpp_fcl/src/hpp_fcl_discrete_managers.cpp create mode 100644 tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp create mode 100644 tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp create mode 100644 tesseract_collision/test/benchmarks/hpp_fcl_discrete_bvh_benchmarks.cpp diff --git a/tesseract_collision/CMakeLists.txt b/tesseract_collision/CMakeLists.txt index 0910205bbb4..06fa05ecdc9 100644 --- a/tesseract_collision/CMakeLists.txt +++ b/tesseract_collision/CMakeLists.txt @@ -94,6 +94,14 @@ if(TESSERACT_BUILD_FCL) add_subdirectory(fcl) endif() +# HPP-FCL +option(TESSERACT_BUILD_HPP_FCL "Build HPP-FCL components" ON) +if(TESSERACT_BUILD_HPP_FCL) + message("Building HPP-FCL components") + list(APPEND SUPPORTED_COMPONENTS hpp_fcl) + add_subdirectory(hpp_fcl) +endif() + # VHACD option(TESSERACT_BUILD_VHACD "Build VHACD components" ON) if(TESSERACT_BUILD_VHACD) @@ -112,20 +120,23 @@ target_compile_definitions(${PROJECT_NAME}_core PRIVATE TESSERACT_CONTACT_MANAGERS_PLUGINS="${CONTACT_MANAGERS_PLUGINS_STRING}") # Testing -if((TESSERACT_ENABLE_TESTING OR TESSERACT_COLLISION_ENABLE_TESTING) AND TESSERACT_BUILD_FCL) +if((TESSERACT_ENABLE_TESTING OR TESSERACT_COLLISION_ENABLE_TESTING) AND TESSERACT_BUILD_FCL AND TESSERACT_BUILD_HPP_FCL) enable_testing() add_run_tests_target(ENABLE ${TESSERACT_ENABLE_RUN_TESTING}) add_subdirectory(test) endif() # Benchmarks -if((TESSERACT_ENABLE_BENCHMARKING OR TESSERACT_COLLISION_ENABLE_BENCHMARKING) AND TESSERACT_BUILD_FCL +if((TESSERACT_ENABLE_BENCHMARKING OR TESSERACT_COLLISION_ENABLE_BENCHMARKING) + AND TESSERACT_BUILD_FCL + AND TESSERACT_BUILD_HPP_FCL AND TESSERACT_BUILD_TEST_SUITE) add_subdirectory(test/benchmarks) endif() # Examples -if((TESSERACT_ENABLE_EXAMPLES OR TESSERACT_COLLISION_ENABLE_EXAMPLES) AND TESSERACT_BUILD_FCL) +if((TESSERACT_ENABLE_EXAMPLES OR TESSERACT_COLLISION_ENABLE_EXAMPLES) AND TESSERACT_BUILD_FCL + AND TESSERACT_BUILD_HPP_FCL) add_subdirectory(examples) endif() diff --git a/tesseract_collision/hpp_fcl/CMakeLists.txt b/tesseract_collision/hpp_fcl/CMakeLists.txt new file mode 100644 index 00000000000..c2e9cb3e901 --- /dev/null +++ b/tesseract_collision/hpp_fcl/CMakeLists.txt @@ -0,0 +1,74 @@ +find_package(octomap REQUIRED) +find_package(hpp-fcl REQUIRED) + +# Create target for HPP-FCL implementation +add_library(${PROJECT_NAME}_hpp_fcl src/hpp_fcl_discrete_managers.cpp src/hpp_fcl_utils.cpp + src/hpp_fcl_collision_object_wrapper.cpp) +target_link_libraries( + ${PROJECT_NAME}_hpp_fcl + PUBLIC ${PROJECT_NAME}_core + Eigen3::Eigen + tesseract::tesseract_geometry + hpp-fcl::hpp-fcl + console_bridge::console_bridge + octomap + octomath) +target_compile_options(${PROJECT_NAME}_hpp_fcl PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_hpp_fcl PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_hpp_fcl PUBLIC ${TESSERACT_COMPILE_DEFINITIONS}) +target_cxx_version(${PROJECT_NAME}_hpp_fcl PUBLIC VERSION ${TESSERACT_CXX_VERSION}) +target_clang_tidy(${PROJECT_NAME}_hpp_fcl ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_code_coverage( + ${PROJECT_NAME}_hpp_fcl + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) +target_include_directories(${PROJECT_NAME}_hpp_fcl PUBLIC "$" + "$") + +add_library(${PROJECT_NAME}_hpp_fcl_factories src/hpp_fcl_factories.cpp) +target_link_libraries(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${PROJECT_NAME}_hpp_fcl) +target_compile_options(${PROJECT_NAME}_hpp_fcl_factories PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${TESSERACT_COMPILE_DEFINITIONS}) +target_clang_tidy(${PROJECT_NAME}_hpp_fcl_factories ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_hpp_fcl_factories PUBLIC VERSION ${TESSERACT_CXX_VERSION}) +target_code_coverage( + ${PROJECT_NAME}_hpp_fcl_factories + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) +target_include_directories( + ${PROJECT_NAME}_hpp_fcl_factories PUBLIC "$" + "$") + +# Add factory library so contact_managers_factory can find these factories by defauult +set(CONTACT_MANAGERS_PLUGINS ${CONTACT_MANAGERS_PLUGINS} "${PROJECT_NAME}_hpp_fcl_factories" PARENT_SCOPE) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME} + DESTINATION include + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" + PATTERN "*.inl" + PATTERN ".svn" EXCLUDE) + +configure_component( + COMPONENT hpp_fcl + NAMESPACE tesseract + TARGETS ${PROJECT_NAME}_hpp_fcl ${PROJECT_NAME}_hpp_fcl_factories + DEPENDENCIES "tesseract_collision COMPONENTS core" "hpp-fcl") + +if(TESSERACT_PACKAGE) + cpack_component( + COMPONENT hpp_fcl + VERSION ${pkg_extracted_version} + DESCRIPTION "Tesseract Collision HPP-FCL components" + COMPONENT_DEPENDS core + LINUX_DEPENDS "hpp-fcl | ${TESSERACT_PACKAGE_PREFIX}hpp_fcl" + WINDOWS_DEPENDS "hpp-fcl | ${TESSERACT_PACKAGE_PREFIX}hpp_fcl") +endif() diff --git a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_collision_object_wrapper.h b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_collision_object_wrapper.h new file mode 100644 index 00000000000..6c9fe3debfd --- /dev/null +++ b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_collision_object_wrapper.h @@ -0,0 +1,74 @@ +/** + * @file hpp_fcl_collision_object_wrapper.h + * @brief Collision Object Wrapper to modify AABB with contact distance threshold + * + * @author Levi Armstrong + * @date April 14, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H +#define TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +/** + * @brief This is a wrapper around HPP-FCL Collision Object Class which allows you to expand the AABB by the contact + * dist. + * + * This significantly improves performance when making distance requests if performing a contact tests type FIRST. + */ +class HPP_FCLCollisionObjectWrapper : public hpp::fcl::CollisionObject +{ +public: + using hpp::fcl::CollisionObject::CollisionObject; + + /** + * @brief Set the collision objects contact distance threshold. + * + * This automatically calls updateAABB() which increases the AABB by the contact distance. + * @param contact_distance The contact distance threshold. + */ + void setContactDistanceThreshold(double contact_distance); + + /** + * @brief Get the collision objects contact distance threshold. + * @return The contact distance threshold. + */ + double getContactDistanceThreshold() const; + + /** + * @brief Update the internal AABB. This must be called instead of the base class computeAABB(). + * + * After setting the collision objects transform this must be called. + */ + void updateAABB(); + +protected: + double contact_distance_{ 0 }; /**< @brief The contact distance threshold. */ +}; + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl + +#endif // TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H diff --git a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_discrete_managers.h b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_discrete_managers.h new file mode 100644 index 00000000000..aee0770df69 --- /dev/null +++ b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_discrete_managers.h @@ -0,0 +1,155 @@ +/** + * @file hpp_fcl_discrete_managers.h + * @brief Tesseract ROS HPP-FCL contact checker implementation. + * + * @author Levi Armstrong + * @date Dec 18, 2017 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (BSD) + * @par + * All rights reserved. + * @par + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * @par + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * @par + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H +#define TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H + +#include +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +/** @brief A HPP-FCL implementation of the discrete contact manager */ +class HPP_FCLDiscreteBVHManager : public DiscreteContactManager +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + HPP_FCLDiscreteBVHManager(std::string name = "HPP_FCLDiscreteBVHManager"); + ~HPP_FCLDiscreteBVHManager() override = default; + HPP_FCLDiscreteBVHManager(const HPP_FCLDiscreteBVHManager&) = delete; + HPP_FCLDiscreteBVHManager& operator=(const HPP_FCLDiscreteBVHManager&) = delete; + HPP_FCLDiscreteBVHManager(HPP_FCLDiscreteBVHManager&&) = delete; + HPP_FCLDiscreteBVHManager& operator=(HPP_FCLDiscreteBVHManager&&) = delete; + + std::string getName() const override final; + + DiscreteContactManager::UPtr clone() const override final; + + bool addCollisionObject(const std::string& name, + const int& mask_id, + const CollisionShapesConst& shapes, + const tesseract_common::VectorIsometry3d& shape_poses, + bool enabled = true) override final; + + const CollisionShapesConst& getCollisionObjectGeometries(const std::string& name) const override final; + + const tesseract_common::VectorIsometry3d& + getCollisionObjectGeometriesTransforms(const std::string& name) const override final; + + bool hasCollisionObject(const std::string& name) const override final; + + bool removeCollisionObject(const std::string& name) override final; + + bool enableCollisionObject(const std::string& name) override final; + + bool disableCollisionObject(const std::string& name) override final; + + bool isCollisionObjectEnabled(const std::string& name) const override final; + + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override final; + + void setCollisionObjectsTransform(const std::vector& names, + const tesseract_common::VectorIsometry3d& poses) override final; + + void setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) override final; + + const std::vector& getCollisionObjects() const override final; + + void setActiveCollisionObjects(const std::vector& names) override final; + + const std::vector& getActiveCollisionObjects() const override final; + + void setCollisionMarginData( + CollisionMarginData collision_margin_data, + CollisionMarginOverrideType override_type = CollisionMarginOverrideType::REPLACE) override final; + + void setDefaultCollisionMarginData(double default_collision_margin) override final; + + void setPairCollisionMarginData(const std::string& name1, + const std::string& name2, + double collision_margin) override final; + + const CollisionMarginData& getCollisionMarginData() const override final; + + void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + + IsContactAllowedFn getIsContactAllowedFn() const override final; + + void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; + + /** + * @brief Add a fcl collision object to the manager + * @param cow The tesseract fcl collision object + */ + void addCollisionObject(const COW::Ptr& cow); + +private: + std::string name_; + + /** @brief Broad-phase Collision Manager for active collision objects */ + std::unique_ptr static_manager_; + + /** @brief Broad-phase Collision Manager for active collision objects */ + std::unique_ptr dynamic_manager_; + + Link2COW link2cow_; /**< @brief A map of all (static and active) collision objects being managed */ + std::vector active_; /**< @brief A list of the active collision objects */ + std::vector collision_objects_; /**< @brief A list of the collision objects */ + CollisionMarginData collision_margin_data_; /**< @brief The contact distance threshold */ + IsContactAllowedFn fn_; /**< @brief The is allowed collision function */ + std::size_t fcl_co_count_{ 0 }; /**< @brief The number fcl collision objects */ + + /** @brief This is used to store static collision objects to update */ + std::vector static_update_; + + /** @brief This is used to store dynamic collision objects to update */ + std::vector dynamic_update_; + + /** @brief This function will update internal data when margin data has changed */ + void onCollisionMarginDataChanged(); +}; + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl +#endif // TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H diff --git a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h new file mode 100644 index 00000000000..992ce82f93a --- /dev/null +++ b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h @@ -0,0 +1,42 @@ +/** + * @file hpp_fcl_factories.h + * @brief Factories for loading hpp-fcl contact managers as plugins + * + * @author Levi Armstrong + * @date October 25, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H +#define TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H + +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +class HPP_FCLDiscreteBVHManagerFactory : public DiscreteContactManagerFactory +{ +public: + DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; +}; + +TESSERACT_PLUGIN_ANCHOR_DECL(HPP_FCLFactoriesAnchor) + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl +#endif // TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H diff --git a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_utils.h b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_utils.h new file mode 100644 index 00000000000..ff268473b85 --- /dev/null +++ b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_utils.h @@ -0,0 +1,289 @@ +/** + * @file hpp_fcl_utils.h + * @brief Tesseract ROS HPP-FCL Utility Functions. + * + * @author Levi Armstrong + * @date Dec 18, 2017 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (BSD) + * @par + * All rights reserved. + * @par + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * @par + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * @par + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TESSERACT_COLLISION_HPP_FCL_UTILS_H +#define TESSERACT_COLLISION_HPP_FCL_UTILS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +using CollisionGeometryPtr = std::shared_ptr; +using CollisionObjectPtr = std::shared_ptr; +using CollisionObjectRawPtr = hpp::fcl::CollisionObject*; +using CollisionObjectConstPtr = std::shared_ptr; + +enum CollisionFilterGroups +{ + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + AllFilter = -1 // all bits sets: DefaultFilter | StaticFilter | KinematicFilter +}; + +/** + * @brief This is a Tesseract link collision object wrapper which add items specific to tesseract. It is a wrapper + * around a tesseract link which may contain several collision objects. + */ +class CollisionObjectWrapper +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CollisionObjectWrapper() = default; + CollisionObjectWrapper(std::string name, + const int& type_id, + CollisionShapesConst shapes, + tesseract_common::VectorIsometry3d shape_poses); + + short int m_collisionFilterGroup{ CollisionFilterGroups::KinematicFilter }; + short int m_collisionFilterMask{ CollisionFilterGroups::StaticFilter | CollisionFilterGroups::KinematicFilter }; + bool m_enabled{ true }; + + const std::string& getName() const { return name_; } + const int& getTypeID() const { return type_id_; } + /** \brief Check if two objects point to the same source object */ + bool sameObject(const CollisionObjectWrapper& other) const + { + return name_ == other.name_ && type_id_ == other.type_id_ && shapes_.size() == other.shapes_.size() && + shape_poses_.size() == other.shape_poses_.size() && + std::equal(shapes_.begin(), shapes_.end(), other.shapes_.begin()) && + std::equal(shape_poses_.begin(), + shape_poses_.end(), + other.shape_poses_.begin(), + [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); + } + + const CollisionShapesConst& getCollisionGeometries() const { return shapes_; } + + const tesseract_common::VectorIsometry3d& getCollisionGeometriesTransforms() const { return shape_poses_; } + + void setCollisionObjectsTransform(const Eigen::Isometry3d& pose) + { + world_pose_ = pose; + for (unsigned i = 0; i < collision_objects_.size(); ++i) + { + CollisionObjectPtr& co = collision_objects_[i]; + auto tf = pose * shape_poses_[i]; + co->setTransform(hpp::fcl::Transform3f(tf.rotation(), tf.translation())); + co->updateAABB(); // This a tesseract function that updates abb to take into account contact distance + } + } + + void setContactDistanceThreshold(double contact_distance) + { + contact_distance_ = contact_distance; + for (auto& co : collision_objects_) + co->setContactDistanceThreshold(contact_distance_); + } + + double getContactDistanceThreshold() const { return contact_distance_; } + const Eigen::Isometry3d& getCollisionObjectsTransform() const { return world_pose_; } + const std::vector& getCollisionObjects() const { return collision_objects_; } + std::vector& getCollisionObjects() { return collision_objects_; } + const std::vector& getCollisionObjectsRaw() const { return collision_objects_raw_; } + std::vector& getCollisionObjectsRaw() { return collision_objects_raw_; } + std::shared_ptr clone() const + { + auto clone_cow = std::make_shared(); + clone_cow->name_ = name_; + clone_cow->type_id_ = type_id_; + clone_cow->shapes_ = shapes_; + clone_cow->shape_poses_ = shape_poses_; + clone_cow->collision_geometries_ = collision_geometries_; + + clone_cow->collision_objects_.reserve(collision_objects_.size()); + clone_cow->collision_objects_raw_.reserve(collision_objects_.size()); + for (const auto& co : collision_objects_) + { + assert(std::dynamic_pointer_cast(co) != nullptr); + auto collObj = + std::make_shared(*std::static_pointer_cast(co)); + collObj->setUserData(clone_cow.get()); + collObj->setTransform(co->getTransform()); + collObj->updateAABB(); + clone_cow->collision_objects_.push_back(collObj); + clone_cow->collision_objects_raw_.push_back(collObj.get()); + } + + clone_cow->m_collisionFilterGroup = m_collisionFilterGroup; + clone_cow->m_collisionFilterMask = m_collisionFilterMask; + clone_cow->m_enabled = m_enabled; + return clone_cow; + } + + /** + * @brief Given fcl collision shape get the index to the links collision shape + * @param co fcl collision shape + * @return links collision shape index + */ + int getShapeIndex(const hpp::fcl::CollisionObject* co) const; + +protected: + std::string name_; // name of the collision object + int type_id_{ -1 }; // user defined type id + Eigen::Isometry3d world_pose_{ Eigen::Isometry3d::Identity() }; /**< @brief Collision Object World Transformation */ + CollisionShapesConst shapes_; + tesseract_common::VectorIsometry3d shape_poses_; + std::vector collision_geometries_; + std::vector collision_objects_; + /** + * @brief The raw pointer is also stored because FCL accepts vectors for batch process. + * Note: They are updating the API to Shared Pointers but the broadphase has not been updated yet. + */ + std::vector collision_objects_raw_; + + double contact_distance_{ 0 }; /**< @brief The contact distance threshold */ +}; + +CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom); + +using COW = CollisionObjectWrapper; +using Link2COW = std::map; +using Link2ConstCOW = std::map; + +inline COW::Ptr createFCLCollisionObject(const std::string& name, + const int& type_id, + const CollisionShapesConst& shapes, + const tesseract_common::VectorIsometry3d& shape_poses, + bool enabled) +{ + // dont add object that does not have geometry + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) + { + CONSOLE_BRIDGE_logDebug("ignoring link %s", name.c_str()); + return nullptr; + } + + auto new_cow = std::make_shared(name, type_id, shapes, shape_poses); + + new_cow->m_enabled = enabled; + CONSOLE_BRIDGE_logDebug("Created collision object for link %s", new_cow->getName().c_str()); + return new_cow; +} + +/** + * @brief Update collision objects filters + * @param active The active collision objects + * @param cow The collision object to update + * @param static_manager Broadphasse manager for static objects + * @param dynamic_manager Broadphase manager for dynamic objects + */ +inline void updateCollisionObjectFilters(const std::vector& active, + const COW::Ptr& cow, + const std::unique_ptr& static_manager, + const std::unique_ptr& dynamic_manager) +{ + // For descrete checks we can check static to kinematic and kinematic to + // kinematic + if (!isLinkActive(active, cow->getName())) + { + if (cow->m_collisionFilterGroup != CollisionFilterGroups::StaticFilter) + { + std::vector& objects = cow->getCollisionObjects(); + // This link was dynamic but is now static + for (auto& co : objects) + dynamic_manager->unregisterObject(co.get()); + + for (auto& co : objects) + static_manager->registerObject(co.get()); + } + cow->m_collisionFilterGroup = CollisionFilterGroups::StaticFilter; + } + else + { + if (cow->m_collisionFilterGroup != CollisionFilterGroups::KinematicFilter) + { + std::vector& objects = cow->getCollisionObjects(); + // This link was static but is now dynamic + for (auto& co : objects) + static_manager->unregisterObject(co.get()); + + for (auto& co : objects) + dynamic_manager->registerObject(co.get()); + } + cow->m_collisionFilterGroup = CollisionFilterGroups::KinematicFilter; + } + + // If the group is StaticFilter then the Mask is KinematicFilter, then StaticFilter groups can only collide with + // KinematicFilter groups. If the group is KinematicFilter then the mask is StaticFilter and KinematicFilter meaning + // that KinematicFilter groups can collide with both StaticFilter and KinematicFilter groups. + if (cow->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + cow->m_collisionFilterMask = CollisionFilterGroups::KinematicFilter; + } + else + { + cow->m_collisionFilterMask = CollisionFilterGroups::StaticFilter | CollisionFilterGroups::KinematicFilter; + } +} + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wnon-virtual-dtor" +struct CollisionCallback : hpp::fcl::CollisionCallBackBase +{ + ContactTestData* cdata{}; + bool collide(hpp::fcl::CollisionObject* o1, hpp::fcl::CollisionObject* o2) override; +}; + +struct DistanceCollisionCallback : hpp::fcl::CollisionCallBackBase +{ + ContactTestData* cdata{}; + bool collide(hpp::fcl::CollisionObject* o1, hpp::fcl::CollisionObject* o2) override; +}; +#pragma GCC diagnostic pop + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl +#endif // TESSERACT_COLLISION_HPP_FCL_UTILS_H diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_collision_object_wrapper.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_collision_object_wrapper.cpp new file mode 100644 index 00000000000..a9bfa9a0d56 --- /dev/null +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_collision_object_wrapper.cpp @@ -0,0 +1,57 @@ +/** + * @file hpp_fcl_collision_object_wrapper.cpp + * @brief Collision Object Wrapper to modify AABB with contact distance threshold + * + * @author Levi Armstrong + * @date April 14, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +void HPP_FCLCollisionObjectWrapper::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + updateAABB(); +} + +double HPP_FCLCollisionObjectWrapper::getContactDistanceThreshold() const { return contact_distance_; } + +void HPP_FCLCollisionObjectWrapper::updateAABB() +{ + if (t.rotation().isIdentity()) + { + aabb = translate(cgeom->aabb_local, t.translation()); + hpp::fcl::Vec3f delta = hpp::fcl::Vec3f::Constant(contact_distance_); + aabb.min_ -= delta; + aabb.max_ += delta; + } + else + { + hpp::fcl::Vec3f center = (t * cgeom->aabb_center).translation(); + hpp::fcl::Vec3f delta = hpp::fcl::Vec3f::Constant(cgeom->aabb_radius + contact_distance_); + aabb.min_ = center - delta; + aabb.max_ = center + delta; + } +} + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_discrete_managers.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_discrete_managers.cpp new file mode 100644 index 00000000000..f95e3bb0b1a --- /dev/null +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_discrete_managers.cpp @@ -0,0 +1,402 @@ +/** + * @file hpp_fcl_discrete_managers.cpp + * @brief Tesseract ROS HPP-FCL contact checker implementation. + * + * @author Levi Armstrong + * @date Dec 18, 2017 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (BSD) + * @par + * All rights reserved. + * @par + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * @par + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * @par + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +static const CollisionShapesConst EMPTY_COLLISION_SHAPES_CONST; +static const tesseract_common::VectorIsometry3d EMPTY_COLLISION_SHAPES_TRANSFORMS; + +HPP_FCLDiscreteBVHManager::HPP_FCLDiscreteBVHManager(std::string name) : name_(std::move(name)) +{ + static_manager_ = std::make_unique(); + dynamic_manager_ = std::make_unique(); + collision_margin_data_ = CollisionMarginData(0); +} + +std::string HPP_FCLDiscreteBVHManager::getName() const { return name_; } + +DiscreteContactManager::UPtr HPP_FCLDiscreteBVHManager::clone() const +{ + auto manager = std::make_unique(); + + for (const auto& cow : link2cow_) + manager->addCollisionObject(cow.second->clone()); + + manager->setActiveCollisionObjects(active_); + manager->setCollisionMarginData(collision_margin_data_); + manager->setIsContactAllowedFn(fn_); + + return manager; +} + +bool HPP_FCLDiscreteBVHManager::addCollisionObject(const std::string& name, + const int& mask_id, + const CollisionShapesConst& shapes, + const tesseract_common::VectorIsometry3d& shape_poses, + bool enabled) +{ + if (link2cow_.find(name) != link2cow_.end()) + removeCollisionObject(name); + + COW::Ptr new_cow = createFCLCollisionObject(name, mask_id, shapes, shape_poses, enabled); + if (new_cow != nullptr) + { + addCollisionObject(new_cow); + return true; + } + + return false; +} + +const CollisionShapesConst& HPP_FCLDiscreteBVHManager::getCollisionObjectGeometries(const std::string& name) const +{ + auto cow = link2cow_.find(name); + return (link2cow_.find(name) != link2cow_.end()) ? cow->second->getCollisionGeometries() : + EMPTY_COLLISION_SHAPES_CONST; +} + +const tesseract_common::VectorIsometry3d& +HPP_FCLDiscreteBVHManager::getCollisionObjectGeometriesTransforms(const std::string& name) const +{ + auto cow = link2cow_.find(name); + return (link2cow_.find(name) != link2cow_.end()) ? cow->second->getCollisionGeometriesTransforms() : + EMPTY_COLLISION_SHAPES_TRANSFORMS; +} + +bool HPP_FCLDiscreteBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool HPP_FCLDiscreteBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + std::vector& objects = it->second->getCollisionObjects(); + fcl_co_count_ -= objects.size(); + + std::vector static_objs; + static_manager_->getObjects(static_objs); + + std::vector dynamic_objs; + dynamic_manager_->getObjects(dynamic_objs); + + // Must check if object exists in the manager before calling unregister. + // If it does not exist and unregister is called it is undefined behavior + for (auto& co : objects) + { + auto static_it = std::find(static_objs.begin(), static_objs.end(), co.get()); + if (static_it != static_objs.end()) + static_manager_->unregisterObject(co.get()); + + auto dynamic_it = std::find(dynamic_objs.begin(), dynamic_objs.end(), co.get()); + if (dynamic_it != dynamic_objs.end()) + dynamic_manager_->unregisterObject(co.get()); + } + + collision_objects_.erase(std::find(collision_objects_.begin(), collision_objects_.end(), name)); + link2cow_.erase(name); + return true; + } + return false; +} + +bool HPP_FCLDiscreteBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + return true; + } + return false; +} + +bool HPP_FCLDiscreteBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + return true; + } + return false; +} + +bool HPP_FCLDiscreteBVHManager::isCollisionObjectEnabled(const std::string& name) const +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + return it->second->m_enabled; + + return false; +} + +void HPP_FCLDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + const Eigen::Isometry3d& cur_tf = it->second->getCollisionObjectsTransform(); + // Note: If the transform has not changed do not updated to prevent unnecessary re-balancing of the BVH tree + if (!cur_tf.translation().isApprox(pose.translation(), 1e-8) || !cur_tf.rotation().isApprox(pose.rotation(), 1e-8)) + { + it->second->setCollisionObjectsTransform(pose); + if (it->second->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + // Note: Calling update causes a re-balance of the AABB tree, which is expensive + static_manager_->update(it->second->getCollisionObjectsRaw()); + } + else + { + // Note: Calling update causes a re-balance of the AABB tree, which is expensive + dynamic_manager_->update(it->second->getCollisionObjectsRaw()); + } + } + } +} + +void HPP_FCLDiscreteBVHManager::setCollisionObjectsTransform(const std::vector& names, + const tesseract_common::VectorIsometry3d& poses) +{ + assert(names.size() == poses.size()); + static_update_.clear(); + dynamic_update_.clear(); + for (auto i = 0U; i < names.size(); ++i) + { + auto it = link2cow_.find(names[i]); + if (it != link2cow_.end()) + { + const Eigen::Isometry3d& cur_tf = it->second->getCollisionObjectsTransform(); + // Note: If the transform has not changed do not updated to prevent unnecessary re-balancing of the BVH tree + if (!cur_tf.translation().isApprox(poses[i].translation(), 1e-8) || + !cur_tf.rotation().isApprox(poses[i].rotation(), 1e-8)) + { + it->second->setCollisionObjectsTransform(poses[i]); + std::vector& co = it->second->getCollisionObjectsRaw(); + if (it->second->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + static_update_.insert(static_update_.end(), co.begin(), co.end()); + } + else + { + dynamic_update_.insert(dynamic_update_.end(), co.begin(), co.end()); + } + } + } + } + + // This is because FCL supports batch update which only re-balances the tree once + if (!static_update_.empty()) + static_manager_->update(static_update_); + + if (!dynamic_update_.empty()) + dynamic_manager_->update(dynamic_update_); +} + +void HPP_FCLDiscreteBVHManager::setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) +{ + static_update_.clear(); + dynamic_update_.clear(); + for (const auto& transform : transforms) + { + auto it = link2cow_.find(transform.first); + if (it != link2cow_.end()) + { + const Eigen::Isometry3d& cur_tf = it->second->getCollisionObjectsTransform(); + // Note: If the transform has not changed do not updated to prevent unnecessary re-balancing of the BVH tree + if (!cur_tf.translation().isApprox(transform.second.translation(), 1e-8) || + !cur_tf.rotation().isApprox(transform.second.rotation(), 1e-8)) + { + it->second->setCollisionObjectsTransform(transform.second); + std::vector& co = it->second->getCollisionObjectsRaw(); + if (it->second->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + static_update_.insert(static_update_.end(), co.begin(), co.end()); + } + else + { + dynamic_update_.insert(dynamic_update_.end(), co.begin(), co.end()); + } + } + } + } + + // This is because FCL supports batch update which only re-balances the tree once + if (!static_update_.empty()) + static_manager_->update(static_update_); + + if (!dynamic_update_.empty()) + dynamic_manager_->update(dynamic_update_); +} + +const std::vector& HPP_FCLDiscreteBVHManager::getCollisionObjects() const { return collision_objects_; } + +void HPP_FCLDiscreteBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + for (auto& co : link2cow_) + updateCollisionObjectFilters(active_, co.second, static_manager_, dynamic_manager_); + + // This causes a refit on the bvh tree. + dynamic_manager_->update(); + static_manager_->update(); +} + +const std::vector& HPP_FCLDiscreteBVHManager::getActiveCollisionObjects() const { return active_; } +void HPP_FCLDiscreteBVHManager::setCollisionMarginData(CollisionMarginData collision_margin_data, + CollisionMarginOverrideType override_type) +{ + collision_margin_data_.apply(collision_margin_data, override_type); + onCollisionMarginDataChanged(); +} + +void HPP_FCLDiscreteBVHManager::setDefaultCollisionMarginData(double default_collision_margin) +{ + collision_margin_data_.setDefaultCollisionMargin(default_collision_margin); + onCollisionMarginDataChanged(); +} + +void HPP_FCLDiscreteBVHManager::setPairCollisionMarginData(const std::string& name1, + const std::string& name2, + double collision_margin) +{ + collision_margin_data_.setPairCollisionMargin(name1, name2, collision_margin); + onCollisionMarginDataChanged(); +} + +const CollisionMarginData& HPP_FCLDiscreteBVHManager::getCollisionMarginData() const { return collision_margin_data_; } +void HPP_FCLDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { fn_ = fn; } +IsContactAllowedFn HPP_FCLDiscreteBVHManager::getIsContactAllowedFn() const { return fn_; } + +void HPP_FCLDiscreteBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) +{ + ContactTestData cdata(active_, collision_margin_data_, fn_, request, collisions); + + if (collision_margin_data_.getMaxCollisionMargin() > 0) + { + DistanceCollisionCallback distanceCallback; + distanceCallback.cdata = &cdata; + + // TODO: Should the order be flipped? + if (!static_manager_->empty()) + static_manager_->collide(dynamic_manager_.get(), &distanceCallback); + + // It looks like the self check is as fast as selfDistanceContactTest even though it is N^2 + if (!cdata.done && !dynamic_manager_->empty()) + dynamic_manager_->collide(&distanceCallback); + } + else + { + CollisionCallback collisionCallback; + collisionCallback.cdata = &cdata; + + // TODO: Should the order be flipped? + if (!static_manager_->empty()) + static_manager_->collide(dynamic_manager_.get(), &collisionCallback); + + // It looks like the self check is as fast as selfDistanceContactTest even though it is N^2 + if (!cdata.done && !dynamic_manager_->empty()) + dynamic_manager_->collide(&collisionCallback); + } +} + +void HPP_FCLDiscreteBVHManager::addCollisionObject(const COW::Ptr& cow) +{ + std::size_t cnt = cow->getCollisionObjectsRaw().size(); + fcl_co_count_ += cnt; + static_update_.reserve(fcl_co_count_); + dynamic_update_.reserve(fcl_co_count_); + link2cow_[cow->getName()] = cow; + collision_objects_.push_back(cow->getName()); + + std::vector& objects = cow->getCollisionObjects(); + if (cow->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + // If static add to static manager + for (auto& co : objects) + static_manager_->registerObject(co.get()); + } + else + { + for (auto& co : objects) + dynamic_manager_->registerObject(co.get()); + } + + // If active links is not empty update filters to replace the active links list + if (!active_.empty()) + updateCollisionObjectFilters(active_, cow, static_manager_, dynamic_manager_); + + // This causes a refit on the bvh tree. + dynamic_manager_->update(); + static_manager_->update(); +} + +void HPP_FCLDiscreteBVHManager::onCollisionMarginDataChanged() +{ + static_update_.clear(); + dynamic_update_.clear(); + + for (auto& cow : link2cow_) + { + cow.second->setContactDistanceThreshold(collision_margin_data_.getMaxCollisionMargin() / 2.0); + std::vector& co = cow.second->getCollisionObjectsRaw(); + if (cow.second->m_collisionFilterGroup == CollisionFilterGroups::StaticFilter) + { + static_update_.insert(static_update_.end(), co.begin(), co.end()); + } + else + { + dynamic_update_.insert(dynamic_update_.end(), co.begin(), co.end()); + } + } + + if (!static_update_.empty()) + static_manager_->update(static_update_); + + if (!dynamic_update_.empty()) + dynamic_manager_->update(dynamic_update_); +} +} // namespace tesseract_collision::tesseract_collision_hpp_fcl diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp new file mode 100644 index 00000000000..1bff078c507 --- /dev/null +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp @@ -0,0 +1,45 @@ +/** + * @file hpp_fcl_factories.cpp + * @brief Factories for loading fcl contact managers as plugins + * + * @author Levi Armstrong + * @date October 25, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +DiscreteContactManager::UPtr HPP_FCLDiscreteBVHManagerFactory::create(const std::string& name, + const YAML::Node& /*config*/) const +{ + return std::make_unique(name); +} + +TESSERACT_PLUGIN_ANCHOR_IMPL(HPP_FCLFactoriesAnchor) // LCOV_EXCL_LINE + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl + +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_DISCRETE_MANAGER_PLUGIN( + tesseract_collision::tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManagerFactory, + HPP_FCLDiscreteBVHManagerFactory); diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp new file mode 100644 index 00000000000..7f8a5d41336 --- /dev/null +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp @@ -0,0 +1,386 @@ +/** + * @file fcl_utils.cpp + * @brief Tesseract ROS FCL Utility Functions. + * + * @author Levi Armstrong + * @date Dec 18, 2017 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (BSD) + * @par + * All rights reserved. + * @par + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * @par + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * @par + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_collision::tesseract_collision_hpp_fcl +{ +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Plane::ConstPtr& geom) +{ + return std::make_shared(geom->getA(), geom->getB(), geom->getC(), geom->getD()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Box::ConstPtr& geom) +{ + return std::make_shared(geom->getX(), geom->getY(), geom->getZ()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Sphere::ConstPtr& geom) +{ + return std::make_shared(geom->getRadius()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Cylinder::ConstPtr& geom) +{ + return std::make_shared(geom->getRadius(), geom->getLength()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Cone::ConstPtr& geom) +{ + return std::make_shared(geom->getRadius(), geom->getLength()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Capsule::ConstPtr& geom) +{ + return std::make_shared(geom->getRadius(), geom->getLength()); +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Mesh::ConstPtr& geom) +{ + int vertex_count = geom->getVertexCount(); + int triangle_count = geom->getFaceCount(); + const tesseract_common::VectorVector3d& vertices = *(geom->getVertices()); + const Eigen::VectorXi& triangles = *(geom->getFaces()); + + auto g = std::make_shared>(); + if (vertex_count > 0 && triangle_count > 0) + { + std::vector tri_indices(static_cast(triangle_count)); + for (int i = 0; i < triangle_count; ++i) + { + assert(triangles[4L * i] == 3); + tri_indices[static_cast(i)] = hpp::fcl::Triangle(static_cast(triangles[(4 * i) + 1]), + static_cast(triangles[(4 * i) + 2]), + static_cast(triangles[(4 * i) + 3])); + } + + g->beginModel(); + g->addSubModel(vertices, tri_indices); + g->endModel(); + + return g; + } + + CONSOLE_BRIDGE_logError("The mesh is empty!"); + return nullptr; +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::ConvexMesh::ConstPtr& geom) +{ + int vertex_count = geom->getVertexCount(); + int triangle_count = geom->getFaceCount(); + const tesseract_common::VectorVector3d& vertices = *(geom->getVertices()); + const Eigen::VectorXi& triangles = *(geom->getFaces()); + + if (vertex_count > 0 && triangle_count > 0) + { + std::vector tri_indices(static_cast(triangle_count)); + for (int i = 0; i < triangle_count; ++i) + { + assert(triangles[4L * i] == 3); + tri_indices[static_cast(i)] = hpp::fcl::Triangle(static_cast(triangles[(4 * i) + 1]), + static_cast(triangles[(4 * i) + 2]), + static_cast(triangles[(4 * i) + 3])); + } + + return CollisionGeometryPtr( + new hpp::fcl::Convex(false, + const_cast*>(geom->getVertices()->data()), + vertex_count, + tri_indices.data(), + triangle_count)); + } + + CONSOLE_BRIDGE_logError("The mesh is empty!"); + return nullptr; +} + +CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Octree::ConstPtr& geom) +{ + switch (geom->getSubType()) + { + case tesseract_geometry::Octree::SubType::BOX: + { + return CollisionGeometryPtr(new hpp::fcl::OcTree(geom->getOctree())); + } + default: + { + CONSOLE_BRIDGE_logError("This hpp-fcl octree sub shape type (%d) is not supported for geometry octree", + static_cast(geom->getSubType())); + return nullptr; + } + } +} + +CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) +{ + switch (geom->getType()) + { + case tesseract_geometry::GeometryType::PLANE: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::BOX: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::SPHERE: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::CYLINDER: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::CONE: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::CAPSULE: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::MESH: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::CONVEX_MESH: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + case tesseract_geometry::GeometryType::OCTREE: + { + return createShapePrimitive(std::static_pointer_cast(geom)); + } + default: + { + CONSOLE_BRIDGE_logError("This geometric shape type (%d) is not supported using hpp-fcl yet", + static_cast(geom->getType())); + return nullptr; + } + } +} + +bool CollisionCallback::collide(hpp::fcl::CollisionObject* o1, hpp::fcl::CollisionObject* o2) +{ + if (cdata->done) + return true; + + const auto* cd1 = static_cast(o1->getUserData()); + const auto* cd2 = static_cast(o2->getUserData()); + + bool needs_collision = cd1->m_enabled && cd2->m_enabled && + (cd1->m_collisionFilterGroup & cd2->m_collisionFilterMask) && // NOLINT + (cd2->m_collisionFilterGroup & cd1->m_collisionFilterMask) && // NOLINT + !isContactAllowed(cd1->getName(), cd2->getName(), cdata->fn, false); + + assert(std::find(cdata->active->begin(), cdata->active->end(), cd1->getName()) != cdata->active->end() || + std::find(cdata->active->begin(), cdata->active->end(), cd2->getName()) != cdata->active->end()); + + if (!needs_collision) + return false; + + std::size_t num_contacts = (cdata->req.contact_limit > 0) ? static_cast(cdata->req.contact_limit) : + std::numeric_limits::max(); + if (cdata->req.type == ContactTestType::FIRST) + num_contacts = 1; + + hpp::fcl::CollisionResult col_result; + hpp::fcl::CollisionRequest col_request; + col_request.num_max_contacts = num_contacts; + col_request.enable_contact = cdata->req.calculate_penetration; + col_request.gjk_variant = hpp::fcl::GJKVariant::NesterovAcceleration; + hpp::fcl::collide(o1, o2, col_request, col_result); + + if (col_result.isCollision()) + { + const Eigen::Isometry3d& tf1 = cd1->getCollisionObjectsTransform(); + const Eigen::Isometry3d& tf2 = cd2->getCollisionObjectsTransform(); + Eigen::Isometry3d tf1_inv = tf1.inverse(); + Eigen::Isometry3d tf2_inv = tf2.inverse(); + + for (size_t i = 0; i < col_result.numContacts(); ++i) + { + const hpp::fcl::Contact& fcl_contact = col_result.getContact(i); + 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.subshape_id[0] = static_cast(fcl_contact.b1); + contact.subshape_id[1] = static_cast(fcl_contact.b2); + contact.nearest_points[0] = fcl_contact.pos; + contact.nearest_points[1] = fcl_contact.pos; + contact.nearest_points_local[0] = tf1_inv * contact.nearest_points[0]; + contact.nearest_points_local[1] = tf2_inv * contact.nearest_points[1]; + contact.transform[0] = tf1; + contact.transform[1] = tf2; + contact.type_id[0] = cd1->getTypeID(); + contact.type_id[1] = cd2->getTypeID(); + contact.distance = -1.0 * fcl_contact.penetration_depth; + contact.normal = fcl_contact.normal; + + ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd1->getName(), cd2->getName()); + const auto it = cdata->res->find(pc); + bool found = (it != cdata->res->end() && !it->second.empty()); + + processResult(*cdata, contact, pc, found); + } + } + + return cdata->done; +} + +bool DistanceCollisionCallback::collide(hpp::fcl::CollisionObject* o1, hpp::fcl::CollisionObject* o2) +{ + if (cdata->done) + return true; + + const auto* cd1 = static_cast(o1->getUserData()); + const auto* cd2 = static_cast(o2->getUserData()); + + bool needs_collision = cd1->m_enabled && cd2->m_enabled && + (cd1->m_collisionFilterGroup & cd2->m_collisionFilterMask) && // NOLINT + (cd2->m_collisionFilterGroup & cd1->m_collisionFilterMask) && // NOLINT + !isContactAllowed(cd1->getName(), cd2->getName(), cdata->fn, false); + + assert(std::find(cdata->active->begin(), cdata->active->end(), cd1->getName()) != cdata->active->end() || + std::find(cdata->active->begin(), cdata->active->end(), cd2->getName()) != cdata->active->end()); + + if (!needs_collision) + return false; + + hpp::fcl::DistanceResult fcl_result; + hpp::fcl::DistanceRequest fcl_request(true); + fcl_request.gjk_variant = hpp::fcl::GJKVariant::NesterovAcceleration; + double d = hpp::fcl::distance(o1, o2, fcl_request, fcl_result); + + if (d < cdata->collision_margin_data.getMaxCollisionMargin()) + { + const Eigen::Isometry3d& tf1 = cd1->getCollisionObjectsTransform(); + const Eigen::Isometry3d& tf2 = cd2->getCollisionObjectsTransform(); + Eigen::Isometry3d tf1_inv = tf1.inverse(); + Eigen::Isometry3d tf2_inv = tf2.inverse(); + + 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.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]; + contact.nearest_points[1] = fcl_result.nearest_points[1]; + contact.nearest_points_local[0] = tf1_inv * contact.nearest_points[0]; + contact.nearest_points_local[1] = tf2_inv * contact.nearest_points[1]; + contact.transform[0] = tf1; + contact.transform[1] = tf2; + contact.type_id[0] = cd1->getTypeID(); + contact.type_id[1] = cd2->getTypeID(); + contact.distance = fcl_result.min_distance; + contact.normal = (fcl_result.min_distance * (contact.nearest_points[1] - contact.nearest_points[0])).normalized(); + + ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd1->getName(), cd2->getName()); + const auto it = cdata->res->find(pc); + bool found = (it != cdata->res->end() && !it->second.empty()); + + processResult(*cdata, contact, pc, found); + } + + return cdata->done; +} + +CollisionObjectWrapper::CollisionObjectWrapper(std::string name, + const int& type_id, + CollisionShapesConst shapes, + tesseract_common::VectorIsometry3d shape_poses) + : name_(std::move(name)), type_id_(type_id), shapes_(std::move(shapes)), shape_poses_(std::move(shape_poses)) +{ + assert(!shapes_.empty()); // NOLINT + assert(!shape_poses_.empty()); // NOLINT + assert(!name_.empty()); // NOLINT + assert(shapes_.size() == shape_poses_.size()); // NOLINT + + m_collisionFilterGroup = CollisionFilterGroups::KinematicFilter; + m_collisionFilterMask = CollisionFilterGroups::StaticFilter | CollisionFilterGroups::KinematicFilter; + + collision_geometries_.reserve(shapes_.size()); + collision_objects_.reserve(shapes_.size()); + collision_objects_raw_.reserve(shapes_.size()); + for (std::size_t i = 0; i < shapes_.size(); ++i) // NOLINT + { + CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]); + if (subshape != nullptr) + { + collision_geometries_.push_back(subshape); + auto co = std::make_shared(subshape); + co->setUserData(this); + co->setTransform(hpp::fcl::Transform3f(shape_poses_[i].rotation(), shape_poses_[i].translation())); + co->updateAABB(); + collision_objects_.push_back(co); + collision_objects_raw_.push_back(co.get()); + } + } +} + +int CollisionObjectWrapper::getShapeIndex(const hpp::fcl::CollisionObject* co) const +{ + 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; +} + +} // namespace tesseract_collision::tesseract_collision_hpp_fcl diff --git a/tesseract_collision/package.xml b/tesseract_collision/package.xml index d0ad2d7db95..8bbfb7c9771 100644 --- a/tesseract_collision/package.xml +++ b/tesseract_collision/package.xml @@ -21,6 +21,7 @@ bullet bullet-extras fcl + hpp-fcl libconsole-bridge-dev tesseract_support diff --git a/tesseract_collision/test/CMakeLists.txt b/tesseract_collision/test/CMakeLists.txt index 2198304736e..20a25ce0fc2 100644 --- a/tesseract_collision/test/CMakeLists.txt +++ b/tesseract_collision/test/CMakeLists.txt @@ -22,6 +22,7 @@ macro(add_gtest test_name test_file) ${PROJECT_NAME}_test_suite ${PROJECT_NAME}_bullet ${PROJECT_NAME}_fcl + ${PROJECT_NAME}_hpp_fcl tesseract::tesseract_geometry tesseract::tesseract_scene_graph console_bridge::console_bridge @@ -47,7 +48,8 @@ macro(add_gtest test_name test_file) ${PROJECT_NAME}_core ${PROJECT_NAME}_test_suite ${PROJECT_NAME}_bullet - ${PROJECT_NAME}_fcl) + ${PROJECT_NAME}_fcl + ${PROJECT_NAME}_hpp_fcl) add_dependencies(run_tests ${test_name}) endmacro() diff --git a/tesseract_collision/test/benchmarks/CMakeLists.txt b/tesseract_collision/test/benchmarks/CMakeLists.txt index c29402661d1..482fd1ff5a0 100644 --- a/tesseract_collision/test/benchmarks/CMakeLists.txt +++ b/tesseract_collision/test/benchmarks/CMakeLists.txt @@ -15,6 +15,7 @@ macro(add_benchmark benchmark_name benchmark_file) ${PROJECT_NAME}_test_suite_benchmarks ${PROJECT_NAME}_bullet ${PROJECT_NAME}_fcl + ${PROJECT_NAME}_hpp_fcl tesseract::tesseract_geometry tesseract::tesseract_scene_graph console_bridge @@ -28,13 +29,15 @@ macro(add_benchmark benchmark_name benchmark_file) ${benchmark_name} ${PROJECT_NAME}_test_suite_benchmarks ${PROJECT_NAME}_bullet - ${PROJECT_NAME}_fcl) + ${PROJECT_NAME}_fcl + ${PROJECT_NAME}_hpp_fcl) add_run_benchmark_target(${benchmark_name}) endmacro() add_benchmark(${PROJECT_NAME}_bullet_discrete_simple_benchmarks bullet_discrete_simple_benchmarks.cpp) add_benchmark(${PROJECT_NAME}_bullet_discrete_bvh_benchmarks bullet_discrete_bvh_benchmarks.cpp) add_benchmark(${PROJECT_NAME}_fcl_discrete_bvh_benchmarks fcl_discrete_bvh_benchmarks.cpp) +add_benchmark(${PROJECT_NAME}_hpp_fcl_discrete_bvh_benchmarks hpp_fcl_discrete_bvh_benchmarks.cpp) # Create target that profiles the collision checkers. add_executable(${PROJECT_NAME}_profile collision_profile.cpp) @@ -42,6 +45,7 @@ target_link_libraries( ${PROJECT_NAME}_profile PRIVATE ${PROJECT_NAME}_bullet ${PROJECT_NAME}_fcl + ${PROJECT_NAME}_hpp_fcl tesseract::tesseract_geometry tesseract::tesseract_support ${Boost_LIBRARIES} diff --git a/tesseract_collision/test/benchmarks/collision_profile.cpp b/tesseract_collision/test/benchmarks/collision_profile.cpp index 3889601db62..141f3649de3 100644 --- a/tesseract_collision/test/benchmarks/collision_profile.cpp +++ b/tesseract_collision/test/benchmarks/collision_profile.cpp @@ -11,6 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include @@ -172,11 +173,14 @@ void runDiscreteProfile(bool use_single_link, bool use_convex_mesh, double conta auto bt_simple_checker = std::make_shared(); auto bt_bvh_checker = std::make_shared(); auto fcl_bvh_checker = std::make_shared(); + auto hpp_fcl_bvh_checker = std::make_shared(); std::vector poses = getTransforms(50); - std::vector checkers = { bt_simple_checker, bt_bvh_checker, fcl_bvh_checker }; - std::vector checker_names = { "BtSimple", "BtBVH", "FCLBVH" }; - std::vector checker_contacts = { 0, 0, 0 }; + std::vector checkers = { + bt_simple_checker, bt_bvh_checker, fcl_bvh_checker, hpp_fcl_bvh_checker + }; + std::vector checker_names = { "BtSimple", "BtBVH", "FCLBVH", "HPP_FCLBHV" }; + std::vector checker_contacts = { 0, 0, 0, 0 }; std::printf("Total number of shape: %d\n", int(DIM * DIM * DIM)); // for (std::size_t i = 0; i < checkers.size(); ++i) diff --git a/tesseract_collision/test/benchmarks/hpp_fcl_discrete_bvh_benchmarks.cpp b/tesseract_collision/test/benchmarks/hpp_fcl_discrete_bvh_benchmarks.cpp new file mode 100644 index 00000000000..753bc10701c --- /dev/null +++ b/tesseract_collision/test/benchmarks/hpp_fcl_discrete_bvh_benchmarks.cpp @@ -0,0 +1,314 @@ +#include +#include + +#include +#include +#include +#include + +using namespace tesseract_collision; +using namespace test_suite; +using namespace tesseract_geometry; + +// Removes some of the long running tests (greater than 30s). Set to false to run everything. +static bool RUN_QUICK = true; + +int main(int argc, char** argv) +{ + const tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager::ConstPtr checker = + std::make_shared(); + + ////////////////////////////////////// + // Clone + ////////////////////////////////////// + { + std::vector num_links = { 0, 2, 4, 8, 16, 32, 64, 128, 256, 512 }; + std::function BM_CLONE_FUNC = BM_CLONE; + for (const auto& num_link : num_links) + { + std::string name = "BM_CLONE_" + checker->getName() + "_ACTIVE_OBJ_" + std::to_string(num_link); + benchmark::RegisterBenchmark(name.c_str(), + BM_CLONE_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(GeometryType::BOX), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(GeometryType::BOX), + Eigen::Isometry3d::Identity(), + ContactTestType::ALL), + num_link) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } + } + + ////////////////////////////////////// + // contactTest + ////////////////////////////////////// + std::function BM_CONTACT_TEST_FUNC = BM_CONTACT_TEST; + + // Make vector of all shapes to try + // Cone, Sphere, and Capsule are absurdly slow. It doesn't even make sense to run them (4/7/2020). + std::vector geometry_types = { + GeometryType::BOX, GeometryType::CYLINDER, GeometryType::CONE, GeometryType::SPHERE, GeometryType::CAPSULE + }; + if (RUN_QUICK) + { + geometry_types.pop_back(); + geometry_types.pop_back(); + geometry_types.pop_back(); + } + + std::vector test_types = { + ContactTestType::ALL, ContactTestType::FIRST, ContactTestType::CLOSEST, ContactTestType::LIMITED + }; + + // In Collision + { + for (const auto& test_type : test_types) + { + // Loop over all primitive combinations + for (const auto& type1 : geometry_types) + { + for (const auto& type2 : geometry_types) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = "BM_CONTACT_TEST_0_" + checker->getName() + "_" + + ContactTestTypeStrings[static_cast(test_type)] + "_" + + GeometryTypeStrings[type1] + "_" + GeometryTypeStrings[type2]; + benchmark::RegisterBenchmark(name.c_str(), + BM_CONTACT_TEST_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(type1), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(type2), + tf.translate(Eigen::Vector3d(0.0001, 0, 0)), + test_type)) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } + } + } + } + // Not in collision. Within contact threshold + { + for (const auto& test_type : test_types) + { + // Loop over all primitive combinations + for (const auto& type1 : geometry_types) + { + for (const auto& type2 : geometry_types) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = "BM_CONTACT_TEST_1_" + checker->getName() + "_" + + ContactTestTypeStrings[static_cast(test_type)] + "_" + + GeometryTypeStrings[type1] + "_" + GeometryTypeStrings[type2]; + benchmark::RegisterBenchmark(name.c_str(), + BM_CONTACT_TEST_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(type1), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(type2), + tf.translate(Eigen::Vector3d(0.6, 0, 0)), + test_type)) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } + } + } + } + + // Not in collision. Outside contact threshold + { + for (const auto& test_type : test_types) + { + // Loop over all primitive combinations + for (const auto& type1 : geometry_types) + { + for (const auto& type2 : geometry_types) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = "BM_CONTACT_TEST_2_" + checker->getName() + "_" + + ContactTestTypeStrings[static_cast(test_type)] + "_" + + GeometryTypeStrings[type1] + "_" + GeometryTypeStrings[type2]; + benchmark::RegisterBenchmark(name.c_str(), + BM_CONTACT_TEST_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(type1), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(type2), + tf.translate(Eigen::Vector3d(2, 0, 0)), + test_type)) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } + } + } + } + + ////////////////////////////////////// + // setCollisionObjectTransform + ////////////////////////////////////// + { + std::function BM_SELECT_RANDOM_OBJECT_FUNC = BM_SELECT_RANDOM_OBJECT; + benchmark::RegisterBenchmark("BM_SELECT_RANDOM_OBJECT", BM_SELECT_RANDOM_OBJECT_FUNC, 256) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + { + std::function BM_SET_COLLISION_OBJECTS_TRANSFORM_SINGLE_FUNC = + BM_SET_COLLISION_OBJECTS_TRANSFORM_SINGLE; + std::vector num_links = { 2, 4, 8, 16, 32, 64, 128, 256, 512 }; + + for (const auto& num_link : num_links) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = + "BM_SET_COLLISION_OBJECTS_TRANSFORM_SINGLE_" + checker->getName() + "_ACTIVE_OBJ_" + std::to_string(num_link); + benchmark::RegisterBenchmark(name.c_str(), + BM_SET_COLLISION_OBJECTS_TRANSFORM_SINGLE_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(GeometryType::BOX), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(GeometryType::BOX), + tf.translate(Eigen::Vector3d(2, 0, 0)), + ContactTestType::ALL), + num_link) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + } + { + std::function BM_SET_COLLISION_OBJECTS_TRANSFORM_VECTOR_FUNC = + BM_SET_COLLISION_OBJECTS_TRANSFORM_VECTOR; + std::vector num_links = { 2, 4, 8, 16, 32, 64, 128, 256, 512 }; + + for (const auto& num_link : num_links) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = + "BM_SET_COLLISION_OBJECTS_TRANSFORM_VECTOR_" + checker->getName() + "_ACTIVE_OBJ_" + std::to_string(num_link); + benchmark::RegisterBenchmark(name.c_str(), + BM_SET_COLLISION_OBJECTS_TRANSFORM_VECTOR_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(GeometryType::BOX), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(GeometryType::BOX), + tf.translate(Eigen::Vector3d(2, 0, 0)), + ContactTestType::ALL), + num_link) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + } + { + std::function BM_SET_COLLISION_OBJECTS_TRANSFORM_MAP_FUNC = + BM_SET_COLLISION_OBJECTS_TRANSFORM_MAP; + std::vector num_links = { 2, 4, 8, 16, 32, 64, 128, 256, 512 }; + + for (const auto& num_link : num_links) + { + auto tf = Eigen::Isometry3d::Identity(); + std::string name = + "BM_SET_COLLISION_OBJECTS_TRANSFORM_MAP_" + checker->getName() + "_ACTIVE_OBJ_" + std::to_string(num_link); + benchmark::RegisterBenchmark(name.c_str(), + BM_SET_COLLISION_OBJECTS_TRANSFORM_MAP_FUNC, + DiscreteBenchmarkInfo(checker, + CreateUnitPrimative(GeometryType::BOX), + Eigen::Isometry3d::Identity(), + CreateUnitPrimative(GeometryType::BOX), + tf.translate(Eigen::Vector3d(2, 0, 0)), + ContactTestType::ALL), + num_link) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + } + ////////////////////////////////////// + // Large Dataset contactTest + ////////////////////////////////////// + if (std::string(BENCHMARK_ARGS) != "CI_ONLY") + { + std::function + BM_LARGE_DATASET_MULTILINK_FUNC = BM_LARGE_DATASET_MULTILINK; + std::vector edge_sizes = { 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12 }; + + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_MULTILINK_" + checker->getName() + "_CONVEX_MESH_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark(name.c_str(), + BM_LARGE_DATASET_MULTILINK_FUNC, + clone, + edge_size, + tesseract_geometry::GeometryType::CONVEX_MESH) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_MULTILINK_" + checker->getName() + "_PRIMATIVE_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark( + name.c_str(), BM_LARGE_DATASET_MULTILINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + // These last two took 45s and 120s. Too long to run in CI, and impractical for our purposes anyway. + if (RUN_QUICK) + { + edge_sizes.pop_back(); + edge_sizes.pop_back(); + } + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_MULTILINK_" + checker->getName() + "_DETAILED_MESH_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark( + name.c_str(), BM_LARGE_DATASET_MULTILINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::MESH) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond); + } + std::function + BM_LARGE_DATASET_SINGLELINK_FUNC = BM_LARGE_DATASET_SINGLELINK; + + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_SINGLELINK_" + checker->getName() + "_CONVEX_MESH_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark(name.c_str(), + BM_LARGE_DATASET_SINGLELINK_FUNC, + clone, + edge_size, + tesseract_geometry::GeometryType::CONVEX_MESH) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_SINGLELINK_" + checker->getName() + "_PRIMATIVE_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark( + name.c_str(), BM_LARGE_DATASET_SINGLELINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kNanosecond); + } + for (const auto& edge_size : edge_sizes) + { + DiscreteContactManager::Ptr clone = checker->clone(); + std::string name = + "BM_LARGE_DATASET_SINGLELINK_" + checker->getName() + "_DETAILED_MESH_EDGE_SIZE_" + std::to_string(edge_size); + benchmark::RegisterBenchmark( + name.c_str(), BM_LARGE_DATASET_SINGLELINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::MESH) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond); + } + } + + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); +} diff --git a/tesseract_collision/test/collision_box_box_unit.cpp b/tesseract_collision/test/collision_box_box_unit.cpp index ef4255c3a29..76b58eebb6d 100644 --- a/tesseract_collision/test/collision_box_box_unit.cpp +++ b/tesseract_collision/test/collision_box_box_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -46,6 +47,17 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxBoxConvexHullUnit) // NO test_suite::runTest(checker, true); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxBoxUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, false); +} + +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxBoxConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, true); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_box_capsule_unit.cpp b/tesseract_collision/test/collision_box_capsule_unit.cpp index 72f3f27511f..d13bffcbf88 100644 --- a/tesseract_collision/test/collision_box_capsule_unit.cpp +++ b/tesseract_collision/test/collision_box_capsule_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -28,6 +29,11 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxCapsuleUnit) // NOLINT test_suite::runTest(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxCapsuleUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_box_cone_unit.cpp b/tesseract_collision/test/collision_box_cone_unit.cpp index f17e815fe4a..f1ecb35e009 100644 --- a/tesseract_collision/test/collision_box_cone_unit.cpp +++ b/tesseract_collision/test/collision_box_cone_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -28,6 +29,11 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxConeUnit) // NOLINT test_suite::runTest(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxConeUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_box_cylinder_unit.cpp b/tesseract_collision/test/collision_box_cylinder_unit.cpp index 2701b2b5897..1c43f3cc973 100644 --- a/tesseract_collision/test/collision_box_cylinder_unit.cpp +++ b/tesseract_collision/test/collision_box_cylinder_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -22,14 +23,20 @@ TEST(TesseractCollisionUnit, BulletDiscreteBVHCollisionBoxCylinderUnit) // NOLI test_suite::runTest(checker); } -// TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxCylinderUnit) -//{ -// TODO: Currently this fails when using FCL. An issue has been created -// and they are currently addressing the issue. +TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxCylinderUnit) +{ + // TODO: Currently this fails when using FCL. An issue has been created + // and they are currently addressing the issue. + + tesseract_collision_fcl::FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} -// tesseract::tesseract_fcl::FCLDiscreteBVHManager checker; -// runTest(checker); -//} +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxCylinderUnit) +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} int main(int argc, char** argv) { diff --git a/tesseract_collision/test/collision_box_sphere_unit.cpp b/tesseract_collision/test/collision_box_sphere_unit.cpp index 9ac5978f945..d6bbdea8371 100644 --- a/tesseract_collision/test/collision_box_sphere_unit.cpp +++ b/tesseract_collision/test/collision_box_sphere_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -46,6 +47,17 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionBoxSphereConvexHullUnit) // test_suite::runTest(checker, true); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxSphereUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, false); +} + +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionBoxSphereConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, true); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_clone_unit.cpp b/tesseract_collision/test/collision_clone_unit.cpp index 93c6caca97c..35edfd54659 100644 --- a/tesseract_collision/test/collision_clone_unit.cpp +++ b/tesseract_collision/test/collision_clone_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -29,6 +30,11 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionCloneUnit) // NOLINT // currently } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionCloneUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, 0.001, 0.001, 0.001); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_compound_compound_unit.cpp b/tesseract_collision/test/collision_compound_compound_unit.cpp index a6234f25887..cce574d8b52 100644 --- a/tesseract_collision/test/collision_compound_compound_unit.cpp +++ b/tesseract_collision/test/collision_compound_compound_unit.cpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -30,6 +31,11 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionCompoundCompoundUnit) // NO test_suite::runTest(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionCompoundCompoundUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} TEST(TesseractCollisionUnit, BulletContinuousSimpleCollisionCompoundCompoundUnit) // NOLINT { tesseract_collision_bullet::BulletCastSimpleManager checker; diff --git a/tesseract_collision/test/collision_large_dataset_unit.cpp b/tesseract_collision/test/collision_large_dataset_unit.cpp index a631967cea2..2558b1373c1 100644 --- a/tesseract_collision/test/collision_large_dataset_unit.cpp +++ b/tesseract_collision/test/collision_large_dataset_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -46,6 +47,18 @@ TEST(TesseractCollisionLargeDataSetUnit, FCLDiscreteBVHCollisionLargeDataSetUnit test_suite::runTest(checker); } +TEST(TesseractCollisionLargeDataSetUnit, HPP_FCLDiscreteBVHCollisionLargeDataSetConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, true); +} + +TEST(TesseractCollisionLargeDataSetUnit, HPP_FCLDiscreteBVHCollisionLargeDataSetUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_mesh_mesh_unit.cpp b/tesseract_collision/test/collision_mesh_mesh_unit.cpp index 65bc43601b7..9d6dd040c63 100644 --- a/tesseract_collision/test/collision_mesh_mesh_unit.cpp +++ b/tesseract_collision/test/collision_mesh_mesh_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -28,6 +29,11 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionMeshMeshUnit) // NOLINT test_suite::runTest(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionMeshMeshUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_multi_threaded_unit.cpp b/tesseract_collision/test/collision_multi_threaded_unit.cpp index 7eee7593dd2..d49b1a3e20d 100644 --- a/tesseract_collision/test/collision_multi_threaded_unit.cpp +++ b/tesseract_collision/test/collision_multi_threaded_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -46,6 +47,17 @@ TEST(TesseractCollisionMultiThreadedUnit, FCLDiscreteBVHCollisionMultiThreadedUn test_suite::runTest(checker); } +TEST(TesseractCollisionMultiThreadedUnit, HPP_FCLDiscreteBVHCollisionMultiThreadedConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, true); +} + +TEST(TesseractCollisionMultiThreadedUnit, HPP_FCLDiscreteBVHCollisionMultiThreadedUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_octomap_mesh_unit.cpp b/tesseract_collision/test/collision_octomap_mesh_unit.cpp index bdf40231ea0..349d818879e 100644 --- a/tesseract_collision/test/collision_octomap_mesh_unit.cpp +++ b/tesseract_collision/test/collision_octomap_mesh_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -25,13 +26,20 @@ TEST(TesseractCollisionUnit, BulletDiscreteBVHCollisionOctomapSphereMeshUnit) / test_suite::runTest(checker, tesseract_common::getTempPath() + "BulletDiscreteBVHCollisionOctomapSphereMeshUnit.ply"); } -// TODO: There is an issue with fcl octomap collision type. Either with Tesseract implementation or fcl +// // TODO: There is an issue with fcl octomap collision type. Either with Tesseract implementation or fcl // TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionOctomapSphereMeshUnit) -//{ +// { // tesseract_collision_fcl::FCLDiscreteBVHManager checker; // test_suite::runTest(checker, tesseract_common::getTempPath() + "FCLDiscreteBVHCollisionOctomapSphereMeshUnit.ply"); // // TODO: There appears to be an issue in fcl for octomap::OcTree. -//} +// } + +// TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionOctomapSphereMeshUnit) +// { +// tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; +// test_suite::runTest(checker, tesseract_common::getTempPath() + +// "HPP_FCLDiscreteBVHCollisionOctomapSphereMeshUnit.ply"); +// } /** @brief This is to test the shape id and sub-shape id of the contact results. */ int main(int argc, char** argv) diff --git a/tesseract_collision/test/collision_octomap_sphere_unit.cpp b/tesseract_collision/test/collision_octomap_sphere_unit.cpp index 3a1b74fb025..b5581af349b 100644 --- a/tesseract_collision/test/collision_octomap_sphere_unit.cpp +++ b/tesseract_collision/test/collision_octomap_sphere_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -46,6 +47,18 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionOctomapSphereConvexHullUnit) test_suite::runTest(checker, 0.16, true); // TODO: There appears to be an issue in fcl for octomap::OcTree. } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionOctomapSphereUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, 0.16, false); +} + +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionOctomapSphereConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, 0.16, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/collision_sphere_sphere_unit.cpp b/tesseract_collision/test/collision_sphere_sphere_unit.cpp index b57e78409ef..3d8410431a5 100644 --- a/tesseract_collision/test/collision_sphere_sphere_unit.cpp +++ b/tesseract_collision/test/collision_sphere_sphere_unit.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; @@ -49,6 +50,18 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionSphereSphereConvexHullUnit) test_suite::detail::runTestConvex3(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionSphereSphereUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, false); +} + +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHCollisionSphereSphereConvexHullUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker, false); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_collision/test/contact_manager_plugins.yaml b/tesseract_collision/test/contact_manager_plugins.yaml index 4209235a988..5354f35019a 100644 --- a/tesseract_collision/test/contact_manager_plugins.yaml +++ b/tesseract_collision/test/contact_manager_plugins.yaml @@ -4,6 +4,7 @@ contact_manager_plugins: search_libraries: - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories + - tesseract_collision_hpp_fcl_factories discrete_plugins: default: BulletDiscreteBVHManager plugins: @@ -13,6 +14,8 @@ contact_manager_plugins: class: BulletDiscreteSimpleManagerFactory FCLDiscreteBVHManager: class: FCLDiscreteBVHManagerFactory + HPP_FCLDiscreteBVHManager: + class: HPP_FCLDiscreteBVHManagerFactory continuous_plugins: default: BulletCastBVHManager plugins: diff --git a/tesseract_collision/test/contact_managers_config_unit.cpp b/tesseract_collision/test/contact_managers_config_unit.cpp index f8e7b84d38a..f35337718c6 100644 --- a/tesseract_collision/test/contact_managers_config_unit.cpp +++ b/tesseract_collision/test/contact_managers_config_unit.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include using namespace tesseract_collision; @@ -68,6 +69,12 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHContactManagerConfigUnit) // NOLINT test_suite::runTest(checker); } +TEST(TesseractCollisionUnit, HPP_FCLDiscreteBVHContactManagerConfigUnit) // NOLINT +{ + tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} + TEST(TesseractCollisionUnit, CombineContactAllowedFnUnit) // NOLINT { { // tesseract_collision::ACMOverrideType::NONE diff --git a/tesseract_collision/test/contact_managers_factory_static_unit.cpp b/tesseract_collision/test/contact_managers_factory_static_unit.cpp index e928bb54c33..2131f6ce3c0 100644 --- a/tesseract_collision/test/contact_managers_factory_static_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_static_unit.cpp @@ -46,6 +46,7 @@ TEST(TesseractContactManagersFactoryUnit, StaticLoadPlugin) // NOLINT search_libraries: - tesseract_collision_bullet_factories_not_there - tesseract_collision_fcl_factories_not_there + - tesseract_collision_hpp_fcl_factories_not_there discrete_plugins: default: BulletDiscreteBVHManager plugins: @@ -55,6 +56,8 @@ TEST(TesseractContactManagersFactoryUnit, StaticLoadPlugin) // NOLINT class: BulletDiscreteSimpleManagerFactory FCLDiscreteBVHManager: class: FCLDiscreteBVHManagerFactory + HPP_FCLDiscreteBVHManager: + class: HPP_FCLDiscreteBVHManagerFactory continuous_plugins: default: BulletCastBVHManager plugins: diff --git a/tesseract_collision/test/contact_managers_factory_unit.cpp b/tesseract_collision/test/contact_managers_factory_unit.cpp index a6a3f50de1e..375145ce271 100644 --- a/tesseract_collision/test/contact_managers_factory_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_unit.cpp @@ -58,7 +58,7 @@ void runContactManagersFactoryTest(const tesseract_common::fs::path& config_path { std::set sl = factory.getSearchLibraries(); - EXPECT_EQ(sl.size(), 2); + EXPECT_EQ(sl.size(), 3); for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) { @@ -66,7 +66,7 @@ void runContactManagersFactoryTest(const tesseract_common::fs::path& config_path } } - EXPECT_EQ(discrete_plugins.size(), 3); + EXPECT_EQ(discrete_plugins.size(), 4); for (auto cm_it = discrete_plugins.begin(); cm_it != discrete_plugins.end(); ++cm_it) { auto name = cm_it->first.as(); @@ -135,6 +135,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadStringPluginTest) // NOLINT search_libraries: - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories + - tesseract_collision_hpp_fcl_factories discrete_plugins: default: BulletDiscreteBVHManager plugins: @@ -144,6 +145,8 @@ TEST(TesseractContactManagersFactoryUnit, LoadStringPluginTest) // NOLINT class: BulletDiscreteSimpleManagerFactory FCLDiscreteBVHManager: class: FCLDiscreteBVHManagerFactory + HPP_FCLDiscreteBVHManager: + class: HPP_FCLDiscreteBVHManagerFactory continuous_plugins: default: BulletCastBVHManager plugins: @@ -173,7 +176,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadStringPluginTest) // NOLINT { std::set sl = factory.getSearchLibraries(); - EXPECT_EQ(sl.size(), 2); + EXPECT_EQ(sl.size(), 3); for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) { @@ -181,7 +184,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadStringPluginTest) // NOLINT } } - EXPECT_EQ(discrete_plugins.size(), 3); + EXPECT_EQ(discrete_plugins.size(), 4); for (auto cm_it = discrete_plugins.begin(); cm_it != discrete_plugins.end(); ++cm_it) { auto name = cm_it->first.as(); @@ -206,7 +209,7 @@ TEST(TesseractContactManagersFactoryUnit, PluginFactorAPIUnit) // NOLINT EXPECT_FALSE(factory.getSearchPaths().empty()); EXPECT_EQ(factory.getSearchPaths().size(), 1); EXPECT_FALSE(factory.getSearchLibraries().empty()); - EXPECT_EQ(factory.getSearchLibraries().size(), 2); + EXPECT_EQ(factory.getSearchLibraries().size(), 3); EXPECT_EQ(factory.getDiscreteContactManagerPlugins().size(), 0); EXPECT_EQ(factory.getContinuousContactManagerPlugins().size(), 0); EXPECT_ANY_THROW(factory.getDefaultDiscreteContactManagerPlugin()); // NOLINT @@ -216,11 +219,11 @@ TEST(TesseractContactManagersFactoryUnit, PluginFactorAPIUnit) // NOLINT factory.addSearchPath("/usr/local/lib"); EXPECT_EQ(factory.getSearchPaths().size(), 2); - EXPECT_EQ(factory.getSearchLibraries().size(), 2); + EXPECT_EQ(factory.getSearchLibraries().size(), 3); factory.addSearchLibrary("tesseract_collision"); EXPECT_EQ(factory.getSearchPaths().size(), 2); - EXPECT_EQ(factory.getSearchLibraries().size(), 3); + EXPECT_EQ(factory.getSearchLibraries().size(), 4); { tesseract_common::PluginInfoMap map = factory.getDiscreteContactManagerPlugins(); @@ -308,6 +311,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyDiscretePluginTest) // NOLINT search_libraries: - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories + - tesseract_collision_hpp_fcl_factories discrete_plugins: default: BulletDiscreteBVHManager plugins: @@ -316,7 +320,9 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyDiscretePluginTest) // NOLINT BulletDiscreteSimpleManager: class: BulletDiscreteSimpleManagerFactory FCLDiscreteBVHManager: - class: FCLDiscreteBVHManagerFactory)"; + class: FCLDiscreteBVHManagerFactory + HPP_FCLDiscreteBVHManager: + class: HPP_FCLDiscreteBVHManagerFactory)"; ContactManagersPluginFactory factory(config); YAML::Node plugin_config = YAML::Load(config); @@ -338,7 +344,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyDiscretePluginTest) // NOLINT { std::set sl = factory.getSearchLibraries(); - EXPECT_EQ(sl.size(), 2); + EXPECT_EQ(sl.size(), 3); for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) { @@ -346,7 +352,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyDiscretePluginTest) // NOLINT } } - EXPECT_EQ(discrete_plugins.size(), 3); + EXPECT_EQ(discrete_plugins.size(), 4); for (auto cm_it = discrete_plugins.begin(); cm_it != discrete_plugins.end(); ++cm_it) { auto name = cm_it->first.as(); @@ -364,6 +370,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyContinuousPluginTest) // NOLI search_libraries: - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories + - tesseract_collision_hpp_fcl_factories continuous_plugins: default: BulletCastBVHManager plugins: @@ -392,7 +399,7 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyContinuousPluginTest) // NOLI { std::set sl = factory.getSearchLibraries(); - EXPECT_EQ(sl.size(), 2); + EXPECT_EQ(sl.size(), 3); for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) { From ce59b4000da8db2481472db79f7a89a47e321fbf Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 6 Feb 2024 13:53:15 +0100 Subject: [PATCH 2/6] Add hpp-fcl to dependencies.repos --- dependencies.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dependencies.repos b/dependencies.repos index 076e79c04bd..4885e6359fd 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -10,6 +10,10 @@ local-name: fcl uri: https://github.com/flexible-collision-library/fcl.git version: 0.6.1 +- git: + local-name: hpp-fcl + uri: https://github.com/humanoid-path-planner/hpp-fcl.git + version: v2.4.1 - git: local-name: octomap uri: https://github.com/OctoMap/octomap.git From ecc1618c6733c7cc41bafd2e5eea5ddf1e75b504 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 6 Feb 2024 14:23:55 +0100 Subject: [PATCH 3/6] Disable building of python interface for hpp-fcl --- .github/workflows/code_quality.yml | 2 +- .github/workflows/ubuntu.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/code_quality.yml b/.github/workflows/code_quality.yml index d9eae2525fb..5b78800b2af 100644 --- a/.github/workflows/code_quality.yml +++ b/.github/workflows/code_quality.yml @@ -53,7 +53,7 @@ jobs: ccache-prefix: ${{ matrix.distro }} vcs-file: dependencies.repos run-tests: false - upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release + upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_INTERFACE=OFF target-path: target_ws/src target-args: --cmake-args ${{ matrix.env.TARGET_CMAKE_ARGS }} diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 89d69a63d03..e857ac02a1c 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -48,6 +48,6 @@ jobs: with: ccache-prefix: ${{ matrix.distro }} vcs-file: dependencies.repos - upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release + upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_INTERFACE=OFF target-path: target_ws/src target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON From 4e5553edad7059ab7f4e7e7e50a42736616d0250 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 6 Feb 2024 19:12:21 +0100 Subject: [PATCH 4/6] NOLINT const_cast --- tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp index 7f8a5d41336..1a53edbfc45 100644 --- a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp @@ -131,12 +131,12 @@ CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::ConvexMesh:: static_cast(triangles[(4 * i) + 3])); } - return CollisionGeometryPtr( - new hpp::fcl::Convex(false, - const_cast*>(geom->getVertices()->data()), - vertex_count, - tri_indices.data(), - triangle_count)); + return CollisionGeometryPtr(new hpp::fcl::Convex( + false, + const_cast*>(geom->getVertices()->data()), // NOLINT + vertex_count, + tri_indices.data(), + triangle_count)); } CONSOLE_BRIDGE_logError("The mesh is empty!"); From e87265ac60a5731a42442e80472cc5c658f27275 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 19 Feb 2024 15:15:08 +0100 Subject: [PATCH 5/6] Update to hpp-fcl devel branch --- dependencies.repos | 2 +- .../hpp_fcl/src/hpp_fcl_utils.cpp | 27 ++++++++++--------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/dependencies.repos b/dependencies.repos index 4885e6359fd..cb2d0152ef5 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -13,7 +13,7 @@ - git: local-name: hpp-fcl uri: https://github.com/humanoid-path-planner/hpp-fcl.git - version: v2.4.1 + version: devel - git: local-name: octomap uri: https://github.com/OctoMap/octomap.git diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp index 1a53edbfc45..2dbeb73342f 100644 --- a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp @@ -117,26 +117,28 @@ CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::ConvexMesh:: { int vertex_count = geom->getVertexCount(); int triangle_count = geom->getFaceCount(); - const tesseract_common::VectorVector3d& vertices = *(geom->getVertices()); const Eigen::VectorXi& triangles = *(geom->getFaces()); if (vertex_count > 0 && triangle_count > 0) { - std::vector tri_indices(static_cast(triangle_count)); + // TODO: This is just a copy to get rid of the constness + auto vertices = std::make_shared>(static_cast(vertex_count)); + for (int i = 0; i < vertex_count; ++i) + { + vertices->at(static_cast(i)) = geom->getVertices()->at(i); + } + + auto tri_indices = std::make_shared>(static_cast(triangle_count)); for (int i = 0; i < triangle_count; ++i) { assert(triangles[4L * i] == 3); - tri_indices[static_cast(i)] = hpp::fcl::Triangle(static_cast(triangles[(4 * i) + 1]), - static_cast(triangles[(4 * i) + 2]), - static_cast(triangles[(4 * i) + 3])); + tri_indices->at(static_cast(i)) = hpp::fcl::Triangle(static_cast(triangles[(4 * i) + 1]), + static_cast(triangles[(4 * i) + 2]), + static_cast(triangles[(4 * i) + 3])); } - return CollisionGeometryPtr(new hpp::fcl::Convex( - false, - const_cast*>(geom->getVertices()->data()), // NOLINT - vertex_count, - tri_indices.data(), - triangle_count)); + return CollisionGeometryPtr( + new hpp::fcl::Convex(vertices, vertex_count, tri_indices, triangle_count)); } CONSOLE_BRIDGE_logError("The mesh is empty!"); @@ -326,7 +328,8 @@ bool DistanceCollisionCallback::collide(hpp::fcl::CollisionObject* o1, hpp::fcl: contact.type_id[0] = cd1->getTypeID(); contact.type_id[1] = cd2->getTypeID(); contact.distance = fcl_result.min_distance; - contact.normal = (fcl_result.min_distance * (contact.nearest_points[1] - contact.nearest_points[0])).normalized(); + // contact.normal = (fcl_result.min_distance * (contact.nearest_points[1] - contact.nearest_points[0])).normalized(); + contact.normal = fcl_result.normal; ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd1->getName(), cd2->getName()); const auto it = cdata->res->find(pc); From 1b8cde1d844ca958d2d594300c11d61a650da153 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 18 Jul 2024 14:52:42 +0200 Subject: [PATCH 6/6] Leverage forward declarations to improve compile times --- .../tesseract_collision/hpp_fcl/hpp_fcl_factories.h | 3 ++- tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp | 7 ++++--- tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h index 992ce82f93a..d2f91f2b462 100644 --- a/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h +++ b/tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h @@ -33,7 +33,8 @@ namespace tesseract_collision::tesseract_collision_hpp_fcl class HPP_FCLDiscreteBVHManagerFactory : public DiscreteContactManagerFactory { public: - DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(HPP_FCLFactoriesAnchor) diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp index 1bff078c507..b0534537d8f 100644 --- a/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_factories.cpp @@ -26,11 +26,12 @@ #include #include +#include namespace tesseract_collision::tesseract_collision_hpp_fcl { -DiscreteContactManager::UPtr HPP_FCLDiscreteBVHManagerFactory::create(const std::string& name, - const YAML::Node& /*config*/) const +std::unique_ptr +HPP_FCLDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node& /*config*/) const { return std::make_unique(name); } @@ -42,4 +43,4 @@ TESSERACT_PLUGIN_ANCHOR_IMPL(HPP_FCLFactoriesAnchor) // LCOV_EXCL_LINE // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_DISCRETE_MANAGER_PLUGIN( tesseract_collision::tesseract_collision_hpp_fcl::HPP_FCLDiscreteBVHManagerFactory, - HPP_FCLDiscreteBVHManagerFactory); + HPP_FCLDiscreteBVHManagerFactory) diff --git a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp index 2dbeb73342f..5234a0ebc56 100644 --- a/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp +++ b/tesseract_collision/hpp_fcl/src/hpp_fcl_utils.cpp @@ -149,7 +149,7 @@ CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Octree::Cons { switch (geom->getSubType()) { - case tesseract_geometry::Octree::SubType::BOX: + case tesseract_geometry::OctreeSubType::BOX: { return CollisionGeometryPtr(new hpp::fcl::OcTree(geom->getOctree())); }