From 575727541edc5680fd6ceb42e772493d4bf58d40 Mon Sep 17 00:00:00 2001
From: Erdal Pekel <info@erdalpekel.de>
Date: Mon, 14 Jan 2019 17:30:07 +0100
Subject: [PATCH 1/2] Applied changes for Panda Gazebo integration

---
 franka_description/robots/hand.xacro          |  15 +++
 franka_description/robots/panda.gazebo.xacro  |  94 +++++++++++++++
 .../robots/panda.transmission.xacro           | 112 ++++++++++++++++++
 franka_description/robots/panda_arm.xacro     |  67 ++++++++++-
 .../robots/panda_arm_hand.urdf.xacro          |   8 ++
 5 files changed, 295 insertions(+), 1 deletion(-)
 create mode 100644 franka_description/robots/panda.gazebo.xacro
 create mode 100644 franka_description/robots/panda.transmission.xacro

diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 1cd3777e1..7d48c1b82 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -19,6 +19,11 @@
           <mesh filename="package://franka_description/meshes/collision/hand.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.68"/>
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
+      </inertial>
     </link>
     <link name="${ns}_leftfinger">
       <visual>
@@ -31,6 +36,11 @@
           <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.01"/>
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
+      </inertial>
     </link>
     <link name="${ns}_rightfinger">
       <visual>
@@ -45,6 +55,11 @@
           <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.01"/>
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
+      </inertial>
     </link>
     <joint name="${ns}_finger_joint1" type="prismatic">
       <parent link="${ns}_hand"/>
diff --git a/franka_description/robots/panda.gazebo.xacro b/franka_description/robots/panda.gazebo.xacro
new file mode 100644
index 000000000..02822caa5
--- /dev/null
+++ b/franka_description/robots/panda.gazebo.xacro
@@ -0,0 +1,94 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="panda_gazebo" params="robot_name">
+
+        <!-- Link0 -->
+        <gazebo reference="${robot_name}_link0">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link1 -->
+        <gazebo reference="${robot_name}_link1">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link2 -->
+        <gazebo reference="${robot_name}_link2">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link3 -->
+        <gazebo reference="${robot_name}_link3">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link4 -->
+        <gazebo reference="${robot_name}_link4">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link5 -->
+        <gazebo reference="${robot_name}_link5">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link6 -->
+        <gazebo reference="${robot_name}_link6">
+            <material>Gazebo/White</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link7 -->
+        <gazebo reference="${robot_name}_link7">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- Link8 -->
+        <gazebo reference="${robot_name}_link8">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- LinkHand -->
+        <gazebo reference="${robot_name}_hand">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- LinkRightFinger -->
+        <gazebo reference="${robot_name}_rightfinger">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+        <!-- LinkLeftFinger -->
+        <gazebo reference="${robot_name}_leftfinger">
+            <material>Gazebo/Grey</material>
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+        </gazebo>
+
+    </xacro:macro>
+
+</robot>
+
diff --git a/franka_description/robots/panda.transmission.xacro b/franka_description/robots/panda.transmission.xacro
new file mode 100644
index 000000000..343bd7ad7
--- /dev/null
+++ b/franka_description/robots/panda.transmission.xacro
@@ -0,0 +1,112 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="panda_transmission" params="robot_name">
+
+
+        <!-- Load Gazebo lib and set the robot namespace -->
+        <gazebo>
+            <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+                <!-- <robotNamespace>/${robot_name}</robotNamespace> -->
+            </plugin>
+        </gazebo>
+
+        <transmission name="${robot_name}_tran_1">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint1">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_1">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_2">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint2">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_2">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_3">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint3">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_3">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_4">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint4">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_4">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_5">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint5">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_5">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_6">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint6">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_6">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_tran_7">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_joint7">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_7">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_leftfinger">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_finger_joint1">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_finger_joint1">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${robot_name}_rightfinger">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${robot_name}_finger_joint2">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_finger_joint2">
+                <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+    </xacro:macro>
+</robot>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 672c881ca..427f279db 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -8,6 +8,18 @@
         <origin rpy="${rpy}" xyz="${xyz}"/>
       </joint>
     </xacro:unless>
+
+    <xacro:property name="joint_damping" value="1.0"/>
+
+    <link name="world" />
+
+    <joint name="robot_to_world" type="fixed">
+      <parent link="world" />
+      <child link="${arm_id}_link0" />
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <dynamics damping="${joint_damping}"/>
+    </joint>
+
     <link name="${arm_id}_link0">
       <visual>
         <geometry>
@@ -19,6 +31,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link0.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="3.06"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <link name="${arm_id}_link1">
       <visual>
@@ -31,6 +48,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link1.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="2.34"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint1" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -39,6 +61,7 @@
       <child link="${arm_id}_link1"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link2">
       <visual>
@@ -51,6 +74,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link2.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="2.36"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint2" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
@@ -59,6 +87,7 @@
       <child link="${arm_id}_link2"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link3">
       <visual>
@@ -71,6 +100,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link3.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="2.38"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint3" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -79,6 +113,7 @@
       <child link="${arm_id}_link3"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link4">
       <visual>
@@ -91,6 +126,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link4.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="2.43"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint4" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
@@ -99,6 +139,7 @@
       <child link="${arm_id}_link4"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link5">
       <visual>
@@ -111,6 +152,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link5.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="3.5"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </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"/>
@@ -119,6 +165,7 @@
       <child link="${arm_id}_link5"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link6">
       <visual>
@@ -131,6 +178,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link6.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1.47"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint6" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
@@ -139,6 +191,7 @@
       <child link="${arm_id}_link6"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
     <link name="${arm_id}_link7">
       <visual>
@@ -151,6 +204,11 @@
           <mesh filename="package://${description_pkg}/meshes/collision/link7.stl"/>
         </geometry>
       </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.45"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
     </link>
     <joint name="${arm_id}_joint7" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -159,8 +217,15 @@
       <child link="${arm_id}_link7"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <dynamics damping="${joint_damping}"/>
     </joint>
-    <link name="${arm_id}_link8"/>
+    <link name="${arm_id}_link8">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.0"/>
+        <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
+      </inertial>
+    </link>
     <joint name="${arm_id}_joint8" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0.107"/>
       <parent link="${arm_id}_link7"/>
diff --git a/franka_description/robots/panda_arm_hand.urdf.xacro b/franka_description/robots/panda_arm_hand.urdf.xacro
index 4c0f2fcea..f86ee2e40 100644
--- a/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -1,7 +1,15 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="robot_name" default="panda"/>
+  
   <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
   <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+  <xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
+  <xacro:include filename="$(find franka_description)/robots/panda.transmission.xacro"/>
+
   <xacro:panda_arm />
   <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+
+  <xacro:panda_gazebo robot_name="$(arg robot_name)" />
+  <xacro:panda_transmission robot_name="$(arg robot_name)" />
 </robot>

From dfdd17d3aa311e9e1b7e6ced5547755cdaa7b89a Mon Sep 17 00:00:00 2001
From: Erdal Pekel <info@erdalpekel.de>
Date: Mon, 14 Jan 2019 17:38:35 +0100
Subject: [PATCH 2/2] State changes to repository.

---
 README.md | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/README.md b/README.md
index 1df34a2f3..63362382e 100644
--- a/README.md
+++ b/README.md
@@ -4,6 +4,19 @@
 
 See the [Franka Control Interface (FCI) documentation][fci-docs] for more information.
 
+## My Changes
+
+In order to integrate the panda robot into gazebo I made changes to the following files:
+
+- franka_description/robots/hand.xacro
+- franka_description/robots/panda_arm.xacro
+- franka_description/robots/panda_arm_hand.urdf.xacro
+
+I added the following files to the repository:
+
+- franka_description/robots/panda.gazebo.xacro
+- franka_description/robots/panda.transmission.xacro
+
 ## License
 
 All packages of `franka_ros` are licensed under the [Apache 2.0 license][apache-2.0].