Skip to content

Commit 3ea809a

Browse files
rjoomenrjoomen
committed
Add hpp-fcl as a collision checker
1 parent f0e5732 commit 3ea809a

32 files changed

+2029
-29
lines changed

‎tesseract_collision/CMakeLists.txt‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,14 @@ if(TESSERACT_BUILD_FCL)
9494
add_subdirectory(fcl)
9595
endif()
9696

97+
# HPP-FCL
98+
option(TESSERACT_BUILD_HPP_FCL "Build HPP-FCL components" ON)
99+
if(TESSERACT_BUILD_HPP_FCL)
100+
message("Building HPP-FCL components")
101+
list(APPEND SUPPORTED_COMPONENTS hpp_fcl)
102+
add_subdirectory(hpp_fcl)
103+
endif()
104+
97105
# VHACD
98106
option(TESSERACT_BUILD_VHACD "Build VHACD components" ON)
99107
if(TESSERACT_BUILD_VHACD)
@@ -112,20 +120,23 @@ target_compile_definitions(${PROJECT_NAME}_core
112120
PRIVATE TESSERACT_CONTACT_MANAGERS_PLUGINS="${CONTACT_MANAGERS_PLUGINS_STRING}")
113121

114122
# Testing
115-
if((TESSERACT_ENABLE_TESTING OR TESSERACT_COLLISION_ENABLE_TESTING) AND TESSERACT_BUILD_FCL)
123+
if((TESSERACT_ENABLE_TESTING OR TESSERACT_COLLISION_ENABLE_TESTING) AND TESSERACT_BUILD_FCL AND TESSERACT_BUILD_HPP_FCL)
116124
enable_testing()
117125
add_run_tests_target(ENABLE ${TESSERACT_ENABLE_RUN_TESTING})
118126
add_subdirectory(test)
119127
endif()
120128

