cyy/sim2sim_mujoco_example
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: cyy:dd4aaa816fbbf9756526be701e3aa894e83e4fb3
Branches Tags
cyy:main cyy:wtw
..
compare: cyy:0bd1294433799e19a4dbea476b5b0c4dbf248dbd
Branches Tags
cyy:wtw cyy:main

10 Commits
dd4aaa816f ... 0bd1294433

Author SHA1 Message Date
cyylinux
0bd1294433 wtw init 2026-06-21 22:41:35 +08:00
MobKBK
8070238165 walk_ok 2026-04-14 04:59:28 +08:00
MobKBK
d988ed941b init cyy 2026-04-05 07:37:08 +08:00
TrivasZhang
f3300ff1bf add license 2022-08-30 17:46:24 +08:00
TrivasZhang
e20035d1d2 add license 2022-08-30 17:43:05 +08:00
TrivasZhang
e6166e3f29 Merge pull request #3 from wangcongrobot/main 
fix xml error
 2022-08-30 17:07:34 +08:00
Cong Wang
f6266b80e5 fix xml error 2022-07-06 09:59:17 +08:00
Hongbo Zhu
d9a3e31d1a add go1 2021-11-10 20:10:35 +08:00
Hongbo Zhu
d12271b5a1 Update laikago.xml 2021-11-03 14:51:59 +08:00
Hongbo Zhu
c782dbb9e8 Update aliengo.xml 2021-11-03 14:51:33 +08:00
25 changed files with 1625 additions and 38 deletions
Expand all files Collapse all files

9
.claude/settings.json Normal file
View File

@@ -0,0 +1,9 @@
{
"permissions": {
"allow": [
"Bash(curl -s \"https://api.github.com/repos/zzzJie-Robot/Go2Arm_Lab/contents/source/Go2Arm_Lab/Go2Arm_Lab/tasks/manager_based/go2arm_lab\")",
"Read(//home/8x54zj-m/walk-these-ways/**)",
"Bash(python3 -c \"import ast; ast.parse\\(open\\('go1_walk_these_ways_inference.py'\\).read\\(\\)\\)\")"
]
}
}

29
LICENSE Normal file
View File

@@ -0,0 +1,29 @@
BSD 3-Clause License
Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

BIN
__pycache__/go1_stairs_sim2sim_inference.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/go1_stairs_sim2sim_inference.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/go1_unitree_rl_inference.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/go1_walk_these_ways_inference.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/go1_walk_these_ways_inference.cpython-38.pyc Normal file
View File

Binary file not shown.

23
data/a1/urdf/a1.xml → data/a1/xml/a1.xml
View File

@@ -1,7 +1,6 @@
<mujoco model="a1_description"> <mujoco model="a1_description">
<compiler angle="radian" meshdir="../meshes/" /> <compiler angle="radian" meshdir="../meshes/" />
<size njmax="500" nconmax="100" /> <size njmax="500" nconmax="100" />
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/> <option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
<default> <default>
@@ -10,7 +9,7 @@
<light castshadow="false" diffuse="1 1 1"/> <light castshadow="false" diffuse="1 1 1"/>
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/> <motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
<camera fovy="60"/> <camera fovy="60"/>
<joint damping="0.01" armature="0.01"/> <joint damping="0.01" armature="0.01" frictionloss="0.2" />
</default> </default>
@@ -28,18 +27,25 @@
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/> <material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
</asset> </asset>
<visual>
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
</visual>
<worldbody> <worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/> <light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/> <camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/> <geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
<body name="trunk" pos="0 0 0.5"> <body name="trunk" pos="0 0 0.3">
<inertial pos="0.0127283 0.00218554 0.000514891" quat="0.00186575 0.711506 0.000389649 0.702677" mass="4.714" diaginertia="0.0648213 0.0565803 0.0169323" /> <inertial pos="0.0127283 0.00218554 0.000514891" quat="0.00186575 0.711506 0.000389649 0.702677" mass="4.714" diaginertia="0.0648213 0.0565803 0.0169323" />
<joint type="free" /> <joint type="free" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
<geom size="0.1335 0.097 0.057" type="box" rgba="0.913725 0.913725 0.847059 0" /> <geom size="0.1335 0.066 0.057" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" /> <geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" /> <geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" />
<site name="imu" pos="0 0 0"/>
<body name="FR_hip" pos="0.183 -0.047 0"> <body name="FR_hip" pos="0.183 -0.047 0">
<inertial pos="-0.003311 -0.000635 3.1e-05" quat="0.507528 0.506268 0.491507 0.494499" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" /> <inertial pos="-0.003311 -0.000635 3.1e-05" quat="0.507528 0.506268 0.491507 0.494499" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" />
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" /> <joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
@@ -166,6 +172,15 @@
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/> <jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/> <jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
<accelerometer name="Body_Acc" site="imu"/>
<gyro name="Body_Gyro" site="imu"/>
<framepos name="Body_Pos" objtype="site" objname="imu"/>
<framequat name="Body_Quat" objtype="site" objname="imu"/>
</sensor> </sensor>
</mujoco> </mujoco>

2
data/aliengo/urdf/aliengo.urdf
View File

@@ -49,7 +49,7 @@
</joint> </joint>
</xacro:if> --> </xacro:if> -->
<link name="world"/> <link name="world"/>
<joint name="" type="floating"> <joint name="floating_base" type="floating">
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/> <parent link="world"/>
<child link="trunk"/> <child link="trunk"/>

