Robocon/RC_WheelLeg
4
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[software] 添加16DOF早期训练仿真与Sim2Real闭环

Browse Source
This commit is contained in:
eastzio
2026-07-21 16:15:14 +08:00
parent 9bd22225f9
commit e9e2c946b3
681 changed files with 137221 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
05_software/train/rc_mjlab/sim2sim/input_dev/keyboard.py
+59
View File
@@ -0,0 +1,59 @@
from pynput import keyboard
class KeyboardCommandController:
"""Provides smoothed velocity commands via keyboard input."""
def __init__(self, max_x_vel=2.0, max_yaw_vel=2.0, acc_step=0.05, dec_step=0.1):
self.current_cmd = [0.0, 0.0, 0.0]
self.max_x_vel = max_x_vel
self.max_yaw_vel = max_yaw_vel
self.acc_step = acc_step
self.dec_step = dec_step
self.pressed_keys = set()
self.listener = keyboard.Listener(
on_press=self.on_press,
on_release=self.on_release
)
def start(self):
self.listener.start()
print("[InputDev] Keyboard control active: UP(fwd) DOWN(bwd) LEFT(yawL) RIGHT(yawR)")
def stop(self):
self.listener.stop()
def on_press(self, key):
self.pressed_keys.add(key)
def on_release(self, key):
if key in self.pressed_keys:
self.pressed_keys.remove(key)
def get_command(self):
target_vx = 0.0
target_dyaw = 0.0
if keyboard.Key.up in self.pressed_keys:
target_vx += self.max_x_vel
if keyboard.Key.down in self.pressed_keys:
target_vx -= self.max_x_vel
if keyboard.Key.left in self.pressed_keys:
target_dyaw += self.max_yaw_vel
if keyboard.Key.right in self.pressed_keys:
target_dyaw -= self.max_yaw_vel
# Smooth X velocity
step_x = self.acc_step if target_vx != 0 else self.dec_step
if self.current_cmd[0] < target_vx:
self.current_cmd[0] = min(self.current_cmd[0] + step_x, target_vx)
elif self.current_cmd[0] > target_vx:
self.current_cmd[0] = max(self.current_cmd[0] - step_x, target_vx)
# Smooth Yaw velocity
step_yaw = self.acc_step * 2.0 if target_dyaw != 0 else self.dec_step * 2.0
if self.current_cmd[2] < target_dyaw:
self.current_cmd[2] = min(self.current_cmd[2] + step_yaw, target_dyaw)
elif self.current_cmd[2] > target_dyaw:
self.current_cmd[2] = max(self.current_cmd[2] - step_yaw, target_dyaw)
return self.current_cmd.copy()
05_software/train/rc_mjlab/sim2sim/interface/mujoco_io.py
+183
View File
@@ -0,0 +1,183 @@
import os
import mujoco
import numpy as np
from pathlib import Path
from tools.math_utils import get_gravity_orientation
class LowPassFilter:
def __init__(self, cutoff_freq, dt, dim):
self.alpha = dt / (dt + 1.0 / (2.0 * np.pi * cutoff_freq))
self.y_prev = None
def filter(self, x):
if self.y_prev is None:
self.y_prev = x.copy()
y = self.alpha * x + (1.0 - self.alpha) * self.y_prev
self.y_prev = y.copy()
return y
class MuJoCoIO:
"""Handles MuJoCo simulation environment initialization and IO."""
def __init__(self, terrain_xml_path, robot_xml_path, hfield_dir):
# Create temp XML
temp_xml = self._create_sim2sim_xml(terrain_xml_path, robot_xml_path, hfield_dir)
print("[MuJoCoIO] Loading MuJoCo model...")
spec = mujoco.MjSpec.from_file(str(temp_xml))
# Override actuators to match mjlab exactly
self._rebuild_actuators(spec)
self.m = spec.compile()
# Boost headlight
self.m.vis.headlight.ambient[:] = [0.6, 0.6, 0.6]
self.m.vis.headlight.diffuse[:] = [0.8, 0.8, 0.8]
self.d = mujoco.MjData(self.m)
# Joint definitions
self.leg_joint_names = [
"fl_hip_abduction_joint", "fl_hip_pitch_joint", "fl_knee_joint",
"fr_hip_abduction_joint", "fr_hip_pitch_joint", "fr_knee_joint",
"rl_hip_abduction_joint", "rl_hip_pitch_joint", "rl_knee_joint",
"rr_hip_abduction_joint", "rr_hip_pitch_joint", "rr_knee_joint",
]
self.wheel_joint_names = [
"fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint", "rr_wheel_joint",
]
self.all_joint_names = self.leg_joint_names + self.wheel_joint_names
self.qpos_ids = np.array([
self.m.jnt_qposadr[mujoco.mj_name2id(self.m, mujoco.mjtObj.mjOBJ_JOINT, n)]
for n in self.all_joint_names
])
self.qvel_ids = np.array([
self.m.jnt_dofadr[mujoco.mj_name2id(self.m, mujoco.mjtObj.mjOBJ_JOINT, n)]
for n in self.all_joint_names
])
self.ctrl_ids = np.array([
mujoco.mj_name2id(self.m, mujoco.mjtObj.mjOBJ_ACTUATOR, n)
for n in self.all_joint_names
])
sim_dt = self.m.opt.timestep
self.control_dt = sim_dt * int(round(0.02 / sim_dt))
self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=self.control_dt, dim=12)
self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=self.control_dt, dim=4)
def _create_sim2sim_xml(self, terrain_xml_path, robot_xml_path, hfield_dir):
with open(terrain_xml_path, "r", encoding="utf-8") as f:
content = f.read()
robot_xml_abs = str(robot_xml_path.absolute()).replace("\\", "/")
content = content.replace(
'<include file="go2w.xml"/>', f'<include file="{robot_xml_abs}"/>'
)
hfield_1 = str((hfield_dir / "height_field.png").absolute()).replace("\\", "/")
hfield_2 = str((hfield_dir / "unitree_hfield.png").absolute()).replace("\\", "/")
content = content.replace("../height_field.png", hfield_1)
content = content.replace("../unitree_hfield.png", hfield_2)
out_xml_path = robot_xml_path.parent / "sim2sim_temp.xml"
with open(out_xml_path, "w", encoding="utf-8") as f:
f.write(content)
os.chdir(str(robot_xml_path.parent))
return out_xml_path
def _rebuild_actuators(self, spec):
actuators_to_delete = list(spec.actuators)
for act in actuators_to_delete:
spec.delete(act)
KP_LEG, KD_LEG = 40.0, 1.0
KD_WHEEL = 0.5
EFFORT_LIMIT = 17.0
leg_jnames = [
"fl_hip_abduction_joint", "fr_hip_abduction_joint",
"rl_hip_abduction_joint", "rr_hip_abduction_joint",
"fl_hip_pitch_joint", "fr_hip_pitch_joint",
"rl_hip_pitch_joint", "rr_hip_pitch_joint",
"fl_knee_joint", "fr_knee_joint",
"rl_knee_joint", "rr_knee_joint",
]
wheel_jnames = ["fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint", "rr_wheel_joint"]
for jname in leg_jnames:
act = spec.add_actuator(name=jname, target=jname)
act.trntype = mujoco.mjtTrn.mjTRN_JOINT
act.dyntype = mujoco.mjtDyn.mjDYN_NONE
act.gaintype = mujoco.mjtGain.mjGAIN_FIXED
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
act.gainprm[0] = KP_LEG
act.biasprm[1] = -KP_LEG
act.biasprm[2] = -KD_LEG
act.forcelimited = True
act.forcerange[:] = [-EFFORT_LIMIT, EFFORT_LIMIT]
act.inheritrange = 0.0
act.ctrllimited = False
for jname in wheel_jnames:
act = spec.add_actuator(name=jname, target=jname)
act.trntype = mujoco.mjtTrn.mjTRN_JOINT
act.dyntype = mujoco.mjtDyn.mjDYN_NONE
act.gaintype = mujoco.mjtGain.mjGAIN_FIXED
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
act.gainprm[0] = KD_WHEEL
act.biasprm[2] = -KD_WHEEL
act.forcelimited = True
act.forcerange[:] = [-EFFORT_LIMIT, EFFORT_LIMIT]
act.inheritrange = 0.0
act.ctrllimited = False
l = spec.worldbody.add_light()
l.pos[:] = [3.7, -9.0, 4.0]
l.dir[:] = [0.0, 0.0, -1.0]
l.diffuse[:] = [0.8, 0.8, 0.8]
l.specular[:] = [0.3, 0.3, 0.3]
def reset_robot(self, default_dof_pos):
print("[MuJoCoIO] Dropping robot to floor...")
self.d.qpos[:3] = [3.7, -9.0, 0.6]
self.d.qpos[self.qpos_ids] = default_dof_pos
self.d.ctrl[self.ctrl_ids[:12]] = default_dof_pos[:12]
self.d.ctrl[self.ctrl_ids[12:]] = 0.0
for _ in range(500):
mujoco.mj_step(self.m, self.d)
def get_obs_53d(self, command, default_dof_pos, last_actions_raw):
quat_wxyz = self.d.qpos[3:7].copy()
ang_vel_body = self.d.qvel[3:6].copy()
base_ang_vel = (ang_vel_body * 0.25).astype(np.float32)
projected_gravity = get_gravity_orientation(quat_wxyz)
dof_pos = self.d.qpos[self.qpos_ids]
dof_vel = self.d.qvel[self.qvel_ids]
joint_pos_rel = (dof_pos[:12] - default_dof_pos[:12]).astype(np.float32)
joint_vel_leg = (dof_vel[:12] * 0.05).astype(np.float32)
wheel_vel = (dof_vel[12:] * 0.05).astype(np.float32)
obs = np.concatenate([
base_ang_vel, # 3
projected_gravity, # 3
command, # 3
joint_pos_rel, # 12
joint_vel_leg, # 12
wheel_vel, # 4
last_actions_raw, # 16
])
return obs
def send_actions(self, scaled_actions, default_dof_pos):
act = scaled_actions + default_dof_pos
act = np.clip(act, -100, 100)
# Apply Low Pass Filter
act[:12] = self.lpf_legs.filter(act[:12])
act[12:] = self.lpf_wheels.filter(act[12:])
for i in range(min(len(act), len(self.d.ctrl))):
self.d.ctrl[self.ctrl_ids[i]] = act[i]
05_software/train/rc_mjlab/sim2sim/main.py
+108
View File
@@ -0,0 +1,108 @@
import time
import torch
import numpy as np
import mujoco.viewer
from pathlib import Path
# 导入我们的模块化组件
from input_dev.keyboard import KeyboardCommandController
from policy.policy_runner import PolicyRunner
from interface.mujoco_io import MuJoCoIO
from tools.logger import SimpleLogger
def main():
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
# 路径设置 (兼容 rc_mjlab/sim2sim 目录结构)
project_root = Path(__file__).parent.parent.absolute()
terrain_dir = Path(__file__).parent / "terrain"
terrain_xml = terrain_dir / "scene_terrain.xml"
robot_xml = project_root / "mjcf" / "wheelleg.xml"
policy_path = {
"rough": project_root / "model_rough.pt",
"crawl": project_root / "model_crawl.pt"
}
# 1. 初始化 MuJoCo IO 接口
print("\n[Main] Initializing MuJoCo Environment...")
io = MuJoCoIO(terrain_xml, robot_xml, terrain_dir)
# 2. 初始化 Policy 推理层
print("\n[Main] Initializing Policy Runner...")
runner = PolicyRunner(policy_path, device)
# 3. 初始化键盘控制器 (带平滑加减速)
print("\n[Main] Initializing Input Controller...")
kb = KeyboardCommandController(max_x_vel=1.0, max_yaw_vel=1.0)
kb.start()
# 4. 初始化数据日志记录器
print("\n[Main] Initializing Data Logger...")
logger = SimpleLogger(log_dir=str(project_root / "sim2sim"))
# 机器人复位
io.reset_robot(runner.default_dof_pos)
runner.reset()
# 时序控制计算
control_dt = io.control_dt # 通常是 0.02s (50Hz)
sim_steps_per_control = int(round(control_dt / io.m.opt.timestep)) # 通常是 10
next_exec_time = time.perf_counter()
viewer_counter = 0
print(f"\n[Main] Starting Control Loop (Control DT: {control_dt:.3f}s)")
try:
with mujoco.viewer.launch_passive(io.m, io.d) as viewer:
# 初始相机视角
viewer.cam.distance = 5.0
viewer.cam.elevation = -20.0
viewer.cam.azimuth = 45.0
while viewer.is_running():
# [1] 获取用户指令
command = kb.get_command()
# [2] 读取环境观测值
obs = io.get_obs_53d(command, runner.default_dof_pos, runner.last_actions)
# [3] 神经网络推理 (包含历史堆叠处理)
scaled_actions, raw_actions = runner.step(obs)
# [4] 下发动作到仿真器 (包含低通滤波)
io.send_actions(scaled_actions, runner.default_dof_pos)
# [5] 记录日志
logger.update(io.d.time, io.m, io.d, command)
# [6] 推进物理仿真
for _ in range(sim_steps_per_control):
mujoco.mj_step(io.m, io.d)
# [7] 降低渲染频率以节省性能 (25Hz 渲染)
viewer_counter += 1
if viewer_counter >= 2:
base_id = mujoco.mj_name2id(io.m, mujoco.mjtObj.mjOBJ_BODY, "base_link")
if base_id != -1:
viewer.cam.lookat[:] = io.d.xpos[base_id]
viewer.sync()
viewer_counter = 0
# [8] 高精度时序锁帧 (完全对标真机 RTOS 逻辑)
next_exec_time += control_dt
now = time.perf_counter()
sleep_time = next_exec_time - now
if sleep_time > 0:
time.sleep(sleep_time)
elif sleep_time < -control_dt:
# 如果发生严重掉帧，重置时钟，避免疯狂快进
next_exec_time = now
finally:
print("\n[Main] Shutting down...")
kb.stop()
logger.save() # 保存数据到 txt
if __name__ == "__main__":
main()
05_software/train/rc_mjlab/sim2sim/nav_sim2sim.py
+1382
View File
File diff suppressed because it is too large Load Diff
05_software/train/rc_mjlab/sim2sim/policy/policy_runner.py
+260
View File
@@ -0,0 +1,260 @@
import torch
import torch.nn as nn
import numpy as np
from collections import deque
from pathlib import Path
from pynput import keyboard
# ============================================================
# Policy Model
# ============================================================
class PolicyMLP(nn.Module):
def __init__(self, obs_dim=318, action_dim=16):
super().__init__()
self.register_buffer("obs_mean", torch.zeros(obs_dim))
self.register_buffer("obs_std", torch.ones(obs_dim))
self.net = nn.Sequential(
nn.Linear(obs_dim, 512), nn.ELU(),
nn.Linear(512, 256), nn.ELU(),
nn.Linear(256, 128), nn.ELU(),
nn.Linear(128, action_dim),
)
def forward(self, x):
x = (x - self.obs_mean) / self.obs_std
return self.net(x)
def load_policy(model_path, device):
ckpt = torch.load(model_path, map_location=device, weights_only=False)
state_dict = ckpt["actor_state_dict"]
# Check mlp.0.weight to determine obs_dim
weight_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
obs_dim = state_dict[weight_key].shape[1]
model = PolicyMLP(obs_dim=obs_dim, action_dim=16)
my_sd = {}
for k, v in state_dict.items():
if k.startswith("mlp."):
my_sd[k.replace("mlp.", "net.")] = v
elif k.startswith("net."):
my_sd[k] = v
elif k == "obs_normalizer._mean":
my_sd["obs_mean"] = v.squeeze()
elif k == "obs_normalizer._var":
my_sd["obs_std"] = torch.sqrt(v.squeeze() + 1e-5)
model.load_state_dict(my_sd, strict=False)
model.eval()
model.to(device)
return model
# ============================================================
# Policy Runner with Smooth Dual-Policy Blending Transition
# ============================================================
class PolicyRunner:
"""Handles multi-policy execution and smooth linear blending transitions."""
def __init__(self, policy_path, device, history_length=6, obs_dim=53):
self.device = device
self.history_length = history_length
self.obs_dim = obs_dim
# Automatically resolve rough and crawl policy paths
if isinstance(policy_path, dict):
self.policy_paths = policy_path
else:
p_path = Path(policy_path)
policy_dir = p_path.parent
crawl_path = policy_dir / "logs" / "crawl.pt"
if not crawl_path.exists():
crawl_path = policy_dir / "crawl.pt"
self.policy_paths = {
"rough": p_path,
"crawl": crawl_path if crawl_path.exists() else p_path
}
# Load both policy networks
self.policies = {}
for name, path in self.policy_paths.items():
print(f"[PolicyRunner] Loading {name} policy from: {path}")
if Path(path).exists():
self.policies[name] = load_policy(path, device)
else:
print(f"[PolicyRunner] WARNING: {name} policy file not found! Falling back to rough.")
self.policies[name] = load_policy(self.policy_paths["rough"], device)
# Default DOF positions for each policy
self.default_dof_poses = {
"rough": np.array([0.0, 0.9, -1.8] * 4 + [0.0] * 4, dtype=np.float32),
"crawl": np.array([
0.4, 1.65, -2.55, # FL (Left)
-0.4, 1.65, -2.55, # FR (Right)
0.4, 1.65, -2.55, # RL (Left)
-0.4, 1.65, -2.55, # RR (Right)
0.0, 0.0, 0.0, 0.0 # Wheels
], dtype=np.float32)
}
# Separate observation history buffers for each policy
self.obs_histories = {
name: deque(maxlen=self.history_length) for name in self.policies.keys()
}
# Separate previous actions for each policy to maintain correct history stacking
self.last_actions_dict = {
name: np.zeros(16, dtype=np.float32) for name in self.policies.keys()
}
# Blending and state transition variables
self.current_policy_name = "rough"
self.transition_in_progress = False
self.transition_old_name = None
self.transition_step = 0
self.transition_steps = 50 # 1.0s at 50Hz (N=50 steps)
# Action scale settings (HIP=0.125, KNEE/THIGH=0.25, WHEEL=5.0)
self.HIP_SCALE = 0.125
self.LEG_POS_SCALE = 0.25
self.WHEEL_VEL_SCALE = 5.0
self.action_scale = np.array([
self.HIP_SCALE, self.LEG_POS_SCALE, self.LEG_POS_SCALE, # FL
self.HIP_SCALE, self.LEG_POS_SCALE, self.LEG_POS_SCALE, # FR
self.HIP_SCALE, self.LEG_POS_SCALE, self.LEG_POS_SCALE, # RL
self.HIP_SCALE, self.LEG_POS_SCALE, self.LEG_POS_SCALE, # RR
self.WHEEL_VEL_SCALE, self.WHEEL_VEL_SCALE, self.WHEEL_VEL_SCALE, self.WHEEL_VEL_SCALE
], dtype=np.float32)
# Background keyboard listener for seamless switcher keys ('1' and '2')
self.listener = keyboard.Listener(on_press=self._on_press)
self.listener.start()
print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
def _on_press(self, key):
try:
if hasattr(key, 'char') and key.char is not None:
if key.char == '1':
self.trigger_transition("rough")
elif key.char == '2':
self.trigger_transition("crawl")
except Exception as e:
print(f"[PolicyRunner] Error in key listener: {e}")
def trigger_transition(self, target_name):
if target_name not in self.policies:
return
if target_name == self.current_policy_name and not self.transition_in_progress:
return
# If already transitioning, override target or complete the current one
self.transition_old_name = self.current_policy_name
self.current_policy_name = target_name
self.transition_in_progress = True
self.transition_step = 0
print(f"\n[PolicyRunner] Smooth Transition: {self.transition_old_name.upper()} -> {self.current_policy_name.upper()} over 1.0s...")
@property
def default_dof_pos(self) -> np.ndarray:
"""Dynamic property returning the blended default pose during transition."""
if self.transition_in_progress:
alpha = self.transition_step / self.transition_steps
return (1.0 - alpha) * self.default_dof_poses[self.transition_old_name] + alpha * self.default_dof_poses[self.current_policy_name]
return self.default_dof_poses[self.current_policy_name]
@property
def last_actions(self) -> np.ndarray:
"""Dynamic property returning the blended previous raw actions for logging/main.py."""
if self.transition_in_progress:
alpha = self.transition_step / self.transition_steps
return (1.0 - alpha) * self.last_actions_dict[self.transition_old_name] + alpha * self.last_actions_dict[self.current_policy_name]
return self.last_actions_dict[self.current_policy_name]
def reset(self):
for name in self.obs_histories.keys():
self.obs_histories[name].clear()
self.last_actions_dict[name] = np.zeros(16, dtype=np.float32)
self.transition_in_progress = False
self.transition_old_name = None
self.transition_step = 0
def step(self, current_obs_53d):
"""
Receives raw 53D observation (computed in main.py using blended default_dof_pos),
re-aligns observation for each policy dynamically, runs inference, and blends output actions.
This implementation uses a local state snapshot to prevent multi-threaded race conditions
with the keyboard background listener thread.
"""
# 1. Take atomic snapshot of state variables
in_progress = self.transition_in_progress
current_name = self.current_policy_name
old_name = self.transition_old_name
step_idx = self.transition_step
total_steps = self.transition_steps
# 2. Compute blended default dof pos using snapshot variables
if in_progress:
alpha = step_idx / total_steps
default_dof_pos_blended = (1.0 - alpha) * self.default_dof_poses[old_name] + alpha * self.default_dof_poses[current_name]
else:
default_dof_pos_blended = self.default_dof_poses[current_name]
# Update histories for BOTH policies with their respective mathematically aligned observations
for name in self.policies.keys():
# Align relative joint positions: obs[9:21] is joint_pos_rel
obs_policy = current_obs_53d.copy()
# Math: q_rel_policy = q_rel_blended + q_default_blended - q_default_policy
# Since current_obs_53d was computed as (q - q_default_blended), this recovers (q - q_default_policy)
obs_policy[9:21] = obs_policy[9:21] + default_dof_pos_blended[:12] - self.default_dof_poses[name][:12]
# Inject policy-specific previous actions
obs_policy[37:53] = self.last_actions_dict[name]
# Populate queue
if len(self.obs_histories[name]) == 0:
for _ in range(self.history_length):
self.obs_histories[name].append(obs_policy.copy())
else:
self.obs_histories[name].append(obs_policy.copy())
# Decide which policies to run using snapshot variables
active_policies = [current_name]
if in_progress:
active_policies.append(old_name)
raw_actions_out = {}
for name in active_policies:
# Flatten observation history
obs_history_array = np.array(self.obs_histories[name])
term_dims = [3, 3, 3, 12, 12, 4, 16]
term_histories = np.split(obs_history_array, np.cumsum(term_dims)[:-1], axis=1)
flat_obs = np.concatenate([h.flatten() for h in term_histories])
obs_tensor = torch.tensor(flat_obs, device=self.device, dtype=torch.float32).unsqueeze(0)
with torch.no_grad():
raw_actions = self.policies[name](obs_tensor).squeeze(0).cpu().numpy()
raw_actions = np.clip(raw_actions, -100.0, 100.0)
self.last_actions_dict[name] = raw_actions.copy()
raw_actions_out[name] = raw_actions
# Blend actions if transitioning using snapshot variables
if in_progress:
alpha = step_idx / total_steps
raw_actions_blended = (1.0 - alpha) * raw_actions_out[old_name] + alpha * raw_actions_out[current_name]
# Step transition state machine on the class state safely
self.transition_step += 1
if self.transition_step >= self.transition_steps:
self.transition_in_progress = False
print(f"[PolicyRunner] Switch to {self.current_policy_name.upper()} complete!")
else:
raw_actions_blended = raw_actions_out[current_name]
# Scale blended actions to physical outputs
scaled_actions = raw_actions_blended * self.action_scale
return scaled_actions, raw_actions_blended
05_software/train/rc_mjlab/sim2sim/sim2sim.py
+438
View File
@@ -0,0 +1,438 @@
"""
Sim2Sim: Deploy rc_mjlab policy in MuJoCo with RC_MAP terrain.
Actuator setup matches mjlab training exactly:
- Legs: <position> actuator (kp=40, kd=1) — d.ctrl = target_position
- Wheels: <velocity> actuator (kd=0.5) — d.ctrl = target_velocity
The original XML's <general> actuators are overridden in Python to match
the mjlab BuiltinPositionActuator / BuiltinVelocityActuator setup.
"""
import os
import time
import math
import torch
import torch.nn as nn
import mujoco
import mujoco.viewer
import numpy as np
import pygame
from pathlib import Path
# ============================================================
# Policy Model
# ============================================================
class PolicyMLP(nn.Module):
def __init__(self, obs_dim=318, action_dim=16):
super().__init__()
self.register_buffer("obs_mean", torch.zeros(obs_dim))
self.register_buffer("obs_std", torch.ones(obs_dim))
self.net = nn.Sequential(
nn.Linear(obs_dim, 512), nn.ELU(),
nn.Linear(512, 256), nn.ELU(),
nn.Linear(256, 128), nn.ELU(),
nn.Linear(128, action_dim),
)
def forward(self, x):
x = (x - self.obs_mean) / self.obs_std
return self.net(x)
def load_policy(model_path, device):
ckpt = torch.load(model_path, map_location=device, weights_only=False)
state_dict = ckpt["actor_state_dict"]
model = PolicyMLP()
my_sd = {}
for k, v in state_dict.items():
if k.startswith("mlp."):
my_sd[k.replace("mlp.", "net.")] = v
elif k == "obs_normalizer._mean":
my_sd["obs_mean"] = v.squeeze()
elif k == "obs_normalizer._var":
my_sd["obs_std"] = torch.sqrt(v.squeeze() + 1e-5)
model.load_state_dict(my_sd, strict=False)
model.eval()
model.to(device)
return model
# ============================================================
# Math Utilities
# ============================================================
def get_gravity_orientation(quat_wxyz):
"""
Compute projected gravity in body frame from quaternion [w,x,y,z].
Proven formula from DreamWaQ-sim2sim reference (easy_math.py).
"""
qw, qx, qy, qz = quat_wxyz
gx = 2.0 * (-qz * qx + qw * qy)
gy = -2.0 * (qz * qy + qw * qx)
gz = 1.0 - 2.0 * (qw * qw + qz * qz)
return np.array([gx, gy, gz], dtype=np.float32)
def quat_rotate_inverse(quat_wxyz, v):
"""
Rotate vector v from world frame to body frame.
quat_wxyz: [w, x, y, z] (as stored in MuJoCo qpos[3:7])
v: [3] world-frame vector
Same formula as go2w_sim2sim/lab2mujoco.py world2self (no conjugate).
"""
q_w = quat_wxyz[0]
q_vec = quat_wxyz[1:]
a = v * (2.0 * q_w * q_w - 1.0)
b = np.cross(q_vec, v) * q_w * 2.0
c = q_vec * np.dot(q_vec, v) * 2.0
return a - b + c
# ============================================================
# XML Preparation
# ============================================================
def create_sim2sim_xml(terrain_xml_path, robot_xml_path, hfield_dir, out_xml_path):
with open(terrain_xml_path, "r", encoding="utf-8") as f:
content = f.read()
robot_xml_abs = str(robot_xml_path.absolute()).replace("\\", "/")
content = content.replace(
'<include file="go2w.xml"/>', f'<include file="{robot_xml_abs}"/>'
)
hfield_1 = str((hfield_dir / "height_field.png").absolute()).replace("\\", "/")
hfield_2 = str((hfield_dir / "unitree_hfield.png").absolute()).replace("\\", "/")
content = content.replace("../height_field.png", hfield_1)
content = content.replace("../unitree_hfield.png", hfield_2)
with open(out_xml_path, "w", encoding="utf-8") as f:
f.write(content)
class LowPassFilter:
def __init__(self, cutoff_freq, dt, dim):
self.alpha = dt / (dt + 1.0 / (2.0 * math.pi * cutoff_freq))
self.y_prev = np.zeros(dim, dtype=np.float64)
def filter(self, x):
y = self.alpha * x + (1.0 - self.alpha) * self.y_prev
self.y_prev = y.copy()
return y
# ============================================================
# Main
# ============================================================
def main():
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
project_root = Path(__file__).parent.parent.absolute()
# Local terrain directory to make rc_mjlab independent
terrain_dir = Path(__file__).parent / "terrain"
terrain_xml = terrain_dir / "scene_terrain.xml"
robot_xml = Path(__file__).parent.parent / "mjcf" / "wheelleg.xml"
policy_path = Path(__file__).parent.parent / "model_1700.pt"
hfield_dir = terrain_dir
temp_xml = project_root / "mjcf" / "sim2sim_temp.xml"
create_sim2sim_xml(terrain_xml, robot_xml, hfield_dir, temp_xml)
print("Loading MuJoCo model...")
os.chdir(str(project_root / "mjcf"))
# Build model using MjSpec (same as mjlab training) to avoid
# broken <include> which renames joints and drops actuators.
spec = mujoco.MjSpec.from_file(str(temp_xml))
# Delete existing XML actuators (mjlab's get_spec() does this)
actuators_to_delete = list(spec.actuators)
for act in actuators_to_delete:
spec.delete(act)
# Rebuild actuators matching mjlab training config exactly
KP_LEG = 40.0
KD_LEG = 1.0
KD_WHEEL = 0.5
EFFORT_LIMIT = 17.0
leg_joint_names = [
"fl_hip_abduction_joint", "fr_hip_abduction_joint",
"rl_hip_abduction_joint", "rr_hip_abduction_joint",
"fl_hip_pitch_joint", "fr_hip_pitch_joint",
"rl_hip_pitch_joint", "rr_hip_pitch_joint",
"fl_knee_joint", "fr_knee_joint",
"rl_knee_joint", "rr_knee_joint",
]
wheel_joint_names = [
"fl_wheel_joint", "fr_wheel_joint",
"rl_wheel_joint", "rr_wheel_joint",
]
# Add position actuators for legs (kp=40, kd=1)
for jname in leg_joint_names:
act = spec.add_actuator(name=jname, target=jname)
act.trntype = mujoco.mjtTrn.mjTRN_JOINT
act.dyntype = mujoco.mjtDyn.mjDYN_NONE
act.gaintype = mujoco.mjtGain.mjGAIN_FIXED
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
act.gainprm[0] = KP_LEG
act.biasprm[1] = -KP_LEG
act.biasprm[2] = -KD_LEG
act.forcelimited = True
act.forcerange[:] = [-EFFORT_LIMIT, EFFORT_LIMIT]
act.inheritrange = 0.0
act.ctrllimited = False
# Add velocity actuators for wheels (kd=0.5)
for jname in wheel_joint_names:
act = spec.add_actuator(name=jname, target=jname)
act.trntype = mujoco.mjtTrn.mjTRN_JOINT
act.dyntype = mujoco.mjtDyn.mjDYN_NONE
act.gaintype = mujoco.mjtGain.mjGAIN_FIXED
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
act.gainprm[0] = KD_WHEEL
act.biasprm[2] = -KD_WHEEL
act.forcelimited = True
act.forcerange[:] = [-EFFORT_LIMIT, EFFORT_LIMIT]
act.inheritrange = 0.0
act.ctrllimited = False
# Add an explicit light over the spawn area so it's guaranteed to be bright
l = spec.worldbody.add_light()
l.pos[:] = [3.7, -9.0, 4.0]
l.dir[:] = [0.0, 0.0, -1.0]
l.diffuse[:] = [0.8, 0.8, 0.8]
l.specular[:] = [0.3, 0.3, 0.3]
m = spec.compile()
# Boost global headlight to ensure no dark corners when camera moves
m.vis.headlight.ambient[:] = [0.6, 0.6, 0.6]
m.vis.headlight.diffuse[:] = [0.8, 0.8, 0.8]
d = mujoco.MjData(m)
# Verify actuators
print(f"Model: {m.njnt} joints, {m.nu} actuators")
for i in range(m.nu):
print(f" actuator[{i}] = {mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_ACTUATOR, i)}")
print(f"Loading Policy from {policy_path}...")
policy = load_policy(policy_path, device)
print("Policy loaded successfully.")
# ------------------------------------------------------------------
# Joint Configuration — names match the XML exactly (MjSpec preserves them)
# ------------------------------------------------------------------
leg_joint_names = [
"fl_hip_abduction_joint", "fl_hip_pitch_joint", "fl_knee_joint",
"fr_hip_abduction_joint", "fr_hip_pitch_joint", "fr_knee_joint",
"rl_hip_abduction_joint", "rl_hip_pitch_joint", "rl_knee_joint",
"rr_hip_abduction_joint", "rr_hip_pitch_joint", "rr_knee_joint",
]
wheel_joint_names = [
"fl_wheel_joint",
"fr_wheel_joint",
"rl_wheel_joint",
"rr_wheel_joint",
]
# ------------------------------------------------------------------
all_joint_names = leg_joint_names + wheel_joint_names
qpos_ids = np.array([
m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, n)]
for n in all_joint_names
])
qvel_ids = np.array([
m.jnt_dofadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, n)]
for n in all_joint_names
])
ctrl_ids = np.array([
mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, n)
for n in all_joint_names
])
# Verify all IDs are valid
for i, name in enumerate(all_joint_names):
jid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, name)
assert jid >= 0, f"Joint '{name}' not found!"
assert ctrl_ids[i] >= 0, f"Actuator '{name}' not found!"
print(f"All {len(all_joint_names)} joints and actuators verified.")
# Default joint positions
default_dof_pos = np.array([
0.0, 0.9, -1.8, # FL
0.0, 0.9, -1.8, # FR
0.0, 0.9, -1.8, # RL
0.0, 0.9, -1.8, # RR
0.0, 0.0, 0.0, 0.0, # wheel
], dtype=np.float64)
# Action scales
HIP_SCALE = 0.125
LEG_POS_SCALE = 0.25
WHEEL_VEL_SCALE = 5.0
action_scale = np.array([
HIP_SCALE, LEG_POS_SCALE, LEG_POS_SCALE, # FL
HIP_SCALE, LEG_POS_SCALE, LEG_POS_SCALE, # FR
HIP_SCALE, LEG_POS_SCALE, LEG_POS_SCALE, # RL
HIP_SCALE, LEG_POS_SCALE, LEG_POS_SCALE, # RR
WHEEL_VEL_SCALE, WHEEL_VEL_SCALE, WHEEL_VEL_SCALE, WHEEL_VEL_SCALE
], dtype=np.float64)
# ------------------------------------------------------------------
# Physics & Control
# ------------------------------------------------------------------
sim_dt = m.opt.timestep
decimation = int(round(0.02 / sim_dt)) # 50Hz control
control_dt = sim_dt * decimation
lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=control_dt, dim=12)
lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=control_dt, dim=4)
# History buffer
history_length = 6
obs_dim = 53
obs_history = np.zeros((history_length, obs_dim), dtype=np.float32)
last_actions = np.zeros(16, dtype=np.float32)
command = np.array([0.0, 0.0, 0.0], dtype=np.float32)
# ------------------------------------------------------------------
# Pygame UI
# ------------------------------------------------------------------
pygame.init()
screen = pygame.display.set_mode((400, 300))
pygame.display.set_caption("Go2W Sim2Sim Control")
font = pygame.font.Font(pygame.font.get_default_font(), 24)
def get_obs():
"""Build 53-dim observation matching IsaacLab env_cfgs.py actor_terms order."""
quat_wxyz = d.qpos[3:7].copy()
# MuJoCo d.qvel[3:6] for free joints is ALREADY in body frame
# (unlike cvel which is in world frame). No rotation needed.
ang_vel_body = d.qvel[3:6].copy()
# Body-frame angular velocity * scale
base_ang_vel = (ang_vel_body * 0.25).astype(np.float32)
# Projected gravity
projected_gravity = get_gravity_orientation(quat_wxyz)
# Joint states
dof_pos = d.qpos[qpos_ids]
dof_vel = d.qvel[qvel_ids]
joint_pos_rel = (dof_pos[:12] - default_dof_pos[:12]).astype(np.float32)
joint_vel_leg = (dof_vel[:12] * 0.05).astype(np.float32)
wheel_vel = (dof_vel[12:] * 0.05).astype(np.float32)
obs = np.concatenate([
base_ang_vel, # 3
projected_gravity, # 3
command, # 3
joint_pos_rel, # 12
joint_vel_leg, # 12
wheel_vel, # 4
last_actions, # 16
]) # total = 53
return obs
# ------------------------------------------------------------------
# Initialize: set default pose and let robot settle
# ------------------------------------------------------------------
print("Dropping robot to floor...")
d.qpos[:3] = [3.7, -9.0, 0.6]
d.qpos[qpos_ids] = default_dof_pos
# Set ctrl to default targets so the PD controller holds the pose
d.ctrl[ctrl_ids[:12]] = default_dof_pos[:12] # leg position targets
d.ctrl[ctrl_ids[12:]] = 0.0 # wheel velocity targets = 0
for _ in range(500):
mujoco.mj_step(m, d)
# Fill history buffer
init_obs = get_obs()
for i in range(history_length):
obs_history[i] = init_obs
print("Starting control loop...")
with mujoco.viewer.launch_passive(m, d) as viewer:
while viewer.is_running():
step_start = time.time()
# --- Pygame UI ---
for event in pygame.event.get():
if event.type == pygame.QUIT:
viewer.close()
pygame.quit()
return
keys = pygame.key.get_pressed()
cmd_vx, cmd_vy, cmd_wz = 0.0, 0.0, 0.0
if keys[pygame.K_UP]: cmd_vx = 1.0
if keys[pygame.K_DOWN]: cmd_vx = -1.0
if keys[pygame.K_LEFT]: cmd_vy = 0.5
if keys[pygame.K_RIGHT]: cmd_vy = -0.5
if keys[pygame.K_a]: cmd_wz = 1.0
if keys[pygame.K_d]: cmd_wz = -1.0
command[0] = cmd_vx
command[1] = cmd_vy
command[2] = cmd_wz
screen.fill((30, 30, 30))
screen.blit(font.render("Go2W Sim2Sim Control", True, (255, 255, 255)), (20, 20))
screen.blit(font.render(f"VX (UP/DOWN): {cmd_vx:.1f}", True, (0, 255, 0)), (20, 60))
screen.blit(font.render(f"VY (LEFT/RIGHT): {cmd_vy:.1f}", True, (0, 255, 0)), (20, 100))
screen.blit(font.render(f"WZ (A/D): {cmd_wz:.1f}", True, (0, 255, 0)), (20, 140))
screen.blit(font.render(f"Time: {d.time:.1f}s", True, (200, 200, 200)), (20, 200))
pygame.display.flip()
# --- Policy inference at control frequency ---
obs = get_obs()
# update history buffer
obs_history = np.roll(obs_history, -1, axis=0)
obs_history[-1] = obs
# mjlab observation layout: flatten history per-term, then concatenate
term_dims = [3, 3, 3, 12, 12, 4, 16]
term_histories = np.split(obs_history, np.cumsum(term_dims)[:-1], axis=1)
flat_obs = np.concatenate([h.flatten() for h in term_histories])
# policy inference
pi_input = torch.tensor(flat_obs, dtype=torch.float32, device=device).unsqueeze(0)
with torch.no_grad():
actions = policy(pi_input).squeeze(0).cpu().numpy()
actions = np.clip(actions, -100.0, 100.0)
last_actions[:] = actions
# Scale and filter
scaled = actions * action_scale
leg_targets = lpf_legs.filter(scaled[:12])
wheel_targets = lpf_wheels.filter(scaled[12:])
# Apply to MuJoCo ctrl:
# Legs: position targets (offset by default pose)
d.ctrl[ctrl_ids[:12]] = leg_targets + default_dof_pos[:12]
# Wheels: velocity targets
d.ctrl[ctrl_ids[12:]] = wheel_targets
# Step simulation (decimation steps per control step)
for _ in range(decimation):
mujoco.mj_step(m, d)
viewer.sync()
elapsed = time.time() - step_start
sleep_time = control_dt - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
pygame.quit()
if __name__ == "__main__":
main()
05_software/train/rc_mjlab/sim2sim/terrain/height_field.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.8 KiB

