diff --git a/pal_tiago/tiago.xml b/pal_tiago/tiago.xml
index c96cba4b..136a97c2 100644
--- a/pal_tiago/tiago.xml
+++ b/pal_tiago/tiago.xml
@@ -31,183 +31,173 @@
     <mesh file="arm/arm_5-wrist-2017.stl"/>
     <mesh file="arm/arm_6-wrist-2017.stl"/>
     <mesh file="gripper_link.stl"/>
-    <mesh file="gripper_finger_link1.stl" scale="-1 -1 1"/>
-    <mesh file="gripper_finger_link.stl"/>
+    <mesh file="gripper_finger_link.stl" scale="-1 -1 1"/>
+    <mesh file="gripper_finger_link1.stl"/>
   </asset>
 
-  <default class="main">
-    <geom contype="0" conaffinity="1" group="1"/>
+  <default>
+    <geom contype="0" conaffinity="1" group="1" type="mesh" mass="0"/>
+    <joint axis="0 0 1"/>
     <default class="arm">
       <geom contype="1" conaffinity="1"/>
+      <joint frictionloss="1"/>
+    </default>
+    <default class="wheel">
+      <geom condim="6"/>
     </default>
   </default>
 
   <worldbody>
     <body name="base_link">
       <joint name="reference" type="free"/>
-      <geom pos="0 0 0.0985" type="mesh" density="0" material="white" mesh="base"/>
-      <geom pos="0.202 0 0.0945" type="mesh" density="0" material="gray10" mesh="sick_tim551"/>
-      <geom pos="-0.183 0.183 0.2715" quat="0.382683 0 0 0.92388" type="mesh" density="0" material="gray10"
-        mesh="srf05"/>
-      <geom pos="-0.259 0 0.2715" quat="0 0 0 1" type="mesh" density="0" material="gray10" mesh="srf05"/>
-      <geom pos="-0.183 -0.183 0.2715" quat="-0.382683 0 0 0.92388" type="mesh" density="0" material="gray10"
-        mesh="srf05"/>
-      <geom pos="-0.201 0.1062 0.2935" type="mesh" density="0" material="black" mesh="antenna"/>
-      <geom pos="-0.201 -0.1062 0.2935" type="mesh" density="0" material="black" mesh="antenna"/>
-      <geom pos="-0.062 0 0.2915" type="mesh" density="0" material="mercury" mesh="torso_fix"/>
-      <geom size="0.09 0.1 0.275" pos="-0.062 0 0.5685" type="box" density="0" material="gray10"/>
-      <geom pos="0 0 0.13" type="mesh" density="0" material="orange" mesh="base_ring"/>
+      <geom pos="0 0 0.0985" material="white" mesh="base"/>
+      <geom pos="0.202 0 0.0945" material="gray10" mesh="sick_tim551"/>
+      <geom pos="-0.183 0.183 0.2715" quat="0.382683 0 0 0.92388" material="gray10" mesh="srf05"/>
+      <geom pos="-0.259 0 0.2715" quat="0 0 0 1" material="gray10" mesh="srf05"/>
+      <geom pos="-0.183 -0.183 0.2715" quat="-0.382683 0 0 0.92388" material="gray10" mesh="srf05"/>
+      <geom pos="-0.201 0.1062 0.2935" material="black" mesh="antenna"/>
+      <geom pos="-0.201 -0.1062 0.2935" material="black" mesh="antenna"/>
+      <geom pos="-0.062 0 0.2915" material="mercury" mesh="torso_fix"/>
+      <geom size="0.09 0.1 0.275" pos="-0.062 0 0.5685" type="box" material="gray10"/>
+      <geom pos="0 0 0.13" material="orange" mesh="base_ring"/>
       <!-- base link inertial manually added from urdf -->
       <!-- <mass value="28.26649"/>
-      <inertia ixx="0.465408937" ixy="0.002160024" ixz="-0.001760255" iyy="0.483193291" iyz="-0.000655952"
+      <inertia ixx="0.465408937" ixy="0.002160024" ixz="-0.001760255" iyy="0.483193291"
+      iyz="-0.000655952"
       izz="0.550939703"/> -->
       <inertial pos="0 0 0" mass="28.26649"
         fullinertia="0.465408937 0.483193291 0.550939703 0.002160024 -0.001760255 -0.000655952"/>
-      <body name="suspension_right_link" pos="0 0 0.0985">
+      <body name="suspension_right_link" pos="0 0 0.0985" childclass="wheel">
         <inertial pos="0 0 -0.02" mass="10" diaginertia="1 1 1"/>