24
data/aliengo/urdf/aliengo.xml → data/aliengo/xml/aliengo.xml
View File

@@ -7,9 +7,9 @@
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/> <geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
<light castshadow="false" diffuse="1 1 1"/> <light castshadow="false" diffuse="1 1 1"/>
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/> <motor ctrlrange="-44.4 44.4" ctrllimited="true"/>
<camera fovy="60"/> <camera fovy="60"/>
<joint damping="0.01" armature="0.01"/> <joint damping="0.01" armature="0.01" frictionloss="0.2" />
</default> </default>
@@ -27,18 +27,25 @@
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/> <material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
</asset> </asset>
<visual>
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
</visual>
<worldbody> <worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/> <light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/> <camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/> <geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
<body name="trunk" pos="0 0 0.5"> <body name="trunk" pos="0 0 0.4">
<inertial pos="0.00846406 0.00404455 -0.000762916" quat="-3.12625e-05 0.708321 0.00247254 0.705886" mass="9.042" diaginertia="0.174706 0.161175 0.033357" /> <inertial pos="0.00846406 0.00404455 -0.000762916" quat="-3.12625e-05 0.708321 0.00247254 0.705886" mass="9.042" diaginertia="0.174706 0.161175 0.033357" />
<joint type="free" /> <joint type="free" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" />
<geom size="0.3235 0.075 0.056" type="box" rgba="0.2 0.2 0.2 0" /> <geom size="0.18 0.075 0.056" type="box" rgba="0.2 0.2 0.2 0" />
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" /> <geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" /> <geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" />
<site name="imu" pos="0 0 0"/>
<body name="FR_hip" pos="0.2399 -0.051 0"> <body name="FR_hip" pos="0.2399 -0.051 0">
<inertial pos="-0.022191 -0.015144 -1.5e-05" quat="0.0135493 0.706835 -0.0117635 0.707151" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" /> <inertial pos="-0.022191 -0.015144 -1.5e-05" quat="0.0135493 0.706835 -0.0117635 0.707151" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" />
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" /> <joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" />
@@ -121,6 +128,7 @@
</body> </body>
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/> <motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
@@ -165,5 +173,13 @@
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/> <jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/> <jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
<accelerometer name="Body_Acc" site="imu"/>
<gyro name="Body_Gyro" site="imu"/>
<framepos name="Body_Pos" objtype="site" objname="imu"/>
<framequat name="Body_Quat" objtype="site" objname="imu"/>
</sensor> </sensor>
</mujoco> </mujoco>

BIN
data/go1/meshes/calf.stl Normal file
View File

Binary file not shown.

BIN
data/go1/meshes/hip.stl Normal file
View File

Binary file not shown.

BIN
data/go1/meshes/thigh.stl Normal file
View File

Binary file not shown.

BIN
data/go1/meshes/thigh_mirror.stl Normal file
View File

Binary file not shown.

BIN
data/go1/meshes/trunk.stl Normal file
View File

Binary file not shown.

704
data/go1/urdf/go1.urdf Normal file
View File

@@ -0,0 +1,704 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="go1_description">
<mujoco>
<compiler
meshdir="../meshes/"
balanceinertia="true"
discardvisual="false" />
</mujoco>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="world"/>
<joint name="" type="floating">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.3762 0.0935 0.114"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.011611 0.004437 0.000108"/>
<mass value="4.8"/>
<inertia ixx="0.016130741919" ixy="0.000593180607" ixz="7.324662e-06" iyy="0.036507810812" iyz="2.0969537e-05" izz="0.044693872053"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1881 -0.04675 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00541 0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FR_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="FR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1881 0.04675 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00541 -0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FL_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="FL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1881 -0.04675 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00541 0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RR_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="RR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1881 0.04675 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00541 -0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RL_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="RL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>

195
data/go1/xml/go1.xml Normal file
View File

@@ -0,0 +1,195 @@
<mujoco model="go1_description">
<compiler angle="radian" meshdir="../meshes/" />
<size njmax="500" nconmax="100" />
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
<default>
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
<light castshadow="false" diffuse="1 1 1"/>
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
<camera fovy="60"/>
<joint damping="1.0" armature="0.01" frictionloss="0.5" />
</default>
=
<asset>
<mesh name="trunk" file="trunk.stl" />
<mesh name="hip" file="hip.stl" />
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
<mesh name="calf" file="calf.stl" />
<mesh name="thigh" file="thigh.stl" />
</asset>
<asset>
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
</asset>
<visual>
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
</visual>
<worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
<body name="trunk" pos="0 0 0.35">
<inertial pos="0.0116053 0.00442221 0.000106692" quat="0.0111438 0.707126 -0.00935374 0.706938" mass="4.801" diaginertia="0.0447997 0.0366257 0.0162187" />
<joint type="free" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
<geom size="0.13 0.04675 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" rgba="0.8 0 0 0" />
<site name="imu" pos="0 0 0"/>
<body name="FR_hip" pos="0.1881 -0.04675 0">
<inertial pos="-0.00406411 -0.0193463 4.50733e-06" quat="0.467526 0.531662 -0.466259 0.530431" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="FR_thigh" pos="0 -0.08 0">
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="FR_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="FL_hip" pos="0.1881 0.04675 0">
<inertial pos="-0.00406411 0.0193463 4.50733e-06" quat="0.531662 0.467526 -0.530431 0.466259" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="FL_thigh" pos="0 0.08 0">
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="FL_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="RR_hip" pos="-0.1881 -0.04675 0">
<inertial pos="0.00406411 -0.0193463 4.50733e-06" quat="0.530431 0.466259 -0.531662 0.467526" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="RR_thigh" pos="0 -0.08 0">
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="RR_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="RL_hip" pos="-0.1881 0.04675 0">
<inertial pos="0.00406411 0.0193463 4.50733e-06" quat="0.466259 0.530431 -0.467526 0.531662" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="RL_thigh" pos="0 0.08 0">
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="RL_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
</actuator>
<sensor>
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
<accelerometer name="Body_Acc" site="imu"/>
<gyro name="Body_Gyro" site="imu"/>
<framepos name="Body_Pos" objtype="site" objname="imu"/>
<framequat name="Body_Quat" objtype="site" objname="imu"/>
</sensor>
</mujoco>

