diff --git a/ur5_moveit_config/config/ur5.srdf b/ur5_moveit_config/config/ur5.srdf index 9b6db329a015b288452bf5ad60b71bbfcb45379d..d5aba4a472ce04b5a5e01295c71991f4391ae279 100644 --- a/ur5_moveit_config/config/ur5.srdf +++ b/ur5_moveit_config/config/ur5.srdf @@ -50,4 +50,5 @@ <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" /> <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" /> <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" /> + <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent" /> </robot> diff --git a/ur_description/urdf/ur5.urdf.xacro b/ur_description/urdf/ur5.urdf.xacro index 448b3083f2c144087ff627f435d8d13d1ef52fde..a3815ecf3f745f644971b079dd0c792f3215d12c 100644 --- a/ur_description/urdf/ur5.urdf.xacro +++ b/ur_description/urdf/ur5.urdf.xacro @@ -282,7 +282,14 @@ <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" /> </joint> - <link name="${prefix}ee_link" /> + <link name="${prefix}ee_link"> + <collision> + <geometry> + <box size="0.01 0.01 0.01"/> + </geometry> + <origin rpy="0 0 0" xyz="-0.01 0 0"/> + </collision> + </link> <xacro:ur5_arm_transmission prefix="${prefix}" /> <xacro:ur5_arm_gazebo prefix="${prefix}" />