05_software/train/rc_mjlab/sim2sim/terrain/scene_terrain.xml
+327
View File
@@ -0,0 +1,327 @@
<mujoco model="go2w scene">
<include file="go2w.xml"/>
<statistic center="3.7 -9.0 0.4" extent="5.0"/>
<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="90" 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"/>
<hfield name="perlin_hfield" size="1.0 0.75 0.2 0.2" file="../height_field.png"/>
<hfield name="image_hfield" size="1.0 1.0 0.02 0.1" file="../unitree_hfield.png"/>
</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" />
<!-- 30cm高墙：旋转90度，沿x轴方向放置，并与T型楼梯中心线 y=-3.50 对齐 -->
<geom pos="1.8 -7.0 0.15"
type="box"
size="0.025 0.5 0.15"
quat="0.7071068 0.0 0.0 0.7071068"
rgba="1.0 0.9 0.4 1.0"/>
<!-- 沙砾碎木坑：x正方向边界与10度斜坡+x边界对齐，y正边界距斜坡y负边界4m -->
<geom pos="4.8361 -12.5 0.075"
type="box"
size="0.5 0.5 0.075"
quat="0.0 0.0 0.0 1.0"
rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.8361 -12.0 0.075"
type="box"
size="0.5 1.0 0.075"
quat="0.0 0.0 0.0 1.0"
rgba="0.75 0.72 0.55 1.0"/>
<!-- 限高杆 -->
<geom pos="6.2 -9.0 0.155"
type="cylinder"
size="0.025 0.155"
quat="1.0 0.0 0.0 0.0"
rgba="0.8 0.1 0.1 1.0" />
<geom pos="5.2 -9.0 0.155"
type="cylinder"
size="0.025 0.155"
quat="1.0 0.0 0.0 0.0"
rgba="0.8 0.1 0.1 1.0" />
<geom pos="5.7 -9.0 0.325"
type="cylinder"
size="0.015 0.5"
quat="0.7071068 0.0 0.7071068 0.0"
rgba="1.0 0.9 0.4 1.0"/>
<!-- 1m × 1m 正方形颜色块，出发区-->
<geom pos="3.7 -9.0 0.0"
type="box"
size="0.5 0.5 0.001"
rgba="1.0 0.0 0.0 0.35"
contype="0"
conaffinity="0" />
<!-- 10cm梯形台阶：T型楼梯，最高平台与 x=5.7 y=-3.5 平台在y轴方向对齐 -->
<geom pos="1.80 -4.75 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0" />
<geom pos="1.80 -4.45 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="1.80 -4.15 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<!-- 最高平台：y = -3.50，与目标平台y轴对齐 -->
<geom pos="1.80 -3.50 0.35" type="box" size="0.5 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="1.80 -2.85 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="1.80 -2.55 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="1.80 -2.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
<!-- 顶部平台向 +x 方向连接地面的10cm台阶，同样y轴移动到 -3.50 -->
<geom pos="2.45 -3.50 0.25" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="2.75 -3.50 0.15" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="3.05 -3.50 0.05" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<!-- 斜坡木桥A木桥B -->
<geom pos="1.8 -0.88 0.1" type="box" size="0.40 0.5 0.005" quat="0.701836 0.086175 -0.086175 0.701836" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="1.8 0.0 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="2.65 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="3.2 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="3.75 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="4.3 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="4.85 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.7 -0.5 0.10" type="box" size="0.5 1.0 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="4.8002 -1.0 0.0951"
type="box"
size="0.4133 0.5 0.005"
quat="0.992546 0.0 -0.121869 0.0"
rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.35 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.65 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.95 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<geom pos="5.7 -3.5 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
<!-- 10度斜坡：宽4m，斜坡 y正方向边缘 与平台 y正方向边缘 对齐 -->
<geom pos="4.6338 -5.0 0.0951"
type="box"
size="0.5759 2.0 0.005"
quat="0.9961947 0.0 -0.0871557 0.0"
rgba="0.75 0.72 0.55 1.0"/>
<!-- 新建10度斜坡：宽3m，高端与前一个10度斜坡高端衔接，向+x方向下坡 -->
<geom pos="5.7681 -5.5 0.0951"
type="box"
size="0.5759 1.5 0.005"
quat="0.9961947 0.0 0.0871557 0.0"
rgba="0.75 0.72 0.55 1.0"/>
<!--绕杆-->
<!-- 直径1m圆形颜色块，仅显示，不碰撞 -->
<geom pos="1.8 -10.1 0.0"
type="cylinder"
size="0.1 0.001"
rgba="1.0 0.0 0.0 0.35"
contype="0"
conaffinity="0" />
<geom pos="3.2 -12.5 0.0"
type="cylinder"
size="0.1 0.001"
rgba="1.0 0.0 0.0 0.35"
contype="0"
conaffinity="0" />
<geom pos="1.55 -12.75 0.0"
type="cylinder"
size="0.1 0.001"
rgba="1.0 0.0 0.0 0.35"
contype="0"
conaffinity="0" />
<!-- 原杆 -->
<geom pos="1.8 -10.5 0.02"
type="cylinder"
size="0.05 0.02"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<geom pos="1.8 -10.5 0.37"
type="cylinder"
size="0.015 0.33"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<!-- y轴负方向第1根：间隔1m -->
<geom pos="1.8 -11.5 0.02"
type="cylinder"
size="0.05 0.02"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<geom pos="1.8 -11.5 0.37"
type="cylinder"
size="0.015 0.33"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<!-- y轴负方向第2根：继续间隔1m -->
<geom pos="1.8 -12.5 0.02"
type="cylinder"
size="0.05 0.02"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<geom pos="1.8 -12.5 0.37"
type="cylinder"
size="0.015 0.33"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<!-- x轴正方向第3根：继续间隔1m -->
<geom pos="2.8 -12.5 0.02"
type="cylinder"
size="0.05 0.02"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<geom pos="2.8 -12.5 0.37"
type="cylinder"
size="0.015 0.33"
quat="1.0 0.0 0.0 0.0"
rgba="0.75 0.72 0.55 1.0" />
<!--===================================================================其他障碍=====================================================================================-->
<!-- 5cm台阶 -->
<geom pos="1.0 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="1.3 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="1.6 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="1.9 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="2.2 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="2.5 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="2.8 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="3.45 2.0 0.375" type="box" size="0.5 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="4.1 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="4.4 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="4.7 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="5.0 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="5.3 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="5.6 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<geom pos="5.9 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
<!-- 斜坡 -->
<geom pos="2.0 4.0 0.1" type="box" size="1.5 0.75 0.005" quat="0.9950041652780258 0.0 -0.09983341664682815 0.0" />
<geom pos="1.4 6.0 0.165" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="1.6 6.0 0.275" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="1.8 6.0 0.385" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="2.0 6.0 0.495" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="2.2 6.0 0.605" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="2.4 6.0 0.715" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="2.5999999999999996 6.0 0.825" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="2.8 6.0 0.9349999999999999" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="3.0 6.0 1.045" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
<geom pos="-2.3179973398407565 5.173660321080885 -0.25" type="box" size="0.2568216778785459 0.2608020098770089 0.2619541072037832" quat="0.9930658271270357 -0.05910360995856133 -0.06731065544310916 0.0761334482746007"/>
<geom pos="-2.3179973398407565 5.3620612607436735 -0.25" type="box" size="0.23778028947050467 0.26051225137569556 0.27137457425982286" quat="0.9959802827127009 0.07435995796871331 0.0004862731479656538 -0.04993632582444846"/>
<geom pos="-2.3179973398407565 5.545897059602109 -0.25" type="box" size="0.23841994611378936 0.2717839884518381 0.22585827399286504" quat="0.9983504429181294 -0.004811901627841528 0.03471877670768104 -0.045473566737405234"/>
<geom pos="-2.3179973398407565 5.7436240471772795 -0.25" type="box" size="0.2552179019048769 0.2548992578792955 0.22547735976326444" quat="0.9968270877924189 -0.029673908987198697 0.06777847718526858 -0.029347814214192768"/>
<geom pos="-2.3179973398407565 5.940214647011584 -0.25" type="box" size="0.24313116620329878 0.2372064979204117 0.26079933745117434" quat="0.9952106364954509 0.05088987714749159 -0.07605843245051555 -0.03436748846516202"/>
<geom pos="-2.3179973398407565 6.165430585901471 -0.25" type="box" size="0.24786042386990592 0.2322559052231109 0.2644037606269708" quat="0.9936075351397807 -0.05017393314139304 0.06986162224674641 -0.07311632022760194"/>
<geom pos="-2.3179973398407565 6.315657865031069 -0.25" type="box" size="0.23704265198840277 0.24982080672772003 0.2530694373586838" quat="0.9981716459301547 0.036179123385437884 0.04523497974247521 0.017269420947002005"/>
<geom pos="-2.3179973398407565 6.489372835072359 -0.25" type="box" size="0.2647428927965494 0.2716292502682415 0.23725049444938928" quat="0.9954041516289313 0.019099981466976248 0.07663366510923347 0.05415761257454444"/>
<geom pos="-2.094119617957536 5.194943631570213 -0.25" type="box" size="0.23176840693038148 0.23782936054799508 0.2282032657053922" quat="0.9973366591969123 0.030692664684553082 -0.030015392527106853 0.058962910104125923"/>
<geom pos="-2.094119617957536 5.441090326561234 -0.25" type="box" size="0.2602142310926322 0.27213502289176367 0.2574009440402366" quat="0.9948915480473169 0.023650461407535205 0.08508706956405496 -0.04890453856469255"/>
<geom pos="-2.094119617957536 5.642958951230403 -0.25" type="box" size="0.24800055056479955 0.24050676557282252 0.23522489807277194" quat="0.9912055235117067 0.06188914817453858 -0.09720856678264424 -0.0650525790586179"/>
<geom pos="-2.094119617957536 5.884810142659838 -0.25" type="box" size="0.24637516954806898 0.24364583504893206 0.2682443460752295" quat="0.9964495083946477 -0.07467561549070643 -0.038754439691442995 0.003165924091257204"/>
<geom pos="-2.094119617957536 6.129893093768145 -0.25" type="box" size="0.2378958269703999 0.25045408022075055 0.24760364656411665" quat="0.9946240462872651 -0.03963038800955788 -0.08917648605658504 0.03464091840526007"/>
<geom pos="-2.094119617957536 6.31135792935651 -0.25" type="box" size="0.2612154064649683 0.2252849660353503 0.26933214617336004" quat="0.997742809172301 0.019590451180868298 0.06391648942914147 -0.006339033565912353"/>
<geom pos="-2.094119617957536 6.4905285510291915 -0.25" type="box" size="0.22768983323309258 0.23025468157122184 0.2748062344121069" quat="0.9940843963100783 0.05105793020576152 0.009164090402224958 -0.0954218016127878"/>
<geom pos="-2.094119617957536 6.650993287072798 -0.25" type="box" size="0.25443787917913946 0.24575387105878618 0.22934424641420587" quat="0.9953832160779188 -0.0075142647566912866 0.08305501342257929 0.04751477371219154"/>
<geom pos="-1.8951777534978893 5.178742263102535 -0.25" type="box" size="0.256127371544264 0.24353861032614957 0.2714578153598507" quat="0.996547165679354 -0.008558846106514282 0.009384988866828401 0.08205129318749771"/>
<geom pos="-1.8951777534978893 5.353646654030374 -0.25" type="box" size="0.23858897994497189 0.2426691342857623 0.26961659613969635" quat="0.997689480058865 0.014145078555906384 0.009115651940537827 -0.06582190381793727"/>
<geom pos="-1.8951777534978893 5.575640096199311 -0.25" type="box" size="0.24430646334884096 0.2676093509240798 0.23693271670520474" quat="0.9976019172675967 -0.06794745094616256 0.006187887696065785 -0.011630503849579829"/>
<geom pos="-1.8951777534978893 5.728196634759373 -0.25" type="box" size="0.25668697148716296 0.2743986770827004 0.23696403861156407" quat="0.9946311966964959 -0.0774649076913635 0.05866511796477466 0.03558615698054331"/>
<geom pos="-1.8951777534978893 5.954288148947323 -0.25" type="box" size="0.2748179458028994 0.25407956175122554 0.25142548243710355" quat="0.9927720663552536 0.03885622734718657 0.06180632563910765 0.09525647469886914"/>
<geom pos="-1.8951777534978893 6.198076908897641 -0.25" type="box" size="0.23522918501552104 0.2714895927340788 0.23659922178360912" quat="0.9944408196408429 0.027722532803612636 -0.06883505448003607 -0.07470376618171105"/>
<geom pos="-1.8951777534978893 6.356335079330304 -0.25" type="box" size="0.253243646653926 0.26648639763264487 0.22751627090926196" quat="0.9924345646356405 0.06351743627133029 0.09661714031088077 -0.041283149154730255"/>
<geom pos="-1.8951777534978893 6.6037178705715744 -0.25" type="box" size="0.2664884535468762 0.26472237049442093 0.2545826559482188" quat="0.996208812433608 0.04792667434016892 0.03088022810200046 -0.06570728596343135"/>
<geom pos="-1.7450143557797366 5.181798049538029 -0.25" type="box" size="0.22764974830145798 0.2314500042225232 0.26635647118774647" quat="0.996420562587967 -0.02530598413067966 -0.01712092616039989 -0.07881968983996092"/>
<geom pos="-1.7450143557797366 5.396539066742657 -0.25" type="box" size="0.24515338305934362 0.25502436912192245 0.23509532716059323" quat="0.9974817825235419 0.033698559354477776 -0.06057332552735373 -0.01501242370995589"/>
<geom pos="-1.7450143557797366 5.5477605493550115 -0.25" type="box" size="0.2368415768982088 0.2653984778068547 0.25193186806340717" quat="0.9933816957092091 -0.04426535864216467 0.0892264769079807 0.057201577537394625"/>
<geom pos="-1.7450143557797366 5.764238738853998 -0.25" type="box" size="0.257475541143157 0.25587521442146555 0.2684267956125346" quat="0.9955849335110497 0.004076118120639651 -0.09299366924131539 0.012091439447074191"/>
<geom pos="-1.7450143557797366 5.942956213887727 -0.25" type="box" size="0.23647345784635188 0.22779489919605103 0.2690566454457882" quat="0.9997372307105926 0.020286738832439623 -0.010113822112874779 0.003410038259163467"/>
<geom pos="-1.7450143557797366 6.162981335139796 -0.25" type="box" size="0.2336152227884161 0.23785626414299832 0.26272786991330355" quat="0.9954569549676405 0.08638675499005252 -0.03150236191317218 -0.024705881136540285"/>
<geom pos="-1.7450143557797366 6.344025407207907 -0.25" type="box" size="0.2364006101773331 0.23674709170116234 0.2660167000427503" quat="0.9941020005048972 -0.09350614404282417 -0.05493059192638792 0.0006660998579442658"/>
<geom pos="-1.7450143557797366 6.5791872438688825 -0.25" type="box" size="0.259653062502181 0.26359758888480966 0.27170867851854713" quat="0.9955751902075182 0.06572529509454274 0.04059253213564211 -0.05350207998588654"/>
<geom pos="-1.4950537649717406 5.196975242498044 -0.25" type="box" size="0.24902209995429173 0.24604186796843594 0.26555264385759036" quat="0.9981753741369879 0.027059282981497578 0.02078151255996191 0.049818133312398136"/>
<geom pos="-1.4950537649717406 5.415093649392617 -0.25" type="box" size="0.22561317519118573 0.23246623591498758 0.2516053992906602" quat="0.9961431033663982 -0.019679813255757937 0.07777937635153075 0.035524515199325105"/>
<geom pos="-1.4950537649717406 5.596072881787104 -0.25" type="box" size="0.2545347271371952 0.2527292932516396 0.272364707011277" quat="0.9987990019474599 -0.0468132050914785 0.002764373695484487 -0.01419280718860587"/>
<geom pos="-1.4950537649717406 5.788444207457658 -0.25" type="box" size="0.257823578756761 0.22815201323013437 0.2506904868770564" quat="0.9907800223935689 0.08207700037219429 -0.06679273748567588 -0.08459931119619979"/>
<geom pos="-1.4950537649717406 5.962479724512656 -0.25" type="box" size="0.23447002037921025 0.260091883647859 0.2613547781123637" quat="0.994185747243896 -0.045779429335849345 -0.09537527521663794 -0.020062420196245392"/>
<geom pos="-1.4950537649717406 6.137620949716146 -0.25" type="box" size="0.25502378303486756 0.24137626830945555 0.26521755821448284" quat="0.9954511067004865 0.04827241542514884 0.023378972724717166 -0.07874193109224191"/>
<geom pos="-1.4950537649717406 6.345742058941403 -0.25" type="box" size="0.23763376291279575 0.27259418014745557 0.24184880568666417" quat="0.9979730938550316 -0.0521706328108408 -0.03256129013590577 0.01636127740178204"/>
<geom pos="-1.4950537649717406 6.548966807280673 -0.25" type="box" size="0.23004532593424165 0.24736965888987583 0.22917624237245732" quat="0.993938322382385 -0.041286377558295506 -0.09233464220936707 0.04308549844056845"/>
<geom pos="-1.2872521554407157 5.194072124237239 -0.25" type="box" size="0.2725857334366114 0.23611730648841156 0.25109418723334265" quat="0.9870880287314421 -0.09538067591152324 -0.0888119213223197 -0.09312460914696315"/>
<geom pos="-1.2872521554407157 5.418639818976418 -0.25" type="box" size="0.2326397456179607 0.2609646699674687 0.2717115772948157" quat="0.9928206111485406 -0.07383059897166898 -0.08473752136579614 0.040936892980589765"/>
<geom pos="-1.2872521554407157 5.655974569843163 -0.25" type="box" size="0.22633109461398734 0.25911291311168594 0.23532484499883452" quat="0.9947769304653071 -0.023834507399340135 0.09176246279588879 -0.037820963666801724"/>
<geom pos="-1.2872521554407157 5.8981628648303595 -0.25" type="box" size="0.27278934520816167 0.2559269445001904 0.26076472835929454" quat="0.9936318869346414 0.0561019165840355 0.09234219275279033 0.0319557140415977"/>
<geom pos="-1.2872521554407157 6.064939027275534 -0.25" type="box" size="0.2481756437845515 0.2613413397088905 0.24788858471207542" quat="0.9992310163557966 0.006919915638721734 0.03652449975321466 -0.012468024618680441"/>
<geom pos="-1.2872521554407157 6.288670079370122 -0.25" type="box" size="0.2574127585385611 0.27445220033632356 0.22507618952620437" quat="0.9984337612791753 0.04947883993440395 -0.0010074967830211658 -0.026093173185657285"/>
<geom pos="-1.2872521554407157 6.4987760234638605 -0.25" type="box" size="0.2442462188069663 0.2639082925274208 0.24918893213917415" quat="0.9910480688126146 0.08639971284060838 -0.0903343762232188 -0.04688832899783648"/>
<geom pos="-1.2872521554407157 6.675611985491267 -0.25" type="box" size="0.2507168814441121 0.26699708557208374 0.26588306060638556" quat="0.9968882348111167 0.009466759168649483 -0.06920917552433178 0.03652831489763918"/>
<geom pos="-1.0678149575697586 5.238251070535694 -0.25" type="box" size="0.2331754586513582 0.22873009754409884 0.2593258638743009" quat="0.9962563091989393 0.014506022500180732 0.069646748226122 0.049114887295595266"/>
<geom pos="-1.0678149575697586 5.471732496077581 -0.25" type="box" size="0.2653495693182789 0.26581370557074685 0.2509273010188512" quat="0.9948019811445705 -0.05898118120008214 0.04034485064897968 -0.07254330845220838"/>
<geom pos="-1.0678149575697586 5.691153717662407 -0.25" type="box" size="0.26309077142424103 0.26536949948987093 0.26566703066149744" quat="0.998331608401392 -0.025703385975953945 -0.05139460706256987 0.005650661991725779"/>
<geom pos="-1.0678149575697586 5.938910564562482 -0.25" type="box" size="0.23082606038737274 0.23890770539441533 0.25941695887199245" quat="0.99040745217393 -0.09494180013051996 0.05600240658814475 -0.08332384846283197"/>
<geom pos="-1.0678149575697586 6.13746533389903 -0.25" type="box" size="0.2676431215498913 0.2569308659994288 0.24597694927356487" quat="0.9955894165044442 0.04511524606234168 -0.003741035154326279 -0.08217258042101841"/>
<geom pos="-1.0678149575697586 6.328213254628996 -0.25" type="box" size="0.24188540919954635 0.25556145122229207 0.2605619987765001" quat="0.9929731934387934 -0.06598945456828725 -0.06809928840246085 0.07079629875087447"/>
<geom pos="-1.0678149575697586 6.554891603524267 -0.25" type="box" size="0.23121916641107804 0.25266417867731916 0.25489063309084503" quat="0.9984565221200774 -0.02714563973423854 0.029423903667849492 0.03849573446816443"/>
<geom pos="-1.0678149575697586 6.718482966003368 -0.25" type="box" size="0.27418971733780156 0.2623437838864593 0.23694037285314332" quat="0.9968347573500278 0.015128289848683302 0.06990516881209438 0.03471121949050888"/>
<geom pos="-0.8851136474992356 5.233272057743225 -0.25" type="box" size="0.2587161160845902 0.2542459313914242 0.25268742624288776" quat="0.9956323623605605 0.07455213252171443 -0.05585558926184348 0.006191260373025539"/>
<geom pos="-0.8851136474992356 5.410759300563648 -0.25" type="box" size="0.24019625793101548 0.2509280955260936 0.26698317271101046" quat="0.9960056830073374 0.025487333399660517 0.08516393482003458 -0.008377318140719903"/>
<geom pos="-0.8851136474992356 5.622546965631826 -0.25" type="box" size="0.22527085609750916 0.22924847380626232 0.23073331588883172" quat="0.9982201065088221 0.016292210993739946 0.013620740418279952 0.05572843307423328"/>
<geom pos="-0.8851136474992356 5.856118124892678 -0.25" type="box" size="0.26833255247166926 0.2512767990265972 0.2502231376336179" quat="0.9914910532543448 0.0657880493114159 0.05073139628229043 -0.10021850784978792"/>
<geom pos="-0.8851136474992356 6.010914595891683 -0.25" type="box" size="0.24926441451407527 0.22868800964152894 0.26501630221174116" quat="0.990736605756697 -0.0765632176593298 -0.09876375004658954 0.05314859727296924"/>
<geom pos="-0.8851136474992356 6.1657945570092965 -0.25" type="box" size="0.26823427325895977 0.263134568634285 0.23692064318485426" quat="0.9903439387899988 0.07219524819688558 0.09310840228148876 0.07305856872598579"/>
<geom pos="-0.8851136474992356 6.383066094410735 -0.25" type="box" size="0.23772929143507357 0.2619329708548053 0.23258884134606547" quat="0.9928463525385446 0.07348810370848206 0.09252710448645944 -0.017156742103069993"/>
<geom pos="-0.8851136474992356 6.551482176174038 -0.25" type="box" size="0.25127741834221506 0.25307864976328337 0.234931895271364" quat="0.994062479132312 -0.084918495271965 -0.05565932062022146 -0.03912386445844099"/>
<geom pos="-0.6441452943469552 5.193190826541208 -0.25" type="box" size="0.27016649011158844 0.23778885128164629 0.25859862297032477" quat="0.9957774962333289 0.08893738517887846 -0.021654911548168992 0.006955883744530942"/>
<geom pos="-0.6441452943469552 5.407266358883468 -0.25" type="box" size="0.24982888074520473 0.26972381569065607 0.2275629646713632" quat="0.9942571418988088 0.02444442288698024 0.050336521935401994 -0.09122193010664256"/>
<geom pos="-0.6441452943469552 5.56363948710079 -0.25" type="box" size="0.24725848381417467 0.2326432801330426 0.2476341019968084" quat="0.9935769557551848 -0.05688836927692267 -0.06858315894908604 0.0697488117593134"/>
<geom pos="-0.6441452943469552 5.716461923308758 -0.25" type="box" size="0.226746163574222 0.25188961955216527 0.24650452053954758" quat="0.9981029078160676 0.03471313574303335 -0.0493522060742233 -0.012245136651074443"/>
<geom pos="-0.6441452943469552 5.896430073001983 -0.25" type="box" size="0.25284949298017617 0.23421620066432108 0.2621382648463894" quat="0.9996413412447853 0.009523771195346772 -0.02374945051322109 -0.007902547492127014"/>
<geom pos="-0.6441452943469552 6.110845613879558 -0.25" type="box" size="0.23421403581041014 0.2504556848552827 0.24096555776925194" quat="0.9982371379292613 0.05906199571247059 0.004184466367765714 0.004097238396071534"/>
<geom pos="-0.6441452943469552 6.301410327752269 -0.25" type="box" size="0.2296817145699627 0.2701766916372966 0.22803599234835198" quat="0.9993879202059675 0.00903361239651759 0.0071600522792900426 -0.03302896372607247"/>
<geom pos="-0.6441452943469552 6.470340592771366 -0.25" type="box" size="0.2742166750011587 0.23990439594655139 0.2609404931878803" quat="0.9954980192986956 -0.06477558432537366 -0.03478863652617152 0.059812774691783824"/>
<geom pos="-0.43033316974190594 5.176946043237525 -0.25" type="box" size="0.2511344405622137 0.26383180387822514 0.2729516287367074" quat="0.9939152287364423 -0.054017256979398534 -0.08418383902399898 -0.0461273810376057"/>
<geom pos="-0.43033316974190594 5.409693792302493 -0.25" type="box" size="0.23878438774624536 0.22858057493504688 0.24808981791323623" quat="0.9942721168135948 -0.06322741578652484 -0.032958930763236916 -0.0796175891553822"/>
<geom pos="-0.43033316974190594 5.573593446642387 -0.25" type="box" size="0.2647545698926724 0.25215736713350106 0.25522885339490087" quat="0.9971918839448468 -0.004732654353889047 0.07222000940536273 0.019241071144380294"/>
<geom pos="-0.43033316974190594 5.819531161619798 -0.25" type="box" size="0.2538318010250847 0.23661725487197 0.26323696729639623" quat="0.9929867679849618 0.07588268800385974 -0.046355180482866284 -0.07791208834634974"/>
<geom pos="-0.43033316974190594 6.047888701789067 -0.25" type="box" size="0.25684720236482583 0.25568474221031684 0.24397094296107352" quat="0.9925256546500537 -0.08214715587137877 0.06615741576234473 0.06138294537877935"/>
<geom pos="-0.43033316974190594 6.283676056613722 -0.25" type="box" size="0.2432692787269645 0.2742601402399693 0.2689467974776609" quat="0.9989897410981501 0.0047037010179340685 0.0014590493232283363 -0.04466814919444852"/>
<geom pos="-0.43033316974190594 6.458239009480634 -0.25" type="box" size="0.2474330649412728 0.2551435998627988 0.23002773988805483" quat="0.9982311862818315 0.0506459597703481 0.025989046975463247 -0.01714802993392417"/>
<geom pos="-0.43033316974190594 6.703309377175586 -0.25" type="box" size="0.2301029552870502 0.2556223065028505 0.2349557527965884" quat="0.9948672058648923 -0.034182171957127715 -0.0030330068097560517 -0.09519255582537542"/>
<geom type="hfield" hfield="perlin_hfield" pos="-1.5 4.0 0.0" quat="1.0 0.0 0.0 0.0"/>
<geom type="hfield" hfield="image_hfield" pos="-1.5 2.0 0.0" quat="0.7073882691671998 0.0 0.0 -0.706825181105366"/>
</worldbody>
</mujoco>
05_software/train/rc_mjlab/sim2sim/terrain/unitree_hfield.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 KiB