1
data/go1/xml/terrain/plane.xml Normal file
View File

@@ -0,0 +1 @@
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1' friction="0.6 0.3 0.3"/>

12
data/go1/xml/terrain/rough.xml Normal file
View File

@@ -0,0 +1,12 @@
<!-- 模拟碎地形 -->
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.4 0.6 0.3 1" pos='0 0 0' size='0 0 1' friction="0.6 0.3 0.3"/>
<geom name='r1' type='box' size='0.15 0.1 0.03' pos='0.5 0.2 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r2' type='box' size='0.1 0.15 0.05' pos='0.8 -0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r3' type='box' size='0.12 0.08 0.04' pos='1.1 0.15 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r4' type='box' size='0.08 0.12 0.06' pos='1.4 -0.2 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r5' type='box' size='0.1 0.1 0.035' pos='1.7 0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r6' type='box' size='0.14 0.09 0.045' pos='2.0 -0.05 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r7' type='box' size='0.09 0.14 0.055' pos='0.3 0.25 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r8' type='box' size='0.11 0.07 0.025' pos='0.6 -0.15 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r9' type='box' size='0.07 0.11 0.05' pos='0.9 0.05 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='r10' type='box' size='0.13 0.08 0.04' pos='1.2 -0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>

6
data/go1/xml/terrain/slope.xml Normal file
View File

@@ -0,0 +1,6 @@
<!-- 斜坡地形-->
<geom name='start_platform' type='box' size='10.0 10.0 0.1' pos='0 0 -0.06' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='start_fill' type='box' size='10.0 10.0 0.2' pos='0 0 -0.26' rgba="0.4 0.35 0.3 1" friction="0.8 0.3 0.3"/>
<geom name='slope' type='box' size='2.5 0.3 0.1' pos='6.8 0 0.10' quat='0.258819 0 0 0.965926' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='top_platform' type='box' size='1.0 0.3 0.06' pos='8.8 0 0.16' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='slope_fill' type='box' size='2.5 0.3 0.25' pos='6.8 0 -0.15' rgba="0.4 0.35 0.3 1" friction="0.8 0.3 0.3"/>

13
data/go1/xml/terrain/stairs.xml Normal file
View File

@@ -0,0 +1,13 @@
<!-- 楼梯地形 -->
<geom name='step0' type='box' size='0.5 10.4 0.06' pos='0 0 -0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step1' type='box' size='0.5 10.4 0.06' pos='0.3 0 -0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step2' type='box' size='0.5 10.4 0.06' pos='0.6 0 0.00' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step3' type='box' size='0.5 10.4 0.06' pos='0.9 0 0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step4' type='box' size='0.5 10.4 0.06' pos='1.2 0 0.12' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step5' type='box' size='0.5 10.4 0.06' pos='1.5 0 0.18' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step6' type='box' size='0.5 10.4 0.06' pos='1.8 0 0.24' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step7' type='box' size='0.5 10.4 0.06' pos='2.1 0 0.30' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step8' type='box' size='0.5 10.4 0.06' pos='2.4 0 0.36' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='step9' type='box' size='0.5 10.4 0.06' pos='2.7 0 0.42' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='platform' type='box' size='0.8 10.4 0.06' pos='3.3 0 0.45' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
<geom name='fill' type='box' size='2.0 10.4 0.45' pos='1.5 0 -0.45' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>

2
data/laikago/urdf/laikago.urdf
View File

@@ -41,7 +41,7 @@
</material> </material>
<link name="world"/> <link name="world"/>
<joint name="" type="floating"> <joint name="floating_base" type="floating">
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/> <parent link="world"/>
<child link="trunk"/> <child link="trunk"/>

25
data/laikago/urdf/laikago.xml → data/laikago/xml/laikago.xml
View File

