diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
index 8cda9df4..ffd8141d 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
@@ -4,7 +4,7 @@
     <inertial>
       <mass value="2.823751482824641"/>
       <origin xyz="0.0024748962432214403 -1.050794470993344e-4 0.023450368787955644" rpy="0 -0 0"/>
-      <inertia ixx="0.004380033669258687" ixy="-1.4369039688385888e-6" ixz="1.4098441290530498e-5" iyy="0.005630606285299466" iyz="-2.3299340994588877e-5" izz="0.003616681010808376"/>
+      <inertia ixx="0.005630606285299466" ixy="1.4369039688385168e-6" ixz="2.329934099458601e-5" iyy="0.004380033669258687" iyz="1.4098441290533537e-5" izz="0.003616681010808376"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -36,8 +36,8 @@
   <link name="l_hip_1">
     <inertial>
       <mass value="2.231768868859629"/>
-      <origin xyz="-0.004015422485162342 0.028247280560774645 -6.811143992383195e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.002695326128467937" ixy="-5.971255499379551e-5" ixz="-5.838535051658256e-6" iyy="0.0022109218149310014" iyz="1.5432160879022375e-5" izz="0.0024610381641843883"/>
+      <origin xyz="-0.0040154224851623455 0.028247280560774652 -6.811143992383195e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.002210921814931002" ixy="5.971255499379551e-5" ixz="-1.5432160879022988e-5" iyy="0.002695326128467937" iyz="-5.838535051658118e-6" izz="0.0024610381641843883"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -69,8 +69,8 @@
   <link name="r_hip_1">
     <inertial>
       <mass value="2.2320852637398954"/>
-      <origin xyz="-0.004032117730258741 -0.028249385885545365 -6.739019720396755e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.0026955458913177294" ixy="6.126720536020865e-5" ixz="6.443355522460479e-6" iyy="0.0022122594774356403" iyz="1.5035655169600434e-5" izz="0.0024608876750051717"/>
+      <origin xyz="-0.004032117730258793 -0.028249385885545365 -6.739019720393702e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.00221225947743564" ixy="-6.126720536020865e-5" ixz="-1.5035655169601288e-5" iyy="0.002695545891317729" iyz="6.443355522460266e-6" izz="0.0024608876750051726"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -102,8 +102,8 @@
   <link name="torso_1">
     <inertial>
       <mass value="2.0645667"/>
-      <origin xyz="-0.009639973621975028 6.168494736287859e-4 0.08244231288506343" rpy="0 -0 0"/>
-      <inertia ixx="0.007406086000000001" ixy="6.975203277646223e-6" ixz="-1.2586871224024975e-5" iyy="0.005947073300000001" iyz="7.227736063238349e-4" izz="0.0031780111"/>
+      <origin xyz="-0.00963997362197503 6.168494736287859e-4 0.08244231288506354" rpy="0 -0 0"/>
+      <inertia ixx="0.007406085999999999" ixy="0" ixz="0" iyy="0.0059470732999999994" iyz="0" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -135,8 +135,8 @@
   <link name="torso_2">
     <inertial>
       <mass value="0.7051627057068411"/>
-      <origin xyz="0.0015724674776760397 0.0036223466761378916 0.026019504666210665" rpy="0 -0 0"/>
-      <inertia ixx="6.675525890436374e-4" ixy="-4.198623927073284e-6" ixz="7.014302140884287e-5" iyy="0.0012590296877101025" iyz="5.398163583772658e-6" izz="0.001145822810927014"/>
+      <origin xyz="0.0015724674776760373 0.0036223466761378907 0.026019504666210766" rpy="0 -0 0"/>
+      <inertia ixx="0.0012590296877101025" ixy="4.1986239270733835e-6" ixz="-5.398163583772818e-6" iyy="6.675525890436374e-4" iyz="7.014302140884287e-5" izz="0.001145822810927014"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -168,8 +168,8 @@
   <link name="chest">
     <inertial>
       <mass value="8.065471632648594"/>
-      <origin xyz="-0.010721051678060343 -3.482064053382885e-4 0.07611063517438789" rpy="0 -0 0"/>
-      <inertia ixx="0.04493883052455447" ixy="-1.7611941682820536e-4" ixz="-1.2962048932620152e-4" iyy="0.04845475402461777" iyz="3.124089280569461e-4" izz="0.03068768729801644"/>
+      <origin xyz="-0.010721051678060345 -3.482064053382885e-4 0.07611063517438793" rpy="0 -0 0"/>
+      <inertia ixx="0.048454754024617776" ixy="1.7611941682820642e-4" ixz="-3.124089280569708e-4" iyy="0.044938830524554475" iyz="-1.2962048932614557e-4" izz="0.030687687298016447"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -201,8 +201,8 @@
   <link name="l_shoulder_1">
     <inertial>
       <mass value="0.7913756606310043"/>
-      <origin xyz="1.482427542201549e-4 0.06782971819041982 5.036196460998787e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="2.0497606829731344e-4" ixz="-1.9449541410974418e-4" iyy="0.01" iyz="5.017853886187208e-5" izz="0.01"/>
+      <origin xyz="1.4824275422014796e-4 0.06782971819041986 5.036196460999898e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -234,8 +234,8 @@
   <link name="neck_2">
     <inertial>
       <mass value="0.12100314943032965"/>
-      <origin xyz="-0.006305773955890981 3.0141314376408408e-5 0.006373188453183354" rpy="0 -0 0"/>
-      <inertia ixx="3.848306673536227e-5" ixy="9.375752008419092e-8" ixz="4.870043501240908e-8" iyy="4.538376659216681e-5" iyz="-8.622664755132242e-6" izz="3.7535538604448434e-5"/>
+      <origin xyz="-0.006305773955890982 3.0141314376407138e-5 0.006373188453183354" rpy="0 -0 0"/>
+      <inertia ixx="4.5383766592166864e-5" ixy="-9.375752008423039e-8" ixz="8.622664755132213e-6" iyy="3.848306673536227e-5" iyz="4.870043501244136e-8" izz="3.7535538604448367e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -267,8 +267,8 @@
   <link name="r_shoulder_1">
     <inertial>
       <mass value="0.7922822880502421"/>
-      <origin xyz="1.7243569867333997e-4 -0.06784040318161476 4.760009452753067e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.0510369914064142e-4" ixz="1.9458763763805837e-4" iyy="0.01" iyz="4.97645732286129e-5" izz="0.01"/>
+      <origin xyz="1.7243569867328445e-4 -0.06784040318161474 4.7600094527533443e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -300,8 +300,8 @@
   <link name="r_shoulder_2">
     <inertial>
       <mass value="0.4392272062642899"/>
-      <origin xyz="0.005127191267809918 -1.6659290147985614e-4 -0.02041441690188461" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="5.024923153599912e-5" ixz="-6.506905358695258e-5" iyy="0.01" iyz="7.819501283435285e-5" izz="0.01"/>
+      <origin xyz="0.005113618901434032 -2.13348557806331e-4 -0.020425805469504787" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -330,8 +330,8 @@
   <link name="r_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="-6.954441358278038e-5 1.6100824876602582e-5 -0.007955156752673734" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.7272716990665913e-6" ixz="1.1152637558250718e-5" iyy="0.01" iyz="5.6833371008971195e-6" izz="0.01"/>
+      <origin xyz="-8.31167799586717e-5 -3.0654831449927755e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -363,8 +363,8 @@
   <link name="r_upper_arm">
     <inertial>
       <mass value="1.7722348545631907"/>
-      <origin xyz="1.6057023129404518e-4 -0.002288738185514838 -0.050105724104489345" rpy="0 -0 0"/>
-      <inertia ixx="0.0033762002427288164" ixy="-7.677378856102588e-5" ixz="-5.13345295872612e-4" iyy="0.00337539101140544" iyz="-2.529139265641443e-4" izz="0.001757755824190023"/>
+      <origin xyz="1.4699786491786937e-4 -0.0023354938418406186 -0.05011711267210951" rpy="0 -0 0"/>
+      <inertia ixx="0.0034219198528275844" ixy="3.081029189714289e-5" ixz="2.5743017599563002e-5" iyy="0.003510878854314856" iyz="-1.0938393403810045e-4" izz="0.0015765483711818404"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -396,8 +396,8 @@
   <link name="r_forearm">
     <inertial>
       <mass value="1.4760712424977045"/>
-      <origin xyz="-0.0182933039283515 -0.0021681316645861815 -0.08813804790687912" rpy="0 -0 0"/>
-      <inertia ixx="0.009999999999999998" ixy="-3.2391145849100215e-5" ixz="-5.651353010468485e-4" iyy="0.010000000000000002" iyz="-2.2574875840364485e-4" izz="0.01"/>
+      <origin xyz="-0.01830687629472739 -0.002214887320910963 -0.08814943647449926" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -429,8 +429,8 @@
   <link name="r_wrist_1">
     <inertial>
       <mass value="0.12269276206782376"/>
-      <origin xyz="-1.4544608855514632e-4 1.5592471276534847e-4 0.02237780081793081" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.2420673813880967e-7" ixz="-1.9257205949543276e-7" iyy="0.01" iyz="-6.889140049546541e-7" izz="0.01"/>
+      <origin xyz="-1.4544607175177166e-4 1.559247070407055e-4 0.022377800821402394" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -462,8 +462,8 @@
   <link name="r_wrist_2">
     <inertial>
       <mass value="0.021188357245777188"/>
-      <origin xyz="3.080739829908197e-11 7.897372672057424e-4 -0.004696960058474059" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.132431331458975e-8" ixz="2.6145918714416895e-7" iyy="0.01" iyz="1.1835010162840832e-7" izz="0.01"/>
+      <origin xyz="5.05499045400093e-11 7.897372606293362e-4 -0.004696960055006527" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -495,8 +495,8 @@
   <link name="r_hand_palm">
     <inertial>
       <mass value="0.2020068404688116"/>
-      <origin xyz="-0.003982157998247627 -0.0021135288978345046 -0.05842371394499357" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.433419081989723e-5" ixz="-2.3106097024043265e-5" iyy="0.01" iyz="4.993825362240387e-6" izz="0.01"/>
+      <origin xyz="-0.003982157972782507 -0.0021135289059891482 -0.05842371394186491" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -528,8 +528,8 @@
   <link name="r_hand_index_1">
     <inertial>
       <mass value="0.019232477290189804"/>
-      <origin xyz="-0.004670558279444528 0.003850331668291379 -0.01852531230737764" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.4613089584480577e-7" ixz="-1.444573423930251e-7" iyy="0.01" iyz="-6.395818558046926e-7" izz="0.01"/>
+      <origin xyz="-0.008420124408709713 0.003850331678233287 -0.01714942611279774" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -561,8 +561,8 @@
   <link name="r_hand_middle_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260456246838e-5 0.0011131046723378002 -0.017428809836249337" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-6.992052843719422e-8" ixz="-3.560988417914359e-7" iyy="0.01" iyz="-1.7213594526882048e-7" izz="0.01"/>
+      <origin xyz="5.610258533731913e-5 0.0011131046827904112 -0.01742880983276912" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -594,8 +594,8 @@
   <link name="r_hand_pinkie_1">
     <inertial>
       <mass value="0.006357585733638135"/>
-      <origin xyz="3.616263242815776e-5 0.0012147050861231743 -0.015053109694843136" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.7647335520761375e-8" ixz="-2.7663833562638734e-7" iyy="0.01" iyz="-4.824304563523185e-8" izz="0.01"/>
+      <origin xyz="3.616260147435227e-5 0.0012147050941129778 -0.015053109692718447" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -627,8 +627,8 @@
   <link name="r_hand_ring_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260444077666e-5 0.001113104590923425 -0.017428809844715287" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.610406857428927e-8" ixz="-3.828604023526623e-7" iyy="0.01" iyz="-6.810300838152728e-8" izz="0.01"/>
+      <origin xyz="5.6102585337293107e-5 0.0011131045999587252 -0.017428809839565573" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -660,8 +660,8 @@
   <link name="r_hand_thumb_1">
     <inertial>
       <mass value="0.02894174189037044"/>
-      <origin xyz="0.018375632420818924 -4.555209342888e-5 0.00212647348637468" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.483972069242782e-6" ixz="-4.227782571629177e-7" iyy="0.01" iyz="3.7277898587524307e-7" izz="0.01"/>
+      <origin xyz="0.0183756324403817 -4.55521018183358e-5 0.002126473485347669" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -693,8 +693,8 @@
   <link name="r_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="-0.014887264450410557 -0.001132972632809287 6.266004727595309e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.6052216178597977e-7" ixz="-8.136362383055484e-8" iyy="0.01" iyz="1.4554843972914e-7" izz="0.01"/>
+      <origin xyz="-0.014887264468035837 -0.0011329726380328586 6.26600593671145e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -726,8 +726,8 @@
   <link name="r_hand_thumb_3">
     <inertial>
       <mass value="0.00683159462238797"/>
-      <origin xyz="0.016086236523113466 0.002652687749746474 4.703178261786234e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.7537742494259307e-7" ixz="-2.1198241519414298e-8" iyy="0.01" iyz="4.311056852137086e-8" izz="0.01"/>
+      <origin xyz="0.016086236544425966 0.0026526877650291103 4.703179416121195e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -759,8 +759,8 @@
   <link name="r_hand_ring_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.43251704611413e-5 0.0027547090528209084 -0.018890876422440472" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-7.58504462601195e-9" ixz="-3.2568363341079027e-7" iyy="0.01" iyz="-4.221363162923806e-8" izz="0.01"/>
+      <origin xyz="4.43251343077225e-5 0.00275470906266162 -0.018890876418029168" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -792,8 +792,8 @@
   <link name="r_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237490764073"/>
-      <origin xyz="2.1391107738796128e-5 0.00277128634729798 -0.01893030588274036" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-5.8381716933055835e-9" ixz="-3.2111976226394747e-7" iyy="0.01" iyz="-3.724756521840787e-8" izz="0.01"/>
+      <origin xyz="2.1391072559044655e-5 0.0027712863562942003 -0.018930305880602127" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -825,8 +825,8 @@
   <link name="r_hand_middle_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.432517100235248e-5 0.0027547091809207447 -0.01889087640358672" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.141336189094091e-8" ixz="-2.9259039740622944e-7" iyy="0.01" iyz="-1.7422053024870367e-7" izz="0.01"/>
+      <origin xyz="4.432513430769475e-5 0.0027547091926723166 -0.018890876399070666" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -858,8 +858,8 @@
   <link name="r_hand_index_2">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="0.010265094459698948 0.001113105001514958 -0.01827635665339833" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-9.537054827379165e-8" ixz="-3.233345932270594e-7" iyy="0.01" iyz="-2.6975812067979636e-7" izz="0.01"/>
+      <origin xyz="5.610258533736423e-5 0.0011131050125119946 -0.01742880980571504" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -891,8 +891,8 @@
   <link name="r_hand_index_3">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="0.018874022948048214 0.0027547026093547033 -0.018834827219235406" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.849762497244115e-8" ixz="-2.514360396660324e-7" iyy="0.01" iyz="-3.0309644254617756e-7" izz="0.01"/>
