diff --git a/robots/panda.transmission.xacro b/robots/panda.transmission.xacro
index f29313877d62e86a3f95779428c1c38783643a92..df24afabfb67ef85ec9201985c5773cca96d8a1e 100644
--- a/robots/panda.transmission.xacro
+++ b/robots/panda.transmission.xacro
@@ -91,10 +91,10 @@
             <transmission name="${robot_name}_leftfinger">
                 <type>transmission_interface/SimpleTransmission</type>
                 <joint name="${robot_name}_finger_joint1">
-                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                 </joint>
                 <actuator name="${robot_name}_finger_joint1">
-                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                     <mechanicalReduction>1</mechanicalReduction>
                 </actuator>
             </transmission>
@@ -102,10 +102,10 @@
             <transmission name="${robot_name}_rightfinger">
                 <type>transmission_interface/SimpleTransmission</type>
                 <joint name="${robot_name}_finger_joint2">
-                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                 </joint>
                 <actuator name="${robot_name}_finger_joint2">
-                    <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                     <mechanicalReduction>1</mechanicalReduction>
                 </actuator>
             </transmission>
diff --git a/robots/panda_arm_hand.urdf.xacro b/robots/panda_arm_hand.urdf.xacro
index 65ac6b9fa17f55ffe5c22b90f4547293c575757a..0ef8f6342c65064bec24bb63a9440c3e8ea3063e 100644
--- a/robots/panda_arm_hand.urdf.xacro
+++ b/robots/panda_arm_hand.urdf.xacro
@@ -16,4 +16,22 @@
   <xacro:panda_gazebo robot_name="$(arg robot_name)" />
   <xacro:panda_transmission robot_name="$(arg robot_name)" load_hand="true" />
   <!-- end for simulation -->
+
+<gazebo>
+        <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
+           <arm>
+               <arm_name>panda_arm</arm_name>
+               <palm_link> panda_link7  </palm_link>
+               <gripper_link> panda_rightfinger </gripper_link>
+              <gripper_link> panda_leftfinger </gripper_link>
+            </arm>
+            <forces_angle_tolerance>100</forces_angle_tolerance>
+            <update_rate>4</update_rate>
+            <grip_count_threshold>4</grip_count_threshold>
+            <max_grip_count>8</max_grip_count>
+            <release_tolerance>0.005</release_tolerance>
+            <disable_collisions_on_attach>false</disable_collisions_on_attach>
+            <contact_topic>__default_topic__</contact_topic>
+        </plugin>
+    </gazebo>
 </robot>