@@ -7,9 +7,9 @@
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/> <geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
<light castshadow="false" diffuse="1 1 1"/> <light castshadow="false" diffuse="1 1 1"/>
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/> <motor ctrlrange="-40.0 40.0" ctrllimited="true"/>
<camera fovy="60"/> <camera fovy="60"/>
<joint damping="0.01" armature="0.01"/> <joint damping="0.01" armature="0.01" frictionloss="0.2" />
</default> </default>
@@ -21,6 +21,11 @@
<mesh name="thigh" file="thigh.stl" /> <mesh name="thigh" file="thigh.stl" />
</asset> </asset>
<visual>
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
</visual>
<asset> <asset>
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/> <texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/> <texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
@@ -31,11 +36,13 @@
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/> <camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/> <geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
<body name="trunk" pos="0 0 0.55"> <body name="trunk" pos="0 0 0.4">
<inertial pos="0.002284 -4.1e-05 0.025165" quat="-0.00605949 0.710803 -0.00734309 0.703327" mass="13.733" diaginertia="0.254491 0.250684 0.0733281" /> <inertial pos="0.002284 -4.1e-05 0.025165" quat="-0.00605949 0.710803 -0.00734309 0.703327" mass="13.733" diaginertia="0.254491 0.250684 0.0733281" />
<joint type="free" /> <joint type="free" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" />
<geom size="0.2808 0.086 0.09375" pos="0 0 0.01675" type="box" rgba="0.2 0.2 0.2 0" /> <geom size="0.2808 0.086 0.09375" pos="0 0 0.01675" type="box" rgba="0.2 0.2 0.2 0" />
<site name="imu" pos="0 0 0"/>
<body name="FR_hip" pos="0.21935 -0.0875 0"> <body name="FR_hip" pos="0.21935 -0.0875 0">
<inertial pos="-0.001568 0.008134 0.000864" quat="0.629953 0.350191 0.35823 0.593462" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" /> <inertial pos="-0.001568 0.008134 0.000864" quat="0.629953 0.350191 0.35823 0.593462" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" />
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.0472 0.872665" /> <joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.0472 0.872665" />
@@ -51,6 +58,7 @@
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" /> <joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" /> <geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
</body> </body>
</body> </body>
</body> </body>
@@ -69,6 +77,7 @@
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" /> <joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" /> <geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
</body> </body>
</body> </body>
</body> </body>
@@ -87,6 +96,7 @@
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" /> <joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" /> <geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
</body> </body>
</body> </body>
</body> </body>
@@ -105,6 +115,7 @@
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" /> <joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" /> <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" /> <geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
</body> </body>
</body> </body>
</body> </body>
@@ -154,6 +165,14 @@
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/> <jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/> <jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
<accelerometer name="Body_Acc" site="imu"/>
<gyro name="Body_Gyro" site="imu"/>
<framepos name="Body_Pos" objtype="site" objname="imu"/>
<framequat name="Body_Quat" objtype="site" objname="imu"/>
</sensor> </sensor>
</mujoco> </mujoco>

593
demo/go1_walk_these_ways_inference.py Normal file
View File

