diff --git a/robots/b2_description/urdf/b2_description.urdf b/robots/b2_description/urdf/b2_description.urdf
index 3e589b9..66eeb9e 100644
--- a/robots/b2_description/urdf/b2_description.urdf
+++ b/robots/b2_description/urdf/b2_description.urdf
@@ -11,7 +11,7 @@
         xyz="0.000458 0.005261 0.000665"
         rpy="0 0 0" />
       <mass
-        value="35.606" />
+        value="35.86" />
       <inertia
         ixx="0.27466"
         ixy="-0.000622"
@@ -47,17 +47,17 @@
     name="lidar_link">
     <inertial>
       <origin
-        xyz="4.22E-15 1.05E-14 -3.05E-16"
+        xyz="0.34218 0 0.17851"
         rpy="0 0 0" />
       <mass
-        value="6.55E-08" />
+        value="4.19E-15" />
       <inertia
-        ixx="1.64E-15"
-        ixy="0"
-        ixz="-1.09E-47"
-        iyy="1.64E-15"
-        iyz="-1.32E-47"
-        izz="1.64E-15" />
+        ixx="1.68E-27"
+        ixy="-3.2E-45"
+        ixz="7.68E-44"
+        iyy="1.68E-27"
+        iyz="-9.26E-47"
+        izz="1.68E-27" />
     </inertial>
     <collision>
       <origin
@@ -87,17 +87,17 @@
     name="f_dc_link">
     <inertial>
       <origin
-        xyz="-3.47E-18 -1.11E-16 1.11E-16"
+        xyz="3.47E-18 0 5.55E-17"
         rpy="0 0 0" />
       <mass
-        value="6.54E-08" />
+        value="4.19E-15" />
       <inertia
-        ixx="1.64E-15"
-        ixy="1.87E-32"
-        ixz="-6.15E-32"
-        iyy="1.64E-15"
-        iyz="4.93E-32"
-        izz="1.64E-15" />
+        ixx="1.68E-27"
+        ixy="-9.81E-45"
+        ixz="-2.54E-44"
+        iyy="1.68E-27"
+        iyz="0"
+        izz="1.68E-27" />
     </inertial>
   </link>
   <joint
@@ -117,17 +117,17 @@
     name="r_dc_link">
     <inertial>
       <origin
-        xyz="1.63E-16 1.39E-16 2.78E-17"
+        xyz="-7.46E-16 -3.33E-16 -1.11E-15"
         rpy="0 0 0" />
       <mass
-        value="6.55E-08" />
+        value="4.19E-15" />
       <inertia
-        ixx="1.64E-15"
-        ixy="-4.94E-44"
-        ixz="4.36E-43"
-        iyy="1.64E-15"
+        ixx="1.68E-27"
+        ixy="-4.31E-44"
+        ixz="2.48E-45"
+        iyy="1.68E-27"
         iyz="0"
-        izz="1.64E-15" />
+        izz="1.68E-27" />
     </inertial>
   </link>
   <joint
@@ -147,17 +147,17 @@
     name="imu_link">
     <inertial>
       <origin
-        xyz="-1.11E-16 -2.81E-16 -1.32E-16"
+        xyz="-2.02E-17 1.73E-17 -1.39E-17"
         rpy="0 0 0" />
       <mass
-        value="6.54E-08" />
+        value="4.19E-15" />
       <inertia
-        ixx="1.64E-15"
-        ixy="0"
-        ixz="-1.09E-47"
-        iyy="1.64E-15"
-        iyz="-1.32E-47"
-        izz="1.64E-15" />
+        ixx="1.68E-27"
+        ixy="1.84E-44"
+        ixz="-7.35E-44"
+        iyy="1.68E-27"
+        iyz="-3.67E-44"
+        izz="1.68E-27" />
     </inertial>
   </link>
   <joint
@@ -180,7 +180,7 @@
         xyz="0.33989 -0.000168 0.12029"
         rpy="0 0 0" />
       <mass
-        value="3.366" />
+        value="3.6885" />
       <inertia
         ixx="0.026455"
         ixy="3.15E-05"
@@ -218,7 +218,7 @@
         xyz="-0.37448 0.000488 0.006495"
         rpy="0 0 0" />
       <mass
