[feature_request] Improve robot collision geometries

[rickstaa]

We are currently trying to get the Noetic version of the panda_moveit_config, which targets franka_ros v0.8.0, released. While doing this, however, we noticed that due 616055e being applied, MoveIt cannot control the Panda robot anymore correctly. This is because 616055e replaces the collision meshes with collision geometries + a safety distance. However, these new collision geometries are a bit coarse, which causes the robot hand movement to be very restrictive (see gif below). #126 (comment) explained the reason for this change:

Regarding the mesh vs geometry differences: Geometries plus a safety distance are used for the real hardware since this relates to its internal collision model. However, in simulation, convex cylinders (plus a safety distance) can easily overlap with other links causing weird effects in Gazebo, so we chose to go with simple meshes here. Remember: You can always assemble and specify your own URDF according to what collision model you want right now.

We have worked around this issue by disabling collision checking between joint 5/6 and joint8 in the MoveIt srdf. However, this solution is suboptimal since MoveIt does now return plans that contain collisions on the real robot.

Therefore, it would be great to improve these collision geometries so that they do not cause self-collisions in positions that are possible on the real robot while keeping them closely related to the collision geometries used in the FCI. I am happy to help but I do not have access to the Collision Geometries that are used in the FCI.

Another, sub-optimal, solution would be to merge #154, allowing us to use the meshes for the https://github.com/ros-planning/moveit_tutorials while the more accurate Geometries + safety distance are used when planning with the real robot. Yet another, sub-optimal solution would to merge #153.

Steps to reproduce the problem

1. Clone https://github.com/rickstaa/panda_moveit_config/tree/noetic-devel-update in a catkin workspace together with the latest version of franka_ros.
2. Checkout the noetic-devel-update-collision-problem branch.
3. Build the catkin workspace.
4. Start the demo launch file roslaunch panda_moveit_config demo.launch.
5. Add the motion planning plugin to Rviz.
6. Try out moving the arm in Rviz and see how restrictive the hand movement is.

[rickstaa]

Also see moveit/panda_moveit_config#35

[rickstaa]

Will be fixed when #188 is merged.

[Yitian-Xian]

Hi, @rickstaa, may I know that if we can modify the collision meshes into something that only contains triangles? Currently, you can check https://github.com/AndrejOrsula/panda_ign_moveit2/tree/master/panda_description, and you will see that mesh is very coarse.

[rickstaa]

Hey @Yitian-Xian, @frankaemika could change the collision geometries to triangles. I, however, suspect that this will be too computationally intensive. Neverthelesss, @gollth is currently working with @rhaschke on improving the collision geometries (see #199).