-        <joint name="suspension_right_joint" axis="0 0 1" type="slide" range="-0.005 0.005" damping="100"/>
+        <joint name="suspension_right_joint" type="slide" range="-0.005 0.005" damping="100"/>
         <body name="wheel_right_link" pos="0 -0.2022 0" quat="1 -1 0 0">
           <inertial pos="0 0 -0.00207" quat="0.296456 0.641958 -0.296458 0.641963" mass="1.82362"
             diaginertia="0.0083924 0.00499747 0.00499738"/>
-          <joint name="wheel_right_joint" axis="0 0 1"/>
-          <geom type="mesh" density="0" material="gray10" mesh="wheel" condim="6"/>
+          <joint name="wheel_right_joint"/>
+          <geom material="gray10" mesh="wheel"/>
         </body>
       </body>
-      <body name="suspension_left_link" pos="0 0 0.0985">
+      <body name="suspension_left_link" pos="0 0 0.0985" childclass="wheel">
         <inertial pos="0 0 -0.02" mass="10" diaginertia="1 1 1"/>
-        <joint name="suspension_left_joint" axis="0 0 1" type="slide" range="-0.005 0.005" damping="100"/>
+        <joint name="suspension_left_joint" type="slide" range="-0.005 0.005" damping="100"/>
         <body name="wheel_left_link" pos="0 0.2022 0" quat="1 -1 0 0">
           <inertial pos="0 0 0.00207" quat="0.641958 0.296456 -0.641963 0.296458" mass="1.82362"
             diaginertia="0.0083924 0.00499747 0.00499738"/>
-          <joint name="wheel_left_joint" axis="0 0 1"/>
-          <geom quat="0 -1 0 0" type="mesh" density="0" material="gray10" mesh="wheel" condim="6"/>
+          <joint name="wheel_left_joint"/>
+          <geom quat="0 -1 0 0" material="gray10" mesh="wheel"/>
         </body>
       </body>
-      <body name="caster_front_right_1_link" pos="0.1695 -0.102 0.065">
+      <body name="caster_front_right_1_link" pos="0.1695 -0.102 0.065" childclass="wheel">
         <inertial pos="-0.00634599 5.347e-05 -0.0203752" quat="0.966491 -0.00340536 -0.256544 -0.00828025"
           mass="0.0514476" diaginertia="1.66556e-05 1.30896e-05 1.04548e-05"/>
-        <joint name="caster_front_right_1_joint" axis="0 0 1" damping="0.5"/>
-        <geom type="mesh" density="0" material="gray70" mesh="caster_1" condim="6"/>
+        <joint name="caster_front_right_1_joint" damping="0.5"/>
+        <geom material="gray70" mesh="caster_1"/>
         <body name="caster_front_right_2_link" pos="-0.016 0 -0.04" quat="1 -1 0 0">
-          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578"
-            diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
-          <joint name="caster_front_right_2_joint" axis="0 0 1"/>
-          <geom type="mesh" density="0" material="gray10" mesh="caster_2" condim="6"/>
+          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578" diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
+          <joint name="caster_front_right_2_joint"/>
+          <geom material="gray10" mesh="caster_2"/>
         </body>
       </body>
-      <body name="caster_front_left_1_link" pos="0.1695 0.102 0.065">
+      <body name="caster_front_left_1_link" pos="0.1695 0.102 0.065" childclass="wheel">
         <inertial pos="-0.00634599 5.347e-05 -0.0203752" quat="0.966491 -0.00340536 -0.256544 -0.00828025"
           mass="0.0514476" diaginertia="1.66556e-05 1.30896e-05 1.04548e-05"/>
-        <joint name="caster_front_left_1_joint" axis="0 0 1" damping="0.5"/>
-        <geom type="mesh" density="0" material="gray70" mesh="caster_1" condim="6"/>
+        <joint name="caster_front_left_1_joint" damping="0.5"/>
+        <geom material="gray70" mesh="caster_1"/>
         <body name="caster_front_left_2_link" pos="-0.016 0 -0.04" quat="1 -1 0 0">
-          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578"
-            diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
-          <joint name="caster_front_left_2_joint" axis="0 0 1"/>
-          <geom type="mesh" density="0" material="gray10" mesh="caster_2" condim="6"/>
+          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578" diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
+          <joint name="caster_front_left_2_joint"/>
+          <geom material="gray10" mesh="caster_2"/>
         </body>
       </body>
