pal-robotics-forks · thomaspeyrucain · Sep 6, 2024 · Sep 6, 2024 · Sep 6, 2024 · Sep 6, 2024
diff --git a/pal_tiago/assets/arm/arm_1_collision.stl b/pal_tiago/assets/arm/arm_1_collision.stl
diff --git a/pal_tiago/assets/arm/arm_1_color.png b/pal_tiago/assets/arm/arm_1_color.png
diff --git a/pal_tiago/assets/arm/arm_1_normals.png b/pal_tiago/assets/arm/arm_1_normals.png
diff --git a/pal_tiago/assets/arm/arm_1_specular.png b/pal_tiago/assets/arm/arm_1_specular.png
diff --git a/pal_tiago/assets/arm/arm_2_collision.stl b/pal_tiago/assets/arm/arm_2_collision.stl
diff --git a/pal_tiago/assets/arm/arm_2_color.png b/pal_tiago/assets/arm/arm_2_color.png
diff --git a/pal_tiago/assets/arm/arm_2_normals.png b/pal_tiago/assets/arm/arm_2_normals.png
diff --git a/pal_tiago/assets/arm/arm_2_specular.png b/pal_tiago/assets/arm/arm_2_specular.png
diff --git a/pal_tiago/assets/arm/arm_3_collision.stl b/pal_tiago/assets/arm/arm_3_collision.stl
diff --git a/pal_tiago/assets/arm/arm_3_color.png b/pal_tiago/assets/arm/arm_3_color.png
diff --git a/pal_tiago/assets/arm/arm_3_normals.png b/pal_tiago/assets/arm/arm_3_normals.png
diff --git a/pal_tiago/assets/arm/arm_3_specular.png b/pal_tiago/assets/arm/arm_3_specular.png
diff --git a/pal_tiago/assets/arm/arm_4_collision.stl b/pal_tiago/assets/arm/arm_4_collision.stl
diff --git a/pal_tiago/assets/arm/arm_4_color.png b/pal_tiago/assets/arm/arm_4_color.png
diff --git a/pal_tiago/assets/arm/arm_4_normals.png b/pal_tiago/assets/arm/arm_4_normals.png
diff --git a/pal_tiago/assets/arm/arm_4_specular.png b/pal_tiago/assets/arm/arm_4_specular.png
diff --git a/pal_tiago/assets/arm/arm_5-wrist-2010.stl b/pal_tiago/assets/arm/arm_5-wrist-2010.stl
diff --git a/pal_tiago/assets/arm/arm_5_collision-wrist-2010.stl b/pal_tiago/assets/arm/arm_5_collision-wrist-2010.stl
diff --git a/pal_tiago/assets/arm/arm_5_collision-wrist-2017.stl b/pal_tiago/assets/arm/arm_5_collision-wrist-2017.stl
diff --git a/pal_tiago/assets/arm/arm_6-wrist-2010.stl b/pal_tiago/assets/arm/arm_6-wrist-2010.stl
diff --git a/pal_tiago/assets/arm/arm_6_collision-wrist-2010.stl b/pal_tiago/assets/arm/arm_6_collision-wrist-2010.stl
diff --git a/pal_tiago/assets/arm/arm_6_collision-wrist-2017.stl b/pal_tiago/assets/arm/arm_6_collision-wrist-2017.stl
diff --git a/pal_tiago/assets/arm/arm_6_color.png b/pal_tiago/assets/arm/arm_6_color.png
diff --git a/pal_tiago/assets/arm/arm_6_normals.png b/pal_tiago/assets/arm/arm_6_normals.png
diff --git a/pal_tiago/assets/arm/arm_6_specular.png b/pal_tiago/assets/arm/arm_6_specular.png
diff --git a/pal_tiago/assets/base/base_collision.stl b/pal_tiago/assets/base/base_collision.stl
diff --git a/pal_tiago/assets/base/high_resolution/base.stl b/pal_tiago/assets/base/high_resolution/base.stl
diff --git a/pal_tiago/assets/base/high_resolution/base_collision.stl b/pal_tiago/assets/base/high_resolution/base_collision.stl
diff --git a/pal_tiago/assets/cameras_add_on/cameras_add_on.stl b/pal_tiago/assets/cameras_add_on/cameras_add_on.stl
diff --git a/pal_tiago/assets/cameras_add_on/cameras_add_on_collision.stl b/pal_tiago/assets/cameras_add_on/cameras_add_on_collision.stl
diff --git a/pal_tiago/assets/dock/dock.stl b/pal_tiago/assets/dock/dock.stl
diff --git a/pal_tiago/assets/dock/dock_collision.stl b/pal_tiago/assets/dock/dock_collision.stl
diff --git a/pal_tiago/assets/finger_abd_collision.stl b/pal_tiago/assets/finger_abd_collision.stl
diff --git a/pal_tiago/assets/finger_flex_collision.stl b/pal_tiago/assets/finger_flex_collision.stl
diff --git a/pal_tiago/assets/finger_flex_tip_collision.stl b/pal_tiago/assets/finger_flex_tip_collision.stl
diff --git a/pal_tiago/assets/gripper_finger_link.stl → ...go/assets/gripper/gripper_finger_link.stl b/pal_tiago/assets/gripper_finger_link.stl → ...go/assets/gripper/gripper_finger_link.stl
diff --git a/pal_tiago/assets/gripper_link.stl → pal_tiago/assets/gripper/gripper_link.stl b/pal_tiago/assets/gripper_link.stl → pal_tiago/assets/gripper/gripper_link.stl
diff --git a/pal_tiago/assets/gripper_finger_link1.stl b/pal_tiago/assets/gripper_finger_link1.stl
diff --git a/pal_tiago/assets/gripper_finger_link_collision.stl b/pal_tiago/assets/gripper_finger_link_collision.stl
diff --git a/pal_tiago/assets/gripper_link_collision.stl b/pal_tiago/assets/gripper_link_collision.stl
diff --git a/pal_tiago/assets/head/head_1_collision.stl b/pal_tiago/assets/head/head_1_collision.stl
diff --git a/pal_tiago/assets/head/head_2_collision.stl b/pal_tiago/assets/head/head_2_collision.stl
diff --git a/pal_tiago/assets/objects/cover.stl b/pal_tiago/assets/objects/cover.stl
diff --git a/pal_tiago/assets/objects/high_resolution/cover.stl b/pal_tiago/assets/objects/high_resolution/cover.stl
diff --git a/pal_tiago/assets/palm.stl b/pal_tiago/assets/palm.stl
diff --git a/pal_tiago/assets/palm_collision.stl b/pal_tiago/assets/palm_collision.stl
diff --git a/pal_tiago/assets/sensors/bumper.stl b/pal_tiago/assets/sensors/bumper.stl
diff --git a/pal_tiago/assets/sensors/bumper_collision.stl b/pal_tiago/assets/sensors/bumper_collision.stl
diff --git a/pal_tiago/assets/sensors/elp_camera.stl b/pal_tiago/assets/sensors/elp_camera.stl
diff --git a/pal_tiago/assets/sensors/hokuyo_urg_04lx_ug01.stl b/pal_tiago/assets/sensors/hokuyo_urg_04lx_ug01.stl
diff --git a/pal_tiago/assets/sensors/structure_sensor.stl b/pal_tiago/assets/sensors/structure_sensor.stl
diff --git a/pal_tiago/assets/sensors/xtion_pro_live/xtion_pro_live.png b/pal_tiago/assets/sensors/xtion_pro_live/xtion_pro_live.png
diff --git a/pal_tiago/assets/thumb_abd_collision.stl b/pal_tiago/assets/thumb_abd_collision.stl
diff --git a/pal_tiago/assets/torso/torso_lift_collision.stl b/pal_tiago/assets/torso/torso_lift_collision.stl
diff --git a/pal_tiago/assets/torso/torso_lift_collision_core.stl b/pal_tiago/assets/torso/torso_lift_collision_core.stl
diff --git a/pal_tiago/assets/torso/torso_lift_collision_neck.stl b/pal_tiago/assets/torso/torso_lift_collision_neck.stl
diff --git a/pal_tiago/assets/torso/torso_lift_collision_shoulder.stl b/pal_tiago/assets/torso/torso_lift_collision_shoulder.stl
diff --git a/pal_tiago/assets/torso/torso_lift_without_arm.stl b/pal_tiago/assets/torso/torso_lift_without_arm.stl
diff --git a/pal_tiago/assets/wheels/high_resolution/caster_1.stl b/pal_tiago/assets/wheels/high_resolution/caster_1.stl
diff --git a/pal_tiago/assets/wheels/high_resolution/caster_2.stl b/pal_tiago/assets/wheels/high_resolution/caster_2.stl
diff --git a/pal_tiago/assets/wheels/high_resolution/wheel.stl b/pal_tiago/assets/wheels/high_resolution/wheel.stl
diff --git a/pal_tiago/tiago.xml b/pal_tiago/tiago.xml
@@ -30,9 +30,9 @@
     <mesh file="arm/arm_4.stl"/>
     <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_link.stl" scale="-1 -1 1"/>