121129
# Benchmarks
122-
if((TESSERACT_ENABLE_BENCHMARKING OR TESSERACT_COLLISION_ENABLE_BENCHMARKING) AND TESSERACT_BUILD_FCL
130+
if((TESSERACT_ENABLE_BENCHMARKING OR TESSERACT_COLLISION_ENABLE_BENCHMARKING)
131+
AND TESSERACT_BUILD_FCL
132+
AND TESSERACT_BUILD_HPP_FCL
123133
AND TESSERACT_BUILD_TEST_SUITE)
124134
add_subdirectory(test/benchmarks)
125135
endif()
126136

127137
# Examples
128-
if((TESSERACT_ENABLE_EXAMPLES OR TESSERACT_COLLISION_ENABLE_EXAMPLES) AND TESSERACT_BUILD_FCL)
138+
if((TESSERACT_ENABLE_EXAMPLES OR TESSERACT_COLLISION_ENABLE_EXAMPLES) AND TESSERACT_BUILD_FCL
139+
AND TESSERACT_BUILD_HPP_FCL)
129140
add_subdirectory(examples)
130141
endif()
131142

‎tesseract_collision/hpp_fcl/CMakeLists.txt‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
find_package(octomap REQUIRED)
2+
find_package(hpp-fcl REQUIRED)
3+
4+
# Create target for HPP-FCL implementation
5+
add_library(${PROJECT_NAME}_hpp_fcl src/hpp_fcl_discrete_managers.cpp src/hpp_fcl_utils.cpp
6+
src/hpp_fcl_collision_object_wrapper.cpp)
7+
target_link_libraries(
8+
${PROJECT_NAME}_hpp_fcl
9+
PUBLIC ${PROJECT_NAME}_core
10+
Eigen3::Eigen
11+
tesseract::tesseract_geometry
12+
hpp-fcl::hpp-fcl
13+
console_bridge::console_bridge
14+
octomap
15+
octomath)
16+
target_compile_options(${PROJECT_NAME}_hpp_fcl PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
17+
target_compile_options(${PROJECT_NAME}_hpp_fcl PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
18+
target_compile_definitions(${PROJECT_NAME}_hpp_fcl PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
19+
target_cxx_version(${PROJECT_NAME}_hpp_fcl PUBLIC VERSION ${TESSERACT_CXX_VERSION})
20+
target_clang_tidy(${PROJECT_NAME}_hpp_fcl ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
21+
target_code_coverage(
22+
${PROJECT_NAME}_hpp_fcl
23+
PRIVATE
24+
ALL
25+
EXCLUDE ${COVERAGE_EXCLUDE}
26+
ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE})
27+
target_include_directories(${PROJECT_NAME}_hpp_fcl PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
28+
"$<INSTALL_INTERFACE:include>")
29+
30+
add_library(${PROJECT_NAME}_hpp_fcl_factories src/hpp_fcl_factories.cpp)
31+
target_link_libraries(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${PROJECT_NAME}_hpp_fcl)
32+
target_compile_options(${PROJECT_NAME}_hpp_fcl_factories PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
33+
target_compile_options(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
34+
target_compile_definitions(${PROJECT_NAME}_hpp_fcl_factories PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
35+
target_clang_tidy(${PROJECT_NAME}_hpp_fcl_factories ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
36+
target_cxx_version(${PROJECT_NAME}_hpp_fcl_factories PUBLIC VERSION ${TESSERACT_CXX_VERSION})
37+
target_code_coverage(
38+
${PROJECT_NAME}_hpp_fcl_factories
39+
PRIVATE
40+
ALL
41+
EXCLUDE ${COVERAGE_EXCLUDE}
42+
ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE})
43+
target_include_directories(
44+
${PROJECT_NAME}_hpp_fcl_factories PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
45+
"$<INSTALL_INTERFACE:include>")
46+
47+
# Add factory library so contact_managers_factory can find these factories by defauult
48+
set(CONTACT_MANAGERS_PLUGINS ${CONTACT_MANAGERS_PLUGINS} "${PROJECT_NAME}_hpp_fcl_factories" PARENT_SCOPE)
49+
50+
# Mark cpp header files for installation
51+
install(
52+
DIRECTORY include/${PROJECT_NAME}
53+
DESTINATION include
54+
FILES_MATCHING
55+
PATTERN "*.h"
56+
PATTERN "*.hpp"
57+
PATTERN "*.inl"
58+
PATTERN ".svn" EXCLUDE)
59+
60+
configure_component(
61+
COMPONENT hpp_fcl
62+
NAMESPACE tesseract
63+
TARGETS ${PROJECT_NAME}_hpp_fcl ${PROJECT_NAME}_hpp_fcl_factories
64+
DEPENDENCIES "tesseract_collision COMPONENTS core" "hpp-fcl")
65+
66+
if(TESSERACT_PACKAGE)
67+
cpack_component(
68+
COMPONENT hpp_fcl
69+
VERSION ${pkg_extracted_version}
70+
DESCRIPTION "Tesseract Collision HPP-FCL components"
71+
COMPONENT_DEPENDS core
72+
LINUX_DEPENDS "hpp-fcl | ${TESSERACT_PACKAGE_PREFIX}hpp_fcl"
73+
WINDOWS_DEPENDS "hpp-fcl | ${TESSERACT_PACKAGE_PREFIX}hpp_fcl")
74+
endif()

‎tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_collision_object_wrapper.h‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
/**
2+
* @file hpp_fcl_collision_object_wrapper.h
3+
* @brief Collision Object Wrapper to modify AABB with contact distance threshold
4+
*
5+
* @author Levi Armstrong
6+
* @date April 14, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H
27+
#define TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <hpp/fcl/collision_object.h>
32+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
33+
34+
namespace tesseract_collision::tesseract_collision_hpp_fcl
35+
{
36+
/**
37+
* @brief This is a wrapper around HPP-FCL Collision Object Class which allows you to expand the AABB by the contact
38+
* dist.
39+
*
40+
* This significantly improves performance when making distance requests if performing a contact tests type FIRST.
41+
*/
42+
class HPP_FCLCollisionObjectWrapper : public hpp::fcl::CollisionObject
43+
{
44+
public:
45+
using hpp::fcl::CollisionObject::CollisionObject;
46+
47+
/**
48+
* @brief Set the collision objects contact distance threshold.
49+
*
50+
* This automatically calls updateAABB() which increases the AABB by the contact distance.
51+
* @param contact_distance The contact distance threshold.
52+
*/
53+
void setContactDistanceThreshold(double contact_distance);
54+
55+
/**
56+
* @brief Get the collision objects contact distance threshold.
57+
* @return The contact distance threshold.
58+
*/
59+
double getContactDistanceThreshold() const;
60+
61+
/**
62+
* @brief Update the internal AABB. This must be called instead of the base class computeAABB().
63+
*
64+
* After setting the collision objects transform this must be called.
65+
*/
66+
void updateAABB();
67+
68+
protected:
69+
double contact_distance_{ 0 }; /**< @brief The contact distance threshold. */
70+
};
71+
72+
} // namespace tesseract_collision::tesseract_collision_hpp_fcl
73+
74+
#endif // TESSERACT_COLLISION_HPP_FCL_COLLISION_OBJECT_WRAPPER_H

‎tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_discrete_managers.h‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
/**
2+
* @file hpp_fcl_discrete_managers.h
3+
* @brief Tesseract ROS HPP-FCL contact checker implementation.
4+
*
5+
* @author Levi Armstrong
6+
* @date Dec 18, 2017
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2017, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (BSD)
14+
* @par
15+
* All rights reserved.
16+
* @par
17+
* Redistribution and use in source and binary forms, with or without
18+
* modification, are permitted provided that the following conditions
19+
* are met:
20+
* @par
21+
* * Redistributions of source code must retain the above copyright
22+
* notice, this list of conditions and the following disclaimer.
23+
* * Redistributions in binary form must reproduce the above
24+
* copyright notice, this list of conditions and the following
25+
* disclaimer in the documentation and/or other materials provided
26+
* with the distribution.
27+
* @par
28+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
29+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
30+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
31+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
32+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
33+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
34+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
35+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
36+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39+
* POSSIBILITY OF SUCH DAMAGE.
40+
*/
41+
42+
#ifndef TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H
43+
#define TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H
44+
45+
#include <tesseract_collision/core/discrete_contact_manager.h>
46+
#include <tesseract_collision/hpp_fcl/hpp_fcl_utils.h>
47+
48+
namespace tesseract_collision::tesseract_collision_hpp_fcl
49+
{
50+
/** @brief A HPP-FCL implementation of the discrete contact manager */
51+
class HPP_FCLDiscreteBVHManager : public DiscreteContactManager
52+
{
53+
public:
54+
using Ptr = std::shared_ptr<HPP_FCLDiscreteBVHManager>;
55+
using ConstPtr = std::shared_ptr<const HPP_FCLDiscreteBVHManager>;
56+
using UPtr = std::unique_ptr<HPP_FCLDiscreteBVHManager>;
57+
using ConstUPtr = std::unique_ptr<const HPP_FCLDiscreteBVHManager>;
58+
59+
HPP_FCLDiscreteBVHManager(std::string name = "HPP_FCLDiscreteBVHManager");
60+
~HPP_FCLDiscreteBVHManager() override = default;
61+
HPP_FCLDiscreteBVHManager(const HPP_FCLDiscreteBVHManager&) = delete;
62+
HPP_FCLDiscreteBVHManager& operator=(const HPP_FCLDiscreteBVHManager&) = delete;
63+
HPP_FCLDiscreteBVHManager(HPP_FCLDiscreteBVHManager&&) = delete;
64+
HPP_FCLDiscreteBVHManager& operator=(HPP_FCLDiscreteBVHManager&&) = delete;
65+
66+
std::string getName() const override final;
67+
68+
DiscreteContactManager::UPtr clone() const override final;
69+
70+
bool addCollisionObject(const std::string& name,
71+
const int& mask_id,
72+
const CollisionShapesConst& shapes,
73+
const tesseract_common::VectorIsometry3d& shape_poses,
74+
bool enabled = true) override final;
75+
76+
const CollisionShapesConst& getCollisionObjectGeometries(const std::string& name) const override final;
77+
78+
const tesseract_common::VectorIsometry3d&
79+
getCollisionObjectGeometriesTransforms(const std::string& name) const override final;
80+
81+
bool hasCollisionObject(const std::string& name) const override final;
82+
83+
bool removeCollisionObject(const std::string& name) override final;
84+
85+
bool enableCollisionObject(const std::string& name) override final;
86+
87+
bool disableCollisionObject(const std::string& name) override final;
88+
89+
bool isCollisionObjectEnabled(const std::string& name) const override final;
90+
91+
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override final;
92+
93+
void setCollisionObjectsTransform(const std::vector<std::string>& names,
94+
const tesseract_common::VectorIsometry3d& poses) override final;
95+
96+
void setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) override final;
97+
98+
const std::vector<std::string>& getCollisionObjects() const override final;
99+
100+
void setActiveCollisionObjects(const std::vector<std::string>& names) override final;
101+
102+
const std::vector<std::string>& getActiveCollisionObjects() const override final;
103+
104+
void setCollisionMarginData(
105+
CollisionMarginData collision_margin_data,
106+
CollisionMarginOverrideType override_type = CollisionMarginOverrideType::REPLACE) override final;
107+
108+
void setDefaultCollisionMarginData(double default_collision_margin) override final;
109+
110+
void setPairCollisionMarginData(const std::string& name1,
111+
const std::string& name2,
112+
double collision_margin) override final;
113+
114+
const CollisionMarginData& getCollisionMarginData() const override final;
115+
116+
void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
117+
118+
IsContactAllowedFn getIsContactAllowedFn() const override final;
119+
120+
void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
121+
122+
/**
123+
* @brief Add a fcl collision object to the manager
124+
* @param cow The tesseract fcl collision object
125+
*/
126+
void addCollisionObject(const COW::Ptr& cow);
127+
128+
private:
129+
std::string name_;
130+
131+
/** @brief Broad-phase Collision Manager for active collision objects */
132+
std::unique_ptr<hpp::fcl::BroadPhaseCollisionManager> static_manager_;
133+
134+
/** @brief Broad-phase Collision Manager for active collision objects */
135+
std::unique_ptr<hpp::fcl::BroadPhaseCollisionManager> dynamic_manager_;
136+
137+
Link2COW link2cow_; /**< @brief A map of all (static and active) collision objects being managed */
138+
std::vector<std::string> active_; /**< @brief A list of the active collision objects */
139+
std::vector<std::string> collision_objects_; /**< @brief A list of the collision objects */
140+
CollisionMarginData collision_margin_data_; /**< @brief The contact distance threshold */
141+
IsContactAllowedFn fn_; /**< @brief The is allowed collision function */
142+
std::size_t fcl_co_count_{ 0 }; /**< @brief The number fcl collision objects */
143+
144+
/** @brief This is used to store static collision objects to update */
145+
std::vector<CollisionObjectRawPtr> static_update_;
146+
147+
/** @brief This is used to store dynamic collision objects to update */
148+
std::vector<CollisionObjectRawPtr> dynamic_update_;
149+
150+
/** @brief This function will update internal data when margin data has changed */
151+
void onCollisionMarginDataChanged();
152+
};
153+
154+
} // namespace tesseract_collision::tesseract_collision_hpp_fcl
155+
#endif // TESSERACT_COLLISION_HPP_FCL_DISCRETE_MANAGERS_H

‎tesseract_collision/hpp_fcl/include/tesseract_collision/hpp_fcl/hpp_fcl_factories.h‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
/**
2+
* @file hpp_fcl_factories.h
3+
* @brief Factories for loading hpp-fcl contact managers as plugins
4+
*
5+
* @author Levi Armstrong
6+
* @date October 25, 2021
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2021, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H
27+
#define TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H
28+
29+
#include <tesseract_collision/core/contact_managers_plugin_factory.h>
30+
31+
namespace tesseract_collision::tesseract_collision_hpp_fcl
32+
{
33+
class HPP_FCLDiscreteBVHManagerFactory : public DiscreteContactManagerFactory
34+
{
35+
public:
36+
DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final;
37+
};
38+
39+
TESSERACT_PLUGIN_ANCHOR_DECL(HPP_FCLFactoriesAnchor)
40+
41+
} // namespace tesseract_collision::tesseract_collision_hpp_fcl
42+
#endif // TESSERACT_COLLISION_HPP_FCL_FCL_FACTORIES_H

0 commit comments

Comments
 (0)