-      <body name="caster_back_right_1_link" pos="-0.1735 -0.102 0.065">
+      <body name="caster_back_right_1_link" pos="-0.1735 -0.102 0.065" childclass="wheel">
         <inertial pos="-0.00634599 5.347e-05 -0.0203752" quat="0.966491 -0.00340536 -0.256544 -0.00828025"
           mass="0.0514476" diaginertia="1.66556e-05 1.30896e-05 1.04548e-05"/>
-        <joint name="caster_back_right_1_joint" axis="0 0 1" damping="0.5"/>
-        <geom type="mesh" density="0" material="gray70" mesh="caster_1" condim="6"/>
+        <joint name="caster_back_right_1_joint" damping="0.5"/>
+        <geom material="gray70" mesh="caster_1"/>
         <body name="caster_back_right_2_link" pos="-0.016 0 -0.04" quat="1 -1 0 0">
-          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578"
-            diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
-          <joint name="caster_back_right_2_joint" axis="0 0 1"/>
-          <geom type="mesh" density="0" material="gray10" mesh="caster_2" condim="6"/>
+          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578" diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
+          <joint name="caster_back_right_2_joint"/>
+          <geom material="gray10" mesh="caster_2"/>
         </body>
       </body>
-      <body name="caster_back_left_1_link" pos="-0.1735 0.102 0.065">
+      <body name="caster_back_left_1_link" pos="-0.1735 0.102 0.065" childclass="wheel">
         <inertial pos="-0.00634599 5.347e-05 -0.0203752" quat="0.966491 -0.00340536 -0.256544 -0.00828025"
           mass="0.0514476" diaginertia="1.66556e-05 1.30896e-05 1.04548e-05"/>
-        <joint name="caster_back_left_1_joint" axis="0 0 1" damping="0.5"/>
-        <geom type="mesh" density="0" material="gray70" mesh="caster_1" condim="6"/>
+        <joint name="caster_back_left_1_joint" damping="0.5"/>
+        <geom material="gray70" mesh="caster_1"/>
         <body name="caster_back_left_2_link" pos="-0.016 0 -0.04" quat="1 -1 0 0">
-          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578"
-            diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
-          <joint name="caster_back_left_2_joint" axis="0 0 1"/>
-          <geom type="mesh" density="0" material="gray10" mesh="caster_2" condim="6"/>
+          <inertial pos="0 0 0" quat="1 0 0 1" mass="0.0885578" diaginertia="1.541e-05 1.075e-05 1.075e-05"/>
+          <joint name="caster_back_left_2_joint"/>
+          <geom material="gray10" mesh="caster_2"/>
         </body>
       </body>
       <body name="torso_lift_link" pos="-0.062 0 0.8885">
         <inertial pos="0.04361 -0.00116 -0.17334" quat="0.562536 0.0400112 -0.00365739 0.825796" mass="6.93198"
           diaginertia="0.250465 0.244703 0.0893652"/>
-        <joint name="torso_lift_joint" axis="0 0 1" type="slide" range="0 0.35" damping="2000"/>
-        <geom type="mesh" density="0" material="white" mesh="torso_lift_with_arm"/>
+        <joint name="torso_lift_joint" type="slide" range="0 0.35" damping="2000"/>
+        <geom material="white" mesh="torso_lift_with_arm"/>
         <body name="head_1_link" pos="0.182 0 0">
           <inertial pos="-0.00508 0.00237 0.07229" quat="0.688669 -0.0973979 -0.101834 0.711251" mass="0.6222"
             diaginertia="0.00140223 0.00121968 0.000861083"/>
-          <joint name="head_1_joint" axis="0 0 1" range="-1.309 1.309" damping="0.5" frictionloss="1"/>
-          <geom type="mesh" density="0" material="white" mesh="head_1"/>
+          <joint name="head_1_joint" range="-1.309 1.309" damping="0.5"/>
+          <geom material="white" mesh="head_1"/>
           <body name="head_2_link" pos="0.005 0 0.098" quat="1 1 0 0">
             <inertial pos="0.0558274 0.0609987 1.58542e-05" quat="0.89114 -0.000495756 -0.00420876 0.453708"
               mass="0.88693" diaginertia="0.00554181 0.00436261 0.00347586"/>