+      <origin xyz="4.432513430773638e-5 0.0027547026217409343 -0.01889087735725714" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -924,8 +924,8 @@
   <link name="neck_3">
     <inertial>
       <mass value="0.17811376821431446"/>
-      <origin xyz="7.590511663558798e-4 -4.091690693859513e-5 0.03603551654922699" rpy="0 -0 0"/>
-      <inertia ixx="1.0759851740143372e-4" ixy="-1.762946883518242e-7" ixz="-2.5039696740768445e-7" iyy="9.705486922197742e-5" iyz="-6.130672988865115e-6" izz="3.6264154407241086e-5"/>
+      <origin xyz="7.590511663558711e-4 -4.0916906938460536e-5 0.036035516549226765" rpy="0 -0 0"/>
+      <inertia ixx="9.70548692219775e-5" ixy="1.7629468835177367e-7" ixz="6.130672988864901e-6" iyy="1.0759851740143377e-4" iyz="-2.503969674072287e-7" izz="3.626415440724103e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -957,8 +957,8 @@
   <link name="head">
     <inertial>
       <mass value="1.972127893184972"/>
-      <origin xyz="-0.01620091407966073 -0.002268813231724585 0.03500744217627949" rpy="0 -0 0"/>
-      <inertia ixx="0.009883124315185845" ixy="-1.818674034830039e-4" ixz="-3.994074773445533e-5" iyy="0.007991907355798097" iyz="1.9244538979180473e-4" izz="0.00786000669473206"/>
+      <origin xyz="-0.01620091412622722 -0.002268813231724542 0.03500744217497026" rpy="0 -0 0"/>
+      <inertia ixx="0.007991907355842523" ixy="1.8186740348760937e-4" ixz="-1.9244538977657914e-4" iyy="0.009883124315185845" iyz="-3.994074771344834e-5" izz="0.00786000669468763"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -990,8 +990,8 @@
   <link name="realsense">
     <inertial>
       <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600984 0.00451999521864419 0.0027456101541037943" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.787553659410962e-5" ixz="-7.183500075449047e-6" iyy="0.01" iyz="-2.5524452311285135e-5" izz="0.01"/>
+      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1023,8 +1023,8 @@
   <link name="l_shoulder_2">
     <inertial>
       <mass value="0.44093319265387243"/>
-      <origin xyz="0.007505823346010238 2.3402763110361158e-4 -0.02049745115620874" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-5.157553010803304e-5" ixz="6.569228591220418e-5" iyy="0.01" iyz="7.97840956223945e-5" izz="0.01"/>
+      <origin xyz="0.007519395712385758 1.8727197477702573e-4 -0.020486062588588566" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1053,8 +1053,8 @@
   <link name="l_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="6.954441358310651e-5 7.741048802184514e-5 -0.00797793388791404" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.1064791718069957e-6" ixz="-1.1158347995854456e-5" iyy="0.01" iyz="5.801991889044871e-6" izz="0.01"/>
+      <origin xyz="8.311677995862313e-5 3.065483169523153e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1086,8 +1086,8 @@
   <link name="l_upper_arm">
     <inertial>
       <mass value="1.7708527474969076"/>
-      <origin xyz="1.169202849922793e-4 0.0022997741939624616 -0.050076427527425534" rpy="0 -0 0"/>
-      <inertia ixx="0.0033774309282785502" ixy="7.386270169901037e-5" ixz="5.095431789297364e-4" iyy="0.003371639006155085" iyz="-2.533199420811916e-4" izz="0.0017505986785028974"/>
+      <origin xyz="1.3049265136780286e-4 0.0022530185376357925 -0.050065038959805325" rpy="0 -0 0"/>
+      <inertia ixx="0.003417154867942194" ixy="-2.952452997527239e-5" ixz="2.6752323704214618e-5" iyy="0.0035109035513553296" iyz="1.0388063737032838e-4" izz="0.0015716101936389657"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1119,8 +1119,8 @@
   <link name="l_forearm">
     <inertial>
       <mass value="1.4604487031386884"/>
-      <origin xyz="-0.018566835695049105 0.002358680882311198 -0.08920511097559716" rpy="0 -0 0"/>
-      <inertia ixx="0.010000000000000002" ixy="3.918099642968445e-5" ixz="5.442336109701719e-4" iyy="0.010000000000000002" iyz="-2.317105945305375e-4" izz="0.01"/>
+      <origin xyz="-0.018553263328673478 0.0023119252259846124 -0.08919372240797706" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.010000000000000002" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1152,8 +1152,8 @@
   <link name="l_wrist_1">
     <inertial>
       <mass value="0.1226928"/>
-      <origin xyz="1.454507181725892e-4 -1.5595312494159908e-4 0.022377795690742608" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.615793735027637e-7" ixz="5.32121794935847e-7" iyy="0.01" iyz="5.639568021417744e-7" izz="0.01"/>
+      <origin xyz="1.4545073498396788e-4 -1.5595311920038601e-4 0.022377795694245" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1185,8 +1185,8 @@
   <link name="l_wrist_2">
     <inertial>
       <mass value="0.02118836"/>
-      <origin xyz="-7.43814359063788e-9 -7.897157600239013e-4 -0.004696967954283704" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.138492578624664e-8" ixz="-2.617165807665742e-7" iyy="0.01" iyz="1.1837099305566312e-7" izz="0.01"/>
+      <origin xyz="-7.4183919215875535e-9 -7.897157534373368e-4 -0.0046969679508168105" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1218,8 +1218,8 @@
   <link name="l_hand_palm">
     <inertial>
       <mass value="0.20364988213680776"/>
-      <origin xyz="-0.003950502243349457 0.0019881930863947794 -0.058015852442294576" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.4578800404078265e-5" ixz="2.4811654406357777e-5" iyy="0.01" iyz="3.5744967821488006e-6" izz="0.01"/>
+      <origin xyz="-0.003950502224178115 0.0019881930945492288 -0.058015852438740544" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1251,8 +1251,8 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
+      <origin xyz="0.008430306749277854 0.0038510231844406384 -0.017148502808541133" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1284,8 +1284,8 @@
   <link name="l_hand_middle_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.609412499965952e-5 0.0010938363808821183 -0.017422530978911333" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="6.769065023564737e-8" ixz="3.608405680785154e-7" iyy="0.01" iyz="-1.5396152545079645e-7" izz="0.01"/>
+      <origin xyz="5.609414423399586e-5 0.0010938363913530202 -0.0174225309754375" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1317,8 +1317,8 @@
   <link name="l_hand_pinkie_1">
     <inertial>
       <mass value="0.006357586"/>
-      <origin xyz="3.655455845796157e-5 0.001213549159989641 -0.015052861820509966" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="2.5015661539015353e-8" ixz="2.7901203347130377e-7" iyy="0.01" iyz="-3.868557597034084e-8" izz="0.01"/>
+      <origin xyz="3.6554577562467744e-5 0.0012135491682558897 -0.015052861815374408" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1350,8 +1350,8 @@
   <link name="l_hand_ring_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.6109515031453616e-5 0.0011139039467009115 -0.01742875683963907" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.292524499993021e-8" ixz="3.871534855605887e-7" iyy="0.01" iyz="-4.96358923238347e-8" izz="0.01"/>
+      <origin xyz="5.610953414397422e-5 0.001113903955753115 -0.017428756834488968" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1383,8 +1383,8 @@
   <link name="l_hand_thumb_1">
     <inertial>
       <mass value="0.029029542280278556"/>
-      <origin xyz="-0.01803917641362529 -0.0028662725609850115 0.0021177719497344483" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.6089515765306715e-6" ixz="4.604612544346765e-7" iyy="0.01" iyz="3.6759606034343055e-7" izz="0.01"/>
+      <origin xyz="-0.018039176431428286 -0.0028662725726853466 0.0021177719486905056" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1416,8 +1416,8 @@
   <link name="l_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="0.014887264450400842 0.0011329726328125345 6.26600714702108e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.508321707683706e-7" ixz="8.213304862846372e-8" iyy="0.01" iyz="1.4694684276477687e-7" izz="0.01"/>
+      <origin xyz="0.014887264468035857 0.0011329726380328031 6.266005936719776e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1449,8 +1449,8 @@
   <link name="l_hand_thumb_3">
     <inertial>
       <mass value="0.006831595"/>
-      <origin xyz="0.01608625251142736 0.002652688715795104 4.7014270457834684e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.6702502025036103e-7" ixz="2.0823539173591144e-8" iyy="0.01" iyz="4.4870066350370055e-8" izz="0.01"/>
+      <origin xyz="0.016086252532748233 0.0026526887310780456 4.701425890352162e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1482,8 +1482,8 @@
   <link name="l_hand_ring_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.433650413547367e-5 0.002754675449902322 -0.018890859501747292" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.977013947931596e-9" ixz="3.289401428370411e-7" iyy="0.01" iyz="-2.7220703393624056e-8" izz="0.01"/>
+      <origin xyz="4.433654030530642e-5 0.0027546754597598533 -0.01889085949732494" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1515,8 +1515,8 @@
   <link name="l_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237"/>
-      <origin xyz="2.2378257433602194e-5 0.0027712834700559796 -0.018930353880242057" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.9098038872728015e-9" ixz="3.2394130716454584e-7" iyy="0.01" iyz="-3.023638062784071e-8" izz="0.01"/>
+      <origin xyz="2.2378292628411067e-5 0.0027712834790767915 -0.018930353878106154" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1548,8 +1548,8 @@
   <link name="l_hand_middle_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.431628345990049e-5 0.002754722935079862 -0.018890877324266697" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.2930780755833666e-8" ixz="3.226980212060653e-7" iyy="0.01" iyz="-1.6629193018826528e-7" izz="0.01"/>
+      <origin xyz="4.4316320155644157e-5 0.0027547229469275236 -0.018890877319942878" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1581,8 +1581,8 @@
   <link name="l_hand_index_2">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="-0.010155342467888226 0.0011131331804158373 -0.01829967928994858" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="9.396780653347036e-8" ixz="3.2904462665817386e-7" iyy="0.01" iyz="-2.5247311144531184e-7" izz="0.01"/>
+      <origin xyz="5.6100110941745274e-5 0.001113133191428639 -0.01742880406449937" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1614,8 +1614,8 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
+      <origin xyz="4.431356673249148e-5 0.0027546711855188366 -0.018890916917498335" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1647,8 +1647,8 @@
   <link name="r_hip_2">
     <inertial>
       <mass value="1.8425374083524118"/>
-      <origin xyz="-0.004338858395565106 -4.2767854114084125e-5 -0.037115837283052844" rpy="0 -0 0"/>
-      <inertia ixx="0.005459166113270066" ixy="-2.373531512034788e-6" ixz="-1.8523984061175535e-6" iyy="0.003120303249647113" iyz="-0.0010124685247925136" izz="0.0034190028448521583"/>
+      <origin xyz="-0.004338858395569037 -4.276785411407025e-5 -0.03711583728304848" rpy="0 -0 0"/>
+      <inertia ixx="0.003120303249647105" ixy="2.3735315120274186e-6" ixz="0.0010124685247925123" iyy="0.005459166113270065" iyz="-1.8523984061140002e-6" izz="0.003419002844852166"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1677,8 +1677,8 @@
   <link name="r_hip_3">
     <inertial>
       <mass value="2.147900058066585"/>
-      <origin xyz="0.005101012450187747 0.012832092744145249 -0.05168842502826276" rpy="0 -0 0"/>
-      <inertia ixx="0.008149001665389528" ixy="-3.498727117566707e-5" ixz="-1.2572783372468983e-4" iyy="0.007193177523135792" iyz="-2.4096738440553857e-5" izz="0.0057278283877820494"/>
+      <origin xyz="0.0051510124501743006 0.012832092744145235 -0.05168842502826221" rpy="0 -0 0"/>
+      <inertia ixx="0.007193177523135792" ixy="3.498727117566612e-5" ixz="2.4096738440560362e-5" iyy="0.008149001665389528" iyz="-1.2572783372468582e-4" izz="0.0057278283877820494"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1710,8 +1710,8 @@
   <link name="r_upper_leg">
     <inertial>
       <mass value="2.784923431729287"/>
-      <origin xyz="0.008467456382678921 -0.01446847602698749 -0.12674960557185633" rpy="0 -0 0"/>
-      <inertia ixx="0.012109133352085043" ixy="-5.455450241372635e-5" ixz="-4.033104101548086e-4" iyy="0.011860132529998117" iyz="-5.393573740676439e-4" izz="0.0022609706907693226"/>
+      <origin xyz="0.008517474024287675 -0.014468465702474021 -0.12674960557185616" rpy="0 -0 0"/>
+      <inertia ixx="0.01186013252999811" ixy="5.455450241372543e-5" ixz="5.393573740676933e-4" iyy="0.01210913335208505" iyz="-4.0331041015476653e-4" izz="0.0022609706907693243"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1743,8 +1743,8 @@
   <link name="r_lower_leg">
     <inertial>
       <mass value="3.7621532589747924"/>
-      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
-      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
+      <origin xyz="0.0012157689164458522 -0.006184734585333342 -0.21372759879868175" rpy="0 -0 0"/>
+      <inertia ixx="0.04644215616821937" ixy="3.7269274872175814e-5" ixz="-8.519885904633137e-4" iyy="0.047738936736850285" iyz="-0.0018042981721195497" izz="0.004839671863500647"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1776,8 +1776,8 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="0.01" iyz="1.0336975691919178e-5" izz="0.01"/>
+      <origin xyz="-0.0013672309746954776 -6.60186959019704e-4 -0.014087432401547305" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1809,8 +1809,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.010000000000000002"/>
+      <origin xyz="0.04052646811695935 -8.642493098851012e-5 -0.023871143533700234" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.009999999999999998" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1839,8 +1839,8 @@
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466904026456e-9" izz="0.01"/>
+      <origin xyz="5.879202911245884e-4 -3.747924692820703e-9 -0.02000061661590058" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1866,8 +1866,8 @@
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646669939015e-9" izz="0.01"/>
+      <origin xyz="5.879202911244878e-4 -3.747924665065128e-9 -0.020000616615901023" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1896,8 +1896,8 @@
   <link name="l_hip_2">
     <inertial>
       <mass value="1.8338392164825101"/>
-      <origin xyz="-0.004125697068299468 3.917475804630921e-5 -0.03750964603556273" rpy="0 -0 0"/>
-      <inertia ixx="0.005488243677251951" ixy="1.6542815235225134e-6" ixz="-2.190409388735156e-6" iyy="0.003159514595905932" iyz="-9.977368607484041e-4" izz="0.0033984945726571133"/>
+      <origin xyz="-0.004125697068299468 3.917475804632309e-5 -0.03750964603556273" rpy="0 -0 0"/>
+      <inertia ixx="0.003159514595905926" ixy="-1.6542815235215681e-6" ixz="9.977368607484037e-4" iyy="0.005488243677251951" iyz="-2.1904093887368695e-6" izz="0.0033984945726571185"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1926,8 +1926,8 @@
   <link name="l_hip_3">
     <inertial>
       <mass value="2.147524084050246"/>
-      <origin xyz="0.005176125839592961 -0.012828813026822289 -0.051748524474533986" rpy="0 -0 0"/>
-      <inertia ixx="0.008197962692605143" ixy="3.321392954197018e-5" ixz="1.247487484059602e-4" iyy="0.007242161971060034" iyz="-2.5059855054008952e-5" izz="0.005727458772759987"/>
+      <origin xyz="0.005176125839593024 -0.01282881302682233 -0.05174852447453404" rpy="0 -0 0"/>
+      <inertia ixx="0.007242161971060034" ixy="-3.321392954197042e-5" ixz="2.505985505401264e-5" iyy="0.008197962692605146" iyz="1.2474874840596051e-4" izz="0.005727458772759987"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1959,8 +1959,8 @@
   <link name="l_upper_leg">
     <inertial>
       <mass value="2.784923431765453"/>
-      <origin xyz="0.008503494694160165 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
-      <inertia ixx="0.012109807149567942" ixy="4.907145319316716e-5" ixz="3.9208918647670385e-4" iyy="0.01185229395228421" iyz="-5.393933817877802e-4" izz="0.002253627916731069"/>
+      <origin xyz="0.008503494694160183 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
+      <inertia ixx="0.011852293952284196" ixy="-4.907145319317264e-5" ixz="5.393933817879138e-4" iyy="0.012109807149567942" iyz="3.9208918647670385e-4" izz="0.002253627916731084"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1992,8 +1992,8 @@
   <link name="l_lower_leg">
     <inertial>
       <mass value="3.767581145212888"/>
-      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
-      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
+      <origin xyz="0.0014201768088090358 0.006186333743956739 -0.21383298861124922" rpy="0 -0 0"/>
+      <inertia ixx="0.04647110900067475" ixy="-2.1189289797886653e-5" ixz="-8.110710215722959e-4" iyy="0.04776726641499918" iyz="0.0018123762001582078" izz="0.004840491251955261"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2025,8 +2025,8 @@
   <link name="l_ankle_1">
     <inertial>
       <mass value="0.3481278951519182"/>
-      <origin xyz="-0.0013852125067176438 6.601869600096483e-4 -0.0140874324743252" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="8.882076128770337e-6" ixz="-5.5239266791031694e-5" iyy="0.01" iyz="7.868443555727937e-6" izz="0.01"/>
+      <origin xyz="-0.0013852125067176195 6.601869600093568e-4 -0.014087432474324535" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2058,8 +2058,8 @@
   <link name="l_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040522400508854174 -9.790751646714113e-5 -0.023871143533686023" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.9973294407367236e-5" ixz="-1.5070602593012222e-5" iyy="0.01" iyz="-0.0013251325687512945" izz="0.01"/>
+      <origin xyz="0.0405224005088543 -9.790751646740482e-5 -0.02387114353368558" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.010000000000000002" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2088,8 +2088,8 @@
   <link name="l_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911260178e-4 -3.747924276487069e-9 -0.02000061661590191" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646683126688e-9" izz="0.01"/>
+      <origin xyz="5.879202911245329e-4 -3.747924692820703e-9 -0.02000061661590158" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2115,8 +2115,8 @@
   <link name="l_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911259571e-4 -3.747924290364857e-9 -0.02000061661590158" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466744864883e-9" izz="0.01"/>
+      <origin xyz="5.879202911244826e-4 -3.7479246789429155e-9 -0.020000616615901134" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
index 9bb50883..5288b4a1 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -4,7 +4,7 @@
     <inertial>
       <mass value="2.823751482824641"/>
       <origin xyz="0.0024748962432214403 -1.050794470993344e-4 0.023450368787955644" rpy="0 -0 0"/>
-      <inertia ixx="0.004380033669258687" ixy="-1.4369039688385888e-6" ixz="1.4098441290530498e-5" iyy="0.005630606285299466" iyz="-2.3299340994588877e-5" izz="0.003616681010808376"/>
+      <inertia ixx="0.005630606285299466" ixy="1.4369039688385168e-6" ixz="2.329934099458601e-5" iyy="0.004380033669258687" iyz="1.4098441290533537e-5" izz="0.003616681010808376"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -33,8 +33,8 @@
   <link name="l_hip_1">
     <inertial>
       <mass value="2.231768868859629"/>
-      <origin xyz="-0.004015422485162342 0.028247280560774645 -6.811143992383195e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.002695326128467937" ixy="-5.971255499379551e-5" ixz="-5.838535051658256e-6" iyy="0.0022109218149310014" iyz="1.5432160879022375e-5" izz="0.0024610381641843883"/>
+      <origin xyz="-0.0040154224851623455 0.028247280560774652 -6.811143992383195e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.002210921814931002" ixy="5.971255499379551e-5" ixz="-1.5432160879022988e-5" iyy="0.002695326128467937" iyz="-5.838535051658118e-6" izz="0.0024610381641843883"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -63,8 +63,8 @@
   <link name="r_hip_1">
     <inertial>
       <mass value="2.2320852637398954"/>
-      <origin xyz="-0.004032117730258741 -0.028249385885545365 -6.739019720396755e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.0026955458913177294" ixy="6.126720536020865e-5" ixz="6.443355522460479e-6" iyy="0.0022122594774356403" iyz="1.5035655169600434e-5" izz="0.0024608876750051717"/>
+      <origin xyz="-0.004032117730258793 -0.028249385885545365 -6.739019720393702e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.00221225947743564" ixy="-6.126720536020865e-5" ixz="-1.5035655169601288e-5" iyy="0.002695545891317729" iyz="6.443355522460266e-6" izz="0.0024608876750051726"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -93,8 +93,8 @@
   <link name="torso_1">
     <inertial>
       <mass value="2.0645667"/>
-      <origin xyz="-0.009639973621975028 6.168494736287859e-4 0.08244231288506343" rpy="0 -0 0"/>
-      <inertia ixx="0.007406086000000001" ixy="6.975203277646223e-6" ixz="-1.2586871224024975e-5" iyy="0.005947073300000001" iyz="7.227736063238349e-4" izz="0.0031780111"/>
+      <origin xyz="-0.00963997362197503 6.168494736287859e-4 0.08244231288506354" rpy="0 -0 0"/>
+      <inertia ixx="0.007406085999999999" ixy="0" ixz="0" iyy="0.0059470732999999994" iyz="0" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -123,8 +123,8 @@
   <link name="torso_2">
     <inertial>
       <mass value="0.7051627057068411"/>
-      <origin xyz="0.0015724674776760397 0.0036223466761378916 0.026019504666210665" rpy="0 -0 0"/>
-      <inertia ixx="6.675525890436374e-4" ixy="-4.198623927073284e-6" ixz="7.014302140884287e-5" iyy="0.0012590296877101025" iyz="5.398163583772658e-6" izz="0.001145822810927014"/>
+      <origin xyz="0.0015724674776760373 0.0036223466761378907 0.026019504666210766" rpy="0 -0 0"/>
+      <inertia ixx="0.0012590296877101025" ixy="4.1986239270733835e-6" ixz="-5.398163583772818e-6" iyy="6.675525890436374e-4" iyz="7.014302140884287e-5" izz="0.001145822810927014"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -153,8 +153,8 @@
   <link name="chest">
     <inertial>
       <mass value="8.065471632648594"/>
-      <origin xyz="-0.010721051678060343 -3.482064053382885e-4 0.07611063517438789" rpy="0 -0 0"/>
-      <inertia ixx="0.04493883052455447" ixy="-1.7611941682820536e-4" ixz="-1.2962048932620152e-4" iyy="0.04845475402461777" iyz="3.124089280569461e-4" izz="0.03068768729801644"/>
+      <origin xyz="-0.010721051678060345 -3.482064053382885e-4 0.07611063517438793" rpy="0 -0 0"/>
+      <inertia ixx="0.048454754024617776" ixy="1.7611941682820642e-4" ixz="-3.124089280569708e-4" iyy="0.044938830524554475" iyz="-1.2962048932614557e-4" izz="0.030687687298016447"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -183,8 +183,8 @@
   <link name="l_shoulder_1">
     <inertial>
       <mass value="0.7913756606310043"/>
-      <origin xyz="1.482427542201549e-4 0.06782971819041982 5.036196460998787e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="2.0497606829731344e-4" ixz="-1.9449541410974418e-4" iyy="0.01" iyz="5.017853886187208e-5" izz="0.01"/>
+      <origin xyz="1.4824275422014796e-4 0.06782971819041986 5.036196460999898e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -213,8 +213,8 @@
   <link name="neck_2">
     <inertial>
       <mass value="0.12100314943032965"/>
-      <origin xyz="-0.006305773955890981 3.0141314376408408e-5 0.006373188453183354" rpy="0 -0 0"/>
-      <inertia ixx="3.848306673536227e-5" ixy="9.375752008419092e-8" ixz="4.870043501240908e-8" iyy="4.538376659216681e-5" iyz="-8.622664755132242e-6" izz="3.7535538604448434e-5"/>
+      <origin xyz="-0.006305773955890982 3.0141314376407138e-5 0.006373188453183354" rpy="0 -0 0"/>
+      <inertia ixx="4.5383766592166864e-5" ixy="-9.375752008423039e-8" ixz="8.622664755132213e-6" iyy="3.848306673536227e-5" iyz="4.870043501244136e-8" izz="3.7535538604448367e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -243,8 +243,8 @@
   <link name="r_shoulder_1">
     <inertial>
       <mass value="0.7922822880502421"/>
-      <origin xyz="1.7243569867333997e-4 -0.06784040318161476 4.760009452753067e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.0510369914064142e-4" ixz="1.9458763763805837e-4" iyy="0.01" iyz="4.97645732286129e-5" izz="0.01"/>
+      <origin xyz="1.7243569867328445e-4 -0.06784040318161474 4.7600094527533443e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -273,8 +273,8 @@
   <link name="r_shoulder_2">
     <inertial>
       <mass value="0.4392272062642899"/>
-      <origin xyz="0.005127191267809918 -1.6659290147985614e-4 -0.02041441690188461" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="5.024923153599912e-5" ixz="-6.506905358695258e-5" iyy="0.01" iyz="7.819501283435285e-5" izz="0.01"/>
+      <origin xyz="0.005113618901434032 -2.13348557806331e-4 -0.020425805469504787" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -300,8 +300,8 @@
   <link name="r_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="-6.954441358278038e-5 1.6100824876602582e-5 -0.007955156752673734" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.7272716990665913e-6" ixz="1.1152637558250718e-5" iyy="0.01" iyz="5.6833371008971195e-6" izz="0.01"/>
+      <origin xyz="-8.31167799586717e-5 -3.0654831449927755e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -330,8 +330,8 @@
   <link name="r_upper_arm">
     <inertial>
       <mass value="1.7722348545631907"/>
-      <origin xyz="1.6057023129404518e-4 -0.002288738185514838 -0.050105724104489345" rpy="0 -0 0"/>
-      <inertia ixx="0.0033762002427288164" ixy="-7.677378856102588e-5" ixz="-5.13345295872612e-4" iyy="0.00337539101140544" iyz="-2.529139265641443e-4" izz="0.001757755824190023"/>
+      <origin xyz="1.4699786491786937e-4 -0.0023354938418406186 -0.05011711267210951" rpy="0 -0 0"/>
+      <inertia ixx="0.0034219198528275844" ixy="3.081029189714289e-5" ixz="2.5743017599563002e-5" iyy="0.003510878854314856" iyz="-1.0938393403810045e-4" izz="0.0015765483711818404"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -360,8 +360,8 @@
   <link name="r_forearm">
     <inertial>
       <mass value="1.4760712424977045"/>
-      <origin xyz="-0.0182933039283515 -0.0021681316645861815 -0.08813804790687912" rpy="0 -0 0"/>
-      <inertia ixx="0.009999999999999998" ixy="-3.2391145849100215e-5" ixz="-5.651353010468485e-4" iyy="0.010000000000000002" iyz="-2.2574875840364485e-4" izz="0.01"/>
+      <origin xyz="-0.01830687629472739 -0.002214887320910963 -0.08814943647449926" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -390,8 +390,8 @@
   <link name="r_wrist_1">
     <inertial>
       <mass value="0.12269276206782376"/>
-      <origin xyz="-1.4544608855514632e-4 1.5592471276534847e-4 0.02237780081793081" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.2420673813880967e-7" ixz="-1.9257205949543276e-7" iyy="0.01" iyz="-6.889140049546541e-7" izz="0.01"/>
+      <origin xyz="-1.4544607175177166e-4 1.559247070407055e-4 0.022377800821402394" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -420,8 +420,8 @@
   <link name="r_wrist_2">
     <inertial>
       <mass value="0.021188357245777188"/>
-      <origin xyz="3.080739829908197e-11 7.897372672057424e-4 -0.004696960058474059" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.132431331458975e-8" ixz="2.6145918714416895e-7" iyy="0.01" iyz="1.1835010162840832e-7" izz="0.01"/>
+      <origin xyz="5.05499045400093e-11 7.897372606293362e-4 -0.004696960055006527" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -450,8 +450,8 @@
   <link name="r_hand_palm">
     <inertial>
       <mass value="0.2020068404688116"/>
-      <origin xyz="-0.003982157998247627 -0.0021135288978345046 -0.05842371394499357" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.433419081989723e-5" ixz="-2.3106097024043265e-5" iyy="0.01" iyz="4.993825362240387e-6" izz="0.01"/>
+      <origin xyz="-0.003982157972782507 -0.0021135289059891482 -0.05842371394186491" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -480,8 +480,8 @@
   <link name="r_hand_index_1">
     <inertial>
       <mass value="0.019232477290189804"/>
-      <origin xyz="-0.004670558279444528 0.003850331668291379 -0.01852531230737764" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.4613089584480577e-7" ixz="-1.444573423930251e-7" iyy="0.01" iyz="-6.395818558046926e-7" izz="0.01"/>
+      <origin xyz="-0.008420124408709713 0.003850331678233287 -0.01714942611279774" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -510,8 +510,8 @@
   <link name="r_hand_middle_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260456246838e-5 0.0011131046723378002 -0.017428809836249337" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-6.992052843719422e-8" ixz="-3.560988417914359e-7" iyy="0.01" iyz="-1.7213594526882048e-7" izz="0.01"/>
+      <origin xyz="5.610258533731913e-5 0.0011131046827904112 -0.01742880983276912" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -540,8 +540,8 @@
   <link name="r_hand_pinkie_1">
     <inertial>
       <mass value="0.006357585733638135"/>
-      <origin xyz="3.616263242815776e-5 0.0012147050861231743 -0.015053109694843136" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.7647335520761375e-8" ixz="-2.7663833562638734e-7" iyy="0.01" iyz="-4.824304563523185e-8" izz="0.01"/>
+      <origin xyz="3.616260147435227e-5 0.0012147050941129778 -0.015053109692718447" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -570,8 +570,8 @@
   <link name="r_hand_ring_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260444077666e-5 0.001113104590923425 -0.017428809844715287" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.610406857428927e-8" ixz="-3.828604023526623e-7" iyy="0.01" iyz="-6.810300838152728e-8" izz="0.01"/>
+      <origin xyz="5.6102585337293107e-5 0.0011131045999587252 -0.017428809839565573" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -600,8 +600,8 @@
   <link name="r_hand_thumb_1">
     <inertial>
       <mass value="0.02894174189037044"/>
-      <origin xyz="0.018375632420818924 -4.555209342888e-5 0.00212647348637468" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.483972069242782e-6" ixz="-4.227782571629177e-7" iyy="0.01" iyz="3.7277898587524307e-7" izz="0.01"/>
+      <origin xyz="0.0183756324403817 -4.55521018183358e-5 0.002126473485347669" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -630,8 +630,8 @@
   <link name="r_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="-0.014887264450410557 -0.001132972632809287 6.266004727595309e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.6052216178597977e-7" ixz="-8.136362383055484e-8" iyy="0.01" iyz="1.4554843972914e-7" izz="0.01"/>
+      <origin xyz="-0.014887264468035837 -0.0011329726380328586 6.26600593671145e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -660,8 +660,8 @@
   <link name="r_hand_thumb_3">
     <inertial>
       <mass value="0.00683159462238797"/>
-      <origin xyz="0.016086236523113466 0.002652687749746474 4.703178261786234e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.7537742494259307e-7" ixz="-2.1198241519414298e-8" iyy="0.01" iyz="4.311056852137086e-8" izz="0.01"/>
+      <origin xyz="0.016086236544425966 0.0026526877650291103 4.703179416121195e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -690,8 +690,8 @@
   <link name="r_hand_ring_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.43251704611413e-5 0.0027547090528209084 -0.018890876422440472" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-7.58504462601195e-9" ixz="-3.2568363341079027e-7" iyy="0.01" iyz="-4.221363162923806e-8" izz="0.01"/>
+      <origin xyz="4.43251343077225e-5 0.00275470906266162 -0.018890876418029168" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -720,8 +720,8 @@
   <link name="r_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237490764073"/>
-      <origin xyz="2.1391107738796128e-5 0.00277128634729798 -0.01893030588274036" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-5.8381716933055835e-9" ixz="-3.2111976226394747e-7" iyy="0.01" iyz="-3.724756521840787e-8" izz="0.01"/>
+      <origin xyz="2.1391072559044655e-5 0.0027712863562942003 -0.018930305880602127" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -750,8 +750,8 @@
   <link name="r_hand_middle_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.432517100235248e-5 0.0027547091809207447 -0.01889087640358672" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.141336189094091e-8" ixz="-2.9259039740622944e-7" iyy="0.01" iyz="-1.7422053024870367e-7" izz="0.01"/>
+      <origin xyz="4.432513430769475e-5 0.0027547091926723166 -0.018890876399070666" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -780,8 +780,8 @@
   <link name="r_hand_index_2">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="0.010265094459698948 0.001113105001514958 -0.01827635665339833" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-9.537054827379165e-8" ixz="-3.233345932270594e-7" iyy="0.01" iyz="-2.6975812067979636e-7" izz="0.01"/>
+      <origin xyz="5.610258533736423e-5 0.0011131050125119946 -0.01742880980571504" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -810,8 +810,8 @@
   <link name="r_hand_index_3">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="0.018874022948048214 0.0027547026093547033 -0.018834827219235406" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.849762497244115e-8" ixz="-2.514360396660324e-7" iyy="0.01" iyz="-3.0309644254617756e-7" izz="0.01"/>
+      <origin xyz="4.432513430773638e-5 0.0027547026217409343 -0.01889087735725714" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -840,8 +840,8 @@
   <link name="neck_3">
     <inertial>
       <mass value="0.17811376821431446"/>
-      <origin xyz="7.590511663558798e-4 -4.091690693859513e-5 0.03603551654922699" rpy="0 -0 0"/>
-      <inertia ixx="1.0759851740143372e-4" ixy="-1.762946883518242e-7" ixz="-2.5039696740768445e-7" iyy="9.705486922197742e-5" iyz="-6.130672988865115e-6" izz="3.6264154407241086e-5"/>
+      <origin xyz="7.590511663558711e-4 -4.0916906938460536e-5 0.036035516549226765" rpy="0 -0 0"/>
+      <inertia ixx="9.70548692219775e-5" ixy="1.7629468835177367e-7" ixz="6.130672988864901e-6" iyy="1.0759851740143377e-4" iyz="-2.503969674072287e-7" izz="3.626415440724103e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -870,8 +870,8 @@
   <link name="head">
     <inertial>
       <mass value="1.972127893184972"/>
-      <origin xyz="-0.01620091407966073 -0.002268813231724585 0.03500744217627949" rpy="0 -0 0"/>
-      <inertia ixx="0.009883124315185845" ixy="-1.818674034830039e-4" ixz="-3.994074773445533e-5" iyy="0.007991907355798097" iyz="1.9244538979180473e-4" izz="0.00786000669473206"/>
+      <origin xyz="-0.01620091412622722 -0.002268813231724542 0.03500744217497026" rpy="0 -0 0"/>
+      <inertia ixx="0.007991907355842523" ixy="1.8186740348760937e-4" ixz="-1.9244538977657914e-4" iyy="0.009883124315185845" iyz="-3.994074771344834e-5" izz="0.00786000669468763"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -900,8 +900,8 @@
   <link name="realsense">
     <inertial>
       <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600984 0.00451999521864419 0.0027456101541037943" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.787553659410962e-5" ixz="-7.183500075449047e-6" iyy="0.01" iyz="-2.5524452311285135e-5" izz="0.01"/>
+      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -930,8 +930,8 @@
   <link name="l_shoulder_2">
     <inertial>
       <mass value="0.44093319265387243"/>
-      <origin xyz="0.007505823346010238 2.3402763110361158e-4 -0.02049745115620874" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-5.157553010803304e-5" ixz="6.569228591220418e-5" iyy="0.01" iyz="7.97840956223945e-5" izz="0.01"/>
+      <origin xyz="0.007519395712385758 1.8727197477702573e-4 -0.020486062588588566" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -957,8 +957,8 @@
   <link name="l_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="6.954441358310651e-5 7.741048802184514e-5 -0.00797793388791404" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.1064791718069957e-6" ixz="-1.1158347995854456e-5" iyy="0.01" iyz="5.801991889044871e-6" izz="0.01"/>
+      <origin xyz="8.311677995862313e-5 3.065483169523153e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -987,8 +987,8 @@
   <link name="l_upper_arm">
     <inertial>
       <mass value="1.7708527474969076"/>
-      <origin xyz="1.169202849922793e-4 0.0022997741939624616 -0.050076427527425534" rpy="0 -0 0"/>
-      <inertia ixx="0.0033774309282785502" ixy="7.386270169901037e-5" ixz="5.095431789297364e-4" iyy="0.003371639006155085" iyz="-2.533199420811916e-4" izz="0.0017505986785028974"/>
+      <origin xyz="1.3049265136780286e-4 0.0022530185376357925 -0.050065038959805325" rpy="0 -0 0"/>
+      <inertia ixx="0.003417154867942194" ixy="-2.952452997527239e-5" ixz="2.6752323704214618e-5" iyy="0.0035109035513553296" iyz="1.0388063737032838e-4" izz="0.0015716101936389657"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1017,8 +1017,8 @@
   <link name="l_forearm">
     <inertial>
       <mass value="1.4604487031386884"/>
-      <origin xyz="-0.018566835695049105 0.002358680882311198 -0.08920511097559716" rpy="0 -0 0"/>
-      <inertia ixx="0.010000000000000002" ixy="3.918099642968445e-5" ixz="5.442336109701719e-4" iyy="0.010000000000000002" iyz="-2.317105945305375e-4" izz="0.01"/>
+      <origin xyz="-0.018553263328673478 0.0023119252259846124 -0.08919372240797706" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.010000000000000002" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1047,8 +1047,8 @@
   <link name="l_wrist_1">
     <inertial>
       <mass value="0.1226928"/>
-      <origin xyz="1.454507181725892e-4 -1.5595312494159908e-4 0.022377795690742608" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.615793735027637e-7" ixz="5.32121794935847e-7" iyy="0.01" iyz="5.639568021417744e-7" izz="0.01"/>
+      <origin xyz="1.4545073498396788e-4 -1.5595311920038601e-4 0.022377795694245" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1077,8 +1077,8 @@
   <link name="l_wrist_2">
     <inertial>
       <mass value="0.02118836"/>
-      <origin xyz="-7.43814359063788e-9 -7.897157600239013e-4 -0.004696967954283704" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.138492578624664e-8" ixz="-2.617165807665742e-7" iyy="0.01" iyz="1.1837099305566312e-7" izz="0.01"/>
+      <origin xyz="-7.4183919215875535e-9 -7.897157534373368e-4 -0.0046969679508168105" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1107,8 +1107,8 @@
   <link name="l_hand_palm">
     <inertial>
       <mass value="0.20364988213680776"/>
-      <origin xyz="-0.003950502243349457 0.0019881930863947794 -0.058015852442294576" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.4578800404078265e-5" ixz="2.4811654406357777e-5" iyy="0.01" iyz="3.5744967821488006e-6" izz="0.01"/>
+      <origin xyz="-0.003950502224178115 0.0019881930945492288 -0.058015852438740544" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1127,8 +1127,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
-    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
+    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
+    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1137,8 +1137,8 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
+      <origin xyz="0.008430306749277854 0.0038510231844406384 -0.017148502808541133" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1167,8 +1167,8 @@
   <link name="l_hand_middle_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.609412499965952e-5 0.0010938363808821183 -0.017422530978911333" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="6.769065023564737e-8" ixz="3.608405680785154e-7" iyy="0.01" iyz="-1.5396152545079645e-7" izz="0.01"/>
+      <origin xyz="5.609414423399586e-5 0.0010938363913530202 -0.0174225309754375" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1197,8 +1197,8 @@
   <link name="l_hand_pinkie_1">
     <inertial>
       <mass value="0.006357586"/>
-      <origin xyz="3.655455845796157e-5 0.001213549159989641 -0.015052861820509966" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="2.5015661539015353e-8" ixz="2.7901203347130377e-7" iyy="0.01" iyz="-3.868557597034084e-8" izz="0.01"/>
+      <origin xyz="3.6554577562467744e-5 0.0012135491682558897 -0.015052861815374408" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1227,8 +1227,8 @@
   <link name="l_hand_ring_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.6109515031453616e-5 0.0011139039467009115 -0.01742875683963907" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.292524499993021e-8" ixz="3.871534855605887e-7" iyy="0.01" iyz="-4.96358923238347e-8" izz="0.01"/>
+      <origin xyz="5.610953414397422e-5 0.001113903955753115 -0.017428756834488968" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1257,8 +1257,8 @@
   <link name="l_hand_thumb_1">
     <inertial>
       <mass value="0.029029542280278556"/>
-      <origin xyz="-0.01803917641362529 -0.0028662725609850115 0.0021177719497344483" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.6089515765306715e-6" ixz="4.604612544346765e-7" iyy="0.01" iyz="3.6759606034343055e-7" izz="0.01"/>
+      <origin xyz="-0.018039176431428286 -0.0028662725726853466 0.0021177719486905056" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1287,8 +1287,8 @@
   <link name="l_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="0.014887264450400842 0.0011329726328125345 6.26600714702108e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-3.508321707683706e-7" ixz="8.213304862846372e-8" iyy="0.01" iyz="1.4694684276477687e-7" izz="0.01"/>
+      <origin xyz="0.014887264468035857 0.0011329726380328031 6.266005936719776e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1317,8 +1317,8 @@
   <link name="l_hand_thumb_3">
     <inertial>
       <mass value="0.006831595"/>
-      <origin xyz="0.01608625251142736 0.002652688715795104 4.7014270457834684e-5" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-4.6702502025036103e-7" ixz="2.0823539173591144e-8" iyy="0.01" iyz="4.4870066350370055e-8" izz="0.01"/>
+      <origin xyz="0.016086252532748233 0.0026526887310780456 4.701425890352162e-5" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1347,8 +1347,8 @@
   <link name="l_hand_ring_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.433650413547367e-5 0.002754675449902322 -0.018890859501747292" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.977013947931596e-9" ixz="3.289401428370411e-7" iyy="0.01" iyz="-2.7220703393624056e-8" izz="0.01"/>
+      <origin xyz="4.433654030530642e-5 0.0027546754597598533 -0.01889085949732494" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1377,8 +1377,8 @@
   <link name="l_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237"/>
-      <origin xyz="2.2378257433602194e-5 0.0027712834700559796 -0.018930353880242057" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.9098038872728015e-9" ixz="3.2394130716454584e-7" iyy="0.01" iyz="-3.023638062784071e-8" izz="0.01"/>
+      <origin xyz="2.2378292628411067e-5 0.0027712834790767915 -0.018930353878106154" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1407,8 +1407,8 @@
   <link name="l_hand_middle_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.431628345990049e-5 0.002754722935079862 -0.018890877324266697" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="3.2930780755833666e-8" ixz="3.226980212060653e-7" iyy="0.01" iyz="-1.6629193018826528e-7" izz="0.01"/>
+      <origin xyz="4.4316320155644157e-5 0.0027547229469275236 -0.018890877319942878" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1427,8 +1427,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
-    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
+    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
+    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1437,8 +1437,8 @@
   <link name="l_hand_index_2">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="-0.010155342467888226 0.0011131331804158373 -0.01829967928994858" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="9.396780653347036e-8" ixz="3.2904462665817386e-7" iyy="0.01" iyz="-2.5247311144531184e-7" izz="0.01"/>
+      <origin xyz="5.6100110941745274e-5 0.001113133191428639 -0.01742880406449937" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1457,8 +1457,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -1467,8 +1467,8 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
+      <origin xyz="4.431356673249148e-5 0.0027546711855188366 -0.018890916917498335" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1497,8 +1497,8 @@
   <link name="r_hip_2">
     <inertial>
       <mass value="1.8425374083524118"/>
-      <origin xyz="-0.004338858395565106 -4.2767854114084125e-5 -0.037115837283052844" rpy="0 -0 0"/>
-      <inertia ixx="0.005459166113270066" ixy="-2.373531512034788e-6" ixz="-1.8523984061175535e-6" iyy="0.003120303249647113" iyz="-0.0010124685247925136" izz="0.0034190028448521583"/>
+      <origin xyz="-0.004338858395569037 -4.276785411407025e-5 -0.03711583728304848" rpy="0 -0 0"/>
+      <inertia ixx="0.003120303249647105" ixy="2.3735315120274186e-6" ixz="0.0010124685247925123" iyy="0.005459166113270065" iyz="-1.8523984061140002e-6" izz="0.003419002844852166"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1524,8 +1524,8 @@
   <link name="r_hip_3">
     <inertial>
       <mass value="2.147900058066585"/>
-      <origin xyz="0.005101012450187747 0.012832092744145249 -0.05168842502826276" rpy="0 -0 0"/>
-      <inertia ixx="0.008149001665389528" ixy="-3.498727117566707e-5" ixz="-1.2572783372468983e-4" iyy="0.007193177523135792" iyz="-2.4096738440553857e-5" izz="0.0057278283877820494"/>
+      <origin xyz="0.0051510124501743006 0.012832092744145235 -0.05168842502826221" rpy="0 -0 0"/>
+      <inertia ixx="0.007193177523135792" ixy="3.498727117566612e-5" ixz="2.4096738440560362e-5" iyy="0.008149001665389528" iyz="-1.2572783372468582e-4" izz="0.0057278283877820494"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1554,8 +1554,8 @@
   <link name="r_upper_leg">
     <inertial>
       <mass value="2.784923431729287"/>
-      <origin xyz="0.008467456382678921 -0.01446847602698749 -0.12674960557185633" rpy="0 -0 0"/>
-      <inertia ixx="0.012109133352085043" ixy="-5.455450241372635e-5" ixz="-4.033104101548086e-4" iyy="0.011860132529998117" iyz="-5.393573740676439e-4" izz="0.0022609706907693226"/>
+      <origin xyz="0.008517474024287675 -0.014468465702474021 -0.12674960557185616" rpy="0 -0 0"/>
+      <inertia ixx="0.01186013252999811" ixy="5.455450241372543e-5" ixz="5.393573740676933e-4" iyy="0.01210913335208505" iyz="-4.0331041015476653e-4" izz="0.0022609706907693243"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1584,8 +1584,8 @@
   <link name="r_lower_leg">
     <inertial>
       <mass value="3.7621532589747924"/>
-      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
-      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
+      <origin xyz="0.0012157689164458522 -0.006184734585333342 -0.21372759879868175" rpy="0 -0 0"/>
+      <inertia ixx="0.04644215616821937" ixy="3.7269274872175814e-5" ixz="-8.519885904633137e-4" iyy="0.047738936736850285" iyz="-0.0018042981721195497" izz="0.004839671863500647"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1614,8 +1614,8 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="0.01" iyz="1.0336975691919178e-5" izz="0.01"/>
+      <origin xyz="-0.0013672309746954776 -6.60186959019704e-4 -0.014087432401547305" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1644,8 +1644,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.010000000000000002"/>
+      <origin xyz="0.04052646811695935 -8.642493098851012e-5 -0.023871143533700234" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.009999999999999998" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1671,8 +1671,8 @@
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466904026456e-9" izz="0.01"/>
+      <origin xyz="5.879202911245884e-4 -3.747924692820703e-9 -0.02000061661590058" rpy="0 -0 0"/>
+      <inertia ixx="0.009999999999999998" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1698,8 +1698,8 @@
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646669939015e-9" izz="0.01"/>
+      <origin xyz="5.879202911244878e-4 -3.747924665065128e-9 -0.020000616615901023" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1728,8 +1728,8 @@
   <link name="l_hip_2">
     <inertial>
       <mass value="1.8338392164825101"/>
-      <origin xyz="-0.004125697068299468 3.917475804630921e-5 -0.03750964603556273" rpy="0 -0 0"/>
-      <inertia ixx="0.005488243677251951" ixy="1.6542815235225134e-6" ixz="-2.190409388735156e-6" iyy="0.003159514595905932" iyz="-9.977368607484041e-4" izz="0.0033984945726571133"/>
+      <origin xyz="-0.004125697068299468 3.917475804632309e-5 -0.03750964603556273" rpy="0 -0 0"/>
+      <inertia ixx="0.003159514595905926" ixy="-1.6542815235215681e-6" ixz="9.977368607484037e-4" iyy="0.005488243677251951" iyz="-2.1904093887368695e-6" izz="0.0033984945726571185"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1755,8 +1755,8 @@
   <link name="l_hip_3">
     <inertial>
       <mass value="2.147524084050246"/>
-      <origin xyz="0.005176125839592961 -0.012828813026822289 -0.051748524474533986" rpy="0 -0 0"/>
-      <inertia ixx="0.008197962692605143" ixy="3.321392954197018e-5" ixz="1.247487484059602e-4" iyy="0.007242161971060034" iyz="-2.5059855054008952e-5" izz="0.005727458772759987"/>
+      <origin xyz="0.005176125839593024 -0.01282881302682233 -0.05174852447453404" rpy="0 -0 0"/>
+      <inertia ixx="0.007242161971060034" ixy="-3.321392954197042e-5" ixz="2.505985505401264e-5" iyy="0.008197962692605146" iyz="1.2474874840596051e-4" izz="0.005727458772759987"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1785,8 +1785,8 @@
   <link name="l_upper_leg">
     <inertial>
       <mass value="2.784923431765453"/>
-      <origin xyz="0.008503494694160165 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
-      <inertia ixx="0.012109807149567942" ixy="4.907145319316716e-5" ixz="3.9208918647670385e-4" iyy="0.01185229395228421" iyz="-5.393933817877802e-4" izz="0.002253627916731069"/>
+      <origin xyz="0.008503494694160183 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
+      <inertia ixx="0.011852293952284196" ixy="-4.907145319317264e-5" ixz="5.393933817879138e-4" iyy="0.012109807149567942" iyz="3.9208918647670385e-4" izz="0.002253627916731084"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1815,8 +1815,8 @@
   <link name="l_lower_leg">
     <inertial>
       <mass value="3.767581145212888"/>
-      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
-      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
+      <origin xyz="0.0014201768088090358 0.006186333743956739 -0.21383298861124922" rpy="0 -0 0"/>
+      <inertia ixx="0.04647110900067475" ixy="-2.1189289797886653e-5" ixz="-8.110710215722959e-4" iyy="0.04776726641499918" iyz="0.0018123762001582078" izz="0.004840491251955261"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1845,8 +1845,8 @@
   <link name="l_ankle_1">
     <inertial>
       <mass value="0.3481278951519182"/>
-      <origin xyz="-0.0013852125067176438 6.601869600096483e-4 -0.0140874324743252" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="8.882076128770337e-6" ixz="-5.5239266791031694e-5" iyy="0.01" iyz="7.868443555727937e-6" izz="0.01"/>
+      <origin xyz="-0.0013852125067176195 6.601869600093568e-4 -0.014087432474324535" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1875,8 +1875,8 @@
   <link name="l_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040522400508854174 -9.790751646714113e-5 -0.023871143533686023" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-1.9973294407367236e-5" ixz="-1.5070602593012222e-5" iyy="0.01" iyz="-0.0013251325687512945" izz="0.01"/>
+      <origin xyz="0.0405224005088543 -9.790751646740482e-5 -0.02387114353368558" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.010000000000000002" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1902,8 +1902,8 @@
   <link name="l_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911260178e-4 -3.747924276487069e-9 -0.02000061661590191" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646683126688e-9" izz="0.01"/>
+      <origin xyz="5.879202911245329e-4 -3.747924692820703e-9 -0.02000061661590158" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1929,8 +1929,8 @@
   <link name="l_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911259571e-4 -3.747924290364857e-9 -0.02000061661590158" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466744864883e-9" izz="0.01"/>
+      <origin xyz="5.879202911244826e-4 -3.7479246789429155e-9 -0.020000616615901134" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
index c2c0238d..4de41b4c 100644
--- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
@@ -4,7 +4,7 @@
     <inertial>
       <mass value="2.823751482824641"/>
       <origin xyz="0.0024748962432214403 -1.050794470993344e-4 0.023450368787955644" rpy="0 -0 0"/>
-      <inertia ixx="0.004380033669258687" ixy="-1.4369039688385888e-6" ixz="1.4098441290530498e-5" iyy="0.005630606285299466" iyz="-2.3299340994588877e-5" izz="0.003616681010808376"/>
+      <inertia ixx="0.005630606285299466" ixy="1.4369039688385168e-6" ixz="2.329934099458601e-5" iyy="0.004380033669258687" iyz="1.4098441290533537e-5" izz="0.003616681010808376"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -36,8 +36,8 @@
   <link name="l_hip_1">
     <inertial>
       <mass value="2.231768868859629"/>
-      <origin xyz="-0.004015422485162342 0.028247280560774645 -6.811143992383195e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.002695326128467937" ixy="-5.971255499379551e-5" ixz="-5.838535051658256e-6" iyy="0.0022109218149310014" iyz="1.5432160879022375e-5" izz="0.0024610381641843883"/>
+      <origin xyz="-0.0040154224851623455 0.028247280560774652 -6.811143992383195e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.002210921814931002" ixy="5.971255499379551e-5" ixz="-1.5432160879022988e-5" iyy="0.002695326128467937" iyz="-5.838535051658118e-6" izz="0.0024610381641843883"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -69,8 +69,8 @@
   <link name="r_hip_1">
     <inertial>
       <mass value="2.2320852637398954"/>
-      <origin xyz="-0.004032117730258741 -0.028249385885545365 -6.739019720396755e-4" rpy="0 -0 0"/>
-      <inertia ixx="0.0026955458913177294" ixy="6.126720536020865e-5" ixz="6.443355522460479e-6" iyy="0.0022122594774356403" iyz="1.5035655169600434e-5" izz="0.0024608876750051717"/>
+      <origin xyz="-0.004032117730258793 -0.028249385885545365 -6.739019720393702e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.00221225947743564" ixy="-6.126720536020865e-5" ixz="-1.5035655169601288e-5" iyy="0.002695545891317729" iyz="6.443355522460266e-6" izz="0.0024608876750051726"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -102,8 +102,8 @@
   <link name="torso_1">
     <inertial>
       <mass value="2.0645667"/>
-      <origin xyz="-0.009639973621975028 6.168494736287859e-4 0.08244231288506343" rpy="0 -0 0"/>
-      <inertia ixx="0.007406086000000001" ixy="6.975203277646223e-6" ixz="-1.2586871224024975e-5" iyy="0.005947073300000001" iyz="7.227736063238349e-4" izz="0.0031780111"/>
+      <origin xyz="-0.00963997362197503 6.168494736287859e-4 0.08244231288506354" rpy="0 -0 0"/>
+      <inertia ixx="0.007406085999999999" ixy="0" ixz="0" iyy="0.0059470732999999994" iyz="0" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -135,8 +135,8 @@
   <link name="torso_2">
     <inertial>
       <mass value="0.7051627057068411"/>
-      <origin xyz="0.0015724674776760397 0.0036223466761378916 0.026019504666210665" rpy="0 -0 0"/>
-      <inertia ixx="6.675525890436374e-4" ixy="-4.198623927073284e-6" ixz="7.014302140884287e-5" iyy="0.0012590296877101025" iyz="5.398163583772658e-6" izz="0.001145822810927014"/>
+      <origin xyz="0.0015724674776760373 0.0036223466761378907 0.026019504666210766" rpy="0 -0 0"/>
+      <inertia ixx="0.0012590296877101025" ixy="4.1986239270733835e-6" ixz="-5.398163583772818e-6" iyy="6.675525890436374e-4" iyz="7.014302140884287e-5" izz="0.001145822810927014"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -168,8 +168,8 @@
   <link name="chest">
     <inertial>
       <mass value="8.065471632648594"/>
-      <origin xyz="-0.010721051678060343 -3.482064053382885e-4 0.07611063517438789" rpy="0 -0 0"/>
-      <inertia ixx="0.04493883052455447" ixy="-1.7611941682820536e-4" ixz="-1.2962048932620152e-4" iyy="0.04845475402461777" iyz="3.124089280569461e-4" izz="0.03068768729801644"/>
+      <origin xyz="-0.010721051678060345 -3.482064053382885e-4 0.07611063517438793" rpy="0 -0 0"/>
+      <inertia ixx="0.048454754024617776" ixy="1.7611941682820642e-4" ixz="-3.124089280569708e-4" iyy="0.044938830524554475" iyz="-1.2962048932614557e-4" izz="0.030687687298016447"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -201,8 +201,8 @@
   <link name="l_shoulder_1">
     <inertial>
       <mass value="0.7913756606310043"/>
-      <origin xyz="1.482427542201549e-4 0.06782971819041982 5.036196460998787e-4" rpy="0 -0 0"/>
-      <inertia ixx="4.33126825498053e-4" ixy="2.0497606829731344e-4" ixz="-1.9449541410974418e-4" iyy="0.0011548041680336774" iyz="5.017853886187208e-5" izz="0.0011461196178955747"/>
+      <origin xyz="1.4824275422014796e-4 0.06782971819041986 5.036196460999898e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.0012090260632276477" ixy="3.837452877706498e-6" ixz="3.4979966648173376e-7" iyy="3.2970130687487713e-4" iyz="1.0257960656819495e-5" izz="0.0011953232413247816"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -234,8 +234,8 @@
   <link name="neck_2">
     <inertial>
       <mass value="0.12100314943032965"/>
-      <origin xyz="-0.006305773955890981 3.0141314376408408e-5 0.006373188453183354" rpy="0 -0 0"/>
-      <inertia ixx="3.848306673536227e-5" ixy="9.375752008419092e-8" ixz="4.870043501240908e-8" iyy="4.538376659216681e-5" iyz="-8.622664755132242e-6" izz="3.7535538604448434e-5"/>
+      <origin xyz="-0.006305773955890982 3.0141314376407138e-5 0.006373188453183354" rpy="0 -0 0"/>
+      <inertia ixx="4.5383766592166864e-5" ixy="-9.375752008423039e-8" ixz="8.622664755132213e-6" iyy="3.848306673536227e-5" iyz="4.870043501244136e-8" izz="3.7535538604448367e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -267,8 +267,8 @@
   <link name="r_shoulder_1">
     <inertial>
       <mass value="0.7922822880502421"/>
-      <origin xyz="1.7243569867333997e-4 -0.06784040318161476 4.760009452753067e-4" rpy="0 -0 0"/>
-      <inertia ixx="4.3422182324316314e-4" ixy="-2.0510369914064142e-4" ixz="1.9458763763805837e-4" iyy="0.0011552753477506248" iyz="4.97645732286129e-5" izz="0.0011466564773150232"/>
+      <origin xyz="1.7243569867328445e-4 -0.06784040318161474 4.7600094527533443e-4" rpy="0 -0 0"/>
+      <inertia ixx="0.0012096539626434014" ixy="-3.680808987116546e-6" ixz="8.229428308001538e-7" iyy="3.3067105254799123e-4" iyz="-1.0172274013350282e-5" izz="0.0011958286331174221"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -300,8 +300,8 @@
   <link name="r_shoulder_2">
     <inertial>
       <mass value="0.4392272062642899"/>
-      <origin xyz="0.005127191267809918 -1.6659290147985614e-4 -0.02041441690188461" rpy="0 -0 0"/>
-      <inertia ixx="3.9569579075834417e-4" ixy="5.024923153599912e-5" ixz="-6.506905358695258e-5" iyy="3.2092942321175174e-4" iyz="7.819501283435285e-5" izz="2.674404937949735e-4"/>
+      <origin xyz="0.005113618901434032 -2.13348557806331e-4 -0.020425805469504787" rpy="0 -0 0"/>
+      <inertia ixx="2.9042245268154257e-4" ixy="-9.557527654531481e-7" ixz="-9.749291990105912e-5" iyy="4.271705089556584e-4" iyz="-1.2227717602335833e-6" izz="2.6647274612786855e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -330,8 +330,8 @@
   <link name="r_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="-6.954441358278038e-5 1.6100824876602582e-5 -0.007955156752673734" rpy="0 -0 0"/>
-      <inertia ixx="7.889184821987055e-5" ixy="1.7272716990665913e-6" ixz="1.1152637558250718e-5" iyy="7.709796065096908e-5" iyz="5.6833371008971195e-6" izz="1.241020479967083e-4"/>
+      <origin xyz="-8.31167799586717e-5 -3.0654831449927755e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="7.620284216410257e-5" ixy="-3.654997747197785e-7" ixz="-3.6240077897294374e-8" iyy="7.648716704064904e-5" iyz="-6.051355657868799e-8" izz="1.2740184766279634e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -363,8 +363,8 @@
   <link name="r_upper_arm">
     <inertial>
       <mass value="1.7722348545631907"/>
-      <origin xyz="1.6057023129404518e-4 -0.002288738185514838 -0.050105724104489345" rpy="0 -0 0"/>
-      <inertia ixx="0.0033762002427288164" ixy="-7.677378856102588e-5" ixz="-5.13345295872612e-4" iyy="0.00337539101140544" iyz="-2.529139265641443e-4" izz="0.001757755824190023"/>
+      <origin xyz="1.4699786491786937e-4 -0.0023354938418406186 -0.05011711267210951" rpy="0 -0 0"/>
+      <inertia ixx="0.0034219198528275844" ixy="3.081029189714289e-5" ixz="2.5743017599563002e-5" iyy="0.003510878854314856" iyz="-1.0938393403810045e-4" izz="0.0015765483711818404"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -396,8 +396,8 @@
   <link name="r_forearm">
     <inertial>
       <mass value="1.4760712424977045"/>
-      <origin xyz="-0.0182933039283515 -0.0021681316645861815 -0.08813804790687912" rpy="0 -0 0"/>
-      <inertia ixx="0.003957111430628234" ixy="-3.2391145849100215e-5" ixz="-5.651353010468485e-4" iyy="0.004004435813236756" iyz="-2.2574875840364485e-4" izz="0.001668610835071044"/>
+      <origin xyz="-0.01830687629472739 -0.002214887320910963 -0.08814943647449926" rpy="0 -0 0"/>
+      <inertia ixx="0.004018141035792247" ixy="-1.0169837170335636e-6" ixz="-6.092751707960711e-5" iyy="0.004094401103638503" iyz="2.3622606931134418e-5" izz="0.00151761593950533"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -429,8 +429,8 @@
   <link name="r_wrist_1">
     <inertial>
       <mass value="0.12269276206782376"/>
-      <origin xyz="-1.4544608855514632e-4 1.5592471276534847e-4 0.02237780081793081" rpy="0 -0 0"/>
-      <inertia ixx="4.909120593413644e-5" ixy="-1.2420673813880967e-7" ixz="-1.9257205949543276e-7" iyy="4.987546767153799e-5" iyz="-6.889140049546541e-7" izz="5.0753515234514626e-5"/>
+      <origin xyz="-1.4544607175177166e-4 1.559247070407055e-4 0.022377800821402394" rpy="0 -0 0"/>
+      <inertia ixx="4.9948289621641885e-5" ixy="-2.784766720355678e-7" ixz="6.019515820494411e-7" iyy="4.9363344899850536e-5" iyz="-7.101822163237812e-7" izz="5.04085543186966e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -462,8 +462,8 @@
   <link name="r_wrist_2">
     <inertial>
       <mass value="0.021188357245777188"/>
-      <origin xyz="3.080739829908197e-11 7.897372672057424e-4 -0.004696960058474059" rpy="0 -0 0"/>
-      <inertia ixx="1.6848934291760358e-6" ixy="4.132431331458975e-8" ixz="2.6145918714416895e-7" iyy="1.6067563092611862e-6" iyz="1.1835010162840832e-7" izz="2.411003313048518e-6"/>
+      <origin xyz="5.05499045400093e-11 7.897372606293362e-4 -0.004696960055006527" rpy="0 -0 0"/>
+      <inertia ixx="1.5886797233767314e-6" ixy="3.7411538116720276e-13" ixz="-1.6476959268145782e-14" iyy="1.6075566141746972e-6" iyz="7.534291394795915e-8" izz="2.5064167139343093e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -495,8 +495,8 @@
   <link name="r_hand_palm">
     <inertial>
       <mass value="0.2020068404688116"/>
-      <origin xyz="-0.003982157998247627 -0.0021135288978345046 -0.05842371394499357" rpy="0 -0 0"/>
-      <inertia ixx="2.0279675255745254e-4" ixy="1.433419081989723e-5" ixz="-2.3106097024043265e-5" iyy="1.4745140848954725e-4" iyz="4.993825362240387e-6" izz="1.0983268122615596e-4"/>
+      <origin xyz="-0.003982157972782507 -0.0021135289059891482 -0.05842371394186491" rpy="0 -0 0"/>
+      <inertia ixx="1.4279727238409082e-4" ixy="4.3471459905317745e-6" ixz="-1.2099467097336674e-5" iyy="2.1040345151106222e-4" iyz="4.7767131643957866e-6" izz="1.0688011837800253e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -528,8 +528,8 @@
   <link name="r_hand_index_1">
     <inertial>
       <mass value="0.019232477290189804"/>
-      <origin xyz="-0.004670558279444528 0.003850331668291379 -0.01852531230737764" rpy="0 -0 0"/>
-      <inertia ixx="2.8522220170899855e-6" ixy="-1.4613089584480577e-7" ixz="-1.444573423930251e-7" iyy="2.369629411622327e-6" iyz="-6.395818558046926e-7" izz="1.4270535066802096e-6"/>
+      <origin xyz="-0.008420124408709713 0.003850331678233287 -0.01714942611279774" rpy="0 -0 0"/>
+      <inertia ixx="2.6266478205033898e-6" ixy="1.0045828131985182e-7" ixz="-4.4428748588513116e-7" iyy="2.811973348231671e-6" iyz="-3.9646985411500977e-8" izz="1.21028376665746e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -561,8 +561,8 @@
   <link name="r_hand_middle_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260456246838e-5 0.0011131046723378002 -0.017428809836249337" rpy="0 -0 0"/>
-      <inertia ixx="1.4184333919587362e-6" ixy="-6.992052843719422e-8" ixz="-3.560988417914359e-7" iyy="1.5623426808149364e-6" iyz="-1.7213594526882048e-7" izz="4.5180796325247034e-7"/>
+      <origin xyz="5.610258533731913e-5 0.0011131046827904112 -0.01742880983276912" rpy="0 -0 0"/>
+      <inertia ixx="1.5923690026243956e-6" ixy="-7.6859949856616e-10" ixz="-9.576514629966624e-9" iyy="1.5225016121802755e-6" iyz="1.1152097367596593e-7" izz="3.177134212214704e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -594,8 +594,8 @@
   <link name="r_hand_pinkie_1">
     <inertial>
       <mass value="0.006357585733638135"/>
-      <origin xyz="3.616263242815776e-5 0.0012147050861231743 -0.015053109694843136" rpy="0 -0 0"/>
-      <inertia ixx="1.0151021263419818e-6" ixy="-2.7647335520761375e-8" ixz="-2.7663833562638734e-7" iyy="1.1540732735924168e-6" iyz="-4.824304563523185e-8" izz="3.68818316243984e-7"/>
+      <origin xyz="3.616260147435227e-5 0.0012147050941129778 -0.015053109692718447" rpy="0 -0 0"/>
+      <inertia ixx="1.1594224636752146e-6" ixy="1.9700677724003336e-10" ixz="-5.145341462812334e-9" iyy="1.1056270005627898e-6" iyz="9.04498861481507e-8" izz="2.7294425194037764e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -627,8 +627,8 @@
   <link name="r_hand_ring_1">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="5.610260444077666e-5 0.001113104590923425 -0.017428809844715287" rpy="0 -0 0"/>
-      <inertia ixx="1.4002809247665375e-6" ixy="-3.610406857428927e-8" ixz="-3.828604023526623e-7" iyy="1.5856639014534724e-6" iyz="-6.810300838152728e-8" izz="4.4663921792356476e-7"/>
+      <origin xyz="5.6102585337293107e-5 0.0011131045999587252 -0.017428809839565573" rpy="0 -0 0"/>
+      <inertia ixx="1.5923690066834338e-6" ixy="-7.685995334343241e-10" ixz="-9.576514632814455e-9" iyy="1.5225016134675864e-6" iyz="1.115209689762951e-7" izz="3.177134239925537e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -660,8 +660,8 @@
   <link name="r_hand_thumb_1">
     <inertial>
       <mass value="0.02894174189037044"/>
-      <origin xyz="0.018375632420818924 -4.555209342888e-5 0.00212647348637468" rpy="0 -0 0"/>
-      <inertia ixx="6.64388084338799e-6" ixy="1.483972069242782e-6" ixz="-4.227782571629177e-7" iyy="3.197183978767725e-6" iyz="3.7277898587524307e-7" izz="5.817402989911044e-6"/>
+      <origin xyz="0.0183756324403817 -4.55521018183358e-5 0.002126473485347669" rpy="0 -0 0"/>
+      <inertia ixx="2.6227221096875315e-6" ixy="-2.6657118889494294e-7" ixz="3.408188388661989e-7" iyy="7.222316801810369e-6" iyz="1.3067300647554165e-7" izz="5.813428900568859e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -693,8 +693,8 @@
   <link name="r_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="-0.014887264450410557 -0.001132972632809287 6.266004727595309e-5" rpy="0 -0 0"/>
-      <inertia ixx="9.87853595299825e-7" ixy="3.6052216178597977e-7" ixz="-8.136362383055484e-8" iyy="4.960351011902446e-7" iyz="1.4554843972914e-7" izz="1.0917736733090493e-6"/>
+      <origin xyz="-0.014887264468035837 -0.0011329726380328586 6.26600593671145e-5" rpy="0 -0 0"/>
+      <inertia ixx="2.822738702941517e-7" ixy="-9.431423655785553e-8" ixz="-7.94903600654804e-9" iyy="1.1148767778556382e-6" iyz="7.629054803653638e-10" izz="1.1785117216493283e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -726,8 +726,8 @@
   <link name="r_hand_thumb_3">
     <inertial>
       <mass value="0.00683159462238797"/>
-      <origin xyz="0.016086236523113466 0.002652687749746474 4.703178261786234e-5" rpy="0 -0 0"/>
-      <inertia ixx="1.2280141668674389e-6" ixy="4.7537742494259307e-7" ixz="-2.1198241519414298e-8" iyy="5.272192177481829e-7" iyz="4.311056852137086e-8" izz="1.4737205455489289e-6"/>
+      <origin xyz="0.016086236544425966 0.0026526877650291103 4.703179416121195e-5" rpy="0 -0 0"/>
+      <inertia ixx="2.881152136289015e-7" ixy="5.927787437409324e-8" ixz="7.046450596142318e-9" iyy="1.4727314074745805e-6" iyz="-1.1370823402676918e-10" izz="1.4681073090610674e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -759,8 +759,8 @@
   <link name="r_hand_ring_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.43251704611413e-5 0.0027547090528209084 -0.018890876422440472" rpy="0 -0 0"/>
-      <inertia ixx="1.8715176762830657e-6" ixy="-7.58504462601195e-9" ixz="-3.2568363341079027e-7" iyy="1.9323057422890318e-6" iyz="-4.221363162923806e-8" izz="3.827386107622495e-7"/>
+      <origin xyz="4.43251343077225e-5 0.00275470906266162 -0.018890876418029168" rpy="0 -0 0"/>
+      <inertia ixx="1.9331833105730582e-6" ixy="-7.989212176587309e-11" ixz="-7.904608363472145e-9" iyy="1.9365695874997407e-6" iyz="-7.314014027601115e-8" izz="3.168091312615482e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -792,8 +792,8 @@
   <link name="r_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237490764073"/>
-      <origin xyz="2.1391107738796128e-5 0.00277128634729798 -0.01893030588274036" rpy="0 -0 0"/>
-      <inertia ixx="1.8658146064729271e-6" ixy="-5.8381716933055835e-9" ixz="-3.2111976226394747e-7" iyy="1.9250896455343995e-6" iyz="-3.724756521840787e-8" izz="3.799290979284571e-7"/>
+      <origin xyz="2.1391072559044655e-5 0.0027712863562942003 -0.018930305880602127" rpy="0 -0 0"/>
+      <inertia ixx="1.925475606102824e-6" ixy="-4.3197295418411515e-10" ixz="-3.753180689187112e-9" iyy="1.929147380651044e-6" iyz="-7.607784499660943e-8" izz="3.1621036318191297e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -825,8 +825,8 @@
   <link name="r_hand_middle_2">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="4.432517100235248e-5 0.0027547091809207447 -0.01889087640358672" rpy="0 -0 0"/>
-      <inertia ixx="1.8843822940808722e-6" ixy="-3.141336189094091e-8" ixz="-2.9259039740622944e-7" iyy="1.9139438821238177e-6" iyz="-1.7422053024870367e-7" izz="3.882358531291246e-7"/>
+      <origin xyz="4.432513430769475e-5 0.0027547091926723166 -0.018890876399070666" rpy="0 -0 0"/>
+      <inertia ixx="1.933183310574237e-6" ixy="-7.989206743982703e-11" ixz="-7.904608363479557e-9" iyy="1.9365732602456635e-6" iyz="-7.313677768867388e-8" izz="3.168054585139121e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -858,8 +858,8 @@
   <link name="r_hand_index_2">
     <inertial>
       <mass value="0.007486365997467528"/>
-      <origin xyz="0.010265094459698948 0.001113105001514958 -0.01827635665339833" rpy="0 -0 0"/>
-      <inertia ixx="1.436941208581246e-6" ixy="-9.537054827379165e-8" ixz="-3.233345932270594e-7" iyy="1.522576006571057e-6" iyz="-2.6975812067979636e-7" izz="4.7306678855939303e-7"/>
+      <origin xyz="5.610258533736423e-5 0.0011131050125119946 -0.01742880980571504" rpy="0 -0 0"/>
+      <inertia ixx="1.5923689864672536e-6" ixy="-7.685993601390065e-10" ixz="-9.576514618642257e-9" iyy="1.5225016070540494e-6" iyz="1.1152099238402203e-7" izz="3.1771341019039245e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -891,8 +891,8 @@
   <link name="r_hand_index_3">
     <inertial>
       <mass value="0.007251413355667432"/>
