diff --git a/.travis.yml b/.travis.yml index f37cd1bfb5..e5e5af6364 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,7 +24,6 @@ env: matrix: - TEST="clang-format catkin_lint" - ROS_DISTRO=melodic - - ROS_DISTRO=kinetic matrix: # Add a separate config to the matrix, using clang as compiler include: diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 5befb7950c..8f2c95937b 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -13,8 +13,6 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") endif() -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") set(CMAKE_BUILD_TYPE Release) @@ -22,6 +20,8 @@ endif() find_package(Boost REQUIRED system filesystem date_time thread iostreams) find_package(Eigen3 REQUIRED) + # TODO: Move collision detection into separate packages +find_package(Bullet REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL_PC REQUIRED fcl) @@ -144,6 +144,7 @@ catkin_package( OCTOMAP urdfdom urdfdom_headers + BULLET ) @@ -158,6 +159,7 @@ endif() include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} + ${BULLET_INCLUDE_DIRS} ) #catkin_lint: ignore_once external_directory (${VERSION_FILE_PATH}) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 926ca0282a..123c6cc8ff 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -95,6 +95,15 @@ struct Contact /** \brief The type of the second body involved in the contact */ BodyType body_type_2; + + /** \brief The distance percentage between casted poses until collision. + * + * If the value is 0, then the collision occured in the start pose. If the value is 1, then the collision occured in + * the end pose. */ + double percent_interpolation; + + /** \brief The two nearest points connecting the two bodies */ + Eigen::Vector3d nearest_points[2]; }; /** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h index c3ae2f9c9f..331c5a154e 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h @@ -469,7 +469,8 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) ros::WallTime start = ros::WallTime::now(); this->cworld_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); - EXPECT_GE(1.0, t); + // TODO: investigate why bullet collision checking is considerably slower here + EXPECT_GE(5.0, t); // this is not really a failure; it is just that slow; // looking into doing collision checking with a voxel grid. ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h new file mode 100644 index 0000000000..a57733f3dd --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -0,0 +1,258 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(robot_state::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +template +class CollisionDetectorPandaTest : public testing::Test +{ +public: + std::shared_ptr value_; + +protected: + void SetUp() override + { + value_.reset(new CollisionAllocatorType); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); + + acm_->setEntry("panda_link0", "panda_link1", true); + acm_->setEntry("panda_link1", "panda_link2", true); + acm_->setEntry("panda_link2", "panda_link3", true); + acm_->setEntry("panda_link3", "panda_link4", true); + acm_->setEntry("panda_link4", "panda_link5", true); + acm_->setEntry("panda_link5", "panda_link6", true); + acm_->setEntry("panda_link6", "panda_link7", true); + acm_->setEntry("panda_link7", "panda_hand", true); + acm_->setEntry("panda_hand", "panda_rightfinger", true); + acm_->setEntry("panda_hand", "panda_leftfinger", true); + acm_->setEntry("panda_rightfinger", "panda_leftfinger", true); + acm_->setEntry("panda_link5", "panda_link7", true); + acm_->setEntry("panda_link6", "panda_hand", true); + + crobot_ = value_->allocateRobot(robot_model_); + cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); + + robot_state_.reset(new robot_state::RobotState(robot_model_)); + setToHome(*robot_state_); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionRobotPtr crobot_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + robot_state::RobotStatePtr robot_state_; +}; + +TYPED_TEST_CASE_P(CollisionDetectorPandaTest); + +/** \brief Correct setup testing. */ +TYPED_TEST_P(CollisionDetectorPandaTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) +{ + // Sets the joint values to zero which is a colliding configuration + this->robot_state_->setToDefaultValues(); + this->robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); +} + +// TODO: Add collision check capability within world itself and then enable test +/** \brief Two boxes in collision in the world environment. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DISABLED_WorldToWorldCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.5, .5, .5); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos_1 = Eigen::Isometry3d::Identity(); + pos_1.translation().x() = 1; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos_1); + + Eigen::Isometry3d pos_2 = Eigen::Isometry3d::Identity(); + pos_2.translation().x() = 1.2; + this->cworld_->getWorld()->addToObject("box_2", shape_ptr, pos_2); + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); + + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cworld_->getWorld()->moveObject("box", pos1); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cworld_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.max_contacts = 10; + req.contacts = true; + req.verbose = true; + + shapes::Shape* shape = new shapes::Box(.4, .4, .4); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + ASSERT_GE(res.contact_count, 3u); + res.clear(); +} + +/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */ +TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos); + + this->crobot_->setLinkPadding("panda_hand", 0.08); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->crobot_->setLinkPadding("panda_hand", 0.0); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, + DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest); diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 3e87ca6c78..6e1ba5526f 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -1,29 +1,39 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_bullet) add_library(${MOVEIT_LIB_NAME} - src/collision_common.cpp - src/collision_robot_bt.cpp - src/collision_world_bt.cpp + src/collision_robot_bullet.cpp + src/collision_world_bullet.cpp + src/bullet_integration/bullet_utils.cpp + src/bullet_integration/bullet_discrete_bvh_manager.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${LIBFCL_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection + ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} + ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES} + ${BULLET_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -add_library(collision_detector_bt_plugin src/collision_detector_bt_plugin_loader.cpp) -set_target_properties(collision_detector_bt_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(collision_detector_bt_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) +add_library(collision_detector_bullet_plugin src/collision_detector_bullet_plugin_loader.cpp) +set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(collision_detector_bullet_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) -install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bt_plugin +install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -install(FILES ../collision_detector_bt_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES ../collision_detector_bullet_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) + + catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection.cpp) + target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + + catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) + target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_bullet/README.md b/moveit_core/collision_detection_bullet/README.md index c0b85486c0..b808244650 100644 --- a/moveit_core/collision_detection_bullet/README.md +++ b/moveit_core/collision_detection_bullet/README.md @@ -2,5 +2,5 @@ To use Bullet as a collision checker you can: a) create a new planning scene and manually switch to Bullet using the planning_scene.setActiveCollisionDetector() b) manually switch to Bullet using the `CollisionPluginLoader` -c) manually instantiate a `CollisionRobotBt` and `CollisionWorldBt` +c) manually instantiate a `CollisionRobotBullet` and `CollisionWorldBullet` d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group` diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h new file mode 100644 index 0000000000..385ed46f23 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * 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 + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +template +using AlignedVector = std::vector>; + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +/** @brief Checks if contact is allowed or not. Returns true if yes, otherwise false. + * + * The order of strings should not matter, the function should handled by the function. */ +typedef std::function + IsContactAllowedFn; + +enum class CollisionObjectType +{ + USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ + + // all of the following convert the meshes to custom collision objects + CONVEX_HULL = 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex + it will be converted) */ + MULTI_SPHERE = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ + SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ +}; + +/** \brief Bundles the data for a collision query */ +struct ContactTestData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ContactTestData(const std::vector& active, const double& contact_distance, const IsContactAllowedFn& fn, + collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm) + : active(active) + , contact_distance(contact_distance) + , fn(fn) + , acm(acm) + , res(res) + , req(req) + , done(false) + , pair_done(false) + { + } + + const std::vector& active; + + /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ + const double& contact_distance; + + /** \brief User defined function which checks if contact is allowed between two objects */ + const IsContactAllowedFn& fn; + + /** \brief Indicates collision objects which are allowed to be in contact */ + const collision_detection::AllowedCollisionMatrix* acm; + + collision_detection::CollisionResult& res; + const collision_detection::CollisionRequest& req; + + /// Indicates if search is finished + bool done; + + /// Indicates if search between a single pair is finished + bool pair_done; +}; + +} // namespace collision_detection_bullet + +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h new file mode 100644 index 0000000000..8067a160de --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -0,0 +1,191 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementaiton of a discrete bullet manager */ +class BulletDiscreteBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletDiscreteBVHManager(); + + ~BulletDiscreteBVHManager(); + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletDiscreteBVHManagerPtr clone() const; + + /**@brief Add a object to the checker + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @return true if successfully added, otherwise false. */ + bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, bool enabled = true); + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set a single collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const btTransform& pose); + + /**@brief Set a series of collision objects' tranforms + * @param names The names of the object + * @param poses The tranformations in world */ + void setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses); + + /**@brief Set a series of collision objects' tranforms + * @param transforms A transform map */ + void setCollisionObjectsTransform(const AlignedMap& transforms); + + /**@brief Set which collision objects can move + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects can move + * @return A list of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered. + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /** @brief Set the active function for determining if two links are allowed to be in collision */ + void setIsContactAllowedFn(IsContactAllowedFn fn); + + /** @brief Get the active function for determining if two links are allowed to be in collision */ + IsContactAllowedFn getIsContactAllowedFn() const; + + /**@brief Perform a contact test for all objects in the manager + * @param collisions The Contact results data + * @param acm The allowed collision matrix + * @param req The contact request */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm); + + /**@brief Perform a contact test for all objects in the manager and the external ones provided + * @param collisions The Contact results data + * @param req The contact request + * @param acm The allowed collision matrix + * @param req The contact request + * @param cows_external Objects to check which are not part of the manager */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, + const std::vector cows_external); + + /**@brief Add a bullet collision object to the manager + * @param cow The bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow); + + /**@brief Return collision objects + * @return A map of collision objects */ + const std::map& getCollisionObjects() const; + +private: + /** @brief A list of the active collision objects */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The is allowed collision function */ + IsContactAllowedFn fn_; + + /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** @brief A map of all (static and active) collision objects being managed */ + std::map link2cow_; + + /**@brief Perform a contact test for the provided object which is not part of the manager + * @param cow The Collision object + * @param collisions The collision results */ + void contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions); +}; +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h new file mode 100644 index 0000000000..d6462ebefe --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -0,0 +1,803 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * + * 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. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +#define METERS + +const btScalar BULLET_MARGIN = 0.0f; +const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS; +const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS; +const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit +const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported +const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; + +MOVEIT_CLASS_FORWARD(CollisionObjectWrapper) + +/** \brief Converts eigen vector to bullet vector */ +inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) +{ + return btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); +} + +/** \brief Converts bullet vector to eigen vector */ +inline Eigen::Vector3d convertBtToEigen(const btVector3& v) +{ + return Eigen::Vector3d(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); +} + +/** \brief Converts eigen quaternion to bullet quaternion */ +inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q) +{ + return btQuaternion(static_cast(q.x()), static_cast(q.y()), static_cast(q.z()), + static_cast(q.w())); +} + +/** \brief Converts eigen matrix to bullet matrix */ +inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r) +{ + return btMatrix3x3(static_cast(r(0, 0)), static_cast(r(0, 1)), static_cast(r(0, 2)), + static_cast(r(1, 0)), static_cast(r(1, 1)), static_cast(r(1, 2)), + static_cast(r(2, 0)), static_cast(r(2, 1)), static_cast(r(2, 2))); +} + +/** \brief Converts bullet transform to eigen transform */ +inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) +{ + const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0); + const Eigen::Vector3d& tran = t.translation(); + + btMatrix3x3 mat = convertEigenToBt(rot); + btVector3 translation = convertEigenToBt(tran); + + return btTransform(mat, translation); +} + +/** @brief Tesseract bullet collision object. + * + * A wrapper around bullet's collision object which contains specific information related to bullet */ +class CollisionObjectWrapper : public btCollisionObject +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Standard constructor */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types); + + /** \brief Constructor for attached robot objects */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links); + + /** \brief Bitfield specifies to which group the object belongs */ + short int m_collision_filter_group; + + /** \brief Bitfield specifies against which other groups the object is collision checked */ + short int m_collision_filter_mask; + + /** \brief Indicates if the collision object is used for a collision check */ + bool m_enabled; + + /** \brief The robot links the collision objects is allowed to touch */ + std::set m_touch_links; + + /** @brief Get the collision object name */ + const std::string& getName() const + { + return m_name; + } + + /** @brief Get a user defined type */ + const collision_detection::BodyType& getTypeID() const + { + return m_type_id; + } + + /** \brief Check if two CollisionObjectWrapper objects point to the same source object + * \return True if same objects, false otherwise */ + bool sameObject(const CollisionObjectWrapper& other) const + { + return m_name == other.m_name && m_type_id == other.m_type_id && m_shapes.size() == other.m_shapes.size() && + m_shape_poses.size() == other.m_shape_poses.size() && + std::equal(m_shapes.begin(), m_shapes.end(), other.m_shapes.begin()) && + std::equal(m_shape_poses.begin(), m_shape_poses.end(), other.m_shape_poses.begin(), + [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); + } + + /** @brief Get the collision objects axis aligned bounding box + * @param aabb_min The minimum point + * @param aabb_max The maximum point */ + void getAABB(btVector3& aabb_min, btVector3& aabb_max) const + { + getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); + const btScalar& d = getContactProcessingThreshold(); + btVector3 contact_threshold(d, d, d); + aabb_min -= contact_threshold; + aabb_max += contact_threshold; + } + + /** @brief Clones the collision objects but not the collision shape wich is const. + * @return Shared Pointer to the cloned collision object */ + std::shared_ptr clone() + { + std::shared_ptr clone_cow( + new CollisionObjectWrapper(m_name, m_type_id, m_shapes, m_shape_poses, m_collision_object_types, m_data)); + clone_cow->setCollisionShape(getCollisionShape()); + clone_cow->setWorldTransform(getWorldTransform()); + clone_cow->m_collision_filter_group = m_collision_filter_group; + clone_cow->m_collision_filter_mask = m_collision_filter_mask; + clone_cow->m_enabled = m_enabled; + clone_cow->setBroadphaseHandle(nullptr); + clone_cow->m_touch_links = m_touch_links; + clone_cow->setContactProcessingThreshold(this->getContactProcessingThreshold()); + return clone_cow; + } + + /** \brief Manage memory of a raw pointer shape */ + template + void manage(T* t) + { + m_data.push_back(std::shared_ptr(t)); + } + + /** \brief Manage memory of a shared pointer shape */ + template + void manage(std::shared_ptr t) + { + m_data.push_back(t); + } + +protected: + /** @brief Special constructor used by the clone method */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data); + + std::string m_name; + collision_detection::BodyType m_type_id; + + /** @brief The shapes that define the collison object */ + std::vector m_shapes; + + /** @brief The poses of the shapes */ + AlignedVector m_shape_poses; + + /** @brief The shape collision object type to be used */ + std::vector m_collision_object_types; + + /** @brief Manages the collision shape pointer so they get destroyed */ + std::vector> m_data; +}; + +/** \brief Computes the local supporting vertex of a convex shape. + * + * If multiple vertices with equal support products exists, their average is calculated and returned. + * + * \param shape The convex shape to check + * \param localNormal The support direction to search for in shape local coordinates + * \param outsupport The value of the calculated support mapping + * \param outpt The computed support point */ +inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport, + btVector3& outpt) +{ + btVector3 pt_sum(0, 0, 0); + float pt_count = 0; + float max_support = -1000; + + const btPolyhedralConvexShape* pshape = dynamic_cast(shape); + if (pshape) + { + int n_pts = pshape->getNumVertices(); + + for (int i = 0; i < n_pts; ++i) + { + btVector3 pt; + pshape->getVertex(i, pt); + + float sup = pt.dot(localNormal); + if (sup > max_support + BULLET_EPSILON) + { + pt_count = 1; + pt_sum = pt; + max_support = sup; + } + else if (sup < max_support - BULLET_EPSILON) + { + } + else + { + pt_count += 1; + pt_sum += pt; + } + } + outsupport = max_support; + outpt = pt_sum / pt_count; + } + else + { + outpt = shape->localGetSupportingVertexWithoutMargin(localNormal); + outsupport = localNormal.dot(outpt); + } +} + +/** @brief Check if a collision check is required between the provided collision objects + * @param cow1 The first collision object + * @param cow2 The second collision object + * @param acm The contact allowed function pointer + * @param verbose Indicate if verbose information should be printed + * @return True if the two collision objects should be checked for collision, otherwise false */ +inline bool needsCollisionCheck(const CollisionObjectWrapper& cow1, const CollisionObjectWrapper& cow2, + const IsContactAllowedFn& allowed_fn, + const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) +{ + if (!cow1.m_enabled) + return false; + + if (!cow2.m_enabled) + return false; + + if (!((cow2.m_collision_filter_group & cow1.m_collision_filter_mask) && + (cow1.m_collision_filter_group & cow2.m_collision_filter_mask))) + return false; + + if (isContactAllowed(cow1.getName(), cow2.getName(), allowed_fn, acm, verbose)) + return false; + + if (cow1.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow2.getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow1.m_touch_links.find(cow2.getName()) != cow1.m_touch_links.end()) + return false; + + if (cow2.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1.getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow2.m_touch_links.find(cow1.getName()) == cow2.m_touch_links.end()) + return false; + + // TODO: Add check for two objects attached to the same link + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Checking collision btw " << cow1.getName() << " vs " + << cow2.getName()); + + return true; +} + +/** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ +inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, + const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + contact.nearest_points[0] = contact.pos; + contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd0->getTypeID(); + + if (!processResult(collisions, contact, pc, found)) + { + return 0; + } + + return 1; +} + +/** \brief Processes a contact point */ +struct TesseractBridgedManifoldResult : public btManifoldResult +{ + btCollisionWorld::ContactResultCallback& m_resultCallback; + + TesseractBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, + btCollisionWorld::ContactResultCallback& resultCallback) + : btManifoldResult(obj0Wrap, obj1Wrap), m_resultCallback(resultCallback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief Abstract interface for both discrete and continuous reporting of contact points */ +struct BroadphaseContactResultCallback +{ + ContactTestData& collisions_; + double contact_distance_; + bool verbose_; + + BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) + : collisions_(collisions), contact_distance_(contact_distance), verbose_(verbose) + { + } + + virtual ~BroadphaseContactResultCallback() = default; + + virtual bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const + { + return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.fn, collisions_.acm, verbose_); + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, + int index1) = 0; +}; + +/** \brief addSingleResult of this struct is called each time the broadphase check indicates a collision. */ +struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultCallback +{ + DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) + : BroadphaseContactResultCallback(collisions, contact_distance, verbose) + { + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int /*index1*/) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } +}; + +/** \brief Processes a contact point */ +struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult +{ + BroadphaseContactResultCallback& result_callback_; + + TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, + const btCollisionObjectWrapper* obj1Wrap, + BroadphaseContactResultCallback& result_callback) + : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + if (result_callback_.collisions_.done || result_callback_.collisions_.pair_done || + depth > static_cast(result_callback_.contact_distance_)) + { + return; + } + + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief Check a collision object not in the broadphase to the broadphase which may eventually be exposed. + * + * This is copied directly out of BulletWorld */ +struct TesseractSingleContactCallback : public btBroadphaseAabbCallback +{ + btCollisionObject* m_collisionObject; /**< @brief The bullet collision object */ + btCollisionDispatcher* m_dispatcher; /**< @brief The bullet collision dispatcher used for getting object to object + collison algorithm */ + const btDispatcherInfo& m_dispatch_info; /**< @brief The bullet collision dispatcher configuration information */ + btCollisionWorld::ContactResultCallback& m_resultCallback; + + TesseractSingleContactCallback(btCollisionObject* collisionObject, btCollisionDispatcher* dispatcher, + const btDispatcherInfo& dispatch_info, + btCollisionWorld::ContactResultCallback& resultCallback) + : m_collisionObject(collisionObject) + , m_dispatcher(dispatcher) + , m_dispatch_info(dispatch_info) + , m_resultCallback(resultCallback) + { + } + + bool process(const btBroadphaseProxy* proxy) override + { + btCollisionObject* collision_object = static_cast(proxy->m_clientObject); + if (collision_object == m_collisionObject) + return true; + + if (m_resultCallback.needsCollision(collision_object->getBroadphaseHandle())) + { + btCollisionObjectWrapper ob0(nullptr, m_collisionObject->getCollisionShape(), m_collisionObject, + m_collisionObject->getWorldTransform(), -1, -1); + btCollisionObjectWrapper ob1(nullptr, collision_object->getCollisionShape(), collision_object, + collision_object->getWorldTransform(), -1, -1); + + btCollisionAlgorithm* algorithm = m_dispatcher->findAlgorithm(&ob0, &ob1, nullptr, BT_CLOSEST_POINT_ALGORITHMS); + if (algorithm) + { + TesseractBridgedManifoldResult contact_point_result(&ob0, &ob1, m_resultCallback); + contact_point_result.m_closestPointDistanceThreshold = m_resultCallback.m_closestDistanceThreshold; + + // discrete collision detection query + algorithm->processCollision(&ob0, &ob1, m_dispatch_info, &contact_point_result); + + algorithm->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algorithm); + } + } + return true; + } +}; + +/** @brief A callback function that is called as part of the broadphase collision checking. + * + * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for + * collision/distance and the results are stored in collision_. */ +class TesseractCollisionPairCallback : public btOverlapCallback +{ + const btDispatcherInfo& dispatch_info_; + btCollisionDispatcher* dispatcher_; + BroadphaseContactResultCallback& results_callback_; + +public: + TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher, + BroadphaseContactResultCallback& results_callback) + : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback) + { + } + + ~TesseractCollisionPairCallback() override = default; + + bool processOverlap(btBroadphasePair& pair) override + { + results_callback_.collisions_.pair_done = false; + + if (results_callback_.collisions_.done) + { + return false; + } + + const CollisionObjectWrapper* cow0 = static_cast(pair.m_pProxy0->m_clientObject); + const CollisionObjectWrapper* cow1 = static_cast(pair.m_pProxy1->m_clientObject); + + std::pair pair_names{ cow0->getName(), cow1->getName() }; + ROS_DEBUG_STREAM("Processing " << cow0->getName() << " vs " << cow1->getName()); + + if (results_callback_.needsCollision(cow0, cow1)) + { + btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); + + // dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithm) + { + pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS); + } + + if (pair.m_algorithm) + { + TesseractBroadphaseBridgedManifoldResult contact_point_result(&obj0_wrap, &obj1_wrap, results_callback_); + contact_point_result.m_closestPointDistanceThreshold = + static_cast(results_callback_.contact_distance_); + + // discrete collision detection query + pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result); + } + } + return false; + } +}; + +/** \brief Casts a geometric shape into a btCollisionShape */ +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow); + +/** @brief Update a collision objects filters + * @param active A list of active collision objects + * @param cow The collision object to update. + * @param continuous Indicate if the object is a continuous collision object. + * + * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous + * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision + * checking. */ +inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow, + bool continuous) +{ + cow.m_collision_filter_group = btBroadphaseProxy::KinematicFilter; + + if (!isLinkActive(active, cow.getName())) + { + cow.m_collision_filter_group = btBroadphaseProxy::StaticFilter; + } + + if (cow.m_collision_filter_group == btBroadphaseProxy::StaticFilter) + { + cow.m_collision_filter_mask = btBroadphaseProxy::KinematicFilter; + } + else + { + (continuous) ? (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter) : + (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter); + } + + if (cow.getBroadphaseHandle()) + { + cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collision_filter_group; + cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collision_filter_mask; + } +} + +/** \brief Wrapper around constructing a CollisionObjectWrapper */ +inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, + const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool enabled = true) +{ + // dont add object that does not have geometry + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); + return nullptr; + } + + CollisionObjectWrapperPtr new_cow( + new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types)); + + new_cow->m_enabled = enabled; + new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); + + ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); + return new_cow; +} + +// TODO: Unify with other createCollisionObject functions +/** \brief Wrapper around constructing a CollisionObjectWrapper for attached objects */ +inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, + const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links, bool enabled = true) +{ + // dont add object that does not have geometry + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); + return nullptr; + } + + CollisionObjectWrapperPtr new_cow( + new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, touch_links)); + + new_cow->m_enabled = enabled; + new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); + + ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); + return new_cow; +} + +/** \brief Processes a contact after positive broadphase check */ +struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallback +{ + ContactTestData& collisions_; + const CollisionObjectWrapperPtr cow_; + double contact_distance_; + bool verbose_; + + DiscreteCollisionCollector(ContactTestData& collisions, const CollisionObjectWrapperPtr& cow, double contact_distance, + bool verbose = false) + : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) + { + m_closestDistanceThreshold = static_cast(contact_distance); + m_collisionFilterGroup = cow->m_collision_filter_group; + m_collisionFilterMask = cow->m_collision_filter_mask; + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int /*index1*/) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } + + bool needsCollision(btBroadphaseProxy* proxy0) const override + { + return !collisions_.done && + needsCollisionCheck(*cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, + collisions_.acm, verbose_); + } +}; + +/** @brief Update the Broadphase AABB for the input collision object + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + // Update the broadphase aabb + assert(cow->getBroadphaseHandle() != nullptr); + broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); +} + +/** @brief Remove the collision object from broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btBroadphaseProxy* bp = cow->getBroadphaseHandle(); + if (bp) + { + // only clear the cached algorithms + broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get()); + broadphase->destroyProxy(bp, dispatcher.get()); + cow->setBroadphaseHandle(nullptr); + } +} + +/** @brief Add the collision object to broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Added " << cow->getName() << " to broadphase"); + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + // Add the active collision object to the broadphase + int type = cow->getCollisionShape()->getShapeType(); + cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collision_filter_group, + cow->m_collision_filter_mask, dispatcher.get())); +} +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h new file mode 100644 index 0000000000..b4929e6dfb --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -0,0 +1,271 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * 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 + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace collision_detection_bullet +{ +/** + * @brief Get a key for two object to search the collision matrix + * @param obj1 First collision object name + * @param obj2 Second collision object name + * @return The collision pair key + */ +inline std::pair getObjectPairKey(const std::string& obj1, const std::string& obj2) +{ + return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); +} + +/** + * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. + * @param active List of active link names + * @param name The name of link to check if it is active. + */ +inline bool isLinkActive(const std::vector& active, const std::string& name) +{ + return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); +} + +/** + * @brief Determine if contact is allowed between two objects. + * @param name1 The name of the first object + * @param name2 The name of the second object + * @param acm The contact allowed function + * @param verbose If true print debug informaton + * @return True if contact is allowed between the two object, otherwise false. + */ +inline bool isContactAllowed(const std::string& name1, const std::string& name2, const IsContactAllowedFn& acm_fn, + const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) +{ + // do not distance check geoms part of the same object / link / attached body + if (name1 == name2) + return true; + + if (acm_fn != nullptr && acm_fn(name1, name2, acm)) + { + if (verbose) + { + ROS_DEBUG_NAMED("collision_detection.bullet", + "Collision between '%s' and '%s' is allowed. No contacts are computed.", name1.c_str(), + name2.c_str()); + } + return true; + } + + if (verbose) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "Actually checking collisions between %s and %s", name1.c_str(), + name2.c_str()); + } + + return false; +} + +/** \brief Stores a single contact result in the requested way. +* \param found Indicates if a contact for this pair of objects has already been found +* \return Pointer to the newly inserted contact */ +inline collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, + const std::pair& key, bool found) +{ + // add deepest penetration / smallest distance to result + if (cdata.req.distance) + { + if (contact.depth < cdata.res.distance) + { + cdata.res.distance = contact.depth; + } + } + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Contact btw " << key.first << " and " << key.second + << " dist: " << contact.depth); + if (!found) + { + cdata.res.collision = true; + std::vector data; + if (!cdata.req.contacts) + { + cdata.done = true; + return nullptr; + } + else + { + data.reserve(cdata.req.max_contacts_per_pair); + data.emplace_back(contact); + cdata.res.contact_count++; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + cdata.done = true; + } + + if (cdata.req.max_contacts_per_pair == 1u) + { + cdata.pair_done = true; + } + + return &(cdata.res.contacts.insert(std::make_pair(key, data)).first->second.back()); + } + else + { + std::vector& dr = cdata.res.contacts[key]; + dr.emplace_back(contact); + cdata.res.contact_count++; + + if (dr.size() >= cdata.req.max_contacts_per_pair) + { + cdata.pair_done = true; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + cdata.done = true; + } + + return &(dr.back()); + } + + return nullptr; +} + +/** + * @brief Create a convex hull from vertices using Bullet Convex Hull Computer + * @param (Output) vertices A vector of vertices + * @param (Output) faces The first values indicates the number of vertices that define the face followed by the vertice + * index + * @param (input) input A vector of point to create a convex hull from + * @param (input) shrink If positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length + * units towards the center along its normal). + * @param (input) shrinkClamp If positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where + * "innerRadius" is the minimum distance of a face to the center of the convex hull. + * @return The number of faces. If less than zero an error occured when trying to create the convex hull + */ +inline int createConvexHull(AlignedVector& vertices, std::vector& faces, + const AlignedVector& input, double shrink = -1, double shrinkClamp = -1) +{ + vertices.clear(); + faces.clear(); + + btConvexHullComputer conv; + btAlignedObjectArray points; + points.reserve(static_cast(input.size())); + for (const Eigen::Vector3d& v : input) + { + points.push_back(btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + } + + btScalar val = conv.compute(&points[0].getX(), sizeof(btVector3), points.size(), static_cast(shrink), + static_cast(shrinkClamp)); + if (val < 0) + { + ROS_ERROR("Failed to create convex hull"); + return -1; + } + + int num_verts = conv.vertices.size(); + vertices.reserve(static_cast(num_verts)); + for (int i = 0; i < num_verts; i++) + { + btVector3& v = conv.vertices[i]; + vertices.push_back(Eigen::Vector3d(v.getX(), v.getY(), v.getZ())); + } + + int num_faces = conv.faces.size(); + faces.reserve(static_cast(3 * num_faces)); + for (int i = 0; i < num_faces; i++) + { + std::vector face; + face.reserve(3); + + const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]); + int a = source_edge->getSourceVertex(); + face.push_back(a); + + int b = source_edge->getTargetVertex(); + face.push_back(b); + + const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace(); + int c = edge->getTargetVertex(); + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + while (c != a) + { + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + } + faces.push_back(static_cast(face.size())); + faces.insert(faces.end(), face.begin(), face.end()); + } + + return num_faces; +} + +inline bool allowedCollisionCheck(const std::string& body_1, const std::string& body_2, + const collision_detection::AllowedCollisionMatrix* acm) +{ + collision_detection::AllowedCollision::Type allowed_type; + + if (acm != nullptr) + { + if (acm->getEntry(body_1, body_2, allowed_type)) + { + if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed " + << body_1 << " and " << body_2); + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " + << body_2); + return false; + } +} +} // namespace collision_detection_bullet + +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h new file mode 100644 index 0000000000..c192b86df9 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -0,0 +1,109 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2018, Southwest Research Institute + * All rights reserved. + * + * 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 + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ + +#include +#include +#include +#include +#include + +#include + +namespace collision_detection_bullet +{ +/** \brief Recursively traverses robot from root to get all active links +* +* \param active_links Stores the active links +* \param urdf_link The current urdf link representation +* \param active Indicates if link is considered active */ +static inline void getActiveLinkNamesRecursive(std::vector& active_links, + const urdf::LinkConstSharedPtr& urdf_link, bool active) +{ + if (active) + { + active_links.push_back(urdf_link->name); + for (const auto& child_link : urdf_link->child_links) + { + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } + else + { + for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i) + { + const urdf::LinkConstSharedPtr child_link = urdf_link->child_links[i]; + if ((child_link->parent_joint) && (child_link->parent_joint->type != urdf::Joint::FIXED)) + getActiveLinkNamesRecursive(active_links, child_link, true); + else + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } +} + +inline shapes::ShapePtr constructShape(const urdf::Geometry* geom) +{ + shapes::Shape* result = nullptr; + switch (geom->type) + { + case urdf::Geometry::SPHERE: + result = new shapes::Sphere(static_cast(geom)->radius); + break; + case urdf::Geometry::BOX: + { + urdf::Vector3 dim = static_cast(geom)->dim; + result = new shapes::Box(dim.x, dim.y, dim.z); + } + break; + case urdf::Geometry::CYLINDER: + result = new shapes::Cylinder(static_cast(geom)->radius, + static_cast(geom)->length); + break; + case urdf::Geometry::MESH: + { + const urdf::Mesh* mesh = static_cast(geom); + if (!mesh->filename.empty()) + { + Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); + shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale); + result = m; + } + } + break; + default: + ROS_ERROR("Unknown geometry type: %d", static_cast(geom->type)); + break; + } + + return shapes::ShapePtr(result); +} + +inline Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) +{ + Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z); + Eigen::Isometry3d result; + result.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + result.linear() = q.toRotationMatrix(); + return result; +} +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h similarity index 76% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h index b914e8c2a5..513bea0e16 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -34,22 +34,23 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ +#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ #include -#include -#include +#include +#include namespace collision_detection { /** \brief An allocator for Bullet collision detectors */ -class CollisionDetectorAllocatorBt - : public CollisionDetectorAllocatorTemplate +class CollisionDetectorAllocatorBullet + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_bt.cpp + static const std::string NAME; // defined in collision_world_bullet.cpp }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h similarity index 84% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h index d60a29ce64..76182e582c 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -34,18 +34,18 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ #include -#include +#include namespace collision_detection { class CollisionDetectorBtPluginLoader : public CollisionPlugin { public: - virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; + bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; } -#endif // MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h similarity index 69% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h index a0aadf4b4a..de04d45100 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h @@ -34,21 +34,22 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ -#include +#include +#include namespace collision_detection { -class CollisionRobotBt : public CollisionRobot +class CollisionRobotBullet : public CollisionRobot { - friend class CollisionWorldBt; + friend class CollisionWorldBullet; public: - CollisionRobotBt(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionRobotBullet(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionRobotBt(const CollisionRobotBt& other); + CollisionRobotBullet(const CollisionRobotBullet& other); void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state) const override; @@ -81,14 +82,38 @@ class CollisionRobotBt : public CollisionRobot const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; protected: + /** \brief Updates the poses of the objects in the manager according to given robot state */ + void updateTransformsFromState(const robot_state::RobotState& state, + const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; + + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ void updatedPaddingOrScaling(const std::vector& links) override; + /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ + void addAttachedOjects(const robot_state::RobotState& state, + std::vector& cows) const; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different continuous checkSelfCollision functions into a single function */ + void checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkOtherCollision functions into a single function */ void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const CollisionRobot& other_robot, const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construts a bullet collision object out of a robot link */ + void addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link); + + /** \brief Handles all self collision checks */ + collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h similarity index 66% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h index fb614b3c18..ea8473d3dc 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h @@ -34,20 +34,26 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ -#include +#include +#include +#include +#include namespace collision_detection { -class CollisionWorldBt : public CollisionWorld +class CollisionWorldBullet : public CollisionWorld { public: - CollisionWorldBt(); - explicit CollisionWorldBt(const WorldPtr& world); - CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world); - ~CollisionWorldBt() override; + CollisionWorldBullet(); + + explicit CollisionWorldBullet(const WorldPtr& world); + + CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world); + + ~CollisionWorldBullet() override; void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, const robot_state::RobotState& state) const override; @@ -71,16 +77,38 @@ class CollisionWorldBt : public CollisionWorld void setWorld(const WorldPtr& world) override; protected: + /** \brief Bundles the different checkWorldCollision functions into a single function */ void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkRobotCollision functions into a single function */ void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + /** \brief Bundles the different continuous checkRobotCollision functions into a single function */ + void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + /** \brief Adds a world object to the collision managers */ + void addToManager(const World::Object* obj); + + /** \brief Updates a managed collision object with its world representation. + * + * We have three cases: 1) the object is part of the manager and not of world --> delete it + * 2) the object is not in the manager, therefore register to manager, + * 3) the object is in the manager then delete and add the modified */ + void updateManagedObject(const std::string& id); + + /** \brief Handles all discrete collision checks */ + collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; + private: - void initialize(); + /** \brief Callback function executed for each change to the world environment */ void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + World::ObserverHandle observer_handle_; }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp new file mode 100644 index 0000000000..b55b3a7c02 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -0,0 +1,306 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#include + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h" + +namespace collision_detection_bullet +{ +BulletDiscreteBVHManager::BulletDiscreteBVHManager() +{ + dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); + + dispatcher_->registerCollisionCreateFunc( + BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); + + dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & + ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); + + broadphase_.reset(new btDbvtBroadphase()); + + contact_distance_ = 0; +} + +BulletDiscreteBVHManager::~BulletDiscreteBVHManager() +{ + // clean up remaining objects + for (std::pair& cow : link2cow_) + { + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); + } +} + +BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const +{ + BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager()); + + for (const std::pair& cow : link2cow_) + { + CollisionObjectWrapperPtr new_cow = cow.second->clone(); + + assert(new_cow->getCollisionShape()); + assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + new_cow->setWorldTransform(cow.second->getWorldTransform()); + new_cow->setContactProcessingThreshold(static_cast(contact_distance_)); + manager->addCollisionObject(new_cow); + } + + manager->setActiveCollisionObjects(active_); + manager->setContactDistanceThreshold(contact_distance_); + manager->setIsContactAllowedFn(fn_); + + return manager; +} + +bool BulletDiscreteBVHManager::addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool enabled) +{ + CollisionObjectWrapperPtr new_cow = + createCollisionObject(name, mask_id, shapes, shape_poses, collision_object_types, enabled); + if (new_cow != nullptr) + { + addCollisionObject(new_cow); + return true; + } + return false; +} + +bool BulletDiscreteBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool BulletDiscreteBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + removeCollisionObjectFromBroadphase(it->second, broadphase_, dispatcher_); + link2cow_.erase(name); + return true; + } + + return false; +} + +bool BulletDiscreteBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + return true; + } + return false; +} + +bool BulletDiscreteBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + return true; + } + return false; +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + cow->setWorldTransform(convertEigenToBt(pose)); + + // Update Collision Object Broadphase AABB + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const btTransform& pose) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + cow->setWorldTransform(pose); + + // Update Collision Object Broadphase AABB + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses) +{ + assert(names.size() == poses.size()); + for (size_t i = 0u; i < names.size(); ++i) + { + setCollisionObjectsTransform(names[i], poses[i]); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform( + const AlignedMap& transforms) +{ + for (const std::pair& transform : transforms) + { + setCollisionObjectsTransform(transform.first, transform.second); + } +} + +void BulletDiscreteBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + // Now need to update the broadphase with correct aabb + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + updateCollisionObjectFilters(active_, *cow, false); + } +} + +const std::vector& BulletDiscreteBVHManager::getActiveCollisionObjects() const +{ + return active_; +} + +void BulletDiscreteBVHManager::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + assert(cow->getBroadphaseHandle() != nullptr); + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +double BulletDiscreteBVHManager::getContactDistanceThreshold() const +{ + return contact_distance_; +} + +void BulletDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) +{ + fn_ = std::move(fn); +} + +IsContactAllowedFn BulletDiscreteBVHManager::getIsContactAllowedFn() const +{ + return fn_; +} + +void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm) +{ + ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " + << pair_cache->getNumOverlappingPairs()); + + DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " + << collisions.contact_count + << " collisions"); +} + +void BulletDiscreteBVHManager::contactTest( + collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, + const std::vector cows_external) +{ + ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " + << pair_cache->getNumOverlappingPairs()); + + DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); + + for (const CollisionObjectWrapperPtr& cow : cows_external) + { + contactTest(cow, cdata); + } + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " + << collisions.contact_count + << " collisions"); +} + +void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + link2cow_[cow->getName()] = cow; + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); +} + +const std::map& BulletDiscreteBVHManager::getCollisionObjects() const +{ + return link2cow_; +} + +void BulletDiscreteBVHManager::contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions) +{ + btVector3 min_aabb, max_aabb; + cow->getAABB(min_aabb, max_aabb); + + DiscreteCollisionCollector cc(collisions, cow, static_cast(cow->getContactProcessingThreshold())); + TesseractSingleContactCallback contact_cb(cow.get(), dispatcher_.get(), dispatch_info_, cc); + broadphase_->aabbTest(min_aabb, max_aabb, contact_cb); +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp new file mode 100644 index 0000000000..18b21bafaf --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -0,0 +1,343 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * + * 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. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_utils.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + const double* size = geom->size; + btScalar a = static_cast(size[0] / 2); + btScalar b = static_cast(size[1] / 2); + btScalar c = static_cast(size[2] / 2); + + return (new btBoxShape(btVector3(a, b, c))); +} + +btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + return (new btSphereShape(static_cast(geom->radius))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length / 2); + return (new btCylinderShapeZ(btVector3(r, r, l))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length); + return (new btConeShapeZ(r, l)); +} + +btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF); + + if (geom->vertex_count > 0 && geom->triangle_count > 0) + { + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::CONVEX_HULL: + { + // Create a convex hull shape to approximate Trimesh + collision_detection_bullet::AlignedVector input; + collision_detection_bullet::AlignedVector vertices; + std::vector faces; + + input.reserve(geom->vertex_count); + for (unsigned int i = 0; i < geom->vertex_count; ++i) + input.push_back(Eigen::Vector3d(geom->vertices[3 * i], geom->vertices[3 * i + 1], geom->vertices[3 * i + 2])); + + if (collision_detection_bullet::createConvexHull(vertices, faces, input) < 0) + return nullptr; + + btConvexHullShape* subshape = new btConvexHullShape(); + for (const Eigen::Vector3d& v : vertices) + subshape->addPoint( + btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + + return subshape; + } + case CollisionObjectType::USE_SHAPE_TYPE: + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->triangle_count)); + compound->setMargin( + BULLET_MARGIN); // margin: compound seems to have no effect when positive but has an effect when negative + + for (unsigned i = 0; i < geom->triangle_count; ++i) + { + btVector3 v[3]; + for (unsigned x = 0; x < 3; ++x) + { + unsigned idx = geom->triangles[3 * i + x]; + for (unsigned y = 0; y < 3; ++y) + { + v[x][y] = static_cast(geom->vertices[3 * idx + y]); + } + } + + btCollisionShape* subshape = new btTriangleShapeEx(v[0], v[1], v[2]); + if (subshape != nullptr) + { + cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans; + geom_trans.setIdentity(); + compound->addChildShape(geom_trans, subshape); + } + } + + return compound; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry meshs", + static_cast(collision_object_type)); + return nullptr; + } + } + } + ROS_ERROR("The mesh is empty!"); + return nullptr; +} + +btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF || + collision_object_type == CollisionObjectType::MULTI_SPHERE); + + btCompoundShape* subshape = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->octree->size())); + double occupancy_threshold = geom->octree->getOccupancyThres(); + + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::USE_SHAPE_TYPE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btScalar l = static_cast(size / 2); + btBoxShape* childshape = new btBoxShape(btVector3(l, l, l)); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + case CollisionObjectType::MULTI_SPHERE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btSphereShape* childshape = + new btSphereShape(static_cast(std::sqrt(2 * ((size / 2) * (size / 2))))); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry octree", + static_cast(collision_object_type)); + return nullptr; + } + } +} + +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow) +{ + switch (geom->type) + { + case shapes::BOX: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::SPHERE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CYLINDER: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CONE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::MESH: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + case shapes::OCTREE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + default: + { + ROS_ERROR("This geometric shape type (%d) is not supported using BULLET yet", static_cast(geom->type)); + return nullptr; + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) +{ + this->setContactProcessingThreshold(0); + assert(!shapes.empty()); + assert(!shape_poses.empty()); + assert(!collision_object_types.empty()); + assert(!name.empty()); + assert(shapes.size() == shape_poses.size()); + assert(shapes.size() == collision_object_types.size()); + + m_collision_filter_group = btBroadphaseProxy::KinematicFilter; + m_collision_filter_mask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter; + + if (shapes.size() == 1) + { + btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this); + shape->setMargin(BULLET_MARGIN); + manage(shape); + setCollisionShape(shape); + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + } + else + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(m_shapes.size())); + manage(compound); + // margin on compound seems to have no effect when positive but has an effect when negative + compound->setMargin(BULLET_MARGIN); + setCollisionShape(compound); + + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + Eigen::Isometry3d inv_world = m_shape_poses[0].inverse(); + + for (std::size_t j = 0; j < m_shapes.size(); ++j) + { + btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this); + if (subshape != nullptr) + { + manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]); + compound->addChildShape(geom_trans, subshape); + } + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links) + : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types) +{ + m_touch_links = touch_links; +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) + , m_data(data) +{ + this->setContactProcessingThreshold(0); +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp similarity index 95% rename from moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp rename to moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp index 461ea927e3..8279cf6d0d 100644 --- a/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -34,14 +34,14 @@ /* Author: Jens Petit */ -#include +#include #include namespace collision_detection { bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const { - scene->setActiveCollisionDetector(CollisionDetectorAllocatorBt::create(), exclusive); + scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); return true; } } diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp deleted file mode 100644 index 95755456fe..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * 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. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include - -namespace collision_detection -{ -CollisionRobotBt::CollisionRobotBt(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : CollisionRobot(model, padding, scale) -{ -} - -CollisionRobotBt::CollisionRobotBt(const CollisionRobotBt& other) : CollisionRobot(other) -{ -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - checkSelfCollisionHelper(req, res, state, nullptr); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const -{ - checkSelfCollisionHelper(req, res, state, &acm); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); -} - -void CollisionRobotBt::updatedPaddingOrScaling(const std::vector& links) -{ -} - -void CollisionRobotBt::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); -} - -void CollisionRobotBt::distanceOther(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp new file mode 100644 index 0000000000..43a2a2683f --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp @@ -0,0 +1,267 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include + +namespace collision_detection +{ +CollisionRobotBullet::CollisionRobotBullet(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : CollisionRobot(model, padding, scale), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + + manager_->setIsContactAllowedFn(fun); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObjectWrapper(link.second); + } + + std::vector active; + collision_detection_bullet::getActiveLinkNamesRecursive(active, robot_model_->getURDF()->getRoot(), true); + manager_->setActiveCollisionObjects(active); +} + +CollisionRobotBullet::CollisionRobotBullet(const CollisionRobotBullet& other) + : CollisionRobot(other), manager_(other.manager_->clone()) +{ +} + +void CollisionRobotBullet::addAttachedOjects( + const robot_state::RobotState& state, + std::vector& cows) const +{ + std::vector attached_bodies; + state.getAttachedBodies(attached_bodies); + for (const robot_state::AttachedBody*& body : attached_bodies) + { + const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); + std::vector collision_object_types( + attached_body_transform.size(), collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + cows.emplace_back(collision_detection_bullet::createCollisionObject( + body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, + collision_object_types, body->getTouchLinks(), true)); + } +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + // TODO: Not in bullet yet +} + +void CollisionRobotBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + std::vector cows; + addAttachedOjects(state, cows); + collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); + updateTransformsFromState(state, discrete_clone_manager); + discrete_clone_manager->contactTest(res, req, acm, cows); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix& acm) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); +} + +void CollisionRobotBullet::updatedPaddingOrScaling(const std::vector& links) +{ + for (const std::string& link : links) + { + if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end()) + { + addLinkAsCollisionObjectWrapper(robot_model_->getURDF()->links_[link]); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } + } +} + +void CollisionRobotBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); +} + +void CollisionRobotBullet::distanceOther(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); +} + +void CollisionRobotBullet::updateTransformsFromState( + const robot_state::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const +{ + // updating link positions with the current robot state + for (const std::pair& link : + manager->getCollisionObjects()) + { + // select the first of the transformations for each link (composed of multiple shapes...) + manager->setCollisionObjectsTransform(link.first, state.getCollisionBodyTransform(link.first, 0)); + } +} + +void CollisionRobotBullet::addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link) +{ + if (!link->collision_array.empty()) + { + const std::vector& col_array = + link->collision_array.empty() ? std::vector(1, link->collision) : + link->collision_array; + + std::vector shapes; + collision_detection_bullet::AlignedVector shape_poses; + std::vector collision_object_types; + + for (const auto& i : col_array) + { + if (i && i->geometry) + { + shapes::ShapePtr shape = collision_detection_bullet::constructShape(i->geometry.get()); + + if (shape) + { + if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits::epsilon() || + fabs(getLinkPadding(link->name)) >= std::numeric_limits::epsilon()) + { + shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name)); + } + + shapes.push_back(shape); + shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin)); + + if (shape->type == shapes::MESH) + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + } + else + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + } + } + } + + collision_detection_bullet::CollisionObjectWrapperPtr cow = collision_detection_bullet::createCollisionObject( + link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true); + + if (manager_->hasCollisionObject(link->name)) + { + manager_->removeCollisionObject(link->name); + } + manager_->addCollisionObject(cow); + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp deleted file mode 100644 index 530c2d8d00..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * 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. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include -#include - -#include - -namespace collision_detection -{ -const std::string CollisionDetectorAllocatorBt::NAME("Bullet"); - -CollisionWorldBt::CollisionWorldBt() : CollisionWorld() -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldBt::CollisionWorldBt(const WorldPtr& world) : CollisionWorld(world) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldBt::CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world) : CollisionWorld(other, world) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldBt::~CollisionWorldBt() -{ - getWorld()->removeObserver(observer_handle_); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state) const -{ - checkRobotCollisionHelper(req, res, robot, state, nullptr); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelper(req, res, robot, state, &acm); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionWorldBt::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet robot world collision checking not yet implemented"); -} - -void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const -{ - checkWorldCollisionHelper(req, res, other_world, nullptr); -} - -void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const -{ - checkWorldCollisionHelper(req, res, other_world, &acm); -} - -void CollisionWorldBt::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -void CollisionWorldBt::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldBt::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) -{ -} - -void CollisionWorldBt::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -void CollisionWorldBt::distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp new file mode 100644 index 0000000000..9b5d164681 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp @@ -0,0 +1,247 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); + +CollisionWorldBullet::CollisionWorldBullet() + : CollisionWorld(), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + manager_->setIsContactAllowedFn(fun); +} + +CollisionWorldBullet::CollisionWorldBullet(const WorldPtr& world) + : CollisionWorld(world), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); + + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + manager_->setIsContactAllowedFn(fun); +} + +CollisionWorldBullet::CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world) + : CollisionWorld(other, world) +{ + manager_ = other.manager_->clone(); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); +} + +CollisionWorldBullet::~CollisionWorldBullet() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, robot, state, nullptr); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + checkRobotCollisionHelperCCD(req, res, robot, state1, state2, nullptr); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelperCCD(req, res, robot, state1, state2, &acm); +} + +void CollisionWorldBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision checking not implemented yet"); +} + +void CollisionWorldBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + const CollisionRobotBullet& robot_bt = dynamic_cast(robot); + + collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); + std::vector attached_cows; + robot_bt.addAttachedOjects(state, attached_cows); + + for (const std::pair& cow : + robot_bt.manager_->getCollisionObjects()) + { + collision_detection_bullet::CollisionObjectWrapperPtr new_cow = cow.second->clone(); + discrete_clone_manager->addCollisionObject(new_cow); + discrete_clone_manager->setCollisionObjectsTransform(new_cow->getName(), + state.getCollisionBodyTransform(new_cow->getName(), 0)); + } + + discrete_clone_manager->setActiveCollisionObjects(robot_bt.manager_->getActiveCollisionObjects()); + + discrete_clone_manager->contactTest(res, req, acm, attached_cows); +} + +void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world) const +{ + checkWorldCollisionHelper(req, res, other_world, nullptr); +} + +void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, + const AllowedCollisionMatrix& acm) const +{ + checkWorldCollisionHelper(req, res, other_world, &acm); +} + +void CollisionWorldBullet::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); +} + +void CollisionWorldBullet::addToManager(const World::Object* obj) +{ + std::vector collision_object_types; + + for (const shapes::ShapeConstPtr& shape : obj->shapes_) + { + if (shape->type == shapes::MESH) + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + else + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + + manager_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, + collision_object_types, true); +} + +void CollisionWorldBullet::updateManagedObject(const std::string& id) +{ + if (getWorld()->hasObject(id)) + { + auto it = getWorld()->find(id); + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + addToManager(it->second.get()); + } + else + { + addToManager(it->second.get()); + } + } + else + { + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + } + } +} + +void CollisionWorldBullet::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + CollisionWorld::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionWorldBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + manager_->removeCollisionObject(obj->id_); + } + else + { + updateManagedObject(obj->id_); + } +} + +void CollisionWorldBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); +} + +void CollisionWorldBullet::distanceWorld(const DistanceRequest& req, DistanceResult& res, + const CollisionWorld& world) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp similarity index 81% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h rename to moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp index ac0a27aa40..3f22bab5f0 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp @@ -34,15 +34,14 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ +#include +#include -#include -#include +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorBullet); -namespace collision_detection +int main(int argc, char* argv[]) { -// TODO: Add common functionality for all Bullet collision checkers. + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - -#endif diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp new file mode 100644 index 0000000000..339cd15c3b --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorBullet); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 8e73a17709..54b67f67b6 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -29,4 +29,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + + catkin_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) + target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_bullet/src/collision_common.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp similarity index 81% rename from moveit_core/collision_detection_bullet/src/collision_common.cpp rename to moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp index ecae7a988f..b83b267171 100644 --- a/moveit_core/collision_detection_bullet/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -34,10 +34,14 @@ /* Author: Jens Petit */ -#include -#include +#include +#include -namespace collision_detection +INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); + +int main(int argc, char* argv[]) { -// TODO: Add common functionality for all collision detection here. -} // namespace collision_detection + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detector_bt_description.xml b/moveit_core/collision_detector_bullet_description.xml similarity index 100% rename from moveit_core/collision_detector_bt_description.xml rename to moveit_core/collision_detector_bullet_description.xml diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 2d65b4acac..40748377ab 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -22,6 +22,7 @@ assimp boost eigen + bullet eigen_stl_containers libfcl-dev geometric_shapes @@ -57,6 +58,6 @@ - + diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch new file mode 100644 index 0000000000..5c9526b876 --- /dev/null +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -0,0 +1,10 @@ + + + + + + + + + +