具身智能机器人实战教程——模块5（1）

当你按下 WASD 键时，你控制的并不是那 20 多个关节的绝对角度，而是在向一个 AI 模型（策略网络）下达高层业务目标。这就像老板给员工下达指令：你只需要告诉它“往前走”或“向左转”，至于如何协调全身 20 多个关节来维持平衡并完成动作，全部交由策略网络（如 MNN 策略网络）去拆解和执行。同时，策略网络还会实时读取机器人的当前状态，据此决定下一步该怎么走。

人形机器人之所以在近十年才迎来爆发，而不是 10 年或 20 年前，根本原因就在于强化学习、深度学习框架以及软硬件生态终于发展到了成熟阶段。如今的 MNN 网络训练效果已经非常接近真人，动作表现极具观赏性。

1.2运动控制二次开发的技术链路

在代码层面，与机器人控制相关的核心逻辑其实非常简单，本质上就是策略网络的一次 infer（推理）操作。理解这条链路，是进行二次开发的关键。

核心数据流：OBS → Input Tensor → Inferred Action

策略网络的输入是 OBS（Observation，观测到的当前状态），输出是 inferred_action（推理出的动作）。其中，接收推理的参数被称为 input tensor，它主要由三部分拼接而成：

历史数据：机器人过去的状态记录。
上一帧的动作：保证动作的连贯性。
Command（指令）：开发者下发的高层控制意图。

Command 数组参数解析

command 数组是我们进行 WASD 控制的核心抓手，它包含三个关键参数：

参数索引	控制维度	说明
第 1 个参数	前后移动	控制机器人前进或后退的速度/意图
第 2 个参数	左右移动	控制机器人的横向平移
第 3 个参数	原地旋转	控制机器人的偏航角速度（Yaw）

图中红框的内容是command参数，因为我们代码初始设置了1，这也是为什么机器人一开始就能向前行走的原因。

1.3实践案例

提示语：参考16_T800_log.py这个代码编写一个脚本叫18_T800_wasd.py，支持用户使用wsad这几个按钮去控制机器人前后左右移动。

"""
18_T800_wasd.py (教学版: WASD 键盘控制机器人前后左右移动)
基于 16_T800_mnn_logs.py 的可运行 MNN 推理 + PD 控制框架,
将固定的速度指令替换为键盘实时控制的 [vx, vy, vyaw]。
控制方案 (按住才走, 松开渐停, 更直觉):
按住 W : 前进 (vx 渐变到 +1.0 m/s)
按住 S : 后退 (vx 渐变到 -1.0 m/s)
按住 A : 左转 (vyaw 渐变到 +1.5 rad/s)
按住 D : 右转 (vyaw 渐变到 -1.5 rad/s)
空格  : 急停 (全部归零)
Q     : 退出 (关闭 viewer)
实现要点:
用 Windows 全局键状态 API GetAsyncKeyState (ctypes 调用)
关键: GetAsyncKeyState 检测的是"物理按键当前是否按下",
与哪个窗口有焦点无关! 所以 MuJoCo viewer 抢焦点也能收到按键
(之前用 msvcrt 只能在控制台有焦点时收到, viewer 一开就失效)
不需要独立线程, 直接在主循环 100Hz 轮询即可
速度用渐变(ramp): 按住时朝目标值逼近, 松开时朝 0 衰减,
避免指令突变让策略失稳
obs / PD / action_scale 等参数完全对齐官方部署 (同 16 脚本)
运行: D:/miniconda3/envs/t800/python.exe 18_T800_wasd.py
"""
import math
import time
import ctypes
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
====== 1. 常量 (与 16 一致, 对齐官方部署参数) ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
HISTORY = 15
ACTION_SIZE = 22
DECIMATION = 5        # 策略 100Hz
LOG_INTERVAL = 0.1
CMD_SCALE = np.array([2.0, 2.0, 1.0])  # obs 末尾指令缩放 [vx2, vy2, vyaw*1]
ACTION_TO_CTRL = list(range(0, 12)) + list(range(13, 23))
DEFAULT_QPOS = np.zeros(25)
DEFAULT_QPOS[0:12] = [-0.06, 0.0, 0.0, 0.12, -0.06, 0.0,
-0.06, 0.0, 0.0, 0.12, -0.06, 0.0]
DEFAULT_QPOS[13:18] = [0.0, 0.15, 0.0, -0.25, 0.0]
DEFAULT_QPOS[18:23] = [0.0, -0.15, 0.0, -0.25, 0.0]
KP = np.zeros(25); KD = np.zeros(25)
KP[0:12] = [180, 100, 100, 180, 40, 40, 180, 100, 100, 180, 40, 40]
KD[0:12] = [5.0, 3.0, 3.0, 5.0, 0.3, 0.3, 5.0, 3.0, 3.0, 5.0, 0.3, 0.3]
KP[12] = 100;  KD[12] = 5.0
KP[13:18] = [60, 50, 50, 60, 50];  KD[13:18] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[18:23] = [60, 50, 50, 60, 50];  KD[18:23] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[23:25] = [100, 100];            KD[23:25] = [1.0, 1.0]
ACTION_SCALE = np.concatenate([
np.tile([0.5, 0.2, 0.2, 0.5, 0.5, 0.2], 2),
np.tile([0.2, 0.2, 0.05, 0.2, 0.05], 2),
])
OBS_SCALE = np.concatenate([
np.full(22, 1.0), np.full(22, 0.05), np.full(22, 1.0),
np.full(3, 1.0), np.full(3, 1.0),
]).astype(np.float32)
====== 2. 全局键盘检测 (GetAsyncKeyState, 不依赖窗口焦点) ======
Windows 虚拟键码
VK_W = 0x57; VK_A = 0x41; VK_S = 0x53; VK_D = 0x44
VK_SPACE = 0x20; VK_Q = 0x51
_user32 = ctypes.windll.user32
def key_down(vk):
"""检测某物理键当前是否被按下 (不管哪个窗口有焦点)"""
# 返回值的最高位(0x8000)为 1 表示该键当前按下
return _user32.GetAsyncKeyState(vk) & 0x8000 != 0
速度指令 [vx, vy, vyaw]; 用渐变方式更新, 避免突变
cmd = np.array([0.0, 0.0, 0.0])
running = True
VX_TARGET = 1.0       # 按住 W/S 时 vx 的目标值 (m/s)
VYAW_TARGET = 1.5     # 按住 A/D 时 vyaw 的目标值 (rad/s)
RAMP = 2.0            # 速度渐变率 (单位/秒): 约 0.5s 到达目标
DECAY = 3.0           # 松开时衰减率 (比 ramp 快, 快速停下)
def update_cmd(dt):
"""根据按键状态更新速度指令 (渐变式)"""
global running
# 退出
if key_down(VK_Q):
running = False
return
# 急停: 立即归零
if key_down(VK_SPACE):
    cmd[:] = 0.0
    return

# vx: W 前进 / S 后退 (互斥时取后者, 实际不会同时按)
if key_down(VK_W):
    cmd[0] += RAMP * dt
    if cmd[0] &gt; VX_TARGET: cmd[0] = VX_TARGET
elif key_down(VK_S):
    cmd[0] -= RAMP * dt
    if cmd[0] &lt; -VX_TARGET: cmd[0] = -VX_TARGET
else:
    # 松开: 朝 0 衰减
    if cmd[0] &gt; 0:
        cmd[0] = max(0.0, cmd[0] - DECAY * dt)
    else:
        cmd[0] = min(0.0, cmd[0] + DECAY * dt)

# vyaw: A 左转 / D 右转
if key_down(VK_A):
    cmd[2] += RAMP * dt
    if cmd[2] &gt; VYAW_TARGET: cmd[2] = VYAW_TARGET
elif key_down(VK_D):
    cmd[2] -= RAMP * dt
    if cmd[2] &lt; -VYAW_TARGET: cmd[2] = -VYAW_TARGET
else:
    if cmd[2] &gt; 0:
        cmd[2] = max(0.0, cmd[2] - DECAY * dt)
    else:
        cmd[2] = min(0.0, cmd[2] + DECAY * dt)