05_software/train/rc_mjlab/sim2sim/tools/disturbance.py
+121
View File
@@ -0,0 +1,121 @@
import numpy as np
import mujoco
import matplotlib.pyplot as plt
from .math_utils import get_body_velocity
class DisturbanceTester:
def __init__(self, interval=3.0, duration=0.02, force_mag=2000.0):
self.interval = interval
self.duration = duration
self.force_mag = force_mag
self.time_log = []
self.base_vel_log = []
self.vel_log = []
self.force_input_log = []
# Define sensors to plot
self.SENSOR_NAMES_TO_PLOT = {
"FR": ["FR_hip_torque", "FR_thigh_torque", "FR_calf_torque"],
"FL": ["FL_hip_torque", "FL_thigh_torque", "FL_calf_torque"],
"RR": ["RR_hip_torque", "RR_thigh_torque", "RR_calf_torque"],
"RL": ["RL_hip_torque", "RL_thigh_torque", "RL_calf_torque"],
"Wheels": ["FR_wheel_torque", "FL_wheel_torque", "RR_wheel_torque", "RL_wheel_torque"]
}
self.sensor_logs = {name: [] for group in self.SENSOR_NAMES_TO_PLOT.values() for name in group}
def update(self, current_time, m, d, estimated_vel):
cycle_time = current_time % self.interval
is_pushing = cycle_time < self.duration
applied_force = np.zeros(6)
if is_pushing:
applied_force[0] = -self.force_mag
# Apply external force
base_body_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "base_link")
if base_body_id == -1:
base_body_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "trunk")
if base_body_id != -1:
d.xfrc_applied[base_body_id] = applied_force
# Record data
self.time_log.append(current_time)
true_vel_body = get_body_velocity(m, d)
self.base_vel_log.append(true_vel_body)
self.force_input_log.append(applied_force[0])
self.vel_log.append(estimated_vel)
# Record sensor data
for name in self.sensor_logs.keys():
sid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SENSOR, name)
if sid != -1:
adr = m.sensor_adr[sid]
val = d.sensordata[adr]
self.sensor_logs[name].append(val)
else:
self.sensor_logs[name].append(0.0)
def plot_results(self):
print("Generating diagnostic plots...")
if len(self.time_log) == 0:
print("No data recorded. Skipping plot.")
return
time_arr = np.array(self.time_log)
vel_arr = np.array(self.base_vel_log)
force_arr = np.array(self.force_input_log)
fig, axes = plt.subplots(4, 2, figsize=(16, 18), sharex=True)
# 1. External Force
axes[0, 0].plot(time_arr, force_arr, 'r-', linewidth=1.5)
axes[0, 0].set_title("External Push Force (N)")
axes[0, 0].set_ylabel("Force")
axes[0, 0].grid(True)
# 2. Velocity
axes[0, 1].plot(time_arr, vel_arr[:, 0], label='True Vx')
axes[0, 1].plot(time_arr, vel_arr[:, 1], label='True Vy')
est_vel_arr = np.array(self.vel_log)
if est_vel_arr.shape[1] >= 2:
axes[0, 1].plot(time_arr, est_vel_arr[:, 0], '--', label='Est Vx')
axes[0, 1].plot(time_arr, est_vel_arr[:, 1], '--', label='Est Vy')
axes[0, 1].set_title("Base Velocity (m/s)")
axes[0, 1].legend()
axes[0, 1].grid(True)
# 3. Legs
plot_config = [
("FR", axes[1, 1]),
("FL", axes[1, 0]),
("RR", axes[2, 1]),
("RL", axes[2, 0])
]
for group_name, ax in plot_config:
sensor_names = self.SENSOR_NAMES_TO_PLOT[group_name]
labels = ["Hip", "Thigh", "Calf"]
for i, s_name in enumerate(sensor_names):
if s_name in self.sensor_logs:
data = self.sensor_logs[s_name]
ax.plot(time_arr, data, label=labels[i], linewidth=1)
ax.set_title(f"{group_name} Leg Torques")
ax.set_ylabel("Torque (Nm)")
ax.legend(loc='upper right')
ax.grid(True, alpha=0.3)
# 4. Wheels
ax_wheel = axes[3, 0]
wheel_sensors = self.SENSOR_NAMES_TO_PLOT["Wheels"]
for w_name in wheel_sensors:
if w_name in self.sensor_logs:
data = self.sensor_logs[w_name]
short_label = w_name.replace("_wheel_torque", "")
ax_wheel.plot(time_arr, data, label=short_label, linewidth=1)
ax_wheel.set_title("Wheel Torques")
ax_wheel.set_ylabel("Torque (Nm)")
ax_wheel.set_xlabel("Time (s)")
ax_wheel.legend()
ax_wheel.grid(True, alpha=0.3)
axes[3, 1].axis('off')
plt.tight_layout()
plt.show()
05_software/train/rc_mjlab/sim2sim/tools/logger.py
+26
View File
@@ -0,0 +1,26 @@
import time
import numpy as np
from pathlib import Path
from tools.math_utils import get_body_velocity
class SimpleLogger:
def __init__(self, log_dir="."):
self.log_file = Path(log_dir) / f"sim2sim_log_{int(time.time())}.txt"
self.data = []
print(f"[Logger] Will write diagnostic data to: {self.log_file}")
def update(self, sim_time, m, d, command):
true_vel_body = get_body_velocity(m, d)
# 记录 仿真时间、控制指令、真实的机体线速度
self.data.append(
f"{sim_time:.4f}, {command[0]:.4f}, {command[1]:.4f}, {command[2]:.4f}, "
f"{true_vel_body[0]:.4f}, {true_vel_body[1]:.4f}, {true_vel_body[2]:.4f}"
)
def save(self):
print(f"\n[Logger] Saving {len(self.data)} records to {self.log_file}...")
with open(self.log_file, "w") as f:
f.write("time, cmd_vx, cmd_vy, cmd_yaw, true_vx, true_vy, true_vz\n")
for row in self.data:
f.write(row + "\n")
print("[Logger] Save complete.")
05_software/train/rc_mjlab/sim2sim/tools/math_utils.py
+35
View File
@@ -0,0 +1,35 @@
import numpy as np
def get_gravity_orientation(quat_wxyz):
"""Compute projected gravity in body frame from quaternion [w,x,y,z]."""
qw, qx, qy, qz = quat_wxyz
gx = 2.0 * (-qz * qx + qw * qy)
gy = -2.0 * (qz * qy + qw * qx)
gz = 1.0 - 2.0 * (qw * qw + qz * qz)
return np.array([gx, gy, gz], dtype=np.float32)
def quat_rotate_inverse(quat_wxyz, v):
"""Rotate vector v from world frame to body frame."""
q_w = quat_wxyz[0]
q_vec = quat_wxyz[1:]
a = v * (2.0 * q_w * q_w - 1.0)
b = np.cross(q_vec, v) * q_w * 2.0
c = q_vec * np.dot(q_vec, v) * 2.0
return a - b + c
def get_body_velocity(m, d):
"""Calculate body frame linear velocity (vx, vy, vz) from world velocity."""
v_world = d.qvel[0:3]
q = d.qpos[3:7] # MuJoCo freejoint quaternion is at indices 3:7 (x,y,z, w,x,y,z)
w, x, y, z = q
norm = np.sqrt(w*w + x*x + y*y + z*z)
if norm < 1e-6:
return np.array([0.0, 0.0, 0.0], dtype=np.float32)
w, x, y, z = w/norm, x/norm, y/norm, z/norm
R = np.array([
[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
[2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
[2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
])
v_body = R.T @ v_world
return v_body.astype(np.float32)