diff --git a/franka_control/config/panda_gazebo_control.yaml b/franka_control/config/panda_gazebo_control.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2039c945c3d6b6967dd0686658c4a8be68476ca8
--- /dev/null
+++ b/franka_control/config/panda_gazebo_control.yaml
@@ -0,0 +1,40 @@
+joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+position_joint_trajectory_controller:
+    type: effort_controllers/JointTrajectoryController
+    joints:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
+
+    gains:
+        panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+        panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+        panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+        panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+        panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+        panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+        panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
+
+    constraints:
+        goal_time: 2.0
+
+    state_publish_rate: 25
+
+franka_gripper:
+    type: effort_controllers/JointTrajectoryController
+    joints:
+        - panda_finger_joint1
+        - panda_finger_joint2
+
+    gains:
+        panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
+        panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
+
+    state_publish_rate: 25
diff --git a/franka_description/CMakeLists.txt b/franka_description/CMakeLists.txt
index f0de05cbda6a4541a7783ef12f505d5c416d6671..7282c39947f348162715e7e829a4aa5eb0f6e2b9 100644
--- a/franka_description/CMakeLists.txt
+++ b/franka_description/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.4)
+cmake_minimum_required(VERSION 2.8.3)
 project(franka_description)
 
 find_package(catkin REQUIRED)