-            <joint name="head_2_joint" axis="0 0 1" range="-1.0472 0.785398" damping="0.5" frictionloss="1"/>
-            <geom type="mesh" density="0" material="white" mesh="head_2"/>
+            <joint name="head_2_joint" range="-1.0472 0.785398" damping="0.5"/>
+            <geom material="white" mesh="head_2"/>
           </body>
         </body>
         <body name="arm_1_link" pos="0.15505 0.014 -0.151" quat="1 0 0 -1" childclass="arm">
           <inertial pos="0.061191 -0.022397 -0.012835" quat="0.161963 0.538347 0.345223 0.751513" mass="1.56343"
             diaginertia="0.00510233 0.00510233 0.00510233"/>
-          <joint name="arm_1_joint" axis="0 0 1" range="0 2.74889" damping="10" frictionloss="1"/>
-          <geom type="mesh" density="0" material="white" mesh="arm_1"/>
+          <joint name="arm_1_joint" range="0 2.74889" damping="10"/>
+          <geom material="white" mesh="arm_1"/>
           <body name="arm_2_link" pos="0.125 0.0195 -0.031" quat="1 1 0 0">
             <inertial pos="0.030432 0.000229 -0.005942" quat="0.126897 0.72859 0.136233 0.659161" mass="1.8004"
               diaginertia="0.00437229 0.00432701 0.0017837"/>
-            <joint name="arm_2_joint" axis="0 0 1" range="-1.5708 1.09083" damping="10" frictionloss="1"/>
-            <geom type="mesh" density="0" material="gray10" mesh="arm_2"/>
+            <joint name="arm_2_joint" range="-1.5708 1.09083" damping="10"/>
+            <geom material="gray10" mesh="arm_2"/>
             <body name="arm_3_link" pos="0.0895 0 -0.0015" quat="0.5 -0.5 -0.5 0.5">
               <inertial pos="0.007418 -0.004361 -0.134194" quat="0.516615 0.0556881 0.00536911 0.854388" mass="1.8"
                 diaginertia="0.0200771 0.0196154 0.00264853"/>
-              <joint name="arm_3_joint" axis="0 0 1" range="-3.53429 1.5708" damping="10" frictionloss="1"/>
-              <geom type="mesh" density="0" material="gray10" mesh="arm_3"/>
+              <joint name="arm_3_joint" range="-3.53429 1.5708" damping="10"/>
+              <geom material="gray10" mesh="arm_3"/>
               <body name="arm_4_link" pos="-0.02 -0.027 -0.222" quat="0.5 -0.5 -0.5 -0.5">
                 <inertial pos="-0.095658 0.014666 0.018133" quat="-0.161572 0.615556 -0.202229 0.744371" mass="1.4327"
                   diaginertia="0.00906004 0.00831328 0.00177669"/>
-                <joint name="arm_4_joint" axis="0 0 1" range="-0.392699 2.35619" damping="10"
-                  frictionloss="1"/>
-                <geom type="mesh" density="0" material="gray10" mesh="arm_4"/>
+                <joint name="arm_4_joint" range="-0.392699 2.35619" damping="10"/>
+                <geom material="gray10" mesh="arm_4"/>
                 <body name="arm_5_link" pos="-0.162 0.02 0.027" quat="1 0 -1 0">
                   <inertial pos="0.001078 -0.000151 -0.077173" quat="0.999999 -0.000362286 -0.00167006 0.000217394"
                     mass="0.935914" diaginertia="0.00103619 0.000794527 0.000439824"/>
-                  <joint name="arm_5_joint" axis="0 0 1" range="-2.0944 2.0944" damping="1" frictionloss="1"/>
-                  <geom type="mesh" density="0" material="mercury" mesh="arm_5-wrist-2017"/>
+                  <joint name="arm_5_joint" range="-2.0944 2.0944" damping="1"/>
+                  <geom material="mercury" mesh="arm_5-wrist-2017"/>
                   <body name="arm_6_link" pos="0 0 0.15" quat="0.5 -0.5 -0.5 -0.5">
                     <inertial pos="-0.000153 -0.003122 0.000183" quat="0.510936 0.49453 0.508164 0.485956"
                       mass="0.302758" diaginertia="3.85419e-05 3.33205e-05 3.29226e-05"/>
-                    <joint name="arm_6_joint" axis="0 0 1" range="-1.41372 1.41372" damping="1"
-                      frictionloss="1"/>
-                    <geom type="mesh" density="0" material="mercury" mesh="arm_6-wrist-2017"/>
+                    <joint name="arm_6_joint" range="-1.41372 1.41372" damping="1"/>
+                    <geom material="mercury" mesh="arm_6-wrist-2017"/>
                     <body name="arm_7_link" quat="0.5 0.5 0.5 0.5">
                       <inertial pos="-0.000173894 0.000176395 0.0817355"
                         quat="0.997803 -0.00177536 -0.00128747 0.0662101" mass="1.00276"
                         diaginertia="0.0025841 0.0022568 0.000746434"/>
-                      <joint name="arm_7_joint" axis="0 0 1" range="-2.0944 2.0944" damping="1"
-                        frictionloss="1"/>
-                      <geom size="0.005 0.0025" pos="0 0 0.047" quat="-1 0 0 1" type="cylinder"
-                        density="0" material="mercury"/>
-                      <geom size="0.0225 0.00785" pos="0 0 0.05385" quat="-1 0 0 0" type="cylinder" density="0"
-                        material="mercury"/>
-                      <geom size="0.025 0.004875" pos="0 0 0.066575" quat="-1 0 0 1" type="cylinder"
-                        density="0" material="gray10"/>
-                      <geom pos="0 0 0.076575" quat="0 -1 1 0" type="mesh" density="0" material="white"
-                        mesh="gripper_link"/>
+                      <joint name="arm_7_joint" range="-2.0944 2.0944" damping="1"/>
+                      <geom size="0.005 0.0025" pos="0 0 0.047" quat="-1 0 0 1" type="cylinder" material="mercury"/>
+                      <geom size="0.0225 0.00785" pos="0 0 0.05385" quat="-1 0 0 0" type="cylinder" material="mercury"/>
+                      <geom size="0.025 0.004875" pos="0 0 0.066575" quat="-1 0 0 1" type="cylinder" material="gray10"/>
+                      <geom pos="0 0 0.076575" quat="0 -1 1 0" material="white" mesh="gripper_link"/>
                       <body name="gripper_right_finger_link" pos="0 0 0.076575" quat="0 -1 1 0">
                         <inertial pos="-0.00447 0.010766 -0.121013" quat="0.944415 0.0576483 0.0941322 -0.309671"
                           mass="0.10998" diaginertia="0.000161667 0.000161667 0.000161667"/>
-                        <joint name="gripper_right_finger_joint" axis="1 0 0" type="slide" range="0 0.045"
-                          damping="1"/>
-                        <geom quat="4.63268e-05 0 0 1" type="mesh" density="0" material="black" mesh="gripper_finger_link"/>
+                        <joint name="gripper_right_finger_joint" axis="1 0 0" type="slide" range="0 0.045" damping="1"/>
+                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_finger_link"/>
                       </body>
                       <body name="gripper_left_finger_link" pos="0 0 0.076575" quat="0 -1 1 0">
                         <inertial pos="-0.00447 0.010766 -0.121013" quat="0.944415 0.0576483 0.0941322 -0.309671"
                           mass="0.10998" diaginertia="0.000161667 0.000161667 0.000161667"/>
-                        <joint name="gripper_left_finger_joint" axis="-1 0 0" type="slide" range="0 0.045"
-                          damping="1"/>
-                        <geom quat="4.63268e-05 0 0 1" type="mesh" density="0" material="black"
-                          mesh="gripper_finger_link1"/>
+                        <joint name="gripper_left_finger_joint" axis="-1 0 0" type="slide" range="0 0.045" damping="1"/>
+                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_finger_link1"/>
                       </body>
                     </body>
                   </body>
@@ -222,8 +212,8 @@
 
   <!-- Added collision exclude to avoid unwanted contact forces between robot bodies -->
   <contact>
-    <exclude body1="arm_2_link" body2="torso_lift_link" />
-    <exclude body1="gripper_right_finger_link" body2="gripper_left_finger_link" />
+    <exclude body1="arm_2_link" body2="torso_lift_link"/>
+    <exclude body1="gripper_right_finger_link" body2="gripper_left_finger_link"/>
   </contact>
 
 </mujoco>
diff --git a/pal_tiago/tiago_motor.xml b/pal_tiago/tiago_motor.xml
index a87f81d2..05307ab6 100644
--- a/pal_tiago/tiago_motor.xml
+++ b/pal_tiago/tiago_motor.xml
@@ -12,11 +12,14 @@
     <motor name="arm_6_joint_motor" joint="arm_6_joint" forcerange="-6.6 6.6"/>
     <motor name="arm_7_joint_motor" joint="arm_7_joint"/>
     <!-- End effector actuators -->
-    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="1000"/>
-    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="1000"/>
+    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
+    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
     <!-- Head actuators -->
     <position name="head_1_joint_position" joint="head_1_joint" ctrlrange="-1.309 1.309" kp="20"/>
-    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77" kp="20"/>
+    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77"
+      kp="20"/>
     <!-- Torso actuators -->
     <position name="torso_lift_joint_position" joint="torso_lift_joint" ctrlrange="0 0.35" kp="5000"/>
     <!-- Wheel actuators -->
diff --git a/pal_tiago/tiago_position.xml b/pal_tiago/tiago_position.xml
index 65b41b8f..c9f57f62 100644
--- a/pal_tiago/tiago_position.xml
+++ b/pal_tiago/tiago_position.xml
@@ -10,13 +10,16 @@
     <!-- Wrist actuators -->
     <position name="arm_position_5_servo" joint="arm_5_joint" forcerange="-3 3" ctrlrange="-2.0944 2.0944" kp="60"/>
     <position name="arm_position_6_servo" joint="arm_6_joint" forcerange="-6.6 6.6" ctrlrange="-1.41372 1.41372" kp="60"/>
-    <position name="arm_position_7_servo" joint="arm_7_joint"  ctrlrange="-2.0944 2.0944" kp="60"/>
+    <position name="arm_position_7_servo" joint="arm_7_joint" ctrlrange="-2.0944 2.0944" kp="60"/>
     <!-- End effector actuators -->
-    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="500"/>
-    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="500"/>
+    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
+    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
     <!-- Head actuators -->
     <position name="head_1_joint_position" joint="head_1_joint" ctrlrange="-1.309 1.309" kp="20"/>
-    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77" kp="20"/>
+    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77"
+      kp="20"/>
     <!-- Torso actuators -->
     <position name="torso_lift_joint_position" joint="torso_lift_joint" ctrlrange="0 0.35" kp="40000"/>
     <!-- Wheel actuators -->
@@ -25,8 +28,7 @@
   </actuator>
 
   <keyframe>
-    <key name="home"
-      ctrl="0.2 -1.34 -0.2 1.94 -1.57 1.37 0 0 0 0 0 0.15 0 0"
+    <key name="home" ctrl="0.2 -1.34 -0.2 1.94 -1.57 1.37 0 0 0 0 0 0.15 0 0"
       qpos="0 0 -0.985 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.15 0 0 0.20 -1.34 -0.20 1.94 -1.57 1.37 0.0 0 0"/>
   </keyframe>
 
diff --git a/pal_tiago/tiago_velocity.xml b/pal_tiago/tiago_velocity.xml
index 2a99b6f2..5d2f0d86 100644
--- a/pal_tiago/tiago_velocity.xml
+++ b/pal_tiago/tiago_velocity.xml
@@ -11,12 +11,15 @@
     <velocity name="arm_velocity_5_servo" joint="arm_5_joint" ctrlrange="-1.95 1.95" forcerange="-3 3" kv="2000"/>
     <velocity name="arm_velocity_6_servo" joint="arm_6_joint" ctrlrange="-1.76 1.76" forcerange="-6.6 6.6" kv="2000"/>
     <velocity name="arm_velocity_7_servo" joint="arm_7_joint" ctrlrange="-1.76 1.76" kv="2000"/>
-    <!-- End effector actuators --> 
-    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="150"/>
-    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045" forcerange="-64 64" kp="150"/>
+    <!-- End effector actuators -->
+    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
+    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" ctrlrange="0 0.045"
+      forcerange="-64 64" kp="1000"/>
     <!-- Head actuators -->
     <position name="head_1_joint_position" joint="head_1_joint" ctrlrange="-1.309 1.309" kp="20"/>
-    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77" kp="20"/>
+    <position name="head_2_joint_position" joint="head_2_joint" ctrlrange="-1.0472 0.785398" forcerange="-2.77 2.77"
+      kp="20"/>
     <!-- Torso actuators -->
     <position name="torso_lift_joint_position" joint="torso_lift_joint" ctrlrange="0 0.35" kp="40000"/>
     <!-- Wheel actuators -->
@@ -25,8 +28,7 @@
   </actuator>
 
   <keyframe>
-    <key name="home"
-      ctrl="0 0 0 0 0 0 0 0 0 0 0 0.15 0 0"
+    <key name="home" ctrl="0 0 0 0 0 0 0 0 0 0 0 0.15 0 0"
       qpos="0 0 -0.985 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.15 0 0 0.20 -1.34 -0.20 1.94 -1.57 1.37 0.0 0 0"/>
   </keyframe>
 
