Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Unitree H1 Walk task #336

Open
wants to merge 26 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
6d54f28
[REF] Increased max message size for grpc servers
AntoSave Apr 26, 2024
7d67c12
[ADD] Add Unitree H1 Walk task
AntoSave May 7, 2024
5b89bc5
[ADD] Add utility for visualization in mujoco
AntoSave May 7, 2024
3d3017f
[ADD] Add utility for plotting live graphs
AntoSave May 7, 2024
22147db
[ADD] Add utility for live parameter editing
AntoSave May 7, 2024
e256ed2
[REF] Model checkpoint for H1 walk
AntoSave May 12, 2024
12a9a21
[DOC] Fix comment
AntoSave May 12, 2024
cbb6cf3
[ADD] Point plotting in mujoco
AntoSave Jul 21, 2024
2edb239
[ADD] python script for h1 walk tuning
AntoSave Jul 21, 2024
97d7924
[ADD] Add a new cost term which penalizes the feet crossing
AntoSave Jul 25, 2024
cfc9ad6
[REM] Useless files for the PR
AntoSave Aug 4, 2024
f902df5
[REM] H1 walk baseline task, as it shouldn't be in the PR
AntoSave Aug 4, 2024
994054c
[REM] Useless comments
AntoSave Aug 4, 2024
d45fde7
[DOC] Updated comments
AntoSave Aug 4, 2024
45b15c8
Merge branch 'main' of https://github.com/google-deepmind/mujoco_mpc …
AntoSave Aug 4, 2024
813e622
Merge branch 'google-deepmind-main' into feature-h1-walk
AntoSave Aug 4, 2024
cb20fdb
[REF] Reduced agent horizon to 0.4s
AntoSave Aug 14, 2024
636f8b7
[REF] Changed agent integrator to 'implicitfast'
AntoSave Aug 14, 2024
9dfcc14
[DOC] Added spaces after '//'
AntoSave Aug 14, 2024
d73cd6a
[REM] Removed useless function and comments
AntoSave Aug 14, 2024
47228ab
[REF] Formatted code with 'clang-format' vscode extension
AntoSave Aug 14, 2024
65132ee
[REF] Removed links to local machine in patch file
AntoSave Aug 14, 2024
9954bc0
- [ADD] Robot now follows orientation reference (mocap quaternion) af…
AntoSave Sep 3, 2024
0e81d03
- [FIX] Fixed a bug in the "Face goal" residual calculation
AntoSave Sep 7, 2024
0282b27
[DOC] Updated comment
AntoSave Sep 7, 2024
6cb23e4
[REF] 'Face Goal' now takes into account the four orientations separa…
AntoSave Sep 23, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions mjpc/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion mjpc/grpc/agent_server.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
2 changes: 1 addition & 1 deletion mjpc/grpc/direct_server.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
2 changes: 1 addition & 1 deletion mjpc/grpc/filter_server.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
2 changes: 1 addition & 1 deletion mjpc/grpc/ui_agent_server.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
12 changes: 12 additions & 0 deletions mjpc/tasks/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
187 changes: 187 additions & 0 deletions mjpc/tasks/h1/h1.xml.patch
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please remove the file links to your machine. You can replace them with something generic like the other .patch files.

Original file line number Diff line number Diff line change
@@ -0,0 +1,187 @@
diff --git a/h1.xml b/h1.xml
--- a/h1.xml
+++ b/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">
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe check to see if this diff can be smaller?

- <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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The xml file should have a new line at the end.

79 changes: 79 additions & 0 deletions mjpc/tasks/h1/walk/task.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
<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.4" />
<numeric name="agent_timestep" data="0.02" />
<numeric name="agent_integrator" data="3" />
<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 torso" dim="1" user="0 0.020 0 1.0" /> <!--kQuadratic-->
<user name="Posture arms" dim="8" user="0 0.020 0 1.0" /> <!--kQuadratic-->
<user name="Face goal" dim="4" 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="2" user="8 1.0 0.0 20.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"/>
<framexaxis name="goal_forward" objtype="xbody" objname="goal"/>
<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"/>
<jointpos name="right_hip_roll" joint="right_hip_roll"/>
<jointpos name="left_hip_roll" joint="left_hip_roll"/>
</sensor>
</mujoco>
Loading