diff --git a/franka_description/meshes/collision/finger.stl b/franka_description/meshes/collision/finger.stl
new file mode 100644
index 0000000000000000000000000000000000000000..3b87289fea8128bcec3e0b4d174b169124e8e444
Binary files /dev/null and b/franka_description/meshes/collision/finger.stl differ
diff --git a/franka_description/meshes/collision/hand.stl b/franka_description/meshes/collision/hand.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4e820902eba7b9d959a2e0cc8091f4b0f09ed77a
Binary files /dev/null and b/franka_description/meshes/collision/hand.stl differ
diff --git a/franka_description/meshes/collision/link0.stl b/franka_description/meshes/collision/link0.stl
new file mode 100644
index 0000000000000000000000000000000000000000..def070c7077c0ddb33bbe16cd6c75f19dd318734
Binary files /dev/null and b/franka_description/meshes/collision/link0.stl differ
diff --git a/franka_description/meshes/collision/link1.stl b/franka_description/meshes/collision/link1.stl
new file mode 100644
index 0000000000000000000000000000000000000000..426bcf2d7a04e067e01ab198d0ccfef63c6846e8
Binary files /dev/null and b/franka_description/meshes/collision/link1.stl differ
diff --git a/franka_description/meshes/collision/link2.stl b/franka_description/meshes/collision/link2.stl
new file mode 100644
index 0000000000000000000000000000000000000000..b369f1599a3c1356611716621f998bd8b5a8863b
Binary files /dev/null and b/franka_description/meshes/collision/link2.stl differ
diff --git a/franka_description/meshes/collision/link3.stl b/franka_description/meshes/collision/link3.stl
new file mode 100644
index 0000000000000000000000000000000000000000..25162eeedf286d1e27fdd4ba38950ae90678bb0f
Binary files /dev/null and b/franka_description/meshes/collision/link3.stl differ
diff --git a/franka_description/meshes/collision/link4.stl b/franka_description/meshes/collision/link4.stl
new file mode 100644
index 0000000000000000000000000000000000000000..76c8c33c3e1e6c184f8c3693b390892c25b179e3
Binary files /dev/null and b/franka_description/meshes/collision/link4.stl differ
diff --git a/franka_description/meshes/collision/link5.stl b/franka_description/meshes/collision/link5.stl
new file mode 100644
index 0000000000000000000000000000000000000000..3006a0b9a695f020e1887128d805d15aaa7fd342
Binary files /dev/null and b/franka_description/meshes/collision/link5.stl differ
diff --git a/franka_description/meshes/collision/link6.stl b/franka_description/meshes/collision/link6.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2e9594a873f97e572ec68a4e0ab6d65f41f5007e
Binary files /dev/null and b/franka_description/meshes/collision/link6.stl differ
diff --git a/franka_description/meshes/collision/link7.stl b/franka_description/meshes/collision/link7.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0532d057507637533d25f4b3ed451f213685a61d
Binary files /dev/null and b/franka_description/meshes/collision/link7.stl differ
diff --git a/franka_description/meshes/visual/finger.dae b/franka_description/meshes/visual/finger.dae
index 8d249b9751ca2a5abf80fe1fcd1a1b951c481ba8..e09f9b80e08696a1a531b15ca44179dce77c1f20 100644
--- a/franka_description/meshes/visual/finger.dae
+++ b/franka_description/meshes/visual/finger.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/hand.dae b/franka_description/meshes/visual/hand.dae
index 454bfcddd36f81a720ba9793b1998bfd8561ff7a..990b1f587a19bd8522cc9e3aa832e99245e548a7 100644
--- a/franka_description/meshes/visual/hand.dae
+++ b/franka_description/meshes/visual/hand.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -154,7 +150,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link0.dae b/franka_description/meshes/visual/link0.dae
index 9d65e4420b46c4498ca5c7d6390120e01443882b..573bd8e628af1468d05072afcdbf807a5d52f450 100644
--- a/franka_description/meshes/visual/link0.dae
+++ b/franka_description/meshes/visual/link0.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -154,7 +150,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -184,7 +179,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -214,7 +208,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -244,7 +237,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -274,7 +266,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -304,7 +295,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -334,7 +324,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -364,7 +353,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link1.dae b/franka_description/meshes/visual/link1.dae
index 47e5b4215f5c9d334b4ba741b571d4aabf2fc404..f55e7e0651c83b0bb73234c7cb509d294c4c8010 100644
--- a/franka_description/meshes/visual/link1.dae
+++ b/franka_description/meshes/visual/link1.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link2.dae b/franka_description/meshes/visual/link2.dae
index 6d5788c831cfb7f6e0fa2ebe0df188673f980ce4..c017aa31c3fb812e0d01b641858a55b04cdc0e10 100644
--- a/franka_description/meshes/visual/link2.dae
+++ b/franka_description/meshes/visual/link2.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link3.dae b/franka_description/meshes/visual/link3.dae
index 895e67be2d17a44d553a1290e9e5fcd89f4f82d1..0a21ad5cc361522f0df1971b841e4a1200b48885 100644
--- a/franka_description/meshes/visual/link3.dae
+++ b/franka_description/meshes/visual/link3.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link4.dae b/franka_description/meshes/visual/link4.dae
index 1d47af8e04af59e4420e5f59f0deaaa3feb4d72f..dce08f85dca332d18ff11177867c245ec2ae3987 100644
--- a/franka_description/meshes/visual/link4.dae
+++ b/franka_description/meshes/visual/link4.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link5.dae b/franka_description/meshes/visual/link5.dae
index b4370bb6401bc1c4b57b5bd2ab9ee89436757110..c8f4f1118f04e31d73926a7038af60d96d3ac34b 100644
--- a/franka_description/meshes/visual/link5.dae
+++ b/franka_description/meshes/visual/link5.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link6.dae b/franka_description/meshes/visual/link6.dae
index 74a80bff2151dbc161238b9e86ab0c1f7d59043c..484159bfbcd06a84600490459210254d42479007 100644
--- a/franka_description/meshes/visual/link6.dae
+++ b/franka_description/meshes/visual/link6.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -154,7 +150,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -184,7 +179,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -214,7 +208,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -244,7 +237,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -274,7 +266,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -304,7 +295,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -334,7 +324,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -364,7 +353,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -394,7 +382,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -424,7 +411,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -454,7 +440,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -484,7 +469,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -514,7 +498,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/meshes/visual/link7.dae b/franka_description/meshes/visual/link7.dae
index e43db9037bafa660557ee193fef8c1d217acb01b..4abeee6bdbd31e5bd30d916957876b16db38f530 100644
--- a/franka_description/meshes/visual/link7.dae
+++ b/franka_description/meshes/visual/link7.dae
@@ -34,7 +34,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -64,7 +63,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -94,7 +92,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -124,7 +121,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -154,7 +150,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -184,7 +179,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -214,7 +208,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
@@ -244,7 +237,6 @@
             <transparent opaque="A_ONE">
               <color>1 1 1 1</color>
             </transparent>
