diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro index 48674a8a4e6d3372e045be0f4edb32af890b4ce7..04bad21b1e9131f27825834de120e8c4730d6466 100644 --- a/config/panda_arm.xacro +++ b/config/panda_arm.xacro @@ -41,6 +41,7 @@ <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/> <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/> <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/> + <disable_collisions link1="panda_link3" link2="panda_link5" reason="Default" /> <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/> <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/> <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/> @@ -50,5 +51,10 @@ <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default"/> <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/> <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/> + <!-- Fix self-collisions between coarse collision geometries provided by franka_description + (see https://github.com/ros-planning/panda_moveit_config/pull/35#pullrequestreview-390715967). + These collisions are handled by joint limits already and thus can be disabled here. --> + <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default"/> + <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default"/> </xacro:macro> </robot>