Empty collision checker template for usage with tesseract and bullet (m…

…oveit#1499)

* Adding documentation to collision detection (moveit#1488)

* CMake adaptions for Tesseract integration

* Empty collision detector template for tesseract and bullet

* Making bullet as a collision plugin available

* Adding missing error messages

* Adding new folders and libraries to cmake

* Fixing the BSD license

* clang-format

moveit_core/CMakeLists.txt

@@ -70,6 +70,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     backtrace/include
     collision_detection/include
     collision_detection_fcl/include
+    collision_detection_bullet/include
     constraint_samplers/include
     controller_manager/include
     distance_field/include
@@ -106,6 +107,7 @@ catkin_package(
     moveit_planning_interface
     moveit_collision_detection
     moveit_collision_detection_fcl
+    moveit_collision_detection_bullet
     moveit_kinematic_constraints
     moveit_planning_scene
     moveit_constraint_samplers
@@ -195,6 +197,7 @@ add_subdirectory(robot_state)
 add_subdirectory(robot_trajectory)
 add_subdirectory(collision_detection)
 add_subdirectory(collision_detection_fcl)
+add_subdirectory(collision_detection_bullet)
 add_subdirectory(kinematic_constraints)
 add_subdirectory(planning_scene)
 add_subdirectory(constraint_samplers)

moveit_core/collision_detection_bullet/CMakeLists.txt

@@ -0,0 +1,29 @@
+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
+)
+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})
+add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+
+add_library(collision_detector_bullet_plugin src/collision_detector_bt_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_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_bullet_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(moveit_resources REQUIRED)
+  include_directories(${moveit_resources_INCLUDE_DIRS})
+endif()

moveit_core/collision_detection_bullet/README.md

@@ -0,0 +1,6 @@
+### Using Bullet as a collision checker [experimental]
+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`
+d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group`

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h

@@ -0,0 +1,48 @@
+/*********************************************************************
+ * 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 */
+
+#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_
+#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_
+
+#include <moveit/collision_detection/world.h>
+#include <moveit/collision_detection/collision_world.h>
+
+namespace collision_detection
+{
+// TODO: Add common functionality for all Bullet collision checkers.
+}
+
+#endif

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h

@@ -0,0 +1,55 @@
+/*********************************************************************
+ * 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 */
+
+#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_
+#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_
+
+#include <moveit/collision_detection/collision_detector_allocator.h>
+#include <moveit/collision_detection_bullet/collision_robot_bt.h>
+#include <moveit/collision_detection_bullet/collision_world_bt.h>
+
+namespace collision_detection
+{
+/** \brief An allocator for Bullet collision detectors */
+class CollisionDetectorAllocatorBt
+    : public CollisionDetectorAllocatorTemplate<CollisionWorldBt, CollisionRobotBt, CollisionDetectorAllocatorBt>
+{
+public:
+  static const std::string NAME;  // defined in collision_world_bt.cpp
+};
+}
+
+#endif

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h

@@ -0,0 +1,51 @@
+/*********************************************************************
+ * 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 */
+
+#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_
+#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_
+
+#include <moveit/collision_detection/collision_plugin.h>
+#include <moveit/collision_detection_bullet/collision_detector_allocator_bt.h>
+
+namespace collision_detection
+{
+class CollisionDetectorBtPluginLoader : public CollisionPlugin
+{
+public:
+  virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const;
+};
+}
+#endif  // MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h

@@ -0,0 +1,94 @@
+/*********************************************************************
+ * 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 */
+
+#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_
+#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_
+
+#include <moveit/collision_detection_bullet/collision_common.h>
+
+namespace collision_detection
+{
+class CollisionRobotBt : public CollisionRobot
+{
+  friend class CollisionWorldBt;
+
+public:
+  CollisionRobotBt(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0);
+
+  CollisionRobotBt(const CollisionRobotBt& other);
+
+  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+                          const robot_state::RobotState& state) const override;
+  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+                          const AllowedCollisionMatrix& acm) const override;
+  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
+                          const robot_state::RobotState& state2) const override;
+  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
+                          const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
+
+  void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+                           const CollisionRobot& other_robot,
+                           const robot_state::RobotState& other_state) const override;
+  void 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 override;
+  void 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 override;
+  void 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 override;
+
+  void distanceSelf(const DistanceRequest& req, DistanceResult& res,
+                    const robot_state::RobotState& state) const override;
+
+  void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
+                     const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override;
+
+protected:
+  void updatedPaddingOrScaling(const std::vector<std::string>& links) override;
+
+  void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+                                const AllowedCollisionMatrix* acm) const;
+  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;
+};
+}
+
+#endif

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h

@@ -0,0 +1,86 @@
+/*********************************************************************
+ * 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 */
+
+#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_
+#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_
+
+#include <moveit/collision_detection_bullet/collision_robot_bt.h>
+
+namespace collision_detection
+{
+class CollisionWorldBt : public CollisionWorld
+{
+public:
+  CollisionWorldBt();
+  explicit CollisionWorldBt(const WorldPtr& world);
+  CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world);
+  ~CollisionWorldBt() override;
+
+  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+                           const robot_state::RobotState& state) const override;
+  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+                           const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override;
+  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+                           const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override;
+  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+                           const robot_state::RobotState& state1, const robot_state::RobotState& state2,
+                           const AllowedCollisionMatrix& acm) const override;
+  void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
+                           const CollisionWorld& other_world) const override;
+  void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
+                           const AllowedCollisionMatrix& acm) const override;
+
+  void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
+                     const robot_state::RobotState& state) const override;
+
+  void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override;
+
+  void setWorld(const WorldPtr& world) override;
+
+protected:
+  void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
+                                 const AllowedCollisionMatrix* acm) const;
+  void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+                                 const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
+
+private:
+  void initialize();
+  void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
+  World::ObserverHandle observer_handle_;
+};
+}
+
+#endif