Merge pull request #95 in SWDEV/franka_ros from feature/SRR-367-add-r…

…cu-collision-volumes-to-robot to develop * commit '4f4235696b2718940aa29ab33817cafed54637cd': adds safety distances documentation. Adds comments to explain the safety distance and robot id. Add changelog entries Adds safety distance to panda arm hand urdf. Adds inital value to safety_distance in panda_arm.xacro Adds panda arm back to arm hand urdf. Modifies the urdf files containing safety_distance." Changes the laucnh file back. deletese collision meshes. Adds the safety distance to the values. changes link 7 collision volume to an equivalent description. Adds hand collisions. Adds rcu collision volumes to robot URDF.
frankaemika · Nov 16, 2018 · e52c03a · e52c03a
2 parents e96e1dc + 4f42356
commit e52c03a
Show file tree

Hide file tree

Showing 16 changed files with 195 additions and 29 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,8 @@ Requires `libfranka` >= 0.5.0
 
  * Moved `panda_moveit_config` to [`ros-planning`](https:/ros-planning/panda_moveit_config)
  * Publish `robot_mode` in `franka_state_controller`
+ * Update collision volumes
+ * URDF: Remove invalid `axis` for `joint8`
  * Added support for ROS Melodic Morenia.
 
 ## 0.6.0 - 2018-08-08

diff --git a/franka_description/meshes/collision/finger.stl b/franka_description/meshes/collision/finger.stl
diff --git a/franka_description/meshes/collision/hand.stl b/franka_description/meshes/collision/hand.stl
diff --git a/franka_description/meshes/collision/link0.stl b/franka_description/meshes/collision/link0.stl
diff --git a/franka_description/meshes/collision/link1.stl b/franka_description/meshes/collision/link1.stl
diff --git a/franka_description/meshes/collision/link2.stl b/franka_description/meshes/collision/link2.stl
diff --git a/franka_description/meshes/collision/link3.stl b/franka_description/meshes/collision/link3.stl
diff --git a/franka_description/meshes/collision/link4.stl b/franka_description/meshes/collision/link4.stl
diff --git a/franka_description/meshes/collision/link5.stl b/franka_description/meshes/collision/link5.stl
diff --git a/franka_description/meshes/collision/link6.stl b/franka_description/meshes/collision/link6.stl
diff --git a/franka_description/meshes/collision/link7.stl b/franka_description/meshes/collision/link7.stl
diff --git a/franka_description/robots/hand.urdf.xacro b/franka_description/robots/hand.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
  <xacro:include filename="hand.xacro"/>
- <xacro:hand ns="panda"/>
+ <xacro:hand ns="panda" safety_distance="0.03"/>
 </robot>
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
- <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' ">
+ <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
+ <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' safety_distance:='0'">
  <xacro:unless value="${connected_to == ''}">
  <joint name="${ns}_hand_joint" type="fixed">
  <parent link="${connected_to}"/>
@@ -15,37 +16,57 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
  <geometry>
- <mesh filename="package://franka_description/meshes/collision/hand.stl"/>
+ <cylinder radius="${0.04+safety_distance}" length="0.1" />
  </geometry>
  </collision>
- </link>
- <link name="${ns}_leftfinger">
- <visual>
+ <collision>
+ <origin xyz="0 -0.05 0.04" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+ <sphere radius="${0.04+safety_distance}" />
  </geometry>
- </visual>
+ </collision>
+ <collision>
+ <origin xyz="0 0.05 0.04" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.04+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}"/>
+ <geometry>
+ <cylinder radius="${0.02+safety_distance}" length="0.1" />
+ </geometry>
+ </collision>
  <collision>
+ <origin xyz="0 -0.05 0.1" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
+ <sphere radius="${0.02+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.05 0.1" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.02+safety_distance}" />
  </geometry>
  </collision>
  </link>
- <link name="${ns}_rightfinger">
+ <link name="${ns}_leftfinger">
  <visual>