-      <origin xyz="0.018874022948048214 0.0027547026093547033 -0.018834827219235406" rpy="0 -0 0"/>
-      <inertia ixx="1.8980608643084195e-6" ixy="-4.849762497244115e-8" ixz="-2.514360396660324e-7" iyy="1.8731359000131537e-6" iyz="-3.0309644254617756e-7" izz="4.1536526501140273e-7"/>
+      <origin xyz="4.432513430773638e-5 0.0027547026217409343 -0.01889087735725714" rpy="0 -0 0"/>
+      <inertia ixx="1.9331833106031376e-6" ixy="-7.989481683614064e-11" ixz="-7.904608320181986e-9" iyy="1.9365664798884807e-6" iyz="-7.313734234921826e-8" izz="3.168122388413564e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -924,8 +924,8 @@
   <link name="neck_3">
     <inertial>
       <mass value="0.17811376821431446"/>
-      <origin xyz="7.590511663558798e-4 -4.091690693859513e-5 0.03603551654922699" rpy="0 -0 0"/>
-      <inertia ixx="1.0759851740143372e-4" ixy="-1.762946883518242e-7" ixz="-2.5039696740768445e-7" iyy="9.705486922197742e-5" iyz="-6.130672988865115e-6" izz="3.6264154407241086e-5"/>
+      <origin xyz="7.590511663558711e-4 -4.0916906938460536e-5 0.036035516549226765" rpy="0 -0 0"/>
+      <inertia ixx="9.70548692219775e-5" ixy="1.7629468835177367e-7" ixz="6.130672988864901e-6" iyy="1.0759851740143377e-4" iyz="-2.503969674072287e-7" izz="3.626415440724103e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -957,8 +957,8 @@
   <link name="head">
     <inertial>
       <mass value="1.972127893184972"/>
-      <origin xyz="-0.01620091407966073 -0.002268813231724585 0.03500744217627949" rpy="0 -0 0"/>
-      <inertia ixx="0.009883124315185845" ixy="-1.818674034830039e-4" ixz="-3.994074773445533e-5" iyy="0.007991907355798097" iyz="1.9244538979180473e-4" izz="0.00786000669473206"/>
+      <origin xyz="-0.01620091412622722 -0.002268813231724542 0.03500744217497026" rpy="0 -0 0"/>
+      <inertia ixx="0.007991907355842523" ixy="1.8186740348760937e-4" ixz="-1.9244538977657914e-4" iyy="0.009883124315185845" iyz="-3.994074771344834e-5" izz="0.00786000669468763"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -990,8 +990,8 @@
   <link name="realsense">
     <inertial>
       <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600984 0.00451999521864419 0.0027456101541037943" rpy="0 -0 0"/>
-      <inertia ixx="1.8708641575714764e-4" ixy="-4.787553659410962e-5" ixz="-7.183500075449047e-6" iyy="1.3364464963624943e-4" iyz="-2.5524452311285135e-5" izz="2.963196622871262e-4"/>
+      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <inertia ixx="1.3363140922191058e-4" ixy="4.7877399688365374e-5" ixz="2.5482221369268495e-5" iyy="1.8708641575714753e-4" iyz="-7.171072206331662e-6" izz="2.963329027014652e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1023,8 +1023,8 @@
   <link name="l_shoulder_2">
     <inertial>
       <mass value="0.44093319265387243"/>
-      <origin xyz="0.007505823346010238 2.3402763110361158e-4 -0.02049745115620874" rpy="0 -0 0"/>
-      <inertia ixx="3.9954225516225387e-4" ixy="-5.157553010803304e-5" ixz="6.569228591220418e-5" iyy="3.2267244497365393e-4" iyz="7.97840956223945e-5" izz="2.7204910861740035e-4"/>
+      <origin xyz="0.007519395712385758 1.8727197477702573e-4 -0.020486062588588566" rpy="0 -0 0"/>
+      <inertia ixx="2.9145977319834226e-4" ixy="1.1454484909976663e-6" ixz="-9.910474228153797e-5" iyy="4.316468020210048e-4" iyz="1.4378502430788573e-6" izz="2.711572335339609e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1053,8 +1053,8 @@
   <link name="l_shoulder_3">
     <inertial>
       <mass value="0.271466272759966"/>
-      <origin xyz="6.954441358310651e-5 7.741048802184514e-5 -0.00797793388791404" rpy="0 -0 0"/>
-      <inertia ixx="7.927191583488577e-5" ixy="-1.1064791718069957e-6" ixz="-1.1158347995854456e-5" iyy="7.672965670990288e-5" iyz="5.801991889044871e-6" izz="1.2409028432272892e-4"/>
+      <origin xyz="8.311677995862313e-5 3.065483169523153e-5 -0.007966545320293866" rpy="0 -0 0"/>
+      <inertia ixx="7.620284216408771e-5" ixy="-3.6549977471418807e-7" ixz="3.624007789477276e-8" iyy="7.648716704064355e-5" iyz="6.05135559486276e-8" izz="1.2740184766278623e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1086,8 +1086,8 @@
   <link name="l_upper_arm">
     <inertial>
       <mass value="1.7708527474969076"/>
-      <origin xyz="1.169202849922793e-4 0.0022997741939624616 -0.050076427527425534" rpy="0 -0 0"/>
-      <inertia ixx="0.0033774309282785502" ixy="7.386270169901037e-5" ixz="5.095431789297364e-4" iyy="0.003371639006155085" iyz="-2.533199420811916e-4" izz="0.0017505986785028974"/>
+      <origin xyz="1.3049265136780286e-4 0.0022530185376357925 -0.050065038959805325" rpy="0 -0 0"/>
+      <inertia ixx="0.003417154867942194" ixy="-2.952452997527239e-5" ixz="2.6752323704214618e-5" iyy="0.0035109035513553296" iyz="1.0388063737032838e-4" izz="0.0015716101936389657"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1119,8 +1119,8 @@
   <link name="l_forearm">
     <inertial>
       <mass value="1.4604487031386884"/>
-      <origin xyz="-0.018566835695049105 0.002358680882311198 -0.08920511097559716" rpy="0 -0 0"/>
-      <inertia ixx="0.003836066755501407" ixy="3.918099642968445e-5" ixz="5.442336109701719e-4" iyy="0.0038743157390891507" iyz="-2.317105945305375e-4" izz="0.0016569853669527875"/>
+      <origin xyz="-0.018553263328673478 0.0023119252259846124 -0.08919372240797706" rpy="0 -0 0"/>
+      <inertia ixx="0.003893738896214435" ixy="-3.4642535094897738e-6" ixz="-4.190258302572787e-5" iyy="0.003967428265005762" iyz="-1.2157659778638894e-5" izz="0.0015062007003231051"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1152,8 +1152,8 @@
   <link name="l_wrist_1">
     <inertial>
       <mass value="0.1226928"/>
-      <origin xyz="1.454507181725892e-4 -1.5595312494159908e-4 0.022377795690742608" rpy="0 -0 0"/>
-      <inertia ixx="4.922390975976726e-5" ixy="3.615793735027637e-7" ixz="5.32121794935847e-7" iyy="4.9875676346755436e-5" iyz="5.639568021417744e-7" izz="5.061705798678361e-5"/>
+      <origin xyz="1.4545073498396788e-4 -1.5595311920038601e-4 0.022377795694245" rpy="0 -0 0"/>
+      <inertia ixx="4.99469100996649e-5" ixy="-2.787171151199186e-7" ixz="-6.017806812164961e-7" iyy="4.93613984105829e-5" iyz="7.090130898393705e-7" izz="5.040833558305853e-5"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1185,8 +1185,8 @@
   <link name="l_wrist_2">
     <inertial>
       <mass value="0.02118836"/>
-      <origin xyz="-7.43814359063788e-9 -7.897157600239013e-4 -0.004696967954283704" rpy="0 -0 0"/>
-      <inertia ixx="1.6847571509740292e-6" ixy="-4.138492578624664e-8" ixz="-2.617165807665742e-7" iyy="1.606318104081993e-6" iyz="1.1837099305566312e-7" izz="2.4109738107740318e-6"/>
+      <origin xyz="-7.4183919215875535e-9 -7.897157534373368e-4 -0.0046969679508168105" rpy="0 -0 0"/>
+      <inertia ixx="1.588238425453937e-6" ixy="-2.165272906156668e-11" ixz="7.915941092897307e-11" iyy="1.6073132474088142e-6" iyz="-7.554073452985005e-8" izz="2.5064973929673043e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1218,8 +1218,8 @@
   <link name="l_hand_palm">
     <inertial>
       <mass value="0.20364988213680776"/>
-      <origin xyz="-0.003950502243349457 0.0019881930863947794 -0.058015852442294576" rpy="0 -0 0"/>
-      <inertia ixx="2.059979835473565e-4" ixy="-1.4578800404078265e-5" ixz="2.4811654406357777e-5" iyy="1.5089523433999236e-4" iyz="3.5744967821488006e-6" izz="1.1031333738865725e-4"/>
+      <origin xyz="-0.003950502224178115 0.0019881930945492288 -0.058015852438740544" rpy="0 -0 0"/>
+      <inertia ixx="1.461813098502197e-4" ixy="-3.793772344466637e-6" ixz="-1.1466988740793722e-5" iyy="2.145411925003367e-4" iyz="-3.736671542252551e-6" izz="1.0648405292544985e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1251,8 +1251,8 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
-      <inertia ixx="2.8516997256950557e-6" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="2.3676120706829812e-6" iyz="-6.425403568227921e-7" izz="1.4278260577276285e-6"/>
+      <origin xyz="0.008430306749277854 0.0038510231844406384 -0.017148502808541133" rpy="0 -0 0"/>
+      <inertia ixx="2.6252721263619075e-6" ixy="-9.94793061395749e-8" ixz="4.4747998977494927e-7" iyy="2.8117732161881507e-6" iyz="-3.9466206080886376e-8" izz="1.2100925115556048e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1284,8 +1284,8 @@
   <link name="l_hand_middle_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.609412499965952e-5 0.0010938363808821183 -0.017422530978911333" rpy="0 -0 0"/>
-      <inertia ixx="1.4176096881635584e-6" ixy="6.769065023564737e-8" ixz="3.608405680785154e-7" iyy="1.5664118150309605e-6" iyz="-1.5396152545079645e-7" izz="4.5016344749841866e-7"/>
+      <origin xyz="5.609414423399586e-5 0.0010938363913530202 -0.0174225309754375" rpy="0 -0 0"/>
+      <inertia ixx="1.5929480475374711e-6" ixy="-9.219821293951626e-10" ixz="-9.505332652309699e-9" iyy="1.5230397093076881e-6" iyz="1.1029328740444945e-7" izz="3.1819719384777893e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1317,8 +1317,8 @@
   <link name="l_hand_pinkie_1">
     <inertial>
       <mass value="0.006357586"/>
-      <origin xyz="3.655455845796157e-5 0.001213549159989641 -0.015052861820509966" rpy="0 -0 0"/>
-      <inertia ixx="1.0134116165159135e-6" ixy="2.5015661539015353e-8" ixz="2.7901203347130377e-7" iyy="1.1550035037591326e-6" iyz="-3.868557597034084e-8" izz="3.69895210497436e-7"/>
+      <origin xyz="3.6554577562467744e-5 0.0012135491682558897 -0.015052861815374408" rpy="0 -0 0"/>
+      <inertia ixx="1.1595706744459758e-6" ixy="1.817145860874702e-10" ixz="-5.131228041325918e-9" iyy="1.1053210733791516e-6" iyz="9.124017197402374e-8" izz="2.734185829473546e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1350,8 +1350,8 @@
   <link name="l_hand_ring_1">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="5.6109515031453616e-5 0.0011139039467009115 -0.01742875683963907" rpy="0 -0 0"/>
-      <inertia ixx="1.3989915224602604e-6" ixy="3.292524499993021e-8" ixz="3.871534855605887e-7" iyy="1.586434348399735e-6" iyz="-4.96358923238347e-8" izz="4.477311919350591e-7"/>
+      <origin xyz="5.610953414397422e-5 0.001113903955753115 -0.017428756834488968" rpy="0 -0 0"/>
+      <inertia ixx="1.5926175322430852e-6" ixy="-7.593027571592815e-10" ixz="-9.630025713700034e-9" iyy="1.522746441882303e-6" iyz="1.1162272723786417e-7" izz="3.177930886696654e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1383,8 +1383,8 @@
   <link name="l_hand_thumb_1">
     <inertial>
       <mass value="0.029029542280278556"/>
-      <origin xyz="-0.01803917641362529 -0.0028662725609850115 0.0021177719497344483" rpy="0 -0 0"/>
-      <inertia ixx="6.5832240305966255e-6" ixy="-1.6089515765306715e-6" ixz="4.604612544346765e-7" iyy="3.426453755052308e-6" iyz="3.6759606034343055e-7" izz="5.817681606427074e-6"/>
+      <origin xyz="-0.018039176431428286 -0.0028662725726853466 0.0021177719486905056" rpy="0 -0 0"/>
+      <inertia ixx="2.7178312304541084e-6" ixy="-3.676108153783739e-7" ixz="-2.7762391665657096e-7" iyy="7.273678916289491e-6" iyz="1.0493561895389714e-7" izz="5.835849245332402e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1416,8 +1416,8 @@
   <link name="l_hand_thumb_2">
     <inertial>
       <mass value="0.00669603218404702"/>
-      <origin xyz="0.014887264450400842 0.0011329726328125345 6.26600714702108e-5" rpy="0 -0 0"/>
-      <inertia ixx="1.00044124022547e-6" ixy="-3.508321707683706e-7" ixz="8.213304862846372e-8" iyy="4.834328147504152e-7" iyz="1.4694684276477687e-7" izz="1.0917883148226653e-6"/>
+      <origin xyz="0.014887264468035857 0.0011329726380328031 6.266005936719776e-5" rpy="0 -0 0"/>
+      <inertia ixx="2.82273870293961e-7" ixy="-9.431423655787125e-8" ixz="7.949036006532621e-9" iyy="1.1148767778556158e-6" iyz="-7.629054803910611e-10" izz="1.1785117216489732e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1449,8 +1449,8 @@
   <link name="l_hand_thumb_3">
     <inertial>
       <mass value="0.006831595"/>
-      <origin xyz="0.01608625251142736 0.002652688715795104 4.7014270457834684e-5" rpy="0 -0 0"/>
-      <inertia ixx="1.2392248328077927e-6" ixy="-4.6702502025036103e-7" ixz="2.0823539173591144e-8" iyy="5.161386632973405e-7" iyz="4.4870066350370055e-8" izz="1.4734420077506914e-6"/>
+      <origin xyz="0.016086252532748233 0.0026526887310780456 4.701425890352162e-5" rpy="0 -0 0"/>
+      <inertia ixx="2.8800798429683427e-7" ixy="5.926758782001814e-8" ixz="6.997805198870725e-9" iyy="1.472681587075844e-6" iyz="-2.1551532736764057e-11" izz="1.4681159324831446e-6"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1482,8 +1482,8 @@
   <link name="l_hand_ring_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.433650413547367e-5 0.002754675449902322 -0.018890859501747292" rpy="0 -0 0"/>
