diff --git a/franka_control/config/panda_gazebo_control.yaml b/franka_control/config/panda_gazebo_control.yaml
new file mode 100644
index 00000000..2039c945
--- /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/meshes/collision/finger.stl b/franka_description/meshes/collision/finger.stl
new file mode 100644
index 00000000..3b87289f
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 00000000..4e820902
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 00000000..def070c7
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 00000000..426bcf2d
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 00000000..b369f159
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 00000000..25162eee
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 00000000..76c8c33c
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 00000000..3006a0b9
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 00000000..2e9594a8
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 00000000..0532d057
Binary files /dev/null and b/franka_description/meshes/collision/link7.stl differ
diff --git a/franka_description/robots/hand.urdf.xacro b/franka_description/robots/hand.urdf.xacro
index a50b18ce..ce58a2c6 100644
--- a/franka_description/robots/hand.urdf.xacro
+++ b/franka_description/robots/hand.urdf.xacro
@@ -1,5 +1,19 @@
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 22e6a087..79a644cf 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -1,7 +1,9 @@
+
-
+
@@ -15,42 +17,58 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -58,6 +76,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -66,6 +98,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -80,7 +127,9 @@
-
+
+
+
diff --git a/franka_description/robots/panda.control.xacro b/franka_description/robots/panda.control.xacro
new file mode 100644
index 00000000..b6088caf
--- /dev/null
+++ b/franka_description/robots/panda.control.xacro
@@ -0,0 +1,28 @@
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/franka_description/robots/panda.gazebo.xacro b/franka_description/robots/panda.gazebo.xacro
new file mode 100644
index 00000000..5849c288
--- /dev/null
+++ b/franka_description/robots/panda.gazebo.xacro
@@ -0,0 +1,33 @@
+
+
+
+
+
+ Gazebo/White
+ 0.2
+ 0.2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Grey
+ 0.2
+ 0.2
+
+
+
+
+
+
+
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index 31e7a2c9..37182247 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,5 +1,24 @@
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index b44b230a..5cb35218 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -1,314 +1,360 @@
+
-
+
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/franka_description/robots/panda_arm_hand.urdf.xacro b/franka_description/robots/panda_arm_hand.urdf.xacro
index b98feb65..0dfc64f0 100644
--- a/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -1,7 +1,5 @@
-
-
-
-
+
+