This commit is contained in:
yangning wu 2024-09-03 21:09:29 +08:00
parent 46f21ab36f
commit 9f60255eb8
30 changed files with 759084 additions and 4 deletions

View File

@ -1,4 +1,4 @@
robot: "go2" # Robot name, "go2", "b2", "b2w", "h1"
robot: "go2" # Robot name, "go2", "b2", "b2w", "h1", "go2w"
robot_scene: "scene_terrain.xml" # Robot scene
domain_id: 1 # Domain id

View File

@ -1,5 +1,5 @@
ROBOT = "go2" # Robot name, "go2", "b2", "b2w", "h1"
ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene.xml" # Robot scene
ROBOT = "go2" # Robot name, "go2", "b2", "b2w", "h1", "go2w"
ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene_terrain.xml" # Robot scene
DOMAIN_ID = 1 # Domain id
INTERFACE = "lo" # Interface
@ -10,5 +10,5 @@ JOYSTICK_DEVICE = 0 # Joystick number
PRINT_SCENE_INFORMATION = True # Print link, joint and sensors information of robot
ENABLE_ELASTIC_BAND = False # Virtual spring band, used for lifting h1
SIMULATE_DT = 0.003 # Need to be larger than the runtime of viewer.sync()
SIMULATE_DT = 0.005 # Need to be larger than the runtime of viewer.sync()
VIEWER_DT = 0.02 # 50 fps for viewer

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

Binary file not shown.

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

Binary file not shown.

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

Binary file not shown.

