Add hpp-fcl as a collision checker

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()
 

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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                                          "$<INSTALL_INTERFACE:include>")
+
+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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                           "$<INSTALL_INTERFACE:include>")
+
+# 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()

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/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <hpp/fcl/collision_object.h>
+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

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 <tesseract_collision/core/discrete_contact_manager.h>
+#include <tesseract_collision/hpp_fcl/hpp_fcl_utils.h>
+
+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<HPP_FCLDiscreteBVHManager>;
+  using ConstPtr = std::shared_ptr<const HPP_FCLDiscreteBVHManager>;
+  using UPtr = std::unique_ptr<HPP_FCLDiscreteBVHManager>;
+  using ConstUPtr = std::unique_ptr<const HPP_FCLDiscreteBVHManager>;
+
+  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<std::string>& names,
+                                    const tesseract_common::VectorIsometry3d& poses) override final;
+
+  void setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) override final;
+
+  const std::vector<std::string>& getCollisionObjects() const override final;
+
+  void setActiveCollisionObjects(const std::vector<std::string>& names) override final;
+
+  const std::vector<std::string>& 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<hpp::fcl::BroadPhaseCollisionManager> static_manager_;
+
+  /** @brief Broad-phase Collision Manager for active collision objects */
+  std::unique_ptr<hpp::fcl::BroadPhaseCollisionManager> dynamic_manager_;
+
+  Link2COW link2cow_;               /**< @brief A map of all (static and active) collision objects being managed */
+  std::vector<std::string> active_; /**< @brief A list of the active collision objects */
+  std::vector<std::string> 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<CollisionObjectRawPtr> static_update_;
+
+  /** @brief This is used to store dynamic collision objects to update */
+  std::vector<CollisionObjectRawPtr> 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

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 <tesseract_collision/core/contact_managers_plugin_factory.h>
+
+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