-            <transparency><float>1</float></transparency>
             <index_of_refraction>
               <float sid="index_of_refraction">1</float>
             </index_of_refraction>
diff --git a/franka_description/robots/hand.urdf.xacro b/franka_description/robots/hand.urdf.xacro
index a50b18ce2195f40e373a97403ab7fc55ab962314..643fc608f05f2bd79f0212e01e1a01086d02bd57 100644
--- 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" safety_distance="0.03"/>
+  <xacro:hand ns="panda"/>
 </robot>
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 22e6a0876a47ca561f717f58217ebc49fd8df203..63e95d1c0a38aabad79d914f6f99277e5620f003 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -1,7 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
-  <!-- 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:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' ">
     <xacro:unless value="${connected_to == ''}">
       <joint name="${ns}_hand_joint" type="fixed">
         <parent link="${connected_to}"/>
@@ -16,41 +15,18 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
         <geometry>
-          <cylinder radius="${0.04+safety_distance}" length="0.1" />
-        </geometry>
-      </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.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>
-          <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}"  />
+          <mesh filename="package://franka_description/meshes/collision/hand.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </link>
     <link name="${ns}_leftfinger">
       <visual>
@@ -58,6 +34,19 @@
           <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
+        </geometry>
+      </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </link>
     <link name="${ns}_rightfinger">
       <visual>
@@ -66,7 +55,21 @@
           <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
-   </link>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/finger.stl"/>
+        </geometry>
+      </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
+    </link>
     <joint name="${ns}_finger_joint1" type="prismatic">
       <parent link="${ns}_hand"/>
       <child link="${ns}_leftfinger"/>
@@ -80,7 +83,6 @@
       <origin xyz="0 0 0.0584" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
       <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
-      <mimic joint="${ns}_finger_joint1" />
     </joint>
   </xacro:macro>
 </robot>
diff --git a/franka_description/robots/panda.gazebo.xacro b/franka_description/robots/panda.gazebo.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..02822caa5688d544fb76b743dcfd9b7e6685facd
--- /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 0000000000000000000000000000000000000000..f29313877d62e86a3f95779428c1c38783643a92
--- /dev/null
+++ b/franka_description/robots/panda.transmission.xacro
@@ -0,0 +1,114 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="panda_transmission" params="robot_name load_hand">
+
+
+        <!-- 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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_1">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_2">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_3">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_4">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_5">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_6">
+                <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${robot_name}_motor_7">
+                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <xacro:if value="${load_hand}">
+            <transmission name="${robot_name}_leftfinger">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="${robot_name}_finger_joint1">
+                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="${robot_name}_finger_joint1">
+                    <hardwareInterface>hardware_interface/PositionJointInterface</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/PositionJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="${robot_name}_finger_joint2">
+                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                    <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+            </transmission>
+        </xacro:if>
+    </xacro:macro>
+</robot>
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index 31e7a2c98393f395ea70c3cf68ca158c40bba08c..2b247b62fc62a2375ff05c376b1315bbcd14a96c 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,5 +1,18 @@
 <?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 safety_distance="0.03"/>