-      <inertia ixx="1.8699438083679598e-6" ixy="3.977013947931596e-9" ixz="3.289401428370411e-7" iyy="1.9331566011413766e-6" iyz="-2.7220703393624056e-8" izz="3.8362910599147074e-7"/>
+      <origin xyz="4.433654030530642e-5 0.0027546754597598533 -0.01889085949732494" rpy="0 -0 0"/>
+      <inertia ixx="1.9331168483007796e-6" ixy="-8.964055458821853e-11" ixz="-7.917778264767445e-9" iyy="1.9366717747954655e-6" iyz="-7.299706676136226e-8" izz="3.1694089240456203e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1515,8 +1515,8 @@
   <link name="l_hand_pinkie_2">
     <inertial>
       <mass value="0.007240237"/>
-      <origin xyz="2.2378257433602194e-5 0.0027712834700559796 -0.018930353880242057" rpy="0 -0 0"/>
-      <inertia ixx="1.8646926594602145e-6" ixy="4.9098038872728015e-9" ixz="3.2394130716454584e-7" iyy="1.924876215564609e-6" iyz="-3.023638062784071e-8" izz="3.808734123700184e-7"/>
+      <origin xyz="2.2378292628411067e-5 0.0027712834790767915 -0.018930353878106154" rpy="0 -0 0"/>
+      <inertia ixx="1.9251949577353665e-6" ixy="-4.6071411032444134e-10" ixz="-3.7359984605543596e-9" iyy="1.9290048779043837e-6" iyz="-7.606337504739766e-8" izz="3.162424517550918e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1548,8 +1548,8 @@
   <link name="l_hand_middle_2">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="4.431628345990049e-5 0.002754722935079862 -0.018890877324266697" rpy="0 -0 0"/>
-      <inertia ixx="1.871772832486776e-6" ixy="3.2930780755833666e-8" ixz="3.226980212060653e-7" iyy="1.915706329588829e-6" iyz="-1.6629193018826528e-7" izz="3.9902034887079913e-7"/>
+      <origin xyz="4.4316320155644157e-5 0.0027547229469275236 -0.018890877319942878" rpy="0 -0 0"/>
+      <inertia ixx="1.933060230194342e-6" ixy="-1.5243567957691928e-10" ixz="-7.8198851655833e-9" iyy="1.9366210236852174e-6" iyz="-7.316224558810137e-8" izz="3.1681825706684464e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1581,8 +1581,8 @@
   <link name="l_hand_index_2">
     <inertial>
       <mass value="0.007486366"/>
-      <origin xyz="-0.010155342467888226 0.0011131331804158373 -0.01829967928994858" rpy="0 -0 0"/>
-      <inertia ixx="1.4355311723018076e-6" ixy="9.396780653347036e-8" ixz="3.2904462665817386e-7" iyy="1.5286679233975063e-6" iyz="-2.5247311144531184e-7" izz="4.6787849880342953e-7"/>
+      <origin xyz="5.6100110941745274e-5 0.001113133191428639 -0.01742880406449937" rpy="0 -0 0"/>
+      <inertia ixx="1.5919421212473372e-6" ixy="-8.197016561018782e-10" ixz="-9.495218356033137e-9" iyy="1.5224880972680802e-6" iyz="1.1144592625272454e-7" izz="3.1764737598732553e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1614,8 +1614,8 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
-      <inertia ixx="1.8961170965705488e-6" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="1.8786285070808614e-6" iyz="-2.891156821380605e-7" izz="4.113501491925148e-7"/>
+      <origin xyz="4.431356673249148e-5 0.0027546711855188366 -0.018890916917498335" rpy="0 -0 0"/>
+      <inertia ixx="1.932816895772585e-6" ixy="-1.721386036062205e-10" ixz="-7.819556637146551e-9" iyy="1.9363709956455972e-6" iyz="-7.306511833741725e-8" izz="3.169078614257433e-7"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1647,8 +1647,8 @@
   <link name="r_hip_2">
     <inertial>
       <mass value="1.8425374083524118"/>
-      <origin xyz="-0.004338858395565106 -4.2767854114084125e-5 -0.037115837283052844" rpy="0 -0 0"/>
-      <inertia ixx="0.005459166113270066" ixy="-2.373531512034788e-6" ixz="-1.8523984061175535e-6" iyy="0.003120303249647113" iyz="-0.0010124685247925136" izz="0.0034190028448521583"/>
+      <origin xyz="-0.004338858395569037 -4.276785411407025e-5 -0.03711583728304848" rpy="0 -0 0"/>
+      <inertia ixx="0.003120303249647105" ixy="2.3735315120274186e-6" ixz="0.0010124685247925123" iyy="0.005459166113270065" iyz="-1.8523984061140002e-6" izz="0.003419002844852166"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1677,8 +1677,8 @@
   <link name="r_hip_3">
     <inertial>
       <mass value="2.147900058066585"/>
-      <origin xyz="0.005101012450187747 0.012832092744145249 -0.05168842502826276" rpy="0 -0 0"/>
-      <inertia ixx="0.008149001665389528" ixy="-3.498727117566707e-5" ixz="-1.2572783372468983e-4" iyy="0.007193177523135792" iyz="-2.4096738440553857e-5" izz="0.0057278283877820494"/>
+      <origin xyz="0.0051510124501743006 0.012832092744145235 -0.05168842502826221" rpy="0 -0 0"/>
+      <inertia ixx="0.007193177523135792" ixy="3.498727117566612e-5" ixz="2.4096738440560362e-5" iyy="0.008149001665389528" iyz="-1.2572783372468582e-4" izz="0.0057278283877820494"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1710,8 +1710,8 @@
   <link name="r_upper_leg">
     <inertial>
       <mass value="2.784923431729287"/>
-      <origin xyz="0.008467456382678921 -0.01446847602698749 -0.12674960557185633" rpy="0 -0 0"/>
-      <inertia ixx="0.012109133352085043" ixy="-5.455450241372635e-5" ixz="-4.033104101548086e-4" iyy="0.011860132529998117" iyz="-5.393573740676439e-4" izz="0.0022609706907693226"/>
+      <origin xyz="0.008517474024287675 -0.014468465702474021 -0.12674960557185616" rpy="0 -0 0"/>
+      <inertia ixx="0.01186013252999811" ixy="5.455450241372543e-5" ixz="5.393573740676933e-4" iyy="0.01210913335208505" iyz="-4.0331041015476653e-4" izz="0.0022609706907693243"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1743,8 +1743,8 @@
   <link name="r_lower_leg">
     <inertial>
       <mass value="3.7621532589747924"/>
-      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
-      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
+      <origin xyz="0.0012157689164458522 -0.006184734585333342 -0.21372759879868175" rpy="0 -0 0"/>
+      <inertia ixx="0.04644215616821937" ixy="3.7269274872175814e-5" ixz="-8.519885904633137e-4" iyy="0.047738936736850285" iyz="-0.0018042981721195497" izz="0.004839671863500647"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1776,8 +1776,8 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
-      <inertia ixx="5.753493395636923e-4" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="2.47340125952152e-4" iyz="1.0336975691919178e-5" izz="4.626760389351452e-4"/>
+      <origin xyz="-0.0013672309746954776 -6.60186959019704e-4 -0.014087432401547305" rpy="0 -0 0"/>
+      <inertia ixx="2.473401259521522e-4" ixy="8.99187320178455e-6" ixz="-1.033697569192024e-5" iyy="5.75349339563699e-4" iyz="5.5239266965789845e-5" izz="4.6267603893513845e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1809,8 +1809,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
-      <inertia ixx="0.009438348902378935" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.002144088293229814" iyz="-0.0013223759105872523" izz="0.009024460701601934"/>
+      <origin xyz="0.04052646811695935 -8.642493098851012e-5 -0.023871143533700234" rpy="0 -0 0"/>
+      <inertia ixx="0.002144088293229702" ixy="2.3071453411493782e-5" ixz="0.0013223759105869606" iyy="0.009438348902378931" iyz="-3.077717707147486e-5" izz="0.009024460701602049"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1839,8 +1839,8 @@
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
-      <inertia ixx="2.6091266876367765e-4" ixy="0" ixz="0" iyy="1.912506447891574e-4" iyz="3.4706466904026456e-9" izz="4.500881026346283e-4"/>
+      <origin xyz="5.879202911245884e-4 -3.747924692820703e-9 -0.02000061661590058" rpy="0 -0 0"/>
+      <inertia ixx="1.912506447891574e-4" ixy="2.357895453228089e-19" ixz="-3.4706466956949253e-9" iyy="2.6091266876367765e-4" iyz="5.375663344600725e-18" izz="4.500881026346283e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1866,8 +1866,8 @@
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
-      <inertia ixx="2.609126687634939e-4" ixy="0" ixz="0" iyy="1.912506447889355e-4" iyz="3.470646669939015e-9" izz="4.5008810263463014e-4"/>
+      <origin xyz="5.879202911244878e-4 -3.747924665065128e-9 -0.020000616615901023" rpy="0 -0 0"/>
+      <inertia ixx="1.9125064478893547e-4" ixy="2.3192252624427304e-19" ixz="-3.4706466752314563e-9" iyy="2.609126687634939e-4" iyz="5.375663150730269e-18" izz="4.5008810263463014e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1896,8 +1896,8 @@
   <link name="l_hip_2">
     <inertial>
       <mass value="1.8338392164825101"/>
-      <origin xyz="-0.004125697068299468 3.917475804630921e-5 -0.03750964603556273" rpy="0 -0 0"/>
-      <inertia ixx="0.005488243677251951" ixy="1.6542815235225134e-6" ixz="-2.190409388735156e-6" iyy="0.003159514595905932" iyz="-9.977368607484041e-4" izz="0.0033984945726571133"/>
+      <origin xyz="-0.004125697068299468 3.917475804632309e-5 -0.03750964603556273" rpy="0 -0 0"/>
+      <inertia ixx="0.003159514595905926" ixy="-1.6542815235215681e-6" ixz="9.977368607484037e-4" iyy="0.005488243677251951" iyz="-2.1904093887368695e-6" izz="0.0033984945726571185"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1926,8 +1926,8 @@
   <link name="l_hip_3">
     <inertial>
       <mass value="2.147524084050246"/>
-      <origin xyz="0.005176125839592961 -0.012828813026822289 -0.051748524474533986" rpy="0 -0 0"/>
-      <inertia ixx="0.008197962692605143" ixy="3.321392954197018e-5" ixz="1.247487484059602e-4" iyy="0.007242161971060034" iyz="-2.5059855054008952e-5" izz="0.005727458772759987"/>
+      <origin xyz="0.005176125839593024 -0.01282881302682233 -0.05174852447453404" rpy="0 -0 0"/>
+      <inertia ixx="0.007242161971060034" ixy="-3.321392954197042e-5" ixz="2.505985505401264e-5" iyy="0.008197962692605146" iyz="1.2474874840596051e-4" izz="0.005727458772759987"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1959,8 +1959,8 @@
   <link name="l_upper_leg">
     <inertial>
       <mass value="2.784923431765453"/>
-      <origin xyz="0.008503494694160165 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
-      <inertia ixx="0.012109807149567942" ixy="4.907145319316716e-5" ixz="3.9208918647670385e-4" iyy="0.01185229395228421" iyz="-5.393933817877802e-4" izz="0.002253627916731069"/>
+      <origin xyz="0.008503494694160183 0.014484874736048647 -0.12674659802303312" rpy="0 -0 0"/>
+      <inertia ixx="0.011852293952284196" ixy="-4.907145319317264e-5" ixz="5.393933817879138e-4" iyy="0.012109807149567942" iyz="3.9208918647670385e-4" izz="0.002253627916731084"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1992,8 +1992,8 @@
   <link name="l_lower_leg">
     <inertial>
       <mass value="3.767581145212888"/>
-      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
-      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
+      <origin xyz="0.0014201768088090358 0.006186333743956739 -0.21383298861124922" rpy="0 -0 0"/>
+      <inertia ixx="0.04647110900067475" ixy="-2.1189289797886653e-5" ixz="-8.110710215722959e-4" iyy="0.04776726641499918" iyz="0.0018123762001582078" izz="0.004840491251955261"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2025,8 +2025,8 @@
   <link name="l_ankle_1">
     <inertial>
       <mass value="0.3481278951519182"/>
-      <origin xyz="-0.0013852125067176438 6.601869600096483e-4 -0.0140874324743252" rpy="0 -0 0"/>
-      <inertia ixx="5.753321096553087e-4" ixy="8.882076128770337e-6" ixz="-5.5239266791031694e-5" iyy="2.4734012682242564e-4" iyz="7.868443555727937e-6" izz="4.6265880787367867e-4"/>
+      <origin xyz="-0.0013852125067176195 6.601869600093568e-4 -0.014087432474324535" rpy="0 -0 0"/>
+      <inertia ixx="2.473401268224257e-4" ixy="-8.882076128769303e-6" ixz="-7.868443555728613e-6" iyy="5.753321096553087e-4" iyz="-5.523926679103168e-5" izz="4.6265880787367867e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2058,8 +2058,8 @@
   <link name="l_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040522400508854174 -9.790751646714113e-5 -0.023871143533686023" rpy="0 -0 0"/>
-      <inertia ixx="0.009438197785206428" ixy="-1.9973294407367236e-5" ixz="-1.5070602593012222e-5" iyy="0.0021370613362321164" iyz="-0.0013251325687512945" izz="0.009017282627430335"/>
+      <origin xyz="0.0405224005088543 -9.790751646740482e-5 -0.02387114353368558" rpy="0 -0 0"/>
+      <inertia ixx="0.00213706133623208" ixy="1.9973294407387216e-5" ixz="0.0013251325687512008" iyy="0.00943819778520643" iyz="-1.5070602593015444e-5" izz="0.00901728262743037"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2088,8 +2088,8 @@
   <link name="l_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911260178e-4 -3.747924276487069e-9 -0.02000061661590191" rpy="0 -0 0"/>
-      <inertia ixx="2.609126687635385e-4" ixy="0" ixz="0" iyy="1.9125064478898708e-4" iyz="3.470646683126688e-9" izz="4.5008810263460987e-4"/>
+      <origin xyz="5.879202911245329e-4 -3.747924692820703e-9 -0.02000061661590158" rpy="0 -0 0"/>
+      <inertia ixx="1.9125064478898706e-4" ixy="2.0351856158593786e-19" ixz="-3.470646686211968e-9" iyy="2.6091266876353844e-4" iyz="9.784057075164868e-21" izz="4.5008810263460987e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2115,8 +2115,8 @@
   <link name="l_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911259571e-4 -3.747924290364857e-9 -0.02000061661590158" rpy="0 -0 0"/>
-      <inertia ixx="2.6091266876349345e-4" ixy="0" ixz="0" iyy="1.912506447889398e-4" iyz="3.4706466744864883e-9" izz="4.50088102634634e-4"/>
+      <origin xyz="5.879202911244826e-4 -3.7479246789429155e-9 -0.020000616615901134" rpy="0 -0 0"/>
+      <inertia ixx="1.912506447889398e-4" ixy="2.0351856158593786e-19" ixz="-3.470646677571385e-9" iyy="2.6091266876349345e-4" iyz="9.784057075164868e-21" izz="4.50088102634634e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>