-        value="0.777" />
+        value="0.795" />
       <inertia
         ixx="0.0028118"
         ixy="1.95E-05"
@@ -253,17 +253,17 @@
     name="FL_hip">
     <inertial>
       <origin
-        xyz="-0.009305 -0.010228 0.000264"
+        xyz="-0.003841 -0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="0.00019234"
-        ixz="-6.76E-06"
-        iyy="0.0046728"
-        iyz="-7.16E-06"
-        izz="0.0034208" />
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="-3.77E-07"
+        iyy="0.0048743"
+        iyz="4E-09"
+        izz="0.0037087" />
     </inertial>
     <visual>
       <origin
@@ -334,17 +334,17 @@
     name="FL_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 -0.035797 -0.044921"
+        xyz="-0.006279 -0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="0.005796"
-        izz="0.0065677" />
+        ixx="0.062299"
+        ixy="0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="0.0083537"
+        izz="0.0081997" />
     </inertial>
     <visual>
       <origin
@@ -418,7 +418,7 @@
         xyz="0.012422 0 -0.12499"
         rpy="0 0 0" />
       <mass
-        value="0.58094" />
+        value="0.404" />
       <inertia
         ixx="0.01143"
         ixy="0"
@@ -515,7 +515,7 @@
         xyz="-0.006511 0 -0.010144"
         rpy="0 0 0" />
       <mass
-        value="0.098183" />
+        value="0.126" />
       <inertia
         ixx="3.8E-05"
         ixy="0"
@@ -629,17 +629,17 @@
     name="FR_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 0.035797 -0.044921"
+        xyz="-0.006279 0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="-0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="-0.005796"
-        izz="0.0065677" />
+        ixx="0.06229908"
+        ixy="-0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="-0.008353671"
+        izz="0.008199745" />
     </inertial>
     <visual>
       <origin
@@ -711,7 +711,7 @@
         xyz="0.012422 0 -0.12499"
         rpy="0 0 0" />
       <mass
-        value="0.58094" />
+        value="0.404" />
       <inertia
         ixx="0.01143"
         ixy="0"
@@ -809,7 +809,7 @@
         xyz="-0.006511 0 -0.010144"
         rpy="0 0 0" />
       <mass
-        value="0.098183" />
+        value="0.126" />
       <inertia
         ixx="3.8E-05"
         ixy="0"
@@ -923,17 +923,17 @@
     name="RL_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 -0.035797 -0.044921"
+        xyz="-0.006279 -0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="0.005796"
-        izz="0.0065677" />
+        ixx="0.06229908"
+        ixy="0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="0.008353671"
+        izz="0.008199745" />
     </inertial>
     <visual>
       <origin
@@ -1005,7 +1005,7 @@
         xyz="0.012422 0 -0.12499"
         rpy="0 0 0" />
       <mass
-        value="0.58094" />
+        value="0.404" />
       <inertia
         ixx="0.01143"
         ixy="0"
@@ -1102,7 +1102,7 @@
         xyz="-0.006511 0 -0.010144"
         rpy="0 0 0" />
       <mass
-        value="0.098183" />
+        value="0.126" />
       <inertia
         ixx="3.8E-05"
         ixy="0"
@@ -1135,17 +1135,17 @@
     name="RR_hip">
     <inertial>
       <origin
-        xyz="0.009305 -0.010228 0.000264"
+        xyz="0.003841 0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="0.00019234"
-        ixz="6.76E-06"
-        iyy="0.0046728"
-        iyz="7.16E-06"
-        izz="0.0034208" />
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="3.77E-07"
+        iyy="0.0048743"
+        iyz="-4E-09"
+        izz="0.0037087" />
     </inertial>
     <visual>
       <origin
@@ -1216,17 +1216,17 @@
     name="RR_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 0.035797 -0.044921"
+        xyz="-0.006279 0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="-0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="-0.005796"
-        izz="0.0065677" />
+        ixx="0.062299"
+        ixy="-0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="-0.0083537"
+        izz="0.0081997" />
     </inertial>
     <visual>
       <origin
@@ -1298,7 +1298,7 @@
         xyz="0.012422 0 -0.12499"
         rpy="0 0 0" />
       <mass
-        value="0.58094" />
+        value="0.404" />
       <inertia
         ixx="0.01143"
         ixy="0"