+
+  <!-- for simulation -->
+  <xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
+  <xacro:include filename="panda.transmission.xacro"/>
+  <!-- end for simulation -->
+
+  <xacro:panda_arm />
+
+  <!-- for simulation -->
+  <xacro:arg name="robot_name" default="panda"/>
+  <xacro:panda_gazebo robot_name="$(arg robot_name)" />
+  <xacro:panda_transmission robot_name="$(arg robot_name)" load_hand="false" />
+  <!-- end for simulation -->
+
 </robot>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index b44b230a6f194588e914f541fe2f311e7cbbd1d3..011b72e77452a50d9358b2d157ef2de827ff75ce 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -1,8 +1,6 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
-  <!-- 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:macro name="panda_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0'">
     <xacro:unless value="${not connected_to}">
       <joint name="${arm_id}_joint_${connected_to}" type="fixed">
         <parent link="${connected_to}"/>
@@ -10,6 +8,19 @@
         <origin rpy="${rpy}" xyz="${xyz}"/>
       </joint>
     </xacro:unless>
+
+    <!-- for simulation -->
+    <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" />
+    </joint>
+
+    <xacro:property name="joint_damping" value="1.0"/>
+    <!-- end for simulation -->
+
     <link name="${arm_id}_link0">
       <visual>
         <geometry>
@@ -17,23 +28,18 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
-        <geometry>
-          <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}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link0.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </link>
     <link name="${arm_id}_link1">
       <visual>
@@ -42,23 +48,18 @@
         </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>
-          <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}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link1.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -67,6 +68,10 @@
       <child link="${arm_id}_link1"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link2">
       <visual>
@@ -75,23 +80,18 @@
         </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>
-          <sphere radius="${0.06+safety_distance}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link2.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -100,6 +100,10 @@
       <child link="${arm_id}_link2"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link3">
       <visual>
@@ -108,23 +112,18 @@
         </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>
-          <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}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link3.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -133,6 +132,10 @@
       <child link="${arm_id}_link3"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link4">
       <visual>
@@ -141,23 +144,18 @@
         </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>
-          <sphere radius="${0.06+safety_distance}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link4.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -166,6 +164,10 @@
       <child link="${arm_id}_link4"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link5">
       <visual>
@@ -174,42 +176,18 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0 0 -0.26" rpy="0 0 0"/>
         <geometry>
-          <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}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link5.stl"/>
         </geometry>
       </collision>
 
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -218,6 +196,10 @@
       <child link="${arm_id}_link5"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link6">
       <visual>
@@ -226,23 +208,18 @@
         </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>
-          <sphere radius="${0.05+safety_distance}" />
+          <mesh filename="package://${description_pkg}/meshes/collision/link6.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -251,6 +228,10 @@
       <child link="${arm_id}_link6"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <link name="${arm_id}_link7">
       <visual>
@@ -259,23 +240,18 @@
         </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>
-          <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}"  />
+          <mesh filename="package://${description_pkg}/meshes/collision/link7.stl"/>
         </geometry>
       </collision>
+
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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"/>
@@ -284,31 +260,25 @@
       <child link="${arm_id}_link7"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+
+      <!-- for simulation -->
+      <dynamics damping="${joint_damping}"/>
+      <!-- end for simulation -->
     </joint>
     <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>
+      <!-- for simulation -->
+      <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>
+      <!-- end for simulation -->
     </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
index b98feb65e1b5bb71baac6bcaea23949903cad514..65ac6b9fa17f55ffe5c22b90f4547293c575757a 100644
--- a/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -2,6 +2,18 @@
 <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 safety_distance="0.03"/>
-  <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.03"/>
+
+  <!-- for simulation -->
+  <xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
+  <xacro:include filename="panda.transmission.xacro"/>
+  <!-- end for simulation -->
+
+  <xacro:panda_arm />
+  <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+
+  <!-- for simulation -->
+  <xacro:arg name="robot_name" default="panda"/>
+  <xacro:panda_gazebo robot_name="$(arg robot_name)" />
+  <xacro:panda_transmission robot_name="$(arg robot_name)" load_hand="true" />
+  <!-- end for simulation -->
 </robot>