google-deepmind · AntoSave · Apr 26, 2024 · May 7, 2024 · May 7, 2024 · May 7, 2024
diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt
@@ -50,6 +50,8 @@ add_library(
   tasks/cartpole/cartpole.h
   tasks/fingers/fingers.cc
   tasks/fingers/fingers.h
+  tasks/h1/walk/walk.cc
+  tasks/h1/walk/walk.h
   tasks/humanoid/interact/interact.cc
   tasks/humanoid/interact/interact.h
   tasks/humanoid/interact/contact_keyframe.cc

diff --git a/mjpc/grpc/agent_server.cc b/mjpc/grpc/agent_server.cc
@@ -48,7 +48,7 @@ int main(int argc, char** argv) {
 
   mjpc::agent_grpc::AgentService service(mjpc::GetTasks(),
                                          absl::GetFlag(FLAGS_mjpc_workers));
-  builder.SetMaxReceiveMessageSize(40 * 1024 * 1024);
+  builder.SetMaxReceiveMessageSize(80 * 1024 * 1024);
   builder.RegisterService(&service);
 
   std::unique_ptr<grpc::Server> server(builder.BuildAndStart());

diff --git a/mjpc/grpc/direct_server.cc b/mjpc/grpc/direct_server.cc
@@ -43,7 +43,7 @@ int main(int argc, char** argv) {
   builder.AddListeningPort(server_address, server_credentials);
 
   mjpc::direct_grpc::DirectService service;
-  builder.SetMaxReceiveMessageSize(40 * 1024 * 1024);
+  builder.SetMaxReceiveMessageSize(80 * 1024 * 1024);
   builder.RegisterService(&service);
 
   std::unique_ptr<grpc::Server> server(builder.BuildAndStart());

diff --git a/mjpc/grpc/filter_server.cc b/mjpc/grpc/filter_server.cc
@@ -43,7 +43,7 @@ int main(int argc, char** argv) {
   builder.AddListeningPort(server_address, server_credentials);
 
   filter_grpc::FilterService service;
-  builder.SetMaxReceiveMessageSize(40 * 1024 * 1024);
+  builder.SetMaxReceiveMessageSize(80 * 1024 * 1024);
   builder.RegisterService(&service);
 
   std::unique_ptr<grpc::Server> server(builder.BuildAndStart());

diff --git a/mjpc/grpc/ui_agent_server.cc b/mjpc/grpc/ui_agent_server.cc
@@ -50,7 +50,7 @@ int main(int argc, char** argv) {
 
   mjpc::MjpcApp app(mjpc::GetTasks());
   mjpc::agent_grpc::UiAgentService service(app.Sim());
-  builder.SetMaxReceiveMessageSize(40 * 1024 * 1024);
+  builder.SetMaxReceiveMessageSize(80 * 1024 * 1024);
   builder.RegisterService(&service);
 
   std::unique_ptr<grpc::Server> server(builder.BuildAndStart());

diff --git a/mjpc/tasks/CMakeLists.txt b/mjpc/tasks/CMakeLists.txt
@@ -195,6 +195,18 @@ add_custom_target(
   COMMAND patch -o ${CMAKE_CURRENT_BINARY_DIR}/op3/op3_modified.xml
           ${CMAKE_CURRENT_BINARY_DIR}/op3/op3.xml
           <${CMAKE_CURRENT_SOURCE_DIR}/op3/op3.xml.patch
+  #H1 WALK TASK
+  # copy h1 model from Menagerie
+  COMMAND ${CMAKE_COMMAND} -E copy
+          ${menagerie_SOURCE_DIR}/unitree_h1/h1.xml
+          ${CMAKE_CURRENT_BINARY_DIR}/h1/h1.xml
+  COMMAND ${CMAKE_COMMAND} -E copy_directory
+          ${menagerie_SOURCE_DIR}/unitree_h1/assets
+          ${CMAKE_CURRENT_BINARY_DIR}/h1/assets
+  # modified h1 model for task
+  COMMAND patch -o ${CMAKE_CURRENT_BINARY_DIR}/h1/h1_modified.xml
+          ${CMAKE_CURRENT_BINARY_DIR}/h1/h1.xml
+          <${CMAKE_CURRENT_SOURCE_DIR}/h1/h1.xml.patch
 
   COMMAND ${Python_EXECUTABLE}
           ${CMAKE_CURRENT_BINARY_DIR}/manipulation/merge_panda_robotiq.py

diff --git a/mjpc/tasks/h1/h1.xml.patch b/mjpc/tasks/h1/h1.xml.patch
@@ -0,0 +1,188 @@
+diff --git a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml
+index 32d3fb9..dfe3948 100644
+--- a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml
++++ b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml
+@@ -1,5 +1,5 @@
+ <mujoco model="h1">
+-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
++  <compiler angle="radian" meshdir="../assets" autolimits="true"/>
+
+   <statistic meansize="0.05"/>
+
+@@ -62,84 +62,86 @@
+         diaginertia="0.0490211 0.0445821 0.00824619"/>
+       <freejoint/>
+       <geom class="visual" mesh="pelvis"/>
+-      <body name="left_hip_yaw_link" pos="0 0.0875 -0.1742">
+-        <inertial pos="-0.04923 0.0001 0.0072" quat="0.69699 0.219193 0.233287 0.641667" mass="2.244"
+-          diaginertia="0.00304494 0.00296885 0.00189201"/>
+-        <joint name="left_hip_yaw" axis="0 0 1" range="-0.43 0.43"/>
+-        <geom class="visual" mesh="left_hip_yaw_link"/>
+-        <geom size="0.06 0.035" pos="-0.067 0 0" quat="0.707123 0 0.70709 0" type="cylinder" class="collision"/>
+-        <body name="left_hip_roll_link" pos="0.039468 0 0">
+-          <inertial pos="-0.0058 -0.00319 -9e-05" quat="0.0438242 0.70721 -0.0729075 0.701867" mass="2.232"
+-            diaginertia="0.00243264 0.00225325 0.00205492"/>
+-          <joint name="left_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
+-          <geom class="visual" mesh="left_hip_roll_link"/>
+-          <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 -0.02 0"/>
+-          <body name="left_hip_pitch_link" pos="0 0.11536 0">
+-            <inertial pos="0.00746 -0.02346 -0.08193" quat="0.979828 0.0513522 -0.0169854 -0.192382" mass="4.152"
+-              diaginertia="0.0829503 0.0821457 0.00510909"/>
+-            <joint name="left_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
+-            <geom class="visual" mesh="left_hip_pitch_link"/>
+-            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
+-            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
+-            <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 -0.07 0"/>
+-            <body name="left_knee_link" pos="0 0 -0.4">
+-              <inertial pos="-0.00136 -0.00512 -0.1384" quat="0.626132 -0.034227 -0.0416277 0.777852" mass="1.721"
+-                diaginertia="0.0125237 0.0123104 0.0019428"/>
+-              <joint name="left_knee" axis="0 1 0" range="-0.26 2.05"/>
+-              <geom class="visual" mesh="left_knee_link"/>
+-              <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
+-              <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
+-              <body name="left_ankle_link" pos="0 0 -0.4">
+-                <inertial pos="0.06722 0.00015 -0.04497" quat="0.489101 0.503197 0.565782 0.432972" mass="0.446"
+-                  diaginertia="0.00220848 0.00218961 0.000214202"/>
+-                <joint name="left_ankle" axis="0 1 0" range="-0.87 0.52"/>
+-                <geom class="visual" mesh="left_ankle_link"/>
+-                <geom class="foot1"/>
+-                <geom class="foot2"/>
+-                <geom class="foot3"/>
+-              </body>
++      <body name="legs"><!--ADDED-->
++        <body name="left_hip_yaw_link" pos="0 0.0875 -0.1742">
++            <inertial pos="-0.04923 0.0001 0.0072" quat="0.69699 0.219193 0.233287 0.641667" mass="2.244"
++            diaginertia="0.00304494 0.00296885 0.00189201"/>
++            <joint name="left_hip_yaw" axis="0 0 1" range="-0.43 0.43"/>
++            <geom class="visual" mesh="left_hip_yaw_link"/>
++            <geom size="0.06 0.035" pos="-0.067 0 0" quat="0.707123 0 0.70709 0" type="cylinder" class="collision"/>
++            <body name="left_hip_roll_link" pos="0.039468 0 0">
++            <inertial pos="-0.0058 -0.00319 -9e-05" quat="0.0438242 0.70721 -0.0729075 0.701867" mass="2.232"
++                diaginertia="0.00243264 0.00225325 0.00205492"/>
++            <joint name="left_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
++            <geom class="visual" mesh="left_hip_roll_link"/>
++            <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 -0.02 0"/>
++            <body name="left_hip_pitch_link" pos="0 0.11536 0">
++                <inertial pos="0.00746 -0.02346 -0.08193" quat="0.979828 0.0513522 -0.0169854 -0.192382" mass="4.152"
++                diaginertia="0.0829503 0.0821457 0.00510909"/>
++                <joint name="left_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
++                <geom class="visual" mesh="left_hip_pitch_link"/>
++                <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
++                <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
++                <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 -0.07 0"/>
++                <body name="left_knee_link" pos="0 0 -0.4">
++                <inertial pos="-0.00136 -0.00512 -0.1384" quat="0.626132 -0.034227 -0.0416277 0.777852" mass="1.721"
++                    diaginertia="0.0125237 0.0123104 0.0019428"/>
++                <joint name="left_knee" axis="0 1 0" range="-0.26 2.05"/>
++                <geom class="visual" mesh="left_knee_link"/>
++                <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
++                <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
++                <body name="left_ankle_link" pos="0 0 -0.4">
++                    <inertial pos="0.06722 0.00015 -0.04497" quat="0.489101 0.503197 0.565782 0.432972" mass="0.446"
++                    diaginertia="0.00220848 0.00218961 0.000214202"/>
++                    <joint name="left_ankle" axis="0 1 0" range="-0.87 0.52"/>
++                    <geom class="visual" mesh="left_ankle_link"/>
++                    <geom class="foot1"/>
++                    <geom class="foot2"/>
++                    <geom class="foot3"/>
++                </body>
++                </body>
++            </body>
+             </body>
+-          </body>
+         </body>
+-      </body>
+-      <body name="right_hip_yaw_link" pos="0 -0.0875 -0.1742">
+-        <inertial pos="-0.04923 -0.0001 0.0072" quat="0.641667 0.233287 0.219193 0.69699" mass="2.244"
+-          diaginertia="0.00304494 0.00296885 0.00189201"/>
+-        <joint name="right_hip_yaw" axis="0 0 1" range="-0.43 0.43"/>
+-        <geom class="visual" mesh="right_hip_yaw_link"/>
+-        <geom size="0.06 0.035" pos="-0.067 0 0" quat="0.707123 0 0.70709 0" type="cylinder" class="collision"/>
+-        <body name="right_hip_roll_link" pos="0.039468 0 0">
+-          <inertial pos="-0.0058 0.00319 -9e-05" quat="-0.0438242 0.70721 0.0729075 0.701867" mass="2.232"
+-            diaginertia="0.00243264 0.00225325 0.00205492"/>
+-          <joint name="right_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
+-          <geom class="visual" mesh="right_hip_roll_link"/>
+-          <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 0.02 0"/>
+-          <body name="right_hip_pitch_link" pos="0 -0.11536 0">
+-            <inertial pos="0.00746 0.02346 -0.08193" quat="0.979828 -0.0513522 -0.0169854 0.192382" mass="4.152"
+-              diaginertia="0.0829503 0.0821457 0.00510909"/>
+-            <joint name="right_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
+-            <geom class="visual" mesh="right_hip_pitch_link"/>
+-            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
+-            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
+-            <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 0.07 0"/>
+-            <body name="right_knee_link" pos="0 0 -0.4">
+-              <inertial pos="-0.00136 0.00512 -0.1384" quat="0.777852 -0.0416277 -0.034227 0.626132" mass="1.721"
+-                diaginertia="0.0125237 0.0123104 0.0019428"/>
+-              <joint name="right_knee" axis="0 1 0" range="-0.26 2.05"/>
+-              <geom class="visual" mesh="right_knee_link"/>
+-              <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
+-              <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
+-              <body name="right_ankle_link" pos="0 0 -0.4">
+-                <inertial pos="0.06722 -0.00015 -0.04497" quat="0.432972 0.565782 0.503197 0.489101" mass="0.446"
+-                  diaginertia="0.00220848 0.00218961 0.000214202"/>
+-                <joint name="right_ankle" axis="0 1 0" range="-0.87 0.52"/>
+-                <geom class="visual" mesh="right_ankle_link"/>
+-                <geom class="foot1"/>
+-                <geom class="foot2"/>
+-                <geom class="foot3"/>
+-              </body>
++        <body name="right_hip_yaw_link" pos="0 -0.0875 -0.1742">
++            <inertial pos="-0.04923 -0.0001 0.0072" quat="0.641667 0.233287 0.219193 0.69699" mass="2.244"
++            diaginertia="0.00304494 0.00296885 0.00189201"/>
++            <joint name="right_hip_yaw" axis="0 0 1" range="-0.43 0.43"/>
++            <geom class="visual" mesh="right_hip_yaw_link"/>
++            <geom size="0.06 0.035" pos="-0.067 0 0" quat="0.707123 0 0.70709 0" type="cylinder" class="collision"/>
++            <body name="right_hip_roll_link" pos="0.039468 0 0">
++            <inertial pos="-0.0058 0.00319 -9e-05" quat="-0.0438242 0.70721 0.0729075 0.701867" mass="2.232"
++                diaginertia="0.00243264 0.00225325 0.00205492"/>
++            <joint name="right_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
++            <geom class="visual" mesh="right_hip_roll_link"/>
++            <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 0.02 0"/>
++            <body name="right_hip_pitch_link" pos="0 -0.11536 0">
++                <inertial pos="0.00746 0.02346 -0.08193" quat="0.979828 -0.0513522 -0.0169854 0.192382" mass="4.152"
++                diaginertia="0.0829503 0.0821457 0.00510909"/>
++                <joint name="right_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
++                <geom class="visual" mesh="right_hip_pitch_link"/>
++                <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
++                <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
++                <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 0.07 0"/>
++                <body name="right_knee_link" pos="0 0 -0.4">
++                <inertial pos="-0.00136 0.00512 -0.1384" quat="0.777852 -0.0416277 -0.034227 0.626132" mass="1.721"
++                    diaginertia="0.0125237 0.0123104 0.0019428"/>
++                <joint name="right_knee" axis="0 1 0" range="-0.26 2.05"/>
++                <geom class="visual" mesh="right_knee_link"/>
++                <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
++                <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
++                <body name="right_ankle_link" pos="0 0 -0.4">
++                    <inertial pos="0.06722 -0.00015 -0.04497" quat="0.432972 0.565782 0.503197 0.489101" mass="0.446"
++                    diaginertia="0.00220848 0.00218961 0.000214202"/>
++                    <joint name="right_ankle" axis="0 1 0" range="-0.87 0.52"/>
++                    <geom class="visual" mesh="right_ankle_link"/>
++                    <geom class="foot1"/>
++                    <geom class="foot2"/>
++                    <geom class="foot3"/>
++                </body>
++                </body>
++            </body>
+             </body>
+-          </body>
+         </body>
+       </body>
+       <body name="torso_link">
+@@ -153,6 +155,7 @@
+         <geom name="torso" class="collision" type="box" size="0.07 0.1 0.22" pos="0 0 0.25"/>
+         <geom name="hip" class="collision" type="capsule" size="0.05" fromto="0 -0.1 -0.05 0 0.1 -0.05"/>
+         <site name="imu" pos="-0.04452 -0.01891 0.27756"/>
++        <site name="upper_torso" pos="0 0 0.40"/><!--ADDED-->
+         <body name="left_shoulder_pitch_link" pos="0.0055 0.15535 0.42999" quat="0.976296 0.216438 0 0">
+           <inertial pos="0.005045 0.053657 -0.015715" quat="0.814858 0.579236 -0.0201072 -0.00936488" mass="1.033"
+             diaginertia="0.00129936 0.000987113 0.000858198"/>
+@@ -239,4 +242,4 @@
+     <motor class="h1" name="right_shoulder_yaw" joint="right_shoulder_yaw" ctrlrange="-18 18"/>
+     <motor class="h1" name="right_elbow" joint="right_elbow" ctrlrange="-18 18"/>
+   </actuator>
+-</mujoco>
++</mujoco>
+\ No newline at end of file
diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml
@@ -0,0 +1,75 @@
+<mujoco model="H1 Locomotion">
+  <include file="../../common.xml"/>
+  <include file="../h1_modified.xml" />
+  <size memory="400K"/>
+
+  <visual>
+    <global offwidth="1280" offheight="720"/>
+  </visual>
+
+  <worldbody>
+    <light pos="0 0 3.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" type="plane" conaffinity="1" size="50 50 .05" material="blue_grid"/>
+    <camera name="top" mode="targetbody" target="pelvis" pos="10.0 1.0 3.5" />
+    <body name="goal" mocap="true" pos=".3 0 0.26">
+      <geom size="0.12" contype="0" conaffinity="0" rgba="0 1 0 .5" group="2"/>
+    </body>
+  </worldbody>
+
+  <custom>
+    <!-- agent -->
+    <numeric name="agent_planner" data="2" />
+    <numeric name="agent_horizon" data="0.6" />
+    <numeric name="agent_timestep" data="0.02" />
+    <numeric name="sampling_spline_points" data="3" />
+    <numeric name="sampling_exploration" data="0.05" />
+    <numeric name="gradient_spline_points" data="5" />
+    <numeric name="residual_Torso" data="1.30 1.0 1.7" />
+    <numeric name="residual_Speed" data="0.70 -3.0 3.0" />
+    <numeric name="residual_FeetDistance" data="0.4 0.0 1.0" />
+    <numeric name="residual_BalanceSpeed" data="0.3 0.0 1.0" />
+  </custom>
+
+  <sensor>
+    <!-- cost -->
+    <user name="Height" dim="1" user="7 5.0 0 25.0 0.1 4.0" /> <!--kSmoothAbs2Loss -->
+    <user name="Pelvis/Feet" dim="1" user="8 1.0 0.0 10.0 0.05" /> <!--kRectifyLoss-->
+    <user name="Balance" dim="2" user="1 5.0 0.0 25.0 0.02 4.0" /> <!--kL22-->
+    <user name="Upright" dim="8" user="2 4.0 0.0 25.0 0.01" /> <!--kL2-->
+    <user name="Posture up" dim="9" user="0 0.020 0 1.0" /> <!--kQuadratic-->
+    <user name="Posture down" dim="10" user="0 0.0 0 1.0" /> <!--kQuadratic-->
+    <user name="Face forward" dim="1" user="7 2.0 0.0 25.0 0.5 3.0" /> <!--kSmoothAbs2Loss --> 
+    <user name="Walk forward" dim="1" user="7 7.88 0.0 25.0 0.5 3.0" /> <!--kSmoothAbs2Loss -->
+    <user name="Move feet" dim="2" user="7 4.0 0 25.0 0.2 4.0" /> <!--kSmoothAbs2Loss -->
+    <user name="Control" dim="19" user="0 0.00005 0.00001 0.001" /> <!--kQuadratic-->
+    <user name="Feet Distance" dim="1" user="7 5.62 0 25.0 0.1 4.0" /> <!--kSmoothAbs2Loss-->
+    <user name="Leg cross" dim="1" user="8 2.0 0.0 10.0 0.05" /> <!--kRectifyLoss-->
+    <user name="Slippage" dim="2" user="0 0.0 0 50.0" /> <!--kQuadratic-->
+
+    <!-- residual -->
+    <framepos name="torso_position" objtype="body" objname="torso_link"/>
+    <subtreecom name="torso_subcom" body="pelvis"/> <!--pelvis is the root node for the H1-->
+    <subtreelinvel name="torso_subcomvel" body="pelvis"/> <!--pelvis is the root node for the H1-->
+    <framepos name="foot_right" objtype="body" objname="right_ankle_link"/>
+    <framepos name="foot_left" objtype="body" objname="left_ankle_link"/>
+    <framepos name="foot_right_xbody" objtype="xbody" objname="right_ankle_link"/>
+    <framepos name="foot_left_xbody" objtype="xbody" objname="left_ankle_link"/>
+    <framepos name="pelvis_position" objtype="body" objname="pelvis"/>
+    <framepos name="goal" objtype="xbody" objname="goal"/>
+    <framezaxis name="torso_up" objtype="site" objname="upper_torso"/><!--upper_torso is between the shoulders-->
+    <framezaxis name="pelvis_up" objtype="xbody" objname="pelvis"/>
+    <framezaxis name="foot_right_up" objtype="xbody" objname="right_ankle_link"/>
+    <framezaxis name="foot_left_up" objtype="xbody" objname="left_ankle_link"/>
+    <frameyaxis name="foot_left_left" objtype="xbody" objname="left_ankle_link"/>
+    <framexaxis name="torso_forward" objtype="site" objname="upper_torso"/>
+    <framexaxis name="pelvis_forward" objtype="xbody" objname="pelvis"/>
+    <framexaxis name="foot_right_forward" objtype="xbody" objname="right_ankle_link"/>
+    <framexaxis name="foot_left_forward" objtype="xbody" objname="left_ankle_link"/>
+    <subtreelinvel name="waist_lower_subcomvel" body="legs"/>
+    <framelinvel name="torso_velocity" objtype="body" objname="torso_link"/>
+    <framelinvel name="foot_right_velocity" objtype="body" objname="right_ankle_link"/>
+    <framelinvel name="foot_left_velocity" objtype="body" objname="left_ankle_link"/>
+    <frameangvel name="foot_right_ang_velocity" objtype="body" objname="right_ankle_link"/>
+    <frameangvel name="foot_left_ang_velocity" objtype="body" objname="left_ankle_link"/>
+  </sensor>
+</mujoco>