Binary file not shown.

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -0,0 +1,338 @@
<mujoco model="go2">
<compiler angle="radian" meshdir="assets" autolimits="true" />
<option cone="elliptic" impratio="100" />
<default>
<default class="go2">
<geom friction="0.4" margin="0.001" condim="1" />
<joint axis="0 1 0" damping="0.1" armature="0.01" frictionloss="0.2" />
<motor ctrlrange="-23.7 23.7" />
<default class="abduction">
<joint axis="1 0 0" range="-1.0472 1.0472" />
</default>
<default class="hip">
<default class="front_hip">
<joint range="-1.5708 3.4907" />
</default>
<default class="back_hip">
<joint range="-0.5236 4.5379" />
</default>
</default>
<default class="knee">
<joint range="-2.7227 -0.83776" />
<motor ctrlrange="-45.43 45.43" />
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2" />
</default>
<default class="collision">
<geom group="3" />
<default class="foot">
<geom size="0.022" pos="-0.002 0 -0.2264" priority="1" condim="6"
friction="0.8 0.02 0.01" />
</default>
</default>
</default>
</default>
<asset>
<material name="metal" rgba=".9 .95 .95 1" />
<material name="black" rgba="0 0 0 1" />
<material name="white" rgba="1 1 1 1" />
<material name="gray" rgba="0.671705 0.692426 0.774270 1" />
<mesh file="base_0.obj" />
<mesh file="base_1.obj" />
<mesh file="base_2.obj" />
<mesh file="base_3.obj" />
<mesh file="base_4.obj" />
<mesh file="hip_0.obj" />
<mesh file="hip_1.obj" />
<mesh file="thigh_0.obj" />
<mesh file="thigh_1.obj" />
<mesh file="thigh_mirror_0.obj" />
<mesh file="thigh_mirror_1.obj" />
<mesh file="calf_mirror_0.obj" />
<mesh file="calf_mirror_1.obj" />
<mesh file="calf.stl" />
<mesh file="calf_mirror.stl" />
<mesh file="foot.obj" />
<mesh file="wheel.stl" />
</asset>
<worldbody>
<body name="base_link" pos="0 0 0.6" childclass="go2">
<inertial pos="0.021112 0 -0.005366" quat="-0.000543471 0.713435 -0.00173769 0.700719"
mass="6.921"
diaginertia="0.107027 0.0980771 0.0244531" />
<freejoint />
<geom mesh="base_0" material="black" class="visual" />
<geom mesh="base_1" material="black" class="visual" />
<geom mesh="base_2" material="black" class="visual" />
<geom mesh="base_3" material="white" class="visual" />
<geom mesh="base_4" material="gray" class="visual" />
<geom size="0.1881 0.04675 0.057" type="box" class="collision" />
<geom size="0.05 0.045" pos="0.285 0 0.01" type="cylinder" class="collision" />
<geom size="0.047" pos="0.293 0 -0.06" class="collision" />
<site name="imu" pos="-0.02557 0 0.04232" />
<body name="FL_hip" pos="0.1934 0.0465 0">
<inertial pos="-0.0054 0.00194 -0.000105" quat="0.497014 0.499245 0.505462 0.498237"
mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967" />
<joint name="FL_hip_joint" class="abduction" />
<geom mesh="hip_0" material="metal" class="visual" />
<geom mesh="hip_1" material="gray" class="visual" />
<geom size="0.046 0.02" pos="0 0.08 0" quat="1 1 0 0" type="cylinder" class="collision" />
<body name="FL_thigh" pos="0 0.0955 0">
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623"
mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787" />
<joint name="FL_thigh_joint" class="front_hip" />
<geom mesh="thigh_0" material="metal" class="visual" />
<geom mesh="thigh_1" material="gray" class="visual" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0"
type="box" class="collision" />
<body name="FL_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 -0.000622121 -0.141417"
quat="0.710672 0.00154099 -0.00450087 0.703508"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05" />
<joint name="FL_calf_joint" class="knee" />
<geom mesh="calf" material="gray" class="visual" />
<geom size="0.012 0.06" pos="0.008 0 -0.06" quat="0.994493 0 -0.104807 0"
type="cylinder" class="collision" />
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0"
type="cylinder" class="collision" />
<body name="FL_wheel_link" pos="0 0 -0.2264">
<inertial pos="-0.0 0.04 -0.0"
quat="0.661419 0.248219 0.250051 0.662108" mass="0.98"
diaginertia="0.003 0.0016 0.0016" />
<joint name="FL_wheel_joint" pos="0 0 0" axis="0 1 0" />
<geom type="mesh" rgba="0.15 0.15 0.15 1" mesh="wheel" pos="0 0.0 0"
quat="0.0 1.0 0.0 0.0" />
<geom pos="0 0.0 0" quat="0.0 1.0 0.0 0.0" type="mesh" mesh="wheel" class="foot" />
</body>
</body>
</body>
</body>
<body name="FR_hip" pos="0.1934 -0.0465 0">
<inertial pos="-0.0054 -0.00194 -0.000105" quat="0.498237 0.505462 0.499245 0.497014"
mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967" />
<joint name="FR_hip_joint" class="abduction" />
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 1 0 0" />
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 1 0 0" />
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder"
class="collision" />
<body name="FR_thigh" pos="0 -0.0955 0">
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533"
mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787" />
<joint name="FR_thigh_joint" class="front_hip" />
<geom mesh="thigh_mirror_0" material="metal" class="visual" />
<geom mesh="thigh_mirror_1" material="gray" class="visual" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0"
type="box" class="collision" />
<body name="FR_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 0.000622121 -0.141417"
quat="0.703508 -0.00450087 0.00154099 0.710672"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05" />
<joint name="FR_calf_joint" class="knee" />
<geom mesh="calf_mirror" material="gray" class="visual" />
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0"
type="cylinder" class="collision" />
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0"
type="cylinder" class="collision" />
<body name="FR_wheel_link" pos="0 0 -0.2264">
<inertial pos="-0.0 -0.04 -0.0"
quat="0.661419 0.248219 0.250051 0.662108" mass="0.98"
diaginertia="0.003 0.0016 0.0016" />
<joint name="FR_wheel_joint" pos="0 0 0" axis="0 1 0" />
<geom type="mesh" rgba="0.15 0.15 0.15 1" mesh="wheel" pos="0 -0.0 0" />
<geom pos="0 0.0 0" type="mesh" mesh="wheel" class="foot"/>
</body>
</body>
</body>
</body>
<body name="RL_hip" pos="-0.1934 0.0465 0">
<inertial pos="0.0054 0.00194 -0.000105" quat="0.505462 0.498237 0.497014 0.499245"
mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967" />
<joint name="RL_hip_joint" class="abduction" />
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 0 1 0" />
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 0 1 0" />
<geom size="0.046 0.02" pos="0 0.08 0" quat="0.707107 0.707107 0 0" type="cylinder"
class="collision" />
<body name="RL_thigh" pos="0 0.0955 0">
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623"
mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787" />
<joint name="RL_thigh_joint" class="back_hip" />
<geom mesh="thigh_0" material="metal" class="visual" />
<geom mesh="thigh_1" material="gray" class="visual" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0"
type="box" class="collision" />
<body name="RL_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 -0.000622121 -0.141417"
quat="0.710672 0.00154099 -0.00450087 0.703508"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05" />
<joint name="RL_calf_joint" class="knee" />
<geom mesh="calf" material="gray" class="visual" />
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0"
type="cylinder" class="collision" />
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0"
type="cylinder" class="collision" />
<body name="RL_wheel_link" pos="0 0 -0.2264">
<inertial pos="-0.0 0.04 -0.0"
quat="0.661419 0.248219 0.250051 0.662108" mass="0.98"
diaginertia="0.003 0.0016 0.0016" />
<joint name="RL_wheel_joint" pos="0 0 0" axis="0 1 0" />
<geom type="mesh" rgba="0.15 0.15 0.15 1" mesh="wheel" pos="0 0.0 0"
quat="0.0 1.0 0.0 0.0" />
<geom pos="0 0.0 0" quat="0.0 1.0 0.0 0.0" type="mesh" mesh="wheel" class="foot"/>
</body>
</body>
</body>
</body>
<body name="RR_hip" pos="-0.1934 -0.0465 0">
<inertial pos="0.0054 -0.00194 -0.000105" quat="0.499245 0.497014 0.498237 0.505462"
mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967" />
<joint name="RR_hip_joint" class="abduction" />
<geom mesh="hip_0" material="metal" class="visual"
quat="2.14617e-09 4.63268e-05 4.63268e-05 -1" />
<geom mesh="hip_1" material="gray" class="visual"
quat="2.14617e-09 4.63268e-05 4.63268e-05 -1" />
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder"
class="collision" />
<body name="RR_thigh" pos="0 -0.0955 0">
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533"
mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787" />
<joint name="RR_thigh_joint" class="back_hip" />
<geom mesh="thigh_mirror_0" material="metal" class="visual" />
<geom mesh="thigh_mirror_1" material="gray" class="visual" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0"
type="box" class="collision" />
<body name="RR_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 0.000622121 -0.141417"
quat="0.703508 -0.00450087 0.00154099 0.710672"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05" />
<joint name="RR_calf_joint" class="knee" />
<geom mesh="calf_mirror" material="gray" class="visual" />
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0"
type="cylinder" class="collision" />
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0"
type="cylinder" class="collision" />
<body name="RR_wheel_link" pos="0 0 -0.2264">
<inertial pos="-0.0 -0.04 -0.0"
quat="0.661419 0.248219 0.250051 0.662108" mass="0.98"
diaginertia="0.003 0.0016 0.0016" />
<joint name="RR_wheel_joint" pos="0 0 0" axis="0 1 0" />
<geom type="mesh" rgba="0.15 0.15 0.15 1" mesh="wheel" pos="0 -0.00 0" />
<geom pos="0 0.0 0" type="mesh" mesh="wheel" class="foot"/>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor class="abduction" name="FR_hip" joint="FR_hip_joint" />
<motor class="hip" name="FR_thigh" joint="FR_thigh_joint" />
<motor class="knee" name="FR_calf" joint="FR_calf_joint" />
<motor class="abduction" name="FL_hip" joint="FL_hip_joint" />
<motor class="hip" name="FL_thigh" joint="FL_thigh_joint" />
<motor class="knee" name="FL_calf" joint="FL_calf_joint" />
<motor class="abduction" name="RR_hip" joint="RR_hip_joint" />
<motor class="hip" name="RR_thigh" joint="RR_thigh_joint" />
<motor class="knee" name="RR_calf" joint="RR_calf_joint" />
<motor class="abduction" name="RL_hip" joint="RL_hip_joint" />
<motor class="hip" name="RL_thigh" joint="RL_thigh_joint" />
<motor class="knee" name="RL_calf" joint="RL_calf_joint" />
<motor ctrlrange="-15 15" name="FR_wheel" joint="FR_wheel_joint" />
<motor ctrlrange="-15 15" name="FL_wheel" joint="FL_wheel_joint" />
<motor ctrlrange="-15 15" name="RR_wheel" joint="RR_wheel_joint" />
<motor ctrlrange="-15 15" name="RL_wheel" joint="RL_wheel_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" />
<jointpos name="FR_wheel_pos" joint="FR_wheel_joint" />
<jointpos name="FL_wheel_pos" joint="FL_wheel_joint" />
<jointpos name="RR_wheel_pos" joint="RR_wheel_joint" />
<jointpos name="RL_wheel_pos" joint="RL_wheel_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" />
<jointvel name="FR_wheel_vel" joint="FR_wheel_joint" noise="0.0"/>
<jointvel name="FL_wheel_vel" joint="FL_wheel_joint" noise="0.0"/>
<jointvel name="RR_wheel_vel" joint="RR_wheel_joint" noise="0.0"/>
<jointvel name="RL_wheel_vel" joint="RL_wheel_joint" noise="0.0"/>
<jointactuatorfrc name="FR_hip_torque" joint="FR_hip_joint" noise="0.01" />
<jointactuatorfrc name="FR_thigh_torque" joint="FR_thigh_joint" noise="0.01" />
<jointactuatorfrc name="FR_calf_torque" joint="FR_calf_joint" noise="0.01" />
<jointactuatorfrc name="FL_hip_torque" joint="FL_hip_joint" noise="0.01" />
<jointactuatorfrc name="FL_thigh_torque" joint="FL_thigh_joint" noise="0.01" />
<jointactuatorfrc name="FL_calf_torque" joint="FL_calf_joint" noise="0.01" />
<jointactuatorfrc name="RR_hip_torque" joint="RR_hip_joint" noise="0.01" />
<jointactuatorfrc name="RR_thigh_torque" joint="RR_thigh_joint" noise="0.01" />
<jointactuatorfrc name="RR_calf_torque" joint="RR_calf_joint" noise="0.01" />
<jointactuatorfrc name="RL_hip_torque" joint="RL_hip_joint" noise="0.01" />
<jointactuatorfrc name="RL_thigh_torque" joint="RL_thigh_joint" noise="0.01" />
<jointactuatorfrc name="RL_calf_torque" joint="RL_calf_joint" noise="0.01" />
<jointactuatorfrc name="FR_wheel_torque" joint="FR_wheel_joint" />
<jointactuatorfrc name="FL_wheel_torque" joint="FL_wheel_joint" />
<jointactuatorfrc name="RR_wheel_torque" joint="RR_wheel_joint" />
<jointactuatorfrc name="RL_wheel_torque" joint="RL_wheel_joint" />
<framequat name="imu_quat" objtype="site" objname="imu" noise="0.0"/>
<gyro name="imu_gyro" site="imu" noise="0.0"/>
<accelerometer name="imu_acc" site="imu" />
<framepos name="frame_pos" objtype="site" objname="imu" />
<framelinvel name="frame_vel" objtype="site" objname="imu" />
</sensor>
</mujoco>