@@ -0,0 +1,593 @@
#!/usr/bin/env python3
"""
Go1 RL Policy 推理demo
默认加载的Walk-These-Ways 预训练模型 + MuJoCo 可视化
模型: body_latest.jit + adaptation_module_latest.jit (GRU-based policy with history)
使用方法:
source /opt/mujoco_project/mujoco_env/bin/activate
python3 /opt/unitree_mujoco/go1_walk_these_ways_inference.py
键盘控制:
W/S: 前进/后退
A/D: 左转/右转
Q/E: 侧向左移/右移
R/F: 抬腿高度 高/低
U/J: 身体前倾/后仰 (pitch)
I/K: 身体右倾/左倾 (roll)
空格: 停止
ESC: 退出
"""
import numpy as np
import mujoco
from mujoco import viewer
import os
import threading
import time
import signal
import queue
g_exit_requested = False
def signal_handler(signum, frame):
global g_exit_requested
g_exit_requested = True
signal.signal(signal.SIGINT, signal_handler)
# ============================================================
# 预训练模型 /opt/walk-these-ways/repo/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545/checkpoints
# ============================================================
MODEL_DIR = "/opt/walk-these-ways/repo/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545/checkpoints"
BODY_MODEL_PATH = os.path.join(MODEL_DIR, "body_latest.jit")
ADAPT_MODEL_PATH = os.path.join(MODEL_DIR, "adaptation_module_latest.jit")
XML_PATH = "/opt/unitree_mujoco/data/go1/xml/go1.xml"
MESH_PATH = "/opt/unitree_mujoco/data/go1/meshes"
TERRAIN_PATH = "/opt/unitree_mujoco/data/go1/xml/terrain"
# 地形配置: "plane", "stairs", "slope", "rough"
TERRAIN = "stairs"
# ============================================================
# Walk-These-Ways 模型参数 (来自 parameters.pkl & deploy配置)
# ============================================================
NUM_OBS = 70 # 单步观测维度
NUM_OBS_HISTORY = 30 # 历史观测步数
OBS_BUFFER_SIZE = NUM_OBS * NUM_OBS_HISTORY # 2100
# 观测缩放 (来自 obs_scales)
SCALE_LIN_VEL = 2.0
SCALE_ANG_VEL = 0.25
SCALE_DOF_POS = 1.0
SCALE_DOF_VEL = 0.05
# 动作缩放 (来自 control.action_scale)
ACTION_SCALE = 0.25
HIP_SCALE_REDUCTION = 0.5 # hip关节额外缩放 (来自 control.hip_scale_reduction)
# 限幅 (来自 normalization)
CLIP_OBSERVATIONS = 100.0
CLIP_ACTIONS = 10.0
# 默认关节角度 (来自 init_state.default_joint_angles, deploy顺序 FL, FR, RL, RR)
# Mujoco 关节顺序是 FR, FL, RR, RL，所以需要映射
# Mujoco: [FR_hip, FR_thigh, FR_calf, FL_hip, FL_thigh, FL_calf, RR_hip, RR_thigh, RR_calf, RL_hip, RL_thigh, RL_calf]
# Deploy: [FL_hip, FL_thigh, FL_calf, FR_hip, FR_thigh, FR_calf, RL_hip, RL_thigh, RL_calf, RR_hip, RR_thigh, RR_calf]
DEFAULT_DOF_POS_DEPLOY = np.array([
0.1, 0.8, -1.5, # FL: hip, thigh, calf
-0.1, 0.8, -1.5, # FR: hip, thigh, calf
0.1, 1.0, -1.5, # RL: hip, thigh, calf
-0.1, 1.0, -1.5, # RR: hip, thigh, calf
], dtype=np.float32)
# Mujoco 默认姿态 (用于初始化Mujoco仿真)
DEFAULT_DOF_POS_MUJOCO = np.array([
-0.1, 0.8, -1.5, # FR: hip, thigh, calf (Mujoco order)
0.1, 0.8, -1.5, # FL: hip, thigh, calf
-0.1, 1.0, -1.5, # RR: hip, thigh, calf
0.1, 1.0, -1.5, # RL: hip, thigh, calf
], dtype=np.float32)
# IsaacGym/Deploy -> Mujoco 关节顺序映射
DEPLOY_TO_MUJOCO_MAPPING = np.array([3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8])
# 速度命令范围
MAX_LIN_VEL = 1.0 # 最大线速度 m/s
MAX_ANG_VEL = 3.0 # 与 deploy 一致
# PD控制参数
KP = 20.0
KD = 0.1
# MuJoCo 摩擦系数配置
FRICTION = [0.6, 0.3, 0.3]
FLOOR_FRICTION = [0.0, 0.0, 0.0] # 训练时地面摩擦为0
# ============================================================
# 步态预设 (与 deploy ElegantGaitProfile 一致)
# ============================================================
GAIT_PRESETS = {
'trot': {
'name': 'Trot (小跑)',
'phase': 0.5, 'offset': 0.0, 'bound': 0.0, 'duration': 0.5,
'description': '对角腿同步'
},
'pace': {
'name': 'Pace (踱步)',
'phase': 0.0, 'offset': 0.0, 'bound': 0.5, 'duration': 0.5,
'description': '同侧腿同步'
},
'bound': {
'name': 'Bound (奔跑)',
'phase': 0.0, 'offset': 0.5, 'bound': 0.0, 'duration': 0.5,
'description': '前后腿同步'
},
'pronk': {
'name': 'Pronk (跳跃)',
'phase': 0.0, 'offset': 0.0, 'bound': 0.0, 'duration': 0.5,
'description': '四腿同时'
},
}
current_gait = 'trot'
# ============================================================
# 键盘输入读取 (独立线程, pynput 事件驱动)
# ============================================================
from pynput import keyboard
class KeyboardReader:
def __init__(self):
self._event_queue = queue.Queue()
self.running = True
self.shared_keys_held = set()
self.shared_one_shot = set()
self._reader_thread = None
self._listener = None
def _normalize_key(self, key):
try:
if hasattr(key, 'char') and key.char is not None:
return key.char.lower()
except:
pass
key_str = str(key)
if key_str == 'Key.esc':
return 'escape'
elif key_str == 'Key.space':
return 'space'
elif key_str.startswith('Key.'):
return key_str.lower()
return key_str.lower()
def _reader_worker(self):
while self.running:
try:
event_type, key = self._event_queue.get(timeout=0.05)
k = self._normalize_key(key)
if event_type == 'press':
self.shared_keys_held.add(k)
self.shared_one_shot.discard(k)
elif event_type == 'release':
self.shared_keys_held.discard(k)
self.shared_one_shot.discard(k)
except queue.Empty:
pass
def init(self):
def on_press(key):
self._event_queue.put(('press', key))
def on_release(key):
self._event_queue.put(('release', key))
try:
self._listener = keyboard.Listener(on_press=on_press, on_release=on_release)
self._listener.start()
self._reader_thread = threading.Thread(target=self._reader_worker, daemon=True)
self._reader_thread.start()
print("[INFO] 键盘监听已启动")
except Exception as e:
print(f"[WARN] 无法初始化键盘监听: {e}")
def is_key_pressed(self, key):
k = self._normalize_key(key) if isinstance(key, str) else self._normalize_key(key)
if k not in self.shared_keys_held or k in self.shared_one_shot:
return False
self.shared_one_shot.add(k)
return True
def is_key_held(self, key):
k = self._normalize_key(key) if isinstance(key, str) else self._normalize_key(key)
return k in self.shared_keys_held
def restore(self):
self.running = False
if self._listener:
self._listener.stop()
# ============================================================
# 辅助函数
# ============================================================
def quat_rotate_inverse(data, v):
"""
四元数逆旋转 (世界坐标系 -> 躯干坐标系)
"""
base_rot = data.xmat[1].reshape(3, 3) # body 1 = trunk
return base_rot.T @ np.array(v, dtype=np.float64)
def compute_observations_wtw(data, prev_action, last_action, commands, clock_inputs, default_dof_pos_mujoco, obs_scales):
"""
计算 Walk-These-Ways 策略的观测向量 (70维)
"""
obs = np.zeros(NUM_OBS, dtype=np.float32)
# 获取四元数 (MuJoCo 格式: x, y, z, w) -> 转换为 (w, x, y, z)
quat_mujoco = data.qpos[3:7] # (x, y, z, w)
quat = np.concatenate([quat_mujoco[3:4], quat_mujoco[0:3]])
# 1. 重力投影到躯干坐标系 (使用单位重力 [0,0,-1])
projected_gravity = quat_rotate_inverse(data, np.array([0., 0., -1.]))
obs[0:3] = projected_gravity
# 2. 命令 (已缩放)
obs[3:18] = commands[:15]
# 3. 相对关节位置 (Mujoco顺序 -> Deploy顺序)
current_joint_pos = data.qpos[7:19]
current_joint_pos = current_joint_pos[DEPLOY_TO_MUJOCO_MAPPING]
default_dof_pos_deploy = default_dof_pos_mujoco[DEPLOY_TO_MUJOCO_MAPPING]
dof_pos_rel = (current_joint_pos - default_dof_pos_deploy) * obs_scales['dof_pos']
obs[18:30] = dof_pos_rel
# 4. 关节速度 * scale (Mujoco顺序 -> Deploy顺序)
joint_vel = data.qvel[6:18]
joint_vel = joint_vel[DEPLOY_TO_MUJOCO_MAPPING]
obs[30:42] = joint_vel * obs_scales['dof_vel']
# 5. 上一步动作 (clipped)
obs[42:54] = np.clip(prev_action, -CLIP_ACTIONS, CLIP_ACTIONS)
# 6. 上上步动作
obs[54:66] = np.clip(last_action, -CLIP_ACTIONS, CLIP_ACTIONS)
# 7. 时钟输入
obs[66:70] = clock_inputs
# 限幅
obs = np.clip(obs, -CLIP_OBSERVATIONS, CLIP_OBSERVATIONS)
return obs
def compute_commands_scale():
"""计算 commands_scale，与 deploy/lcm_agent.py 一致"""
obs_scales = {
'lin_vel': 2.0,
'ang_vel': 0.25,
'dof_pos': 1.0,
'dof_vel': 0.05,
'body_height_cmd': 2.0,
'footswing_height_cmd': 0.15,
'body_pitch_cmd': 0.3,
'body_roll_cmd': 0.3,
'stance_width_cmd': 1.0,
'stance_length_cmd': 1.0,
'aux_reward_cmd': 1.0,
}
commands_scale = np.array([
obs_scales['lin_vel'], obs_scales['lin_vel'],
obs_scales['ang_vel'], obs_scales['body_height_cmd'],
1, 1, 1, 1, 1, # gait params (unscaled)
obs_scales['footswing_height_cmd'],
obs_scales['body_pitch_cmd'],
obs_scales['body_roll_cmd'],
obs_scales['stance_width_cmd'],
obs_scales['stance_length_cmd'],
obs_scales['aux_reward_cmd'],
1, 1, 1, 1, 1 # padding
], dtype=np.float32)
return commands_scale[:15], obs_scales
# ============================================================
# 主程序
# ============================================================
def main():
global current_gait
import torch
# 1. 加载 MuJoCo 模型
import re
os.chdir(MESH_PATH)
with open(XML_PATH, 'r') as f:
xml_content = f.read()
xml_content = xml_content.replace('meshdir="../meshes/"', f'meshdir="{MESH_PATH}"')
xml_content = xml_content.replace('<default>\n=', '<default>\n')
# ---- 地形替换: 使用单独的 terrain XML 文件 ----
if TERRAIN != "none":
terrain_file = os.path.join(TERRAIN_PATH, f"{TERRAIN}.xml")
if os.path.exists(terrain_file):
with open(terrain_file, 'r') as f:
terrain_xml = f.read()
# 删除原有的 floor geom
xml_content = re.sub(r'<geom[^>]*name=[^>]*floor[^>]*/>', '', xml_content)
xml_content = re.sub(r"<geom[^>]*name=[^>]*floor[^>]*/>", '', xml_content)
# 在 </worldbody> 之前插入新地形
xml_content = xml_content.replace('</worldbody>', terrain_xml + '</worldbody>')
print(f"[INFO] 地形已加载: {terrain_file} (TERRAIN={TERRAIN})")
else:
print(f"[WARN] 地形文件不存在: {terrain_file}, 使用默认地面")
model = mujoco.MjModel.from_xml_string(xml_content)
data = mujoco.MjData(model)
print(f"[INFO] MuJoCo Model: {model.nbody} bodies, {model.nq} DoF, {model.nu} actuators")
print(f"[INFO] 地形: {TERRAIN} ({os.path.join(TERRAIN_PATH, TERRAIN + '.xml')})")
print(f"[INFO] Mujoco关节顺序: {[mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(1, 13)]}")
print(f"[INFO] Deploy关节顺序: FL, FR, RL, RR (与Mujoco不同)")
# 2. 设置摩擦系数
floor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "floor")
if floor_id >= 0:
model.geom_friction[floor_id] = FLOOR_FRICTION
print(f"[INFO] 地面摩擦系数设置为: {FLOOR_FRICTION}")
else:
print(f"[INFO] 未找到 floor geom，跳过地面摩擦设置（地形可能已自定义摩擦系数）")
for i in range(model.ngeom):
if i != floor_id:
model.geom_friction[i] = FRICTION
print(f"[INFO] 机器人摩擦系数设置为: {FRICTION}")
# 3. 加载 Walk-These-Ways 模型
body_module = torch.jit.load(BODY_MODEL_PATH, map_location='cpu')
adapt_module = torch.jit.load(ADAPT_MODEL_PATH, map_location='cpu')
body_module.eval()
adapt_module.eval()
print(f"[INFO] Body model loaded from: {BODY_MODEL_PATH}")
print(f"[INFO] Adapt model loaded from: {ADAPT_MODEL_PATH}")
print(f"[INFO] 观测维度: {NUM_OBS}, 历史步数: {NUM_OBS_HISTORY}")
print(f"[INFO] Action scale: {ACTION_SCALE}, Hip scale reduction: {HIP_SCALE_REDUCTION}")
# 4. 计算 commands_scale
commands_scale, obs_scales = compute_commands_scale()
print(f"[INFO] Commands scale: {commands_scale}")
# 5. 初始化观测历史 buffer
obs_buffer = np.zeros(OBS_BUFFER_SIZE, dtype=np.float32) # 30 * 70 = 2100
# 6. 初始化机器人姿态
data.qpos[7:19] = DEFAULT_DOF_POS_MUJOCO
data.qvel[:] = 0.0
data.qpos[2] = 0.35 # 抬高躯干
mujoco.mj_step(model, data)
# 初始化历史 (填充零)
for _ in range(NUM_OBS_HISTORY):
dummy_obs = np.zeros(NUM_OBS, dtype=np.float32)
obs_buffer = np.concatenate([obs_buffer[NUM_OBS:], dummy_obs])
# 7. 初始化键盘控制
keyboard_reader = KeyboardReader()
keyboard_reader.init()
print("[INFO] 键盘控制已启用!")
print(" W/S: 前进/后退 (max 1.0 m/s)")
print(" A/D: 左转/右转 (max 5.0 rad/s)")
print(" Q/E: 侧向左移/右移")
print(" R/F: 抬腿高度 高/低")
print(" U/J: 身体前倾/后仰")
print(" I/K: 身体右倾/左倾")
print(" 1: Trot (小跑) 2: Pace (踱步)")
print(" 3: Bound (奔跑) 4: Pronk (跳跃)")
print(" 空格: 停止")
print(" ESC: 退出")
print(f"[INFO] 当前步态: {GAIT_PRESETS[current_gait]['name']} - {GAIT_PRESETS[current_gait]['description']}")
# 8. 启动交互式查看器
import mujoco.viewer as mv
view = mv.launch_passive(model, data)
print("[INFO] 交互式查看器已启动!")
# 9. 主循环
step_count = 0
last_inference_time = time.time()
inference_interval = 0.02 # 50Hz
# 默认命令
x_vel_cmd = 0.0
y_vel_cmd = 0.0
yaw_vel_cmd = 0.0
body_height_cmd = 0.0
footswing_height_cmd = 0.15
body_pitch_cmd = 0.0
body_roll_cmd = 0.0
gait_frequency_cmd = 3.0
gait_phase_cmd = GAIT_PRESETS[current_gait]['phase']
gait_offset_cmd = GAIT_PRESETS[current_gait]['offset']
gait_bound_cmd = GAIT_PRESETS[current_gait]['bound']
gait_duration_cmd = GAIT_PRESETS[current_gait]['duration']
prev_action = np.zeros(12, dtype=np.float32)
last_action = np.zeros(12, dtype=np.float32)
gait_indices = 0.0
clock_inputs = np.zeros(4, dtype=np.float32)
print("[INFO] 开始 RL 策略推理...")
while view.is_running() and not g_exit_requested:
current_time = time.time()
# 空格: 停止
if keyboard_reader.is_key_pressed(' '):
x_vel_cmd = 0.0
y_vel_cmd = 0.0
yaw_vel_cmd = 0.0
# 步态切换
gait_keys = {'1': 'trot', '2': 'pace', '3': 'bound', '4': 'pronk'}
for key, gait_name in gait_keys.items():
if keyboard_reader.is_key_pressed(key):
if current_gait != gait_name:
current_gait = gait_name
gait_phase_cmd = GAIT_PRESETS[gait_name]['phase']
gait_offset_cmd = GAIT_PRESETS[gait_name]['offset']
gait_bound_cmd = GAIT_PRESETS[gait_name]['bound']
gait_duration_cmd = GAIT_PRESETS[gait_name]['duration']
print(f"[INFO] 切换步态: {GAIT_PRESETS[gait_name]['name']} - {GAIT_PRESETS[gait_name]['description']}")
# 连续动作
if keyboard_reader.is_key_held('w'):
x_vel_cmd = MAX_LIN_VEL
elif keyboard_reader.is_key_held('s'):
x_vel_cmd = -MAX_LIN_VEL
else:
x_vel_cmd = 0.0
if keyboard_reader.is_key_held('q'):
y_vel_cmd = MAX_LIN_VEL
elif keyboard_reader.is_key_held('e'):
y_vel_cmd = -MAX_LIN_VEL
else:
y_vel_cmd = 0.0
if keyboard_reader.is_key_held('a'):
yaw_vel_cmd = MAX_ANG_VEL
elif keyboard_reader.is_key_held('d'):
yaw_vel_cmd = -MAX_ANG_VEL
else:
yaw_vel_cmd = 0.0
# 抬腿高度
if keyboard_reader.is_key_held('r'):
footswing_height_cmd = min(0.35, footswing_height_cmd + 0.005)
elif keyboard_reader.is_key_held('f'):
footswing_height_cmd = max(0.03, footswing_height_cmd - 0.005)
# 身体前后倾斜
if keyboard_reader.is_key_held('u'):
body_pitch_cmd = min(0.4, body_pitch_cmd + 0.01)
elif keyboard_reader.is_key_held('j'):
body_pitch_cmd = max(-0.4, body_pitch_cmd - 0.01)
# 身体左右倾斜
if keyboard_reader.is_key_held('i'):
body_roll_cmd = min(0.0, body_roll_cmd + 0.01)
elif keyboard_reader.is_key_held('k'):
body_roll_cmd = max(0.0, body_roll_cmd - 0.01)
# ESC: 退出
if keyboard_reader.is_key_pressed('escape'):
print("[INFO] ESC pressed, exiting...")
break
# 50Hz 推理
if current_time - last_inference_time >= inference_interval:
# 构建原始命令 (15维)
raw_commands = np.zeros(15, dtype=np.float32)
raw_commands[0] = x_vel_cmd
raw_commands[1] = y_vel_cmd
raw_commands[2] = yaw_vel_cmd
raw_commands[3] = body_height_cmd
raw_commands[4] = gait_frequency_cmd
raw_commands[5] = gait_phase_cmd
raw_commands[6] = gait_offset_cmd
raw_commands[7] = gait_bound_cmd
raw_commands[8] = gait_duration_cmd
raw_commands[9] = footswing_height_cmd
raw_commands[10] = body_pitch_cmd
raw_commands[11] = body_roll_cmd
raw_commands[12] = 0.25 # stance_width
raw_commands[13] = 0.4 # stance_length
# 应用 commands_scale
commands = raw_commands * commands_scale
# 更新 gait indices (每 policy step = 4 * sim_dt = 4 * 0.005 = 0.02)
gait_indices += 0.02 * gait_frequency_cmd
if gait_indices > 1.0:
gait_indices -= 1.0
# 计算 clock_inputs
phase = gait_phase_cmd
offset = gait_offset_cmd
bound = gait_bound_cmd
foot_indices = [
gait_indices + phase + offset + bound, # FL
gait_indices + offset, # FR
gait_indices + bound, # RL
gait_indices + phase # RR
]
clock_inputs[0] = np.sin(2 * np.pi * foot_indices[0])
clock_inputs[1] = np.sin(2 * np.pi * foot_indices[1])
clock_inputs[2] = np.sin(2 * np.pi * foot_indices[2])
clock_inputs[3] = np.sin(2 * np.pi * foot_indices[3])
# 计算当前步观测
obs = compute_observations_wtw(
data, prev_action, last_action, commands, clock_inputs, DEFAULT_DOF_POS_MUJOCO, obs_scales
)
# 更新观测历史 buffer
obs_buffer = np.concatenate([obs_buffer[NUM_OBS:], obs])
# 准备模型输入
obs_buffer_tensor = torch.from_numpy(obs_buffer).float().unsqueeze(0)
obs_tensor = torch.from_numpy(obs).float().unsqueeze(0)
# 推理
with torch.inference_mode():
latent = adapt_module(obs_buffer_tensor)
combined_input = torch.cat([obs_buffer_tensor, latent], dim=1)
action = body_module(combined_input).numpy().flatten()
# 动作后处理
action = np.clip(action, -CLIP_ACTIONS, CLIP_ACTIONS)
# 保存动作历史
last_action = prev_action.copy()
prev_action = action.copy()
last_inference_time = current_time
# ---- PD 控制 ----
action_scaled = prev_action * ACTION_SCALE
# hip关节额外缩放
hip_indices = [0, 3, 6, 9]
for i in hip_indices:
action_scaled[i] *= HIP_SCALE_REDUCTION
joint_targets_mujoco = action_scaled[DEPLOY_TO_MUJOCO_MAPPING] + DEFAULT_DOF_POS_MUJOCO
current_pos = data.qpos[7:19]
current_vel = data.qvel[6:18]
torque = KP * (joint_targets_mujoco - current_pos) - KD * current_vel
data.ctrl[:] = torque
mujoco.mj_step(model, data)
view.sync()
step_count += 1
if step_count % 100 == 0:
trunk_z = data.qpos[2]
lin_vel = np.linalg.norm(data.qvel[0:3])
print(f" Step {step_count}: trunk_z={trunk_z:.3f}m, vel={lin_vel:.3f}m/s")
keyboard_reader.restore()
view.close()
if __name__ == "__main__":
main()

25
mujoco_py_test.py
View File

@@ -1,25 +0,0 @@
#!/usr/bin/env python3
"""
Shows how to toss a capsule to a container.
"""
from re import T
from mujoco_py import load_model_from_path, MjSim, MjViewer
import os
import math
import keyboard
model = load_model_from_path("/path/to/a1.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
sim_state = sim.get_state()
while True:
sim.step()
viewer.render()
#print(" ")
if os.getenv('TESTING') is not None:
break