- <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
  <geometry>
  <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
  </geometry>
  </visual>
- <collision>
+ </link>
+ <link name="${ns}_rightfinger">
+ <visual>
  <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
  <geometry>
- <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
+ <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
  </geometry>
- </collision>
-  </link>
+ </visual>
+ </link>
  <joint name="${ns}_finger_joint1" type="prismatic">
  <parent link="${ns}_hand"/>
  <child link="${ns}_leftfinger"/>

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
  <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
- <xacro:panda_arm />
+ <xacro:panda_arm safety_distance="0.03"/>
 </robot>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
@@ -1,6 +1,8 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
- <xacro:macro name="panda_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0'">
+ <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
+ <!-- arm_id: Namespace of the panda arm. Serves to differentiate between arms in case of multiple instances. -->
+ <xacro:macro name="panda_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0' safety_distance:='0'">
  <xacro:unless value="${not connected_to}">
  <joint name="${arm_id}_joint_${connected_to}" type="fixed">
  <parent link="${connected_to}"/>
@@ -15,8 +17,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link0.stl"/>
+ <cylinder radius="${0.06+safety_distance}" length="0.03" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="-0.06 0 0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="-0.09 0 0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -27,8 +42,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 -0.1915" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.06+safety_distance}" length="0.2830" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.333" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link1.stl"/>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -47,8 +75,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link2.stl"/>
+ <cylinder radius="${0.06+safety_distance}" length="0.12" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -67,8 +108,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 -0.145" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.06+safety_distance}" length="0.15" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.22" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link3.stl"/>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.07" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -87,8 +141,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.06+safety_distance}" length="0.12" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link4.stl"/>
+ <sphere radius="${0.06+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -107,10 +174,42 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 -0.26" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link5.stl"/>
+ <cylinder radius="${0.06+safety_distance}" length="0.1" />
  </geometry>
  </collision>
+ <collision>
+ <origin xyz="0 0 -0.31" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.21" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.06+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.08 -0.13" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.025+safety_distance}" length="0.14" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.08 -0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.025+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.08 -0.20" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.025+safety_distance}" />
+ </geometry>
+ </collision>
+
  </link>
  <joint name="${arm_id}_joint5" type="revolute">
  <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -127,8 +226,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 -0.03" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.05+safety_distance}" length="0.08" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.01" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.05+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.07" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link6.stl"/>
+ <sphere radius="${0.05+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -147,8 +259,21 @@
  </geometry>
  </visual>
  <collision>
+ <origin xyz="0 0 0.01" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${0.04+safety_distance}" length="0.14" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.08" rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://${description_pkg}/meshes/collision/link7.stl"/>
+ <sphere radius="${0.04+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.04+safety_distance}" />
  </geometry>
  </collision>
  </link>
@@ -160,12 +285,30 @@
  <axis xyz="0 0 1"/>
  <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>
- <link name="${arm_id}_link8"/>
+ <link name="${arm_id}_link8">
+ <collision>
+ <origin xyz="0.0424 0.0424 -0.0250" rpy="${pi} ${pi/2} ${pi/2}"/>
+ <geometry>
+ <cylinder radius="${0.03+safety_distance}" length="0.01" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0.0424 0.0424 -0.02" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.03+safety_distance}" />
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0.0424 0.0424 -0.03" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="${0.03+safety_distance}" />
+ </geometry>
+ </collision>
+ </link>
  <joint name="${arm_id}_joint8" type="fixed">
  <origin rpy="0 0 0" xyz="0 0 0.107"/>
  <parent link="${arm_id}_link7"/>
  <child link="${arm_id}_link8"/>
- <axis xyz="0 0 0"/>
  </joint>
  </xacro:macro>
 </robot>
diff --git a/franka_description/robots/panda_arm_hand.urdf.xacro b/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -2,6 +2,6 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
  <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
  <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
- <xacro:panda_arm />
- <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+ <xacro:panda_arm safety_distance="0.03"/>
+ <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.03"/>
 </robot>