====== 3. 加载模型, 设初始姿态 ======
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
BASE_ID = model.body("LINK_BASE").id
nu = model.nu
qposadr = np.array([model.jnt_qposadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
dofadr = np.array([model.jnt_dofadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
初始高度 1.0m (17_T800_scan.py 验证为稳定站立临界点)
data.qpos[0:3] = [0.0, 0.0, 1.0]
data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
data.qpos[7:] = DEFAULT_QPOS
mujoco.mj_forward(model, data)
CTRL_IDX = np.array(ACTION_TO_CTRL)
qa22 = qposadr[CTRL_IDX]
da22 = dofadr[CTRL_IDX]
default22 = DEFAULT_QPOS[CTRL_IDX]
def quat2rpy(quat):
w, x, y, z = quat
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
sinp = max(-1.0, min(1.0, 2 * (w * y - z * x)))
pitch = math.asin(sinp)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
return roll, pitch, yaw
====== 4. 加载 MNN 策略 ======
import MNN  # type: ignore
net = MNN.Interpreter(str(POLICY_FILE))
session = net.createSession()
input_tensor = net.getSessionInput(session)
output_tensor = net.getSessionOutput(session)
def infer(obs):
obs = np.asarray(obs, dtype=np.float32).reshape(1, -1)
host_in = MNN.Tensor(
input_tensor.getShape(), MNN.Halide_Type_Float,
obs, MNN.Tensor_DimensionType_Caffe)
input_tensor.copyFrom(host_in)
net.runSession(session)
out_shape = output_tensor.getShape()
host_out = MNN.Tensor(
out_shape, MNN.Halide_Type_Float,
np.zeros(out_shape, dtype=np.float32),
MNN.Tensor_DimensionType_Caffe)
output_tensor.copyToHostTensor(host_out)
return np.asarray(host_out.getData(), dtype=np.float32).reshape(-1)
====== 5. obs 单帧构造 ======
def make_frame(last_action):
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]
proj_grav = R.T @ np.array([0.0, 0.0, -1.0])
frame = np.concatenate([
data.qpos[qa22] - default22,
data.qvel[da22],
last_action,
ang_vel_body,
proj_grav,
]).astype(np.float32)
return frame * OBS_SCALE
action = np.zeros(ACTION_SIZE, dtype=np.float32)
history = np.tile(make_frame(action), (HISTORY, 1))
target = DEFAULT_QPOS.copy()
====== 6. 日志 (每 100ms) ======
def log_pose():
pos = data.xpos[BASE_ID]
roll, pitch, yaw = quat2rpy(data.xquat[BASE_ID])
print(f"[POSE] t={data.time:5.2f}s | "
f"pos=({pos[0]:+6.3f},{pos[1]:+6.3f},{pos[2]:+6.3f})m | "
f"rpy=({math.degrees(roll):+7.2f},{math.degrees(pitch):+7.2f},"
f"{math.degrees(yaw):+7.2f})deg")
def log_cmd():
print(f"[CMD ] t={data.time:5.2f}s | "
f"vx={cmd[0]:+5.2f}m/s vy={cmd[1]:+5.2f}m/s vyaw={cmd[2]:+5.2f}rad/s")
def log_mnn(infer_ms):
print(f"[MNN ] t={data.time:5.2f}s | infer={infer_ms:5.2f}ms | "
f"action min={action.min():+6.3f} max={action.max():+6.3f}")
def log_pd(tau):
q = data.qpos[qposadr]
err = np.abs(q - target)
print(f"[PD  ] t={data.time:5.2f}s | tau_max={np.abs(tau).max():6.1f}Nm | "
f"track_err max={np.degrees(err.max()):5.2f}deg")
print("-" * 70)
====== 7. 主循环 ======
print("=" * 60)
print("WASD 键盘控制 (按住才走, 松开渐停)")
print("  按住 W/S: 前进/后退  (vx 渐变到 ±1.0 m/s)")
print("  按住 A/D: 左转/右转  (vyaw 渐变到 ±1.5 rad/s)")
print("  空格: 急停归零    Q: 退出")
print("  (按键检测用全局 API, viewer 窗口有焦点也能收到)")
print("=" * 60)
print(f"policy 100Hz, 初始 vx=0, 站立高度 1.0m")
print("-" * 70)
next_log = 0.0
step_count = 0
last_infer_ms = 0.0
last_t = 0.0
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and running:
# --- 7.0 每步更新键盘指令 (渐变) ---
dt = data.time - last_t
last_t = data.time
update_cmd(dt)
    # --- 7.1 每 100Hz 策略推理 ---
    if step_count % DECIMATION == 0:
        obs = np.concatenate([
            history.reshape(-1),
            (cmd * CMD_SCALE).astype(np.float32),  # 末尾 3 维指令
        ])

        t0 = time.perf_counter()
        action = infer(obs)
        last_infer_ms = (time.perf_counter() - t0) * 1000.0

        target = DEFAULT_QPOS.copy()
        target[ACTION_TO_CTRL] += (action * ACTION_SCALE).astype(np.float64)

        history[:-1] = history[1:]
        history[-1] = make_frame(action)

    # --- 7.2 每仿真步 PD 控制 ---
    q = data.qpos[qposadr]
    dq = data.qvel[dofadr]
    tau = KP * (target - q) - KD * dq
    data.ctrl[:] = tau

    mujoco.mj_step(model, data)
    step_count += 1

    # --- 7.3 每 100ms 日志 ---
    if data.time &gt;= next_log:
        log_pose()
        log_cmd()
        log_mnn(last_infer_ms)
        log_pd(tau)
        next_log += LOG_INTERVAL

    viewer.sync()
running = False
print("仿真结束")

第二章：构建木块并让T800踢着木块走

很多初学者觉得"让机器人踢着木块往前走"没什么技术含量：不就是往前走吗？把第 16 集那个键盘 command 设成正向不就行了？

但这一章真正想交付的，不是"会走"这个动作本身，而是整个具身智能开发的关键 loop（闭环）：

感知(Perception) → 规划(Planning) → 执行/控制(Control)

感知：用传感器采集周围物体的信息（木块在哪、离我多远）。
规划：决定"我应该怎么走、用什么策略"，这一步可以结合 T800 的"大小脑"来完成。
控制：拿到规划出的指令后，驱动机器人各个关节的力矩，输出对应的动作。

“踢木块"恰好把这三步全用上了：机器人要先"看见"木块、再算出"该朝哪个方向走”、最后"迈腿去踢"。麻雀虽小，五脏俱全，这正是它被选作入门综合案例的原因。

2.1创建木块

提示语：结合18_T800_wsad.py代码在当前的mujoco环境中给我创建一个木块，白色的正方体，长宽高都是8厘米。注意，不要修改t800.xml文件，不污染官方的源代码。动态的吧木块加入到仿真环境，写一个新的脚本叫19_T800_ctrl_box.py

"""
19_T800_ctrl_box.py (教学版: WASD 控制 + 动态加入白色方块)
基于 18_T800_wasd.py, 额外用临时 wrapper XML 动态注入一个 8cm 白色方块,
不修改 t800.xml 官方源文件。
方块定义:
名称 white_box, 几何 box 8x8x8 cm (size 是半尺寸 = 0.04m)
白色 rgba=(1,1,1,1), 质量 0.5kg, freejoint 可被推动
初始位置 (0.5, 0, 0.04), 在机器人正前方
动态注入方法 (关键技巧):
t800.xml 是完整 <mujoco> 文档, 不能直接修改
创建一个临时 wrapper XML: <include file="t800.xml"/> + 新 <worldbody> 方块
用 from_xml_path 加载 (会自动合并 include 的 worldbody)
加载完成后立即删除临时文件 (模型已在内存中, 不依赖文件)
这样既不污染源码, 也不留垃圾文件
运行: D:/miniconda3/envs/t800/python.exe 19_T800_ctrl_box.py
"""
import math
import time
import ctypes
import tempfile
import os
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
====== 1. 常量 (与 18 一致, 对齐官方部署参数) ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
HISTORY = 15
ACTION_SIZE = 22
DECIMATION = 5
LOG_INTERVAL = 0.1
CMD_SCALE = np.array([2.0, 2.0, 1.0])
ACTION_TO_CTRL = list(range(0, 12)) + list(range(13, 23))
DEFAULT_QPOS = np.zeros(25)
DEFAULT_QPOS[0:12] = [-0.06, 0.0, 0.0, 0.12, -0.06, 0.0,
-0.06, 0.0, 0.0, 0.12, -0.06, 0.0]
DEFAULT_QPOS[13:18] = [0.0, 0.15, 0.0, -0.25, 0.0]
DEFAULT_QPOS[18:23] = [0.0, -0.15, 0.0, -0.25, 0.0]
KP = np.zeros(25); KD = np.zeros(25)
KP[0:12] = [180, 100, 100, 180, 40, 40, 180, 100, 100, 180, 40, 40]
KD[0:12] = [5.0, 3.0, 3.0, 5.0, 0.3, 0.3, 5.0, 3.0, 3.0, 5.0, 0.3, 0.3]
KP[12] = 100;  KD[12] = 5.0
KP[13:18] = [60, 50, 50, 60, 50];  KD[13:18] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[18:23] = [60, 50, 50, 60, 50];  KD[18:23] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[23:25] = [100, 100];            KD[23:25] = [1.0, 1.0]
ACTION_SCALE = np.concatenate([
np.tile([0.5, 0.2, 0.2, 0.5, 0.5, 0.2], 2),
np.tile([0.2, 0.2, 0.05, 0.2, 0.05], 2),
])
OBS_SCALE = np.concatenate([
np.full(22, 1.0), np.full(22, 0.05), np.full(22, 1.0),
np.full(3, 1.0), np.full(3, 1.0),
]).astype(np.float32)
====== 2. 键盘控制 (同 18, GetAsyncKeyState 全局检测) ======
VK_W = 0x57; VK_A = 0x41; VK_S = 0x53; VK_D = 0x44
VK_SPACE = 0x20; VK_Q = 0x51
_user32 = ctypes.windll.user32
def key_down(vk):
return _user32.GetAsyncKeyState(vk) & 0x8000 != 0
cmd = np.array([0.0, 0.0, 0.0])
running = True
VX_TARGET = 1.0
VYAW_TARGET = 1.5
RAMP = 2.0
DECAY = 3.0
def update_cmd(dt):
global running
if key_down(VK_Q):
running = False; return
if key_down(VK_SPACE):
cmd[:] = 0.0; return
# vx
if key_down(VK_W):
cmd[0] = min(cmd[0] + RAMP * dt, VX_TARGET)
elif key_down(VK_S):
cmd[0] = max(cmd[0] - RAMP * dt, -VX_TARGET)
else:
cmd[0] = cmd[0] - DECAY * dt if cmd[0] > 0 else cmd[0] + DECAY * dt
cmd[0] = max(0.0, cmd[0]) if cmd[0] > 0 else min(0.0, cmd[0])
# vyaw
if key_down(VK_A):
cmd[2] = min(cmd[2] + RAMP * dt, VYAW_TARGET)
elif key_down(VK_D):
cmd[2] = max(cmd[2] - RAMP * dt, -VYAW_TARGET)
else:
cmd[2] = cmd[2] - DECAY * dt if cmd[2] > 0 else cmd[2] + DECAY * dt
cmd[2] = max(0.0, cmd[2]) if cmd[2] > 0 else min(0.0, cmd[2])
====== 3. 动态加载模型 (t800.xml + 白色方块, 不污染源码) ======
def load_model_with_box():
"""用临时 wrapper XML 动态注入方块, 加载后删除临时文件"""
wrapper = '''<mujoco>
<include file="t800.xml"/>
<worldbody>
<body name="white_box" pos="0.5 0 0.04">
<freejoint/>
<geom name="box_geom" type="box" size="0.04 0.04 0.04"
rgba="1 1 1 1" mass="0.5"/>
</body>
</worldbody>
</mujoco>'''
# 临时文件放 code2/ 目录, 让 include "t800.xml" 能按相对路径找到
with tempfile.NamedTemporaryFile('w', suffix='.xml',
delete=False, dir='.') as f:
f.write(wrapper)
tmp_path = f.name
try:
model = mujoco.MjModel.from_xml_path(tmp_path)
finally:
os.remove(tmp_path)   # 模型已在内存, 删除临时文件
return model
model = load_model_with_box()
data = mujoco.MjData(model)
BASE_ID = model.body("LINK_BASE").id
BOX_ID = model.body("white_box").id
nu = model.nu
qposadr = np.array([model.jnt_qposadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
dofadr = np.array([model.jnt_dofadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
data.qpos[0:3] = [0.0, 0.0, 1.0]
data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
data.qpos[7:7+25] = DEFAULT_QPOS
mujoco.mj_forward(model, data)
CTRL_IDX = np.array(ACTION_TO_CTRL)
qa22 = qposadr[CTRL_IDX]
da22 = dofadr[CTRL_IDX]
default22 = DEFAULT_QPOS[CTRL_IDX]
def quat2rpy(quat):
w, x, y, z = quat
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
sinp = max(-1.0, min(1.0, 2 * (w * y - z * x)))
pitch = math.asin(sinp)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
return roll, pitch, yaw
====== 4. MNN 策略 ======
import MNN  # type: ignore
net = MNN.Interpreter(str(POLICY_FILE))
session = net.createSession()
input_tensor = net.getSessionInput(session)
output_tensor = net.getSessionOutput(session)
def infer(obs):
obs = np.asarray(obs, dtype=np.float32).reshape(1, -1)
host_in = MNN.Tensor(
input_tensor.getShape(), MNN.Halide_Type_Float,
obs, MNN.Tensor_DimensionType_Caffe)
input_tensor.copyFrom(host_in)
net.runSession(session)
out_shape = output_tensor.getShape()
host_out = MNN.Tensor(
out_shape, MNN.Halide_Type_Float,
np.zeros(out_shape, dtype=np.float32),
MNN.Tensor_DimensionType_Caffe)
output_tensor.copyToHostTensor(host_out)
return np.asarray(host_out.getData(), dtype=np.float32).reshape(-1)
def make_frame(last_action):
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]
proj_grav = R.T @ np.array([0.0, 0.0, -1.0])
frame = np.concatenate([
data.qpos[qa22] - default22,
data.qvel[da22],
last_action,
ang_vel_body,
proj_grav,
]).astype(np.float32)
return frame * OBS_SCALE
action = np.zeros(ACTION_SIZE, dtype=np.float32)
history = np.tile(make_frame(action), (HISTORY, 1))
target = DEFAULT_QPOS.copy()
====== 5. 日志 (每 100ms, 含方块位置) ======
def log_pose():
pos = data.xpos[BASE_ID]
roll, pitch, yaw = quat2rpy(data.xquat[BASE_ID])
box_pos = data.xpos[BOX_ID]
print(f"[POSE] t={data.time:5.2f}s | "
f"robot=({pos[0]:+6.3f},{pos[1]:+6.3f},{pos[2]:+6.3f})m | "
f"rpy=({math.degrees(roll):+6.2f},{math.degrees(pitch):+6.2f},"
f"{math.degrees(yaw):+6.2f})deg")
print(f"[BOX ] t={data.time:5.2f}s | "
f"box=({box_pos[0]:+6.3f},{box_pos[1]:+6.3f},{box_pos[2]:+6.3f})m")
def log_cmd():
print(f"[CMD ] t={data.time:5.2f}s | "
f"vx={cmd[0]:+5.2f}m/s vy={cmd[1]:+5.2f}m/s vyaw={cmd[2]:+5.2f}rad/s")
def log_mnn(infer_ms):
print(f"[MNN ] t={data.time:5.2f}s | infer={infer_ms:5.2f}ms | "
f"action min={action.min():+6.3f} max={action.max():+6.3f}")
def log_pd(tau):
q = data.qpos[qposadr]
err = np.abs(q - target)
print(f"[PD  ] t={data.time:5.2f}s | tau_max={np.abs(tau).max():6.1f}Nm | "
f"track_err max={np.degrees(err.max()):5.2f}deg")
print("-" * 70)
====== 6. 主循环 ======
print("=" * 60)
print("WASD 控制 + 白色方块 (动态注入, 不修改 t800.xml)")
print("  按住 W/S: 前进/后退  A/D: 左转/右转")
print("  空格: 急停    Q: 退出")
print(f"  方块初始位置: (0.5, 0, 0.04)  尺寸: 8x8x8 cm")
print("=" * 60)
next_log = 0.0
step_count = 0
last_infer_ms = 0.0
last_t = 0.0
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and running:
dt = data.time - last_t
last_t = data.time
update_cmd(dt)
    if step_count % DECIMATION == 0:
        obs = np.concatenate([
            history.reshape(-1),
            (cmd * CMD_SCALE).astype(np.float32),
        ])
        t0 = time.perf_counter()
        action = infer(obs)
        last_infer_ms = (time.perf_counter() - t0) * 1000.0
        target = DEFAULT_QPOS.copy()
        target[ACTION_TO_CTRL] += (action * ACTION_SCALE).astype(np.float64)
        history[:-1] = history[1:]
        history[-1] = make_frame(action)

    q = data.qpos[qposadr]
    dq = data.qvel[dofadr]
    tau = KP * (target - q) - KD * dq
    data.ctrl[:] = tau

    mujoco.mj_step(model, data)
    step_count += 1

    if data.time &gt;= next_log:
        log_pose()
        log_cmd()
        log_mnn(last_infer_ms)
        log_pd(tau)
        next_log += LOG_INTERVAL

    viewer.sync()
running = False
print("仿真结束")

这里是木块的配置信息

2.2T800踢着木块走

要让机器人踢木块，第一步是让它"看见"木块。视频里把感知拆成了两个层次的问题：

1. 木块在哪里？（目标识别）

机器人一般用深度相机或 RGB 相机来充当"眼睛"。识别木块位置可以走两条路：

场景	方案	说明
通用物体检测	YOLO / Mask R-CNN	用深度神经网络做目标检测，识别出木块在图像中的位置
简单颜色目标（如红色木块）	OpenCV	不用深度网络，靠颜色阈值分割就能很方便地获取木块

2. 木块离我多远？（深度估计）

光知道"图像里木块在哪个像素"还不够。因为最终我们要控制机器人以 0.4 m/s 的速度前进、左拐、右拐，必须知道木块相对自身的真实距离。这就需要 RGBD（深度）相机：

深度相机能获取环境的深度点云信息；
在深度图里，离相机近的物体颜色偏深，离得远的偏淡；
通过这种方式，就能把"目标位置"和"我与目标之间的距离"对应起来。

有了这个距离信息，机器人才可能做后续的位置控制：判断该向前走、向后走，还是向左/向右拐。

真机提醒：在 MuJoCo 仿真环境里，木块一旦在世界里被创建出来，仿真器拥有"上帝视角"，直接就知道木块的确切坐标，所以本集省略了视觉识别这一步。但如果是真机实操，上面提到的 YOLO、双目相机、RGBD 相机测距以及"手眼标定"，都需要单独去学、去做。

提示语：参考19_T800_ctrl_box.py这个代码帮我写一个代码叫20_T800_HitBox.py，要求机器人踢着木块前进，木块大小设置为0.1m的长宽高。

"""
20_T800_HitBox.py (教学版: 机器人踢着方块前进)
基于 19_T800_ctrl_box.py, 目标: 让机器人持续踢着一个 10cm 方块前进。
与 19 的区别:
方块尺寸: 10x10x10 cm (size=0.05, 半尺寸)
自动跟踪方块: 机器人自动朝方块方向走, 始终保持踢着方块
计算方块相对机器人的方向角 err_yaw = atan2(dy, dx)
自动调整 vyaw 指令让机器人朝向方块
不需要手动按 W, 机器人自动前进踢方块
控制方案:
自动模式 (默认): 机器人自动跟踪并踢方块前进
手动覆盖:
空格: 急停 (退出自动模式)
Q: 退出
R: 切回自动模式
动态注入方块 (同 19): 用临时 wrapper XML, 不修改 t800.xml
运行: D:/miniconda3/envs/t800/python.exe 20_T800_HitBox.py
"""
import math
import time
import ctypes
import tempfile
import os
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
诊断开关: 置 False 则完全不加载木块
WITH_BOX = True
====== 1. 常量 (与 19 一致, 对齐官方部署参数) ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
HISTORY = 15
ACTION_SIZE = 22
DECIMATION = 5
LOG_INTERVAL = 0.1
CMD_SCALE = np.array([2.0, 2.0, 1.0])
ACTION_TO_CTRL = list(range(0, 12)) + list(range(13, 23))
DEFAULT_QPOS = np.zeros(25)
DEFAULT_QPOS[0:12] = [-0.06, 0.0, 0.0, 0.12, -0.06, 0.0,
-0.06, 0.0, 0.0, 0.12, -0.06, 0.0]
DEFAULT_QPOS[13:18] = [0.0, 0.15, 0.0, -0.25, 0.0]
DEFAULT_QPOS[18:23] = [0.0, -0.15, 0.0, -0.25, 0.0]
KP = np.zeros(25); KD = np.zeros(25)
KP[0:12] = [180, 100, 100, 180, 40, 40, 180, 100, 100, 180, 40, 40]
KD[0:12] = [5.0, 3.0, 3.0, 5.0, 0.3, 0.3, 5.0, 3.0, 3.0, 5.0, 0.3, 0.3]
腰部 (idx=12): 策略不控制 (ACTION_TO_CTRL 跳过), 由 PD 锁定在初始角 0.
保持官方 100/5, 不要改硬, 否则干扰策略期望的躯干运动导致摔倒.
KP[12] = 100;  KD[12] = 5.0
KP[13:18] = [60, 50, 50, 60, 50];  KD[13:18] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[18:23] = [60, 50, 50, 60, 50];  KD[18:23] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[23:25] = [100, 100];            KD[23:25] = [1.0, 1.0]
ACTION_SCALE = np.concatenate([
np.tile([0.5, 0.2, 0.2, 0.5, 0.5, 0.2], 2),
np.tile([0.2, 0.2, 0.05, 0.2, 0.05], 2),
])
OBS_SCALE = np.concatenate([
np.full(22, 1.0), np.full(22, 0.05), np.full(22, 1.0),
np.full(3, 1.0), np.full(3, 1.0),
]).astype(np.float32)
====== 2. 键盘控制 ======
VK_SPACE = 0x20; VK_Q = 0x51; VK_R = 0x52
_user32 = ctypes.windll.user32
def key_down(vk):
return _user32.GetAsyncKeyState(vk) & 0x8000 != 0
cmd = np.array([0.0, 0.0, 0.0])
running = True
auto_mode = True             # 自动跟踪方块模式
VX_AUTO = 1.0               # 恒定 1.0 m/s (必须与策略训练速度一致, 减速会导致动作发散摔倒)
YAW_MAX = 0.25              # 偏航修正封顶 (rad/s), 过大易失稳
YAW_P = 0.6                 # 偏航修正比例增益
LOCK_DIST = 0.30            # 贴脚锁定距离 (m): 木块在 0.3m 内则不转, 踢正前方
FALL_PITCH = 30.0
FALL_RESET_TIME = 1.5       # 摔倒后 1.5s 未恢复则 teleport
fall_time = None
====== 3. 动态加载模型 (t800.xml + 10cm 方块) ======
def load_model_with_box():
"""物理木块 10x10x10 cm (size=[0.05 0.05 0.05]), 初始在机器人正前方地面上
木块为真实物理物体 (与地面、机器人正常碰撞). 机器人走向木块, 前摆腿/小腿
把它踢/推向前, 木块被踢开后在前面滚动, 机器人继续追上再踢 — 实现"踢着木块前进".
"""
if not WITH_BOX:
    return mujoco.MjModel.from_xml_path("t800.xml")
wrapper = '''&lt;mujoco&gt;
<include file="t800.xml"/>
<worldbody>
<body name="hit_box" pos="0.45 0 0.05">
<freejoint/>
<geom name="box_geom" type="box" size="0.05 0.05 0.05"
rgba="1 1 1 1" mass="0.03" friction="0.2 0.05 0.01"/>
</body>
</worldbody>
</mujoco>'''
with tempfile.NamedTemporaryFile('w', suffix='.xml',
delete=False, dir='.') as f:
f.write(wrapper)
tmp_path = f.name
try:
model = mujoco.MjModel.from_xml_path(tmp_path)
finally:
os.remove(tmp_path)
return model
model = load_model_with_box()
data = mujoco.MjData(model)
BASE_ID = model.body("LINK_BASE").id
BOX_ID = model.body("hit_box").id if WITH_BOX else 0
nu = model.nu
qposadr = np.array([model.jnt_qposadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
dofadr = np.array([model.jnt_dofadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
data.qpos[0:3] = [0.0, 0.0, 1.0]
data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
data.qpos[7:7+25] = DEFAULT_QPOS
mujoco.mj_forward(model, data)
CTRL_IDX = np.array(ACTION_TO_CTRL)
qa22 = qposadr[CTRL_IDX]
da22 = dofadr[CTRL_IDX]
default22 = DEFAULT_QPOS[CTRL_IDX]
def quat2rpy(quat):
w, x, y, z = quat
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
sinp = max(-1.0, min(1.0, 2 * (w * y - z * x)))
pitch = math.asin(sinp)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
return roll, pitch, yaw
====== 4. MNN 策略 ======
import MNN  # type: ignore
net = MNN.Interpreter(str(POLICY_FILE))
session = net.createSession()
input_tensor = net.getSessionInput(session)
output_tensor = net.getSessionOutput(session)
def infer(obs):
obs = np.asarray(obs, dtype=np.float32).reshape(1, -1)
host_in = MNN.Tensor(
input_tensor.getShape(), MNN.Halide_Type_Float,
obs, MNN.Tensor_DimensionType_Caffe)
input_tensor.copyFrom(host_in)
net.runSession(session)
out_shape = output_tensor.getShape()
host_out = MNN.Tensor(
out_shape, MNN.Halide_Type_Float,
np.zeros(out_shape, dtype=np.float32),
MNN.Tensor_DimensionType_Caffe)
output_tensor.copyToHostTensor(host_out)
return np.asarray(host_out.getData(), dtype=np.float32).reshape(-1)
def make_frame(last_action):
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]
proj_grav = R.T @ np.array([0.0, 0.0, -1.0])
frame = np.concatenate([
data.qpos[qa22] - default22,
data.qvel[da22],
last_action,
ang_vel_body,
proj_grav,
]).astype(np.float32)
return frame * OBS_SCALE
action = np.zeros(ACTION_SIZE, dtype=np.float32)
history = np.tile(make_frame(action), (HISTORY, 1))
target = DEFAULT_QPOS.copy()
====== 5. 自动踢木块指令 (恒速 1.0 m/s + 温和偏航修正对准木块) ======
策略在 vx=1.0, vyaw≈0 下训练 (即 16_T800_mnn_logs.py 的稳定步态).
温和 vyaw 修正 (<0.2 rad/s) 让机器人始终对准木块, 持续踢着它前进, 不会失稳.
def update_auto_cmd():
"""自动模式: 1.0 m/s 前进 + 始终转向对准木块, 持续贴身连踢
远距离始终朝木块转向 (不放弃追踪), 使机器人持续对准木块反复踢.
近距离 (dist&lt;LOCK_DIST) 锁定 vyaw=0, 把木块踢向机器人正前方直线推进,
避免因追着偏斜木块旋转越转越大形成侧向盘旋.
"""
robot = data.xpos[BASE_ID]
box = data.xpos[BOX_ID]
dx = box[0] - robot[0]
dy = box[1] - robot[1]
dist = math.hypot(dx, dy)
if dist &gt; LOCK_DIST:
    target_yaw = math.atan2(dy, dx)
    _, _, yaw = quat2rpy(data.xquat[BASE_ID])
    err_yaw = (target_yaw - yaw + math.pi) % (2 * math.pi) - math.pi
    vyaw = max(-YAW_MAX, min(YAW_MAX, YAW_P * err_yaw))
else:
    vyaw = 0.0  # 贴脚时踢正前方, 直线推进木块
cmd[0] = VX_AUTO
cmd[1] = 0.0
cmd[2] = vyaw
def handle_keys():
global running, auto_mode
if key_down(VK_Q):
running = False
if key_down(VK_SPACE):
auto_mode = False
cmd[:] = 0.0
if key_down(VK_R):
auto_mode = True
====== 6. 日志 (详细诊断版) ======
关节名称 (actuator 顺序 J00..J24, 用于日志标注)
JOINT_NAMES = [
"HP_L_Y", "HP_L_R", "HP_L_P", "KN_L", "AN_L_P", "AN_L_R",   # 左腿 0-5
"HP_R_Y", "HP_R_R", "HP_R_P", "KN_R", "AN_R_P", "AN_R_R",   # 右腿 6-11
"WAIST",                                                      # 躯干 12
"SH_L_R", "SH_L_P", "SH_L_Y", "EL_L", "WR_L",                # 左臂 13-17
"SH_R_R", "SH_R_P", "SH_R_Y", "EL_R", "WR_R",                # 右臂 18-22
"NECK_Y", "NECK_P",                                           # 头 23-24
]
CTRL_NAMES_22 = [JOINT_NAMES[i] for i in ACTION_TO_CTRL]  # 22 个受控关节名
def log_pose():
pos = data.xpos[BASE_ID]
roll, pitch, yaw = quat2rpy(data.xquat[BASE_ID])
box_pos = data.xpos[BOX_ID]
dist = math.hypot(box_pos[0]-pos[0], box_pos[1]-pos[1])
print(f"[POSE] t={data.time:5.2f}s | "
f"robot=({pos[0]:+6.3f},{pos[1]:+6.3f},{pos[2]:+6.3f})m | "
f"rpy=({math.degrees(roll):+6.2f},{math.degrees(pitch):+6.2f},"
f"{math.degrees(yaw):+6.2f})deg")
print(f"[BOX ] t={data.time:5.2f}s | "
f"box=({box_pos[0]:+6.3f},{box_pos[1]:+6.3f},{box_pos[2]:+6.3f})m | "
f"dist={dist:.3f}m")
def log_cmd():
mode = "AUTO" if auto_mode else "STOP"
print(f"[CMD ] t={data.time:5.2f}s | mode={mode} | "
f"vx={cmd[0]:+5.2f}m/s vy={cmd[1]:+5.2f}m/s vyaw={cmd[2]:+5.2f}rad/s")
def log_mnn(infer_ms, obs_full):
"""详细 MNN 日志: action 逐关节分解 + obs 统计"""
# action 逐关节 (22 个, 标注名称)
action_str = " ".join([f"{n}={a:+5.2f}" for n, a in zip(CTRL_NAMES_22, action)])
# obs 分段统计 (1083 = 1080 历史 + 3 指令)
hist = obs_full[:1080].reshape(15, 72)  # 15 帧
last_frame = hist[-1]  # 最新帧
q_diff = last_frame[0:22]    # 关节位置偏差 (已缩放)
qvel = last_frame[22:44]     # 关节速度 (已缩放)
last_a = last_frame[44:66]   # 上一帧 action
ang_vel = last_frame[66:69]  # 基座角速度
proj_g = last_frame[69:72]   # 投影重力
cmd_part = obs_full[1080:1083]
print(f"[MNN ] t={data.time:5.2f}s | infer={infer_ms:5.2f}ms | "
f"action sum={action.sum():+6.2f} sat={np.sum(np.abs(action)>=1.9):2d}/22")
print(f"       action: {action_str}")
print(f"       obs_lastframe: q_diff|s|=[{q_diff.min():+5.2f},{q_diff.max():+5.2f}] "
f"qvel|s|=[{qvel.min():+5.2f},{qvel.max():+5.2f}] "
f"act=[{last_a.min():+5.2f},{last_a.max():+5.2f}]")
print(f"       obs_base: ang_vel=[{ang_vel[0]:+5.2f},{ang_vel[1]:+5.2f},{ang_vel[2]:+5.2f}] "
f"proj_g=[{proj_g[0]:+5.2f},{proj_g[1]:+5.2f},{proj_g[2]:+5.2f}] "
f"cmd=[{cmd_part[0]:+5.2f},{cmd_part[1]:+5.2f},{cmd_part[2]:+5.2f}]")
def log_pd(tau):
"""详细 PD 日志: 逐关节 target vs actual + 跟踪误差"""
q = data.qpos[qposadr]
dq = data.qvel[dofadr]
err = q - target  # 有符号误差
err_abs = np.abs(err)
# 22 个受控关节的 target/actual/err
q22 = q[CTRL_IDX]
t22 = target[CTRL_IDX]
e22 = err[CTRL_IDX]
tau22 = tau[CTRL_IDX]
# 找误差最大的 5 个关节
top5_idx = np.argsort(err_abs[CTRL_IDX])[-5:][::-1]
top5_str = " ".join([f"{CTRL_NAMES_22[i]}:{np.degrees(e22[i]):+5.1f}deg" for i in top5_idx])
print(f"[PD  ] t={data.time:5.2f}s | tau_max={np.abs(tau).max():6.1f}Nm | "
f"track_err max={np.degrees(err_abs.max()):5.2f}deg "
f"rms={np.degrees(np.sqrt((err**2).mean())):5.2f}deg")
print(f"       top5_err: {top5_str}")
print(f"       tau22: [{tau22.min():+6.1f},{tau22.max():+6.1f}]Nm "
f"q22:[{np.degrees(q22).min():+6.1f},{np.degrees(q22).max():+6.1f}]deg "
f"t22:[{np.degrees(t22).min():+6.1f},{np.degrees(t22).max():+6.1f}]deg")
def log_base():
"""基座详细状态: 线速度/角速度/COM 投影/脚接触"""
pos = data.xpos[BASE_ID]
vel = data.qvel[0:3]   # 世界系线速度
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]  # 基体系角速度
# COM 在水平面的投影位置 (x, y)
com_xy = data.subtree_com[BASE_ID][:2]  # base 子树 COM
# 脚部位置 (从 model 找脚 body)
try:
left_foot = data.xpos[model.body("L_ANKLE_LINK").id]
right_foot = data.xpos[model.body("R_ANKLE_LINK").id]
foot_str = f"L=({left_foot[2]:.3f},z) R=({right_foot[2]:.3f},z)"
except:
foot_str = "N/A"
# 接触力 (左脚/右脚)
try:
lf = np.sum(np.abs(data.cfrc_ext[model.body("L_ANKLE_LINK").id]), axis=-1)
rf = np.sum(np.abs(data.cfrc_ext[model.body("R_ANKLE_LINK").id]), axis=-1)
contact_str = f"L_force={np.linalg.norm(lf):6.1f}N R_force={np.linalg.norm(rf):6.1f}N"
except:
contact_str = "N/A"
print(f"[BASE] t={data.time:5.2f}s | vel=({vel[0]:+5.2f},{vel[1]:+5.2f},{vel[2]:+5.2f})m/s | "
f"ang_vel_body=({ang_vel_body[0]:+5.2f},{ang_vel_body[1]:+5.2f},{ang_vel_body[2]:+5.2f})rad/s")
print(f"       com_xy=({com_xy[0]:+5.3f},{com_xy[1]:+5.3f}) | {foot_str} | {contact_str}")
def log_sep():
print("-" * 70)
====== 7. 主循环 ======
print("=" * 60)
print("踢方块前进 (自动跟踪模式)")
print("  自动模式: 机器人朝方块走, 踢着方块前进")
print("  空格: 急停 (退出自动)    R: 恢复自动    Q: 退出")
print("  木板尺寸: 12x12x2.4 cm (超扁, 脚可摆过不绊)  初始位置: (0.45, 0, 0.012)")
print("=" * 60)
next_log = 0.0
step_count = 0
last_infer_ms = 0.0
last_obs = np.zeros(1083, dtype=np.float32)  # 保存最近一次 obs 供日志使用
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and running:
handle_keys()
if auto_mode:
update_auto_cmd()
    if step_count % DECIMATION == 0:
        roll, pitch, _ = quat2rpy(data.xquat[BASE_ID])
        is_fallen = abs(math.degrees(roll)) &gt; FALL_PITCH or abs(math.degrees(pitch)) &gt; FALL_PITCH

        if is_fallen:
            if fall_time is None:
                fall_time = data.time
                print(f"[FALL] t={data.time:5.2f}s | 检测到摔倒 (pitch={math.degrees(pitch):.1f}°), 尝试恢复...")
            cmd[:] = 0.0
            action[:] = 0.0
            target = DEFAULT_QPOS.copy()
            if data.time - fall_time &gt; FALL_RESET_TIME:
                # 把机器人重置到方块后方 0.5m, 朝向方块 (避免机器人卡在方块后方)
                box_pos = data.xpos[BOX_ID]
                # 方块后方 0.5m (x 减小方向, 假设方块朝 +x 方向被推)
                reset_x = box_pos[0] - 0.5
                reset_y = box_pos[1]
                print(f"[RESET] t={data.time:5.2f}s | teleport 重置到方块后方 (x={reset_x:.2f}, y={reset_y:.2f})")
                data.qpos[0:3] = [reset_x, reset_y, 1.0]
                data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]  # 朝向 +x (方块方向)
                data.qpos[7:7+25] = DEFAULT_QPOS
                data.qvel[:] = 0.0
                mujoco.mj_forward(model, data)
                fall_time = None
            history[:] = make_frame(action)
        else:
            fall_time = None
            obs = np.concatenate([
                history.reshape(-1),
                (cmd * CMD_SCALE).astype(np.float32),
            ])
            last_obs = obs.copy()  # 保存供日志用
            t0 = time.perf_counter()
            action = infer(obs)
            last_infer_ms = (time.perf_counter() - t0) * 1000.0
            target = DEFAULT_QPOS.copy()
            target[ACTION_TO_CTRL] += (action * ACTION_SCALE).astype(np.float64)
            history[:-1] = history[1:]
            history[-1] = make_frame(action)

    q = data.qpos[qposadr]
    dq = data.qvel[dofadr]
    tau = KP * (target - q) - KD * dq
    data.ctrl[:] = tau

    mujoco.mj_step(model, data)
    step_count += 1

    # (木块为物理物体, 由机器人步态真实踢/推向前, 无需额外处理)

    if data.time &gt;= next_log:
        log_pose()
        log_cmd()
        log_mnn(last_infer_ms, last_obs)
        log_pd(tau)
        log_base()
        log_sep()
        next_log += LOG_INTERVAL

    viewer.sync()
running = False
print("仿真结束")

第三章：T800拿着物体走路

在具身智能与机器人仿真的开发之路上，从"接触"到"抓取"是跨越物理世界与数字世界鸿沟的关键一步。在第二章中，我们实现了 T800 机器人"踢着木块往前走"的 locomotion 控制。但这仅仅是刚体碰撞的初级应用。当我们进阶到第三章时，真正的工程挑战才刚刚开始。

本文将从一线仿真工程师的实战视角，带你拆解如何用 MuJoCo 结合 Python 实现稳定的"手持物体"行走，并分享一套在真实机器人开发中通用的空间坐标思维框架。

3.1机器人二次开发的"灵魂三问"

无论是做视觉伺服、机械臂运动规划，还是真实世界的抓取任务，在动手写代码前，我们必须先在脑海中建立清晰的空间映射。这套被我称为"灵魂三问"的思考框架，是解决所有正逆运动学问题的基石：

身体的部件在哪里？（例如：机器人的末端执行器/手腕坐标系）
目标物体在哪里？（例如：被抓取木块的中心坐标系）
二者之间的空间关系是什么？（例如：相对位姿变换矩阵）

在仿真环境中，我们不需要像真机那样去跑复杂的视觉算法或手眼标定，因为 MuJoCo 引擎直接为我们提供了上帝视角的绝对坐标。但理解这三者的相对关系，依然是我们编写控制逻辑的核心。

为什么"抓取"比"踢"难那么多？

在上一节"踢木块"的实现中，我们只需要处理简单的刚体碰撞和摩擦力。但"抓握"在真实物理世界中是一个极其复杂的非线性问题：

接触点与摩擦系数：手指与木块的接触面是动态变化的，且受表面材质（摩擦系数）影响极大。
力控难题：夹爪力度难以精确把控。夹得太紧，可能会捏坏木块甚至导致木块受力崩飞；夹得太松，木块会在行走的震动中滑落。
刚性形变：真实的物体和夹爪在受力时都会发生微小的弹性形变，这在基于刚体假设的常规仿真中极难完美复现。

为了快速验证"手持物体行走"的运动学稳定性，我们果断放弃了复杂的夹爪/灵巧手控制，转而使用坐标绑定（Coordinate Binding）策略。

核心逻辑：将木块的坐标系直接绑定到机器人右手手腕的坐标系上。在每一帧的仿真循环中，读取手腕的实时坐标，并强制更新木块的坐标，使其与手腕保持相对固定的空间关系。简单来说就是：手在哪，物体就在哪。

3.2T800拿着物体行走

提示语：结合20_T800_HitBox.py代码给我生成一个脚本叫21_T800_HoldBox.py，不要让机器人踢着木块前进，把木块的坐标绑定在机器人的左手手腕的坐标上。让木块跟随机器人一起移动。回复键盘wasd的控制

"""
21_T800_HoldBox.py (教学版: 机器人手持木块)
基于 20_T800_HitBox.py, 目标: 让木块吸附在机器人左手手腕上, 随机器人一起移动。
与 20 的区别:
不踢木块: 木块坐标每一仿真步被绑定到左手手腕 (LINK_WRIST_END_L) 的世界坐标
木块物理禁碰撞 (contype=0 conaffinity=0), 完全跟随手腕, 不会掉落
恢复 WASD 手动手动控制 (18 号方案): 按住移动 + 速度渐变
控制方案 (WASD 手动, 按住移动):
W: 前进      S: 后退
A: 左转      D: 右转
空格: 急停    Q: 退出
松开按键: 速度朝 0 衰减
动态注入木块 (同 20): 用临时 wrapper XML, 不修改 t800.xml
运行: D:/miniconda3/envs/t800/python.exe 21_T800_HoldBox.py
"""
import math
import time
import ctypes
import tempfile
import os
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
====== 1. 常量 (与 20 一致, 对齐官方部署参数) ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
HISTORY = 15
ACTION_SIZE = 22
DECIMATION = 5
LOG_INTERVAL = 0.1
CMD_SCALE = np.array([2.0, 2.0, 1.0])
ACTION_TO_CTRL = list(range(0, 12)) + list(range(13, 23))
DEFAULT_QPOS = np.zeros(25)
DEFAULT_QPOS[0:12] = [-0.06, 0.0, 0.0, 0.12, -0.06, 0.0,
-0.06, 0.0, 0.0, 0.12, -0.06, 0.0]
DEFAULT_QPOS[13:18] = [0.0, 0.15, 0.0, -0.25, 0.0]
DEFAULT_QPOS[18:23] = [0.0, -0.15, 0.0, -0.25, 0.0]
KP = np.zeros(25); KD = np.zeros(25)
KP[0:12] = [180, 100, 100, 180, 40, 40, 180, 100, 100, 180, 40, 40]
KD[0:12] = [5.0, 3.0, 3.0, 5.0, 0.3, 0.3, 5.0, 3.0, 3.0, 5.0, 0.3, 0.3]
腰部 (idx=12): 策略不控制 (ACTION_TO_CTRL 跳过), 由 PD 锁定在初始角 0.
保持官方 100/5, 不要改硬, 否则干扰策略期望的躯干运动导致摔倒.
KP[12] = 100;  KD[12] = 5.0
KP[13:18] = [60, 50, 50, 60, 50];  KD[13:18] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[18:23] = [60, 50, 50, 60, 50];  KD[18:23] = [1.8, 1.5, 1.5, 1.8, 1.2]
KP[23:25] = [100, 100];            KD[23:25] = [1.0, 1.0]
ACTION_SCALE = np.concatenate([
np.tile([0.5, 0.2, 0.2, 0.5, 0.5, 0.2], 2),
np.tile([0.2, 0.2, 0.05, 0.2, 0.05], 2),
])
OBS_SCALE = np.concatenate([
np.full(22, 1.0), np.full(22, 0.05), np.full(22, 1.0),
np.full(3, 1.0), np.full(3, 1.0),
]).astype(np.float32)
====== 2. 键盘控制 (WASD 按住移动 + 速度渐变) ======
VK_W = 0x57; VK_A = 0x41; VK_S = 0x53; VK_D = 0x44
VK_SPACE = 0x20; VK_Q = 0x51
_user32 = ctypes.windll.user32
def key_down(vk):
"""检测某物理键当前是否被按下 (不依赖窗口焦点)"""
return _user32.GetAsyncKeyState(vk) & 0x8000 != 0
速度指令 [vx, vy, vyaw]; 用渐变方式更新, 避免突变
cmd = np.array([0.0, 0.0, 0.0])
running = True
VX_TARGET = 1.0       # 按住 W/S 时 vx 的目标值 (m/s)
VYAW_TARGET = 1.5     # 按住 A/D 时 vyaw 的目标值 (rad/s)
RAMP = 2.0            # 速度渐变率 (单位/秒): 约 0.5s 到达目标
DECAY = 3.0           # 松开时衰减率 (比 ramp 快, 快速停下)
FALL_PITCH = 30.0
FALL_RESET_TIME = 1.5
fall_time = None
def update_cmd(dt):
"""根据按键状态更新速度指令 (渐变式), WASD 手动控制"""
global running
if key_down(VK_Q):
running = False
return
if key_down(VK_SPACE):
cmd[:] = 0.0
return
# vx: W 前进 / S 后退
if key_down(VK_W):
cmd[0] += RAMP * dt
if cmd[0] > VX_TARGET: cmd[0] = VX_TARGET
elif key_down(VK_S):
cmd[0] -= RAMP * dt
if cmd[0] < -VX_TARGET: cmd[0] = -VX_TARGET
else:
if cmd[0] > 0:
cmd[0] = max(0.0, cmd[0] - DECAY * dt)
else:
cmd[0] = min(0.0, cmd[0] + DECAY * dt)
# vyaw: A 左转 / D 右转
if key_down(VK_A):
cmd[2] += RAMP * dt
if cmd[2] > VYAW_TARGET: cmd[2] = VYAW_TARGET
elif key_down(VK_D):
cmd[2] -= RAMP * dt
if cmd[2] < -VYAW_TARGET: cmd[2] = -VYAW_TARGET
else:
if cmd[2] > 0:
cmd[2] = max(0.0, cmd[2] - DECAY * dt)
else:
cmd[2] = min(0.0, cmd[2] + DECAY * dt)
====== 3. 动态加载模型 (t800.xml + 木块) ======
def load_model_with_box():
"""木块 10x10x10 cm, 物理禁碰撞 (contype=0 conaffinity=0), 由脚本绑定到左手"""
wrapper = '''&lt;mujoco&gt;
<include file="t800.xml"/>
<worldbody>
<body name="hold_box" pos="0.4 0.15 1.0">
<freejoint/>
<geom name="box_geom" type="box" size="0.05 0.05 0.05"
rgba="1 1 1 1" mass="0.03" contype="0" conaffinity="0"/>
</body>
</worldbody>
</mujoco>'''
with tempfile.NamedTemporaryFile('w', suffix='.xml',
delete=False, dir='.') as f:
f.write(wrapper)
tmp_path = f.name
try:
model = mujoco.MjModel.from_xml_path(tmp_path)
finally:
os.remove(tmp_path)
return model
model = load_model_with_box()
data = mujoco.MjData(model)
BASE_ID = model.body("LINK_BASE").id
BOX_ID = model.body("hold_box").id
WRIST_L_ID = model.body("LINK_WRIST_END_L").id
木块 freejoint 的 qpos/dof 起始索引 (追加在机器人最后)
BOX_QADR = model.nq - 7   # 机器人 32 + 木块 freejoint 7
BOX_DADR = model.nv - 6   # 机器人 nv + 木块 freejoint 6
nu = model.nu
qposadr = np.array([model.jnt_qposadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
dofadr = np.array([model.jnt_dofadr[int(model.actuator_trnid[i, 0])]
for i in range(nu)])
data.qpos[0:3] = [0.0, 0.0, 1.0]
data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
data.qpos[7:7+25] = DEFAULT_QPOS
mujoco.mj_forward(model, data)
CTRL_IDX = np.array(ACTION_TO_CTRL)
qa22 = qposadr[CTRL_IDX]
da22 = dofadr[CTRL_IDX]
default22 = DEFAULT_QPOS[CTRL_IDX]
def quat2rpy(quat):
w, x, y, z = quat
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
sinp = max(-1.0, min(1.0, 2 * (w * y - z * x)))
pitch = math.asin(sinp)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
return roll, pitch, yaw
====== 4. MNN 策略 ======
import MNN  # type: ignore
net = MNN.Interpreter(str(POLICY_FILE))
session = net.createSession()
input_tensor = net.getSessionInput(session)
output_tensor = net.getSessionOutput(session)
def infer(obs):
obs = np.asarray(obs, dtype=np.float32).reshape(1, -1)
host_in = MNN.Tensor(
input_tensor.getShape(), MNN.Halide_Type_Float,
obs, MNN.Tensor_DimensionType_Caffe)
input_tensor.copyFrom(host_in)
net.runSession(session)
out_shape = output_tensor.getShape()
host_out = MNN.Tensor(
out_shape, MNN.Halide_Type_Float,
np.zeros(out_shape, dtype=np.float32),
MNN.Tensor_DimensionType_Caffe)
output_tensor.copyToHostTensor(host_out)
return np.asarray(host_out.getData(), dtype=np.float32).reshape(-1)
def make_frame(last_action):
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]
proj_grav = R.T @ np.array([0.0, 0.0, -1.0])
frame = np.concatenate([
data.qpos[qa22] - default22,
data.qvel[da22],
last_action,
ang_vel_body,
proj_grav,
]).astype(np.float32)
return frame * OBS_SCALE
action = np.zeros(ACTION_SIZE, dtype=np.float32)
history = np.tile(make_frame(action), (HISTORY, 1))
target = DEFAULT_QPOS.copy()
====== 5. 木块绑定到左手手腕 ======
BOX_OFF = np.array([0.0, 0.0, 0.0])  # 相对手腕的小偏移 (可调)
def update_box_at_wrist():
"""把木块世界坐标绑定到左手手腕, 使其随机器人一起移动
每仿真步: 木块位置 = 左手手腕世界位置, 姿态保持水平 (world 系).
木块禁碰撞, 且清零自身速度, 所以完全受手腕拖动, 不掉落.
"""
w = data.xpos[WRIST_L_ID] + BOX_OFF
data.qpos[BOX_QADR + 0] = w[0]
data.qpos[BOX_QADR + 1] = w[1]
data.qpos[BOX_QADR + 2] = w[2]
data.qpos[BOX_QADR + 3:BOX_QADR + 7] = [1.0, 0.0, 0.0, 0.0]  # 平放姿态
data.qvel[BOX_DADR:BOX_DADR + 6] = 0.0
====== 6. 日志 (详细诊断版) ======
关节名称 (actuator 顺序 J00..J24, 用于日志标注)
JOINT_NAMES = [
"HP_L_Y", "HP_L_R", "HP_L_P", "KN_L", "AN_L_P", "AN_L_R",   # 左腿 0-5
"HP_R_Y", "HP_R_R", "HP_R_P", "KN_R", "AN_R_P", "AN_R_R",   # 右腿 6-11
"WAIST",                                                      # 躯干 12
"SH_L_R", "SH_L_P", "SH_L_Y", "EL_L", "WR_L",                # 左臂 13-17
"SH_R_R", "SH_R_P", "SH_R_Y", "EL_R", "WR_R",                # 右臂 18-22
"NECK_Y", "NECK_P",                                           # 头 23-24
]
CTRL_NAMES_22 = [JOINT_NAMES[i] for i in ACTION_TO_CTRL]  # 22 个受控关节名
def log_pose():
pos = data.xpos[BASE_ID]
roll, pitch, yaw = quat2rpy(data.xquat[BASE_ID])
box_pos = data.xpos[BOX_ID]
wrist_pos = data.xpos[WRIST_L_ID]
err = np.linalg.norm(box_pos - wrist_pos)
print(f"[POSE] t={data.time:5.2f}s | "
f"robot=({pos[0]:+6.3f},{pos[1]:+6.3f},{pos[2]:+6.3f})m | "
f"rpy=({math.degrees(roll):+6.2f},{math.degrees(pitch):+6.2f},"
f"{math.degrees(yaw):+6.2f})deg")
print(f"[BOX ] t={data.time:5.2f}s | "
f"box=({box_pos[0]:+6.3f},{box_pos[1]:+6.3f},{box_pos[2]:+6.3f})m | "
f"wrist=({wrist_pos[0]:+6.3f},{wrist_pos[1]:+6.3f},{wrist_pos[2]:+6.3f})m | "
f"到手腕偏差={err:.3f}m")
def log_cmd():
print(f"[CMD ] t={data.time:5.2f}s | "
f"vx={cmd[0]:+5.2f}m/s vy={cmd[1]:+5.2f}m/s vyaw={cmd[2]:+5.2f}rad/s")
def log_base():
pos = data.xpos[BASE_ID]
vel = data.qvel[0:3]
R = data.xmat[BASE_ID].reshape(3, 3)
ang_vel_body = R.T @ data.qvel[3:6]
com_xy = data.subtree_com[BASE_ID][:2]
print(f"[BASE] t={data.time:5.2f}s | vel=({vel[0]:+5.2f},{vel[1]:+5.2f},{vel[2]:+5.2f})m/s | "
f"ang_vel_body=({ang_vel_body[0]:+5.2f},{ang_vel_body[1]:+5.2f},{ang_vel_body[2]:+5.2f})rad/s")
print(f"       com_xy=({com_xy[0]:+5.3f},{com_xy[1]:+5.3f})")
def log_sep():
print("-" * 70)
====== 7. 主循环 ======
print("=" * 60)
print("机器人手持木块 (木块绑定左手手腕, 随机器人移动)")
print("  W/S: 前进/后退   A/D: 左转/右转   空格: 急停   Q: 退出")
print("  木块尺寸: 10x10x10 cm, 禁物理碰撞, 完全吸附在左手")
print("=" * 60)
next_log = 0.0
step_count = 0
last_infer_ms = 0.0
last_obs = np.zeros(1083, dtype=np.float32)  # 保存最近一次 obs 供日志使用
dt_policy = model.opt.timestep * DECIMATION   # 策略周期 0.01s
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and running:
update_cmd(dt_policy)
    if step_count % DECIMATION == 0:
        roll, pitch, _ = quat2rpy(data.xquat[BASE_ID])
        is_fallen = abs(math.degrees(roll)) &gt; FALL_PITCH or abs(math.degrees(pitch)) &gt; FALL_PITCH

        if is_fallen:
            if fall_time is None:
                fall_time = data.time
                print(f"[FALL] t={data.time:5.2f}s | 检测到摔倒 (pitch={math.degrees(pitch):.1f}°), 尝试恢复...")
            cmd[:] = 0.0
            action[:] = 0.0
            target = DEFAULT_QPOS.copy()
            if data.time - fall_time &gt; FALL_RESET_TIME:
                print(f"[RESET] t={data.time:5.2f}s | teleport 重置到原点")
                data.qpos[0:3] = [0.0, 0.0, 1.0]
                data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
                data.qpos[7:7+25] = DEFAULT_QPOS
                data.qvel[:] = 0.0
                mujoco.mj_forward(model, data)
                update_box_at_wrist()
                fall_time = None
            history[:] = make_frame(action)
        else:
            fall_time = None
            obs = np.concatenate([
                history.reshape(-1),
                (cmd * CMD_SCALE).astype(np.float32),
            ])
            last_obs = obs.copy()  # 保存供日志用
            t0 = time.perf_counter()
            action = infer(obs)
            last_infer_ms = (time.perf_counter() - t0) * 1000.0
            target = DEFAULT_QPOS.copy()
            target[ACTION_TO_CTRL] += (action * ACTION_SCALE).astype(np.float64)
            history[:-1] = history[1:]
            history[-1] = make_frame(action)

    q = data.qpos[qposadr]
    dq = data.qvel[dofadr]
    tau = KP * (target - q) - KD * dq
    data.ctrl[:] = tau

    mujoco.mj_step(model, data)
    step_count += 1

    # 木块绑定到左手手腕, 随机器人一起移动
    update_box_at_wrist()

    if data.time &gt;= next_log:
        log_pose()
        log_cmd()
        log_base()
        log_sep()
        next_log += LOG_INTERVAL

    viewer.sync()
running = False
print("仿真结束")

总结

本文以众擎 T800 人形机器人为载体，系统讲解了在 MuJoCo 物理仿真环境中进行机器人工程调优与二次开发的完整路径。第一章从认知纠偏入手，阐明 WASD 键盘控制的本质是向策略网络下达高层指令，并拆解了 OBS 到 Inferred Action 的技术链路与 Command 数组参数，最终通过 18_T800_wasd.py 实现键盘实时控制。第二章围绕具身智能的感知—规划—控制闭环，先动态注入 8cm 白色木块，再通过 20_T800_HitBox.py 让机器人踢着木块前进。第三章进阶到手持物体行走，借助"灵魂三问"空间坐标框架，采用坐标绑定策略将木块吸附于左手手腕，实现稳定的手持行走。三章层层递进，从遥控、踢物到抓握，完整覆盖了从刚体碰撞到空间坐标绑定的关键工程实践。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

迈向原生世界状态的统一多模态世界模型，大晓机器人联合南洋理工大学S-Lab发布Puffin-World

DAMO开发者矩阵

AESE2027北京机器人展，奖项免费报展位抢先订

DAMO开发者矩阵

自动化仓储物流管理系统有哪些设备软件方案全景

自动化仓储物流管理系统由多层架构组成：业务层WMS负责库存与业务管理，执行层WES拆解任务并优化作业，调度层RCS协调机器人实时运行，设备层则包含堆垛机、AGV/AMR、穿梭车、分拣线等硬件。系统融合软件（如通天晓、科箭、曼哈顿）与设备，形成“业务→执行→调度→设备”协同闭环。企业应基于业务需求，优先评估软件协同能力与长期运维，分阶段实施，实现少人化、高效化运营。