@@ -1395,7 +1395,7 @@
         xyz="-0.006511 0 -0.010144"
         rpy="0 0 0" />
       <mass
-        value="0.098183" />
+        value="0.126" />
       <inertia
         ixx="3.8E-05"
         ixy="0"
@@ -1428,17 +1428,17 @@
     name="f_oc_link">
     <inertial>
       <origin
-        xyz="-2.78E-17 1.73E-18 0"
+        xyz="-3.39E-15 1.46E-16 -6.11E-16"
         rpy="0 0 0" />
       <mass
-        value="6.55E-08" />
+        value="4.19E-15" />
       <inertia
-        ixx="1.64E-15"
-        ixy="-1.32E-47"
-        ixz="0"
-        iyy="1.64E-15"
-        iyz="1.09E-47"
-        izz="1.64E-15" />
+        ixx="1.68E-27"
+        ixy="-3.89E-44"
+        ixz="5.04E-44"
+        iyy="1.68E-27"
+        iyz="2.81E-45"
+        izz="1.68E-27" />
     </inertial>
     <!-- <visual>
       <origin
@@ -1481,17 +1481,17 @@
     name="r_oc_link">
     <inertial>
       <origin
-        xyz="-1.3E-19 3.47E-18 -5.55E-17"
+        xyz="-0.39143 1.52E-07 -0.011961"
         rpy="0 0 0" />
       <mass
-        value="6.55E-08" />
+        value="4.19E-12" />
       <inertia
-        ixx="1.64E-15"
-        ixy="1.33E-47"
-        ixz="-6.38E-33"
-        iyy="1.64E-15"
-        iyz="-1.64E-47"
-        izz="1.64E-15" />
+        ixx="1.68E-22"
+        ixy="9.03E-39"
+        ixz="0"
+        iyy="1.68E-22"
+        iyz="-1.2E-38"
+        izz="1.68E-22" />
     </inertial>
     <!-- <visual>
       <origin
diff --git a/robots/b2w_description/urdf/b2w_description.urdf b/robots/b2w_description/urdf/b2w_description.urdf
index 5ba0396..08b4181 100644
--- a/robots/b2w_description/urdf/b2w_description.urdf
+++ b/robots/b2w_description/urdf/b2w_description.urdf
@@ -180,7 +180,7 @@
         xyz="0.33989 -0.000168 0.12029"
         rpy="0 0 0" />
       <mass
-        value="3.366" />
+        value="3.6885" />
       <inertia
         ixx="0.026455"
         ixy="3.15E-05"
@@ -218,7 +218,7 @@
         xyz="-0.37448 0.000488 0.006495"
         rpy="0 0 0" />
       <mass
-        value="0.777" />
+        value="0.795" />
       <inertia
         ixx="0.0028118"
         ixy="1.95E-05"
@@ -253,17 +253,17 @@
     name="FL_hip">
     <inertial>
       <origin
-        xyz="-0.009305 -0.010228 0.000264"
+        xyz="-0.003841 -0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="0.00019234"
-        ixz="-6.76E-06"
-        iyy="0.0046728"
-        iyz="-7.16E-06"
-        izz="0.0034208" />
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="-3.77E-07"
+        iyy="0.0048743"
+        iyz="4E-09"
+        izz="0.0037087" />
     </inertial>
     <visual>
       <origin
@@ -310,17 +310,17 @@
     name="FL_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 -0.035797 -0.044921"
+        xyz="-0.006279 -0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="0.005796"
-        izz="0.0065677" />
+        ixx="0.062299"
+        ixy="0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="0.0083537"
+        izz="0.0081997" />
     </inertial>
     <visual>
       <origin
@@ -367,17 +367,17 @@
     name="FL_calf">
     <inertial>
       <origin
-        xyz="0.006095 0.012046 -0.29122"
+        xyz="0.005278 0.017301 -0.29778"
         rpy="0 0 0" />
       <mass
-        value="2.053" />
+        value="2.2906" />
       <inertia
-        ixx="0.028133"
-        ixy="0.00016307"
-        ixz="-0.0010034"
-        iyy="0.029251"
-        iyz="0.0014765"
-        izz="0.0028379" />
+        ixx="0.029283"
+        ixy="0.000221"
+        ixz="-0.001064"
+        iyy="0.030868"
+        iyz="0.002093"
+        izz="0.00326" />
     </inertial>
     <visual>
       <origin
@@ -425,17 +425,17 @@
     name="FL_foot">
     <inertial>
       <origin
-        xyz="0 0.051576 0"
+        xyz="-0.000991 0.051053 -0.00113"
         rpy="0 0 0" />
       <mass
-        value="0.918" />
+        value="1.083" />
       <inertia
-        ixx="0.0032475"
-        ixy="0"
-        ixz="0"
-        iyy="0.0061101"
-        iyz="0"
-        izz="0.003235" />
+        ixx="0.004121"
+        ixy="6E-06"
+        ixz="-4.8E-05"
+        iyy="0.007776"
+        iyz="7E-06"
+        izz="0.004108" />
     </inertial>
     <visual>
       <origin
@@ -481,17 +481,17 @@
     name="FR_hip">
     <inertial>
       <origin
-        xyz="-0.009305 0.010228 0.000264"
+        xyz="-0.003841 0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="-0.00019234"
-        ixz="-6.76E-06"
-        iyy="0.0046728"
-        iyz="7.16E-06"
-        izz="0.0034208" />
+        ixx="0.003318772"
+        ixy="-7.16E-05"
+        ixz="-3.77E-07"
+        iyy="0.004874327"
+        iyz="-4E-09"
+        izz="0.003708731" />
     </inertial>
     <visual>
       <origin
@@ -538,17 +538,17 @@
     name="FR_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 0.035797 -0.044921"
+        xyz="-0.006279 0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="-0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="-0.005796"
-        izz="0.0065677" />
+        ixx="0.06229908"
+        ixy="-0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="-0.008353671"
+        izz="0.008199745" />
     </inertial>
     <visual>
       <origin
@@ -593,17 +593,17 @@
     name="FR_calf">
     <inertial>
       <origin
-        xyz="0.006095 -0.012046 -0.29122"
+        xyz="0.005278 -0.017301 -0.297775"
         rpy="0 0 0" />
       <mass
-        value="2.053" />
+        value="2.290557" />
       <inertia
-        ixx="0.028133"
-        ixy="-0.00016307"
-        ixz="-0.0010034"
-        iyy="0.029251"
-        iyz="-0.0014765"
-        izz="0.0028379" />
+        ixx="0.029283"
+        ixy="-0.000221"
+        ixz="-0.001064"
+        iyy="0.030868"
+        iyz="-0.002093"
+        izz="0.00326" />
     </inertial>
     <visual>
       <origin
@@ -651,17 +651,17 @@
     name="FR_foot">
     <inertial>
       <origin
-        xyz="0 -0.051576 0"
+        xyz="-0.000991 -0.051053 -0.00113"
         rpy="0 0 0" />
       <mass
-        value="0.918" />
+        value="1.083" />
       <inertia
-        ixx="0.0032475"
-        ixy="0"
-        ixz="0"
-        iyy="0.0061101"
-        iyz="0"
-        izz="0.003235" />
+        ixx="0.004121"
+        ixy="-6E-06"
+        ixz="-4.8E-05"
+        iyy="0.007776"
+        iyz="-7E-06"
+        izz="0.004108" />
     </inertial>
     <visual>
       <origin
@@ -707,17 +707,17 @@
     name="RL_hip">
     <inertial>
       <origin
-        xyz="0.009305 -0.010228 0.000264"
+        xyz="0.003841 -0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="-0.00019234"
-        ixz="6.76E-06"
-        iyy="0.0046728"
-        iyz="-7.16E-06"
-        izz="0.0034208" />
+        ixx="0.0033188"
+        ixy="-7.16E-05"
+        ixz="3.77E-07"
+        iyy="0.0048743"
+        iyz="4E-09"
+        izz="0.0037087" />
     </inertial>
     <visual>
       <origin
@@ -764,17 +764,17 @@
     name="RL_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 -0.035797 -0.044921"
+        xyz="-0.006279 -0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="0.005796"
-        izz="0.0065677" />
+        ixx="0.06229908"
+        ixy="0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="0.008353671"
+        izz="0.008199745" />
     </inertial>
     <visual>
       <origin
@@ -819,17 +819,17 @@
     name="RL_calf">
     <inertial>
       <origin
-        xyz="0.006095 0.012046 -0.29122"
+        xyz="0.005278 0.017301 -0.29778"
         rpy="0 0 0" />
       <mass
-        value="2.053" />
+        value="2.2906" />
       <inertia
-        ixx="0.028133"
-        ixy="0.00016307"
-        ixz="-0.0010034"
-        iyy="0.029251"
-        iyz="0.0014765"
-        izz="0.0028379" />
+        ixx="0.029283"
+        ixy="0.000221"
+        ixz="-0.001064"
+        iyy="0.030868"
+        iyz="0.002093"
+        izz="0.00326" />
     </inertial>
     <visual>
       <origin
@@ -877,17 +877,17 @@
     name="RL_foot">
     <inertial>
       <origin
-        xyz="0 0.051576 0"
+        xyz="-0.000991 0.051053 -0.00113"
         rpy="0 0 0" />
       <mass
-        value="0.918" />
+        value="1.083" />
       <inertia
-        ixx="0.0032475"
-        ixy="0"
-        ixz="0"
-        iyy="0.0061101"
-        iyz="0"
-        izz="0.003235" />
+        ixx="0.004121"
+        ixy="6E-06"
+        ixz="-4.8E-05"
+        iyy="0.007776"
+        iyz="7E-06"
+        izz="0.004108" />
     </inertial>
     <visual>
       <origin
@@ -933,17 +933,17 @@
     name="RR_hip">
     <inertial>
       <origin
-        xyz="0.009305 -0.010228 0.000264"
+        xyz="0.003841 0.009068 0"
         rpy="0 0 0" />
       <mass
-        value="2.256" />
+        value="2.673" />
       <inertia
-        ixx="0.0026431"
-        ixy="0.00019234"
-        ixz="6.76E-06"
-        iyy="0.0046728"
-        iyz="7.16E-06"
-        izz="0.0034208" />
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="3.77E-07"
+        iyy="0.0048743"
+        iyz="-4E-09"
+        izz="0.0037087" />
     </inertial>
     <visual>
       <origin
@@ -990,17 +990,17 @@
     name="RR_thigh">
     <inertial>
       <origin
-        xyz="-0.004346 0.035797 -0.044921"
+        xyz="-0.006279 0.032049 -0.057835"
         rpy="0 0 0" />
       <mass
-        value="3.591" />
+        value="4.536" />
       <inertia
-        ixx="0.041718"
-        ixy="-0.00055623"
-        ixz="-0.0022768"
-        iyy="0.041071"
-        iyz="-0.005796"
-        izz="0.0065677" />
+        ixx="0.062299"
+        ixy="-0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="-0.0083537"
+        izz="0.0081997" />
     </inertial>
     <visual>
       <origin
@@ -1045,17 +1045,17 @@
     name="RR_calf">
     <inertial>
       <origin
-        xyz="0.006095 -0.012046 -0.29122"
+        xyz="0.005278 -0.017301 -0.29778"
         rpy="0 0 0" />
       <mass
-        value="2.053" />
+        value="2.2906" />
       <inertia
-        ixx="0.028133"
-        ixy="-0.00016307"
-        ixz="-0.0010034"
-        iyy="0.029251"
-        iyz="-0.0014765"
-        izz="0.0028379" />
+        ixx="0.029283"
+        ixy="-0.000221"
+        ixz="-0.001064"
+        iyy="0.030868"
+        iyz="-0.002093"
+        izz="0.00326" />
     </inertial>
     <visual>
       <origin
@@ -1103,17 +1103,17 @@
     name="RR_foot">
     <inertial>
       <origin
-        xyz="0 -0.051576 0"
+        xyz="-0.000991 -0.051053 -0.00113"
         rpy="0 0 0" />
       <mass
-        value="0.918" />
+        value="1.083" />
       <inertia
-        ixx="0.0032475"
-        ixy="0"
-        ixz="0"
-        iyy="0.0061101"
-        iyz="0"
-        izz="0.003235" />
+        ixx="0.004121"
+        ixy="-6E-06"
+        ixz="-4.8E-05"
+        iyy="0.007776"
+        iyz="-7E-06"
+        izz="0.004108" />
     </inertial>
     <visual>
       <origin