Using Bullet as a Collision Checker (#337)

* Adding Bullet collision tutorial:
  * use copy of visualizing collisions tutorial
  * adapted copied tutorial for Bullet
  * added CCD demonstration to tutorial

* PR review fix

* clang-format and update broken link

* update robot state and maker visualizations

* Replace raw pointers through smart pointers

* Retriggering travis

* fix compiler warnings

* fix clang-tidy

* Fixup

Author: j-petit

CMakeLists.txt

@@ -72,3 +72,4 @@ add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner)
 add_subdirectory(doc/moveit_cpp)
 add_subdirectory(doc/collision_environments)
 add_subdirectory(doc/visualizing_collisions)
+add_subdirectory(doc/bullet_collision_checker)

doc/bullet_collision_checker/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_executable(bullet_collision_checker_tutorial
+    src/bullet_collision_checker_tutorial.cpp)
+
+target_link_libraries(bullet_collision_checker_tutorial
+    ${interactive_markers_LIBRARIES}
+    interactivity_utils
+    ${catkin_LIBRARIES}
+    ${Boost_LIBRARIES})
+
+install(TARGETS bullet_collision_checker_tutorial
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

doc/bullet_collision_checker/bullet_collision_checker.rst

@@ -0,0 +1,58 @@
+Using Bullet for Collision Checking
+===================================
+
+.. image:: images/bullet_collision_visualized.png
+   :width: 600px
+
+In addition to the Flexible Collision Library (FCL), `Bullet Collision Detection <https://pybullet.org/wordpress/>`_ is available as a collision checker. The tutorial builds on top of the `Visualizing Collisions <../visualizing_collisions/visualizing_collisions_tutorial.html>`_ tutorial to show collisions.
+
+Furthermore, Continuous Collision Detection (CCD) is available and demonstrated with Bullet.
+
+Getting Started
+---------------
+If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
+
+Running the Code
+----------------
+Roslaunch the launch file to run the code directly from moveit_tutorials: ::
+
+ roslaunch moveit_tutorials bullet_collision_checker_tutorial.launch
+
+You should now see the Panda robot and a box both with interactive markers which you can drag around. Note that different to FCL, Bullet does not calculate all individual contact points for a shape but only the point of deepest penetration.
+
+.. figure:: images/fcl_vs_bullet_markers.png
+
+   ..
+
+   Left: FCL collision results. Right: Bullet collision results.
+
+Please note that the current implementation of Bullet as a collision detector is not thread safe as the internal collision managers are mutable members.
+
+Continuous Collision Detection
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Additionally, Bullet has continuous collision capabilities. This means that it can be guaranteed that no collision occurs during the transition between two discrete robot states with the environment. To get a demonstration of the CCD click the ``Next`` button of the ``moveit_visual_tools`` panel on the bottom left in RViz. The interactive robot disappears and the robot appears in a state where its hand is just behind a box. On pressing next again, the robot jumps to a configuration where the hand is right in front of the box. In both states, no collision is detected (see terminal output).
+
+.. figure:: images/bullet_CCD_discrete.png
+
+   ..
+
+   Left: robot in configuration 1. Right: robot in configuration 2.
+
+On a following ``Next``, CCD is performed using a casted robot model between the two discrete poses. A collision is reported
+(see terminal output for details).
+
+.. image:: images/bullet_CCD_both_states.png
+   :width: 600px
+
+Pressing ``Next`` one more time finishes the tutorial.
+
+Relevant Code
+-------------
+The entire code can be seen :codedir:`here <bullet_collision_checker>` in the moveit_tutorials GitHub project. A lot of information necessary for understanding how this demo works is left out to keep this tutorial focused on Bullet. Please see `Visualizing Collisions <../visualizing_collisions/visualizing_collisions_tutorial.html>`_ for code explanation regarding the visualizing of the collisions.
+
+.. tutorial-formatter:: ./src/bullet_collision_checker_tutorial.cpp
+
+Launch file
+-----------
+The entire launch file is  :codedir:`here <bullet_collision_checker>` on GitHub. All the code in this tutorial can be compiled and run from the ``moveit_tutorials`` package.

doc/bullet_collision_checker/images/bullet_CCD_both_states.png

@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- load URDF -->
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+
+  <!-- load SRDF -->
+  <param name="robot_description_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'"/>
+
+  <!-- Run RViz with a custom config -->
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_tutorials)/doc/bullet_collision_checker/launch/moveit.rviz" output="screen">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+  </node>
+
+  <!-- If needed, broadcast static tf2 for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 1 /world/panda_link0 /panda_link0 10" />
+
+  <!-- launch interactivity_tutorial -->
+  <node name="bullet_collision_tutorial" pkg="moveit_tutorials" type="bullet_collision_checker_tutorial" respawn="false" output="screen">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+  </node>
+
+</launch>