-    <mesh file="gripper_finger_link1.stl"/>
+    <mesh file="gripper/gripper_link.stl"/>
+    <mesh name="gripper_right_finger_link" file="gripper/gripper_finger_link.stl" scale="-1 -1 1"/>
+    <mesh name="gripper_left_finger_link" file="gripper/gripper_finger_link.stl"/>
   </asset>
 
   <default>
@@ -63,12 +63,12 @@
       <inertial pos="-0.01 -0.002 0.131" mass="47.112" diaginertia="2.074 2.245 0.897"/>
       <body name="wheel_right_link" pos="0 -0.2022 0.0985" quat="1 -1 0 0">
         <inertial pos="0.0 0.0 0.0" mass="4.4" diaginertia="0.014 0.014 0.027"/>
-        <joint name="wheel_right_joint"/>
+        <joint name="wheel_right_joint" actuatorfrcrange="-100 100"/>
         <geom material="gray10" mesh="wheel"/>
       </body>
       <body name="wheel_left_link" pos="0 0.2022 0.0985" quat="1 -1 0 0">
         <inertial pos="0.0 0.0 0.0" mass="4.4" diaginertia="0.014 0.014 0.027"/>
-        <joint name="wheel_left_joint"/>
+        <joint name="wheel_left_joint" actuatorfrcrange="-100 100"/>
         <geom quat="0 -1 0 0" material="gray10" mesh="wheel"/>
       </body>
       <body name="caster_front_right_1_link" pos="0.1695 -0.102 0.065" childclass="wheel">
@@ -117,60 +117,60 @@
       </body>
       <body name="torso_lift_link" pos="-0.062 0 0.8885">
         <inertial pos="0.031711 -0.001004 -0.187315" mass="6.281737" diaginertia="0.230972 0.235034 0.080705"/>
-        <joint name="torso_lift_joint" type="slide" range="0 0.35" damping="2000"/>
+        <joint name="torso_lift_joint" type="slide" range="0 0.35" actuatorfrcrange="-500 500" 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" mass="0.6222" diaginertia="0.00140223 0.00121968 0.000861083"/>
-          <joint name="head_1_joint" range="-1.309 1.309" damping="0.5"/>
+          <joint name="head_1_joint" range="-1.309 1.309" actuatorfrcrange="-5.197 5.197" 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" mass="0.88693" diaginertia="0.00554181 0.00436261 0.00347586"/>
-            <joint name="head_2_joint" range="-1.0472 0.785398" damping="0.5"/>
+            <joint name="head_2_joint" range="-1.0472 0.785398" actuatorfrcrange="-5.197 5.197" damping="0.5"/>
             <geom material="white" mesh="head_2"/>
             <camera name="head_camera" pos="0.0908 0.08 0.0" mode="fixed" euler="0 -1.57 0"
                             fovy="45.5" resolution="640 480"/>
           </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" mass="1.56343" diaginertia="0.00510233 0.00510233 0.00510233"/>
-          <joint name="arm_1_joint" range="0 2.74889" damping="10"/>
+          <joint name="arm_1_joint" range="0 2.74889" actuatorfrcrange="-43 43" 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" mass="1.8004" diaginertia="0.00437229 0.00432701 0.0017837"/>
-            <joint name="arm_2_joint" range="-1.5708 1.09083" damping="10"/>
+            <joint name="arm_2_joint" range="-1.5708 1.09083" actuatorfrcrange="-43 43" 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" mass="1.8" diaginertia="0.0200771 0.0196154 0.00264853"/>
-              <joint name="arm_3_joint" range="-3.53429 1.5708" damping="10"/>
+              <joint name="arm_3_joint" range="-3.53429 1.5708" actuatorfrcrange="-26 26" 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" mass="1.4327" diaginertia="0.002040 0.008424 0.008686"/>
-                <joint name="arm_4_joint" range="-0.392699 2.35619" damping="10"/>
+                <joint name="arm_4_joint" range="-0.392699 2.35619" actuatorfrcrange="-26 26" 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" mass="0.935914" diaginertia="0.00103619 0.000794527 0.000439824"/>
-                  <joint name="arm_5_joint" range="-2.0944 2.0944" damping="1"/>
+                  <joint name="arm_5_joint" range="-2.0944 2.0944" actuatorfrcrange="-3 3" 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" mass="0.302758" diaginertia="3.85419e-05 3.33205e-05 3.29226e-05"/>
-                    <joint name="arm_6_joint" range="-1.41372 1.41372" damping="1"/>
+                    <joint name="arm_6_joint" range="-1.41372 1.41372" actuatorfrcrange="-6.6 6.6" 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" mass="1.00276" diaginertia="0.0025841 0.0022568 0.000746434"/>
-                      <joint name="arm_7_joint" range="-2.0944 2.0944" damping="1"/>
+                      <joint name="arm_7_joint" range="-2.0944 2.0944"  actuatorfrcrange="-6.6 6.6" 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.008658 0.019724 -0.149125" mass="0.107001" diaginertia="0.000242 0.000236 0.000018"/>
                         <joint name="gripper_right_finger_joint" axis="1 0 0" type="slide" range="0 0.045" damping="80" frictionloss="1.0"/>
-                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_finger_link"/>
+                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_right_finger_link"/>
                       </body>
                       <body name="gripper_left_finger_link" pos="0 0 0.076575" quat="0 -1 1 0">
                         <inertial pos="-0.008658 -0.019724 -0.149125" mass="0.107001" diaginertia="0.000242 0.000236 0.000018"/>
                         <joint name="gripper_left_finger_joint" axis="-1 0 0" type="slide" range="0 0.045" damping="80" frictionloss="1.0"/>
-                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_finger_link1"/>
+                        <geom quat="4.63268e-05 0 0 1" material="black" mesh="gripper_left_finger_link"/>
                       </body>
                     </body>
                   </body>

diff --git a/pal_tiago/tiago_motor.xml b/pal_tiago/tiago_motor.xml
@@ -18,13 +18,12 @@
       forcerange="-64 64" kp="1000"/>
     <!-- Head actuators -->
     <position name="head_1_joint_position" joint="head_1_joint" inheritrange="1.0" kp="20"/>
-    <position name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" forcerange="-2.77 2.77"
-      kp="20"/>
+    <position name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" kp="20"/>
     <!-- Torso actuators -->
     <position name="torso_lift_joint_position" joint="torso_lift_joint" inheritrange="1.0" kp="5000"/>
     <!-- Wheel actuators -->
-    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
-    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
+    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" kv="100"/>
+    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" kv="100"/>
   </actuator>
 
 </mujoco>
diff --git a/pal_tiago/tiago_position.xml b/pal_tiago/tiago_position.xml
@@ -1,29 +1,36 @@
 <mujoco model="tiago_position">
 
   <include file="tiago.xml"/>
+
+  <default>
+    <default class="position">
+      <position dampratio="1"/>
+    </default>
+  </default>
+
   <actuator>
     <!-- Arm actuators -->
-    <position name="arm_position_1_position" joint="arm_1_joint" inheritrange="1.0" kp="3000" dampratio="1.0" forcerange="-53 53"/>
-    <position name="arm_position_2_position" joint="arm_2_joint" inheritrange="1.0" kp="3000" dampratio="1.0" forcerange="-53 53"/>
-    <position name="arm_position_3_position" joint="arm_3_joint" inheritrange="1.0" kp="3000" dampratio="1.0" forcerange="-30 30"/>
-    <position name="arm_position_4_position" joint="arm_4_joint" inheritrange="1.0" kp="3000" dampratio="1.0" forcerange="-30 30"/>
+    <position class="position" name="arm_position_1_position" joint="arm_1_joint" inheritrange="1.0" kp="3000"/>
+    <position class="position" name="arm_position_2_position" joint="arm_2_joint" inheritrange="1.0" kp="3000"/>
+    <position class="position" name="arm_position_3_position" joint="arm_3_joint" inheritrange="1.0" kp="3000"/>
+    <position class="position" name="arm_position_4_position" joint="arm_4_joint" inheritrange="1.0" kp="3000"/>
     <!-- Wrist actuators -->
-    <position name="arm_position_5_position" joint="arm_5_joint" inheritrange="1.0" kp="500" dampratio="1.0" forcerange="-3 3"/>
-    <position name="arm_position_6_position" joint="arm_6_joint" inheritrange="1.0" kp="500" dampratio="1.0" forcerange="-6.6 6.6"/>
-    <position name="arm_position_7_position" joint="arm_7_joint" inheritrange="1.0" kp="500" dampratio="1.0" forcerange="-6.6 6.6"/>
+    <position class="position" name="arm_position_5_position" joint="arm_5_joint" inheritrange="1.0" kp="500"/>
+    <position class="position" name="arm_position_6_position" joint="arm_6_joint" inheritrange="1.0" kp="500"/>
+    <position class="position" name="arm_position_7_position" joint="arm_7_joint" inheritrange="1.0" kp="500"/>
     <!-- End effector actuators -->
-    <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" inheritrange="0.999"
+    <position class="position" name="gripper_left_finger_position" joint="gripper_left_finger_joint" inheritrange="0.999"
       forcerange="-5.197 5.197" kp="1000" kv="80"/>
-    <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" inheritrange="0.999"
+    <position class="position" name="gripper_right_finger_position" joint="gripper_right_finger_joint" inheritrange="0.999"
       forcerange="-5.197 5.197" kp="1000" kv="80"/>
     <!-- Head actuators -->
-    <position name="head_1_joint_position" joint="head_1_joint" inheritrange="1.0" kp="20" forcerange="-5.197 5.197"/>
-    <position name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" kp="20" forcerange="-5.197 5.197"/>
+    <position class="position" name="head_1_joint_position" joint="head_1_joint" inheritrange="1.0" kp="20"/>
+    <position class="position" name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" kp="20"/>
     <!-- Torso actuators -->
-    <position name="torso_lift_joint_position" joint="torso_lift_joint" inheritrange="1.0" kp="30000" forcerange="-500 500"/>
+    <position class="position" name="torso_lift_joint_position" joint="torso_lift_joint" inheritrange="1.0" kp="30000"/>
     <!-- Wheel actuators -->
-    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
-    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
+    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" kv="100"/>
+    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" kv="100"/>
   </actuator>
 
   <keyframe>

diff --git a/pal_tiago/tiago_velocity.xml b/pal_tiago/tiago_velocity.xml
@@ -3,27 +3,27 @@
   <include file="tiago.xml"/>
   <actuator>
     <!-- Arm actuators -->
-    <velocity name="arm_velocity_1_servo" joint="arm_1_joint" kv="5000" forcerange="-43 43"/>
-    <velocity name="arm_velocity_2_servo" joint="arm_2_joint" kv="100000" forcerange="-43 43"/>
-    <velocity name="arm_velocity_3_servo" joint="arm_3_joint" kv="5000" forcerange="-26 26"/>
-    <velocity name="arm_velocity_4_servo" joint="arm_4_joint" kv="50000" forcerange="-26 26"/>
+    <velocity name="arm_velocity_1_servo" joint="arm_1_joint" kv="5000"/>
+    <velocity name="arm_velocity_2_servo" joint="arm_2_joint" kv="100000"/>
+    <velocity name="arm_velocity_3_servo" joint="arm_3_joint" kv="5000"/>
+    <velocity name="arm_velocity_4_servo" joint="arm_4_joint" kv="50000"/>
     <!-- Wrist actuators -->
-    <velocity name="arm_velocity_5_servo" joint="arm_5_joint" kv="2000" forcerange="-3 3"/>
-    <velocity name="arm_velocity_6_servo" joint="arm_6_joint" kv="2000" forcerange="-6.6 6.6"/>
-    <velocity name="arm_velocity_7_servo" joint="arm_7_joint" kv="2000" forcerange="-6.6 6.6"/>
+    <velocity name="arm_velocity_5_servo" joint="arm_5_joint" kv="2000"/>
+    <velocity name="arm_velocity_6_servo" joint="arm_6_joint" kv="2000"/>
+    <velocity name="arm_velocity_7_servo" joint="arm_7_joint" kv="2000"/>
     <!-- End effector actuators -->
     <position name="gripper_left_finger_position" joint="gripper_left_finger_joint" inheritrange="0.999"
       forcerange="-5.197 5.197" kp="1000"/>
     <position name="gripper_right_finger_position" joint="gripper_right_finger_joint" inheritrange="0.999"
       forcerange="-5.197 5.197" kp="1000"/>
     <!-- Head actuators -->
-    <position name="head_1_joint_position" joint="head_1_joint" inheritrange="1.0" kp="20" forcerange="-5.197 5.197"/>
-    <position name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" kp="20" forcerange="-5.197 5.197"/>
+    <position name="head_1_joint_position" joint="head_1_joint" inheritrange="1.0" kp="20"/>
+    <position name="head_2_joint_position" joint="head_2_joint" inheritrange="1.0" kp="20"/>
     <!-- Torso actuators -->
-    <position name="torso_lift_joint_position" joint="torso_lift_joint" inheritrange="1.0" kp="40000" forcerange="-2000 2000"/>
+    <position name="torso_lift_joint_position" joint="torso_lift_joint" inheritrange="1.0" kp="40000"/>
     <!-- Wheel actuators -->
-    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
-    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" forcerange="-100 100" kv="100"/>
+    <velocity name="wheel_left_joint_vel" joint="wheel_left_joint" ctrlrange="-5 5" kv="100"/>
+    <velocity name="wheel_right_joint_vel" joint="wheel_right_joint" ctrlrange="-5 5" kv="100"/>
   </actuator>
 
   <keyframe>

diff --git a/pal_tiago_dual/assets/arm/arm_11.stl b/pal_tiago_dual/assets/arm/arm_11.stl
diff --git a/pal_tiago_dual/assets/arm/arm_12.stl b/pal_tiago_dual/assets/arm/arm_12.stl
diff --git a/pal_tiago_dual/assets/arm/arm_13.stl b/pal_tiago_dual/assets/arm/arm_13.stl
diff --git a/pal_tiago_dual/assets/arm/arm_14.stl b/pal_tiago_dual/assets/arm/arm_14.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_collision.stl b/pal_tiago_dual/assets/arm/arm_1_collision.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_collision1.stl b/pal_tiago_dual/assets/arm/arm_1_collision1.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_collision2.stl b/pal_tiago_dual/assets/arm/arm_1_collision2.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_collision3.stl b/pal_tiago_dual/assets/arm/arm_1_collision3.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_collision4.stl b/pal_tiago_dual/assets/arm/arm_1_collision4.stl
diff --git a/pal_tiago_dual/assets/arm/arm_1_color.png b/pal_tiago_dual/assets/arm/arm_1_color.png
diff --git a/pal_tiago_dual/assets/arm/arm_1_normals.png b/pal_tiago_dual/assets/arm/arm_1_normals.png
diff --git a/pal_tiago_dual/assets/arm/arm_1_specular.png b/pal_tiago_dual/assets/arm/arm_1_specular.png
diff --git a/pal_tiago_dual/assets/arm/arm_21.stl b/pal_tiago_dual/assets/arm/arm_21.stl
diff --git a/pal_tiago_dual/assets/arm/arm_22.stl b/pal_tiago_dual/assets/arm/arm_22.stl
diff --git a/pal_tiago_dual/assets/arm/arm_23.stl b/pal_tiago_dual/assets/arm/arm_23.stl
diff --git a/pal_tiago_dual/assets/arm/arm_24.stl b/pal_tiago_dual/assets/arm/arm_24.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_collision.stl b/pal_tiago_dual/assets/arm/arm_2_collision.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_collision1.stl b/pal_tiago_dual/assets/arm/arm_2_collision1.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_collision2.stl b/pal_tiago_dual/assets/arm/arm_2_collision2.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_collision3.stl b/pal_tiago_dual/assets/arm/arm_2_collision3.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_collision4.stl b/pal_tiago_dual/assets/arm/arm_2_collision4.stl
diff --git a/pal_tiago_dual/assets/arm/arm_2_color.png b/pal_tiago_dual/assets/arm/arm_2_color.png
diff --git a/pal_tiago_dual/assets/arm/arm_2_normals.png b/pal_tiago_dual/assets/arm/arm_2_normals.png
diff --git a/pal_tiago_dual/assets/arm/arm_2_specular.png b/pal_tiago_dual/assets/arm/arm_2_specular.png
diff --git a/pal_tiago_dual/assets/arm/arm_31.stl b/pal_tiago_dual/assets/arm/arm_31.stl
diff --git a/pal_tiago_dual/assets/arm/arm_32.stl b/pal_tiago_dual/assets/arm/arm_32.stl
diff --git a/pal_tiago_dual/assets/arm/arm_33.stl b/pal_tiago_dual/assets/arm/arm_33.stl
diff --git a/pal_tiago_dual/assets/arm/arm_34.stl b/pal_tiago_dual/assets/arm/arm_34.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_collision.stl b/pal_tiago_dual/assets/arm/arm_3_collision.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_collision1.stl b/pal_tiago_dual/assets/arm/arm_3_collision1.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_collision2.stl b/pal_tiago_dual/assets/arm/arm_3_collision2.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_collision3.stl b/pal_tiago_dual/assets/arm/arm_3_collision3.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_collision4.stl b/pal_tiago_dual/assets/arm/arm_3_collision4.stl
diff --git a/pal_tiago_dual/assets/arm/arm_3_color.png b/pal_tiago_dual/assets/arm/arm_3_color.png
diff --git a/pal_tiago_dual/assets/arm/arm_3_normals.png b/pal_tiago_dual/assets/arm/arm_3_normals.png
diff --git a/pal_tiago_dual/assets/arm/arm_3_specular.png b/pal_tiago_dual/assets/arm/arm_3_specular.png
diff --git a/pal_tiago_dual/assets/arm/arm_41.stl b/pal_tiago_dual/assets/arm/arm_41.stl
diff --git a/pal_tiago_dual/assets/arm/arm_42.stl b/pal_tiago_dual/assets/arm/arm_42.stl
diff --git a/pal_tiago_dual/assets/arm/arm_43.stl b/pal_tiago_dual/assets/arm/arm_43.stl
diff --git a/pal_tiago_dual/assets/arm/arm_44.stl b/pal_tiago_dual/assets/arm/arm_44.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_collision.stl b/pal_tiago_dual/assets/arm/arm_4_collision.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_collision1.stl b/pal_tiago_dual/assets/arm/arm_4_collision1.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_collision2.stl b/pal_tiago_dual/assets/arm/arm_4_collision2.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_collision3.stl b/pal_tiago_dual/assets/arm/arm_4_collision3.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_collision4.stl b/pal_tiago_dual/assets/arm/arm_4_collision4.stl
diff --git a/pal_tiago_dual/assets/arm/arm_4_color.png b/pal_tiago_dual/assets/arm/arm_4_color.png
diff --git a/pal_tiago_dual/assets/arm/arm_4_normals.png b/pal_tiago_dual/assets/arm/arm_4_normals.png
diff --git a/pal_tiago_dual/assets/arm/arm_4_specular.png b/pal_tiago_dual/assets/arm/arm_4_specular.png
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-2010.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-2010.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20101.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20101.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20102.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20102.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20103.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20103.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20104.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20104.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20171.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20171.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20172.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20172.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20173.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20173.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5-wrist-20174.stl b/pal_tiago_dual/assets/arm/arm_5-wrist-20174.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-2010.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-2010.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20101.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20101.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20102.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20102.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20103.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20103.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20104.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20104.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-2017.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-2017.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20171.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20171.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20172.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20172.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20173.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20173.stl
diff --git a/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20174.stl b/pal_tiago_dual/assets/arm/arm_5_collision-wrist-20174.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-2010.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-2010.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20101.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20101.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20102.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20102.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20103.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20103.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20104.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20104.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20171.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20171.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20172.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20172.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20173.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20173.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6-wrist-20174.stl b/pal_tiago_dual/assets/arm/arm_6-wrist-20174.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-2010.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-2010.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20101.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20101.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20102.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20102.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20103.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20103.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20104.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20104.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-2017.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-2017.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20171.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20171.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20172.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20172.stl
diff --git a/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20173.stl b/pal_tiago_dual/assets/arm/arm_6_collision-wrist-20173.stl