Binary file not shown.

After

Width:  |  Height:  |  Size: 4.8 KiB

View File

@ -0,0 +1,33 @@
<mujoco model="go2w scene">
<include file="go2w.xml"/>
<statistic center="0 0 0.1" extent="0.8"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="-130" elevation="-20"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
<geom pos="1.2 0 0.04" type="box" size="0.1 2 0.04" quat="1.0 0 0 0"/>
<geom pos="1.6 0 0.04" type="box" size="0.1 2 0.04" quat="1.0 0 0 0"/>
<geom pos="2.3 0 0.02" type="box" size="0.2 2 0.15" quat="1.0 0 0 0"/>
<geom pos="2.6 0 0.02" type="box" size="0.22 2 0.3" quat="1.0 0 0 0"/>
<geom pos="2.8 0 0.02" type="box" size="0.23 2 0.45" quat="1.0 0 0 0"/>
<geom pos="3 0 0.02" type="box" size="0.24 2 0.6" quat="1.0 0 0 0"/>
<geom pos="3.2 0 0.02" type="box" size="0.25 2 0.75" quat="1.0 0 0 0"/>
<geom pos="3.4 0 0.02" type="box" size="0.26 2 0.9" quat="1.0 0 0 0"/>
</worldbody>
</mujoco>

File diff suppressed because one or more lines are too long

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB