模块 4：AI 策略驱动机器人运动

第一章：机器人日志

第二章：T800足底调试检查

---

文章目录

前言

第一章：机器人日志

        1.1获取机器人运行日志

        1.2结合日志调试

第二章：T800足底调试检查

总结

---

前言

---

第一章：机器人日志

工程师都会遇到这样的落差：训练日志里 reward 一路上涨，曲线漂亮得让人想直接提交代码，可把权重加载进 MuJoCo viewer 回放，机器人要么一步没动就瘫倒，要么原地高频抖动，要么走出三步后突然侧翻。原因并不神秘。强化学习优化的是奖励函数这个代理指标，而不是“稳定行走”这个真实目标；reward 里没写清楚的漏洞会被策略精准利用（reward hacking）；训练侧与回放侧只要有一份 XML、一个 action scale、一组归一化统计量不一致，行为就会面目全非。

新手调试机器人喜欢运行程序，盯着机器人看它如何运动，如果出现了摔倒、故障就只会在一旁挠头。真正有经验的具身智能工程师不会盯着画面，他们会去查看机器人运动数据，数据能反映所有问题。那什么时候采取看画面呢，只有在真机演示的时候出现了和仿真环境不一样的异常，但是又无法从仿真环境中找出问题所在这时候就要用告诉相机去慢放机器人动作，去观察机器人真实运行过程中的问题、这些问题可能是装配，电压等外部因素导致的。

1.1获取机器人运行日志

提示词：针对15_T800_mnn.py代码，创建一个新的脚本叫16_T800_mnn_logs.py，这个脚本不光要实现15_T800_mnn.py代码的功能，还要添加对应日志，打印机器人的pos、rpy，每隔100ms打印一次，并且还需打印模型推理的信息，pd控制器的相关日志，方便分析机器人的步态。

"""
16_T800_mnn_logs.py (教学版 + 日志)
在 15_T800_mnn.py 基础上, 每 100ms 打印一次机器人位姿日志:
  [POSE] 基座 pos (x,y,z) 与 rpy (roll,pitch,yaw)
  [MNN]  推理耗时 + action 统计 (min/max/mean)
注意:
日志标签用 ASCII ([POSE]/[MNN]), 避免 Windows 终端中文乱码
用"仿真时间节流"(data.time)而不是 step 取模, 保证不同时步下稳定 100ms
四元数转 rpy 用本地 numpy/math 实现 (MuJoCo Python 无 mju_quat2euler)
运行: D:/miniconda3/envs/t800/python.exe 16_T800_mnn_logs.py
"""
import math
import time
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
====== 1. 路径与常量 ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
OBS_SIZE = 1083   # 观测向量维度: 72 * 15 (历史) + 3 (速度指令)
ACTION_SIZE = 22  # 策略输出动作维度
CMD_VX = 1.0      # 前进速度指令 (m/s), 正值=前进
LOG_INTERVAL = 0.1  # 日志间隔: 100ms (仿真秒)
====== 2. 加载 MuJoCo 模型 ======
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
基座 body id (LINK_BASE), 位姿从它身上取
BASE_ID = model.body("LINK_BASE").id
def quat2rpy(quat):
"""四元数 (w, x, y, z) -> (roll, pitch, yaw) rad, ZYX 内蕴约定"""
w, x, y, z = quat
sinr = 2.0 * (w * x + y * z)
cosr = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sinr, cosr)
sinp = max(-1.0, min(1.0, 2.0 * (w * y - z * x)))
pitch = math.asin(sinp)
siny = 2.0 * (w * z + x * y)
cosy = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(siny, cosy)
return roll, pitch, yaw
====== 3. 加载 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):
"""一次 MNN 推理: obs (1083,) -> action (22,)"""
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)
====== 4. 日志函数 (每 100ms 调一次) ======
def log_pose():
"""打印基座位置和姿态"""
pos = data.xpos[BASE_ID]          # 世界坐标 (x, y, z)
quat = data.xquat[BASE_ID]        # 四元数 (w, x, y, z)
roll, pitch, yaw = quat2rpy(quat)
print(f"[POSE] t={data.time:6.2f}s | "
f"pos=({pos[0]:+7.3f},{pos[1]:+7.3f},{pos[2]:+7.3f})m | "
f"rpy=({math.degrees(roll):+7.2f},{math.degrees(pitch):+7.2f},"
f"{math.degrees(yaw):+7.2f})deg")
def log_mnn(action, infer_ms):
"""打印 MNN 推理耗时与 action 统计"""
print(f"[MNN ] t={data.time:6.2f}s | infer={infer_ms:5.2f}ms | "
f"action min={action.min():+6.3f} max={action.max():+6.3f} "
f"mean={action.mean():+6.3f} | first3={action[:3].round(3).tolist()}")
====== 5. 主循环: 推理 + 控制 + 日志 ======
print(f"OBS_SIZE={OBS_SIZE}, ACTION_SIZE={ACTION_SIZE}, CMD_VX={CMD_VX} m/s")
print(f"日志间隔: {LOG_INTERVAL*1000:.0f}ms (仿真时间)")
print("-" * 70)
next_log = 0.0  # 下一次日志的仿真时间点
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
# --- 5.1 构造 obs (教学版: 零向量 + 末尾 3 维填速度指令) ---
obs = np.zeros(OBS_SIZE, dtype=np.float32)
obs[-3] = CMD_VX  # vx 指令 (向前)
obs[-2] = 0.0     # vy 指令 (侧向, 0=直行)
obs[-1] = 0.0     # vyaw 指令 (转向, 0=不转)
    # --- 5.2 MNN 推理 (计时) ---
    t0 = time.perf_counter()
    action = infer(obs)  # shape (22,)
    infer_ms = (time.perf_counter() - t0) * 1000.0
# --- 5.3 写入 ctrl (22 维直接填, 剩 3 个补 0) ---
# MuJoCo 的 ctrllimited=true 会自动按 ctrlrange 截断
data.ctrl[:ACTION_SIZE] = action
data.ctrl[ACTION_SIZE:] = 0.0

# --- 5.4 推进仿真 ---
mujoco.mj_step(model, data)

# --- 5.5 每 100ms 打印一次日志 (仿真时间节流) ---
if data.time >= next_log:
    log_pose()
    log_mnn(action, infer_ms)
    print("-" * 70)
    next_log += LOG_INTERVAL

viewer.sync()
print("仿真结束")

1.2结合日志调试

我们发现，机器人还是无法直立行走。但是我们可以将日志信息粘贴到trae，然后要求他修改代码

提示语：机器人没有稳定走路，直接瘫倒下来，帮我修复16_T800_mnn_logs.py

这是ai从日志中发现的问题，结合日志能有效将问题精确。

"""
16_T800_mnn_logs.py (修复版: 完全对齐官方 T800 RL 部署参数)
控制链路:
  真实状态 -> obs(1083) -> MNN 推理 -> action(22)
    -> q_des = 默认姿态 + action * action_scale -> PD -> 力矩 -> ctrl
参数来源 (官方 SDK, policy 文件同名 t800_260318_150533_60000.mnn):
assets/config/t800pro/rl_walking_example/default.yaml
src/runner/rl_walking_example/rl_walking_example_runner.cc
obs 结构 (每帧 72 维, 顺序必须与训练一致):
[0:22]  关节位置偏差 (q - default)        scale=1.0
[22:44] 关节角速度                        scale=0.05
[44:66] 上一帧 action                     scale=1.0
[66:69] 基座角速度 (基体系)               scale=1.0
[69:72] 投影重力 (基体系)                 scale=1.0
历史 15 帧 (旧->新) 拼接 = 1080, 末尾追加指令 [vx2, vy2, vyaw*1]
策略 100Hz (sim dt=0.002, decimation=5), PD 每仿真步运行
日志: 每 100ms 打印 [POSE]/[MNN]/[PD]
运行: D:/miniconda3/envs/t800/python.exe 16_T800_mnn_logs.py
"""
import math
import time
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
====== 1. 常量 ======
ROOT = Path(file).resolve().parent
POLICY_FILE = ROOT / "policy" / "t800_260318_150533_60000.mnn"
OBS_FRAME = 72
HISTORY = 15
ACTION_SIZE = 22
DECIMATION = 5        # 策略 100Hz: 0.002s * 5 = 0.01s
LOG_INTERVAL = 0.1    # 日志 100ms
速度指令 (m/s, rad/s); 进入 obs 时乘缩放系数 [2.0, 2.0, 1.0]
CMD_VX = 1.0
CMD_VY = 0.0
CMD_VYAW = 0.0
CMD_SCALE = np.array([2.0, 2.0, 1.0])
22 个受控关节 -> 25 个 actuator 的映射 (跳过 idx12 躯干 / idx23,24 头)
ACTION_TO_CTRL = list(range(0, 12)) + list(range(13, 23))
====== 2. 官方默认关节姿态 (25 维, actuator 顺序 J00..J24) ======
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]   # 右腿
idx12 躯干 = 0
DEFAULT_QPOS[13:18] = [0.0, 0.15, 0.0, -0.25, 0.0]        # 左臂 J13-J17
DEFAULT_QPOS[18:23] = [0.0, -0.15, 0.0, -0.25, 0.0]       # 右臂 J18-J22
idx23,24 头 = 0
====== 3. 官方 PD 增益 (25 维) ======
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]  # 头
====== 4. 官方 action_scale (22 维, 受控关节顺序) ======
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 每帧缩放 (72 维)
OBS_SCALE = np.concatenate([
np.full(22, 1.0),    # 关节位置偏差
np.full(22, 0.05),   # 关节速度
np.full(22, 1.0),    # 上一帧 action
np.full(3, 1.0),     # 基座角速度
np.full(3, 1.0),     # 投影重力
]).astype(np.float32)
====== 5. 加载模型, 设初始姿态 ======
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)])
初始: 站立高度 + 默认关节姿态
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
====== 6. 加载 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)
====== 7. 构造单帧 obs (72 维) ======
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,              # 22 位置偏差
data.qvel[da22],                          # 22 关节速度
last_action,                              # 22 上一帧 action
ang_vel_body,                             # 3 基座角速度
proj_grav,                                # 3 投影重力
]).astype(np.float32)
return frame * OBS_SCALE
历史缓冲 (15 帧 × 72), 旧帧在前新帧在后; 首帧填初始状态
action = np.zeros(ACTION_SIZE, dtype=np.float32)
history = np.tile(make_frame(action), (HISTORY, 1))
target = DEFAULT_QPOS.copy()
====== 8. 日志 (每 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_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} "
f"mean={action.mean():+6.3f}")
def log_pd(tau):
q = data.qpos[qposadr]
err = np.abs(q - target)
print(f"[PD  ] t={data.time:5.2f}s | "
f"tau_max={np.abs(tau).max():6.1f}Nm | "
f"track_err max={np.degrees(err.max()):5.2f}deg "
f"rms={np.degrees(np.sqrt((err**2).mean())):5.2f}deg")
print("-" * 70)
====== 9. 主循环 ======
print(f"policy 100Hz (decimation={DECIMATION}), CMD_VX={CMD_VX} m/s")
print("-" * 70)
next_log = 0.0
step_count = 0
last_infer_ms = 0.0
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
# --- 9.1 每 100Hz 策略推理 ---
if step_count % DECIMATION == 0:
obs = np.concatenate([
history.reshape(-1),                                  # 1080
np.array([CMD_VX, CMD_VY, CMD_VYAW], np.float32)
* CMD_SCALE.astype(np.float32),                       # 3 指令
])
        t0 = time.perf_counter()
        action = infer(obs)
        last_infer_ms = (time.perf_counter() - t0) * 1000.0

        # action -> 目标关节角: q_des = default + action * scale
        target = DEFAULT_QPOS.copy()
        target[ACTION_TO_CTRL] += (action * ACTION_SCALE).astype(np.float64)

        # 更新历史帧 (旧帧左移, 新帧放最后)
        history[:-1] = history[1:]
        history[-1] = make_frame(action)

    # --- 9.2 每仿真步 PD 控制 (非受控关节保持默认姿态) ---
    q = data.qpos[qposadr]
    dq = data.qvel[dofadr]
    tau = KP * (target - q) - KD * dq
    data.ctrl[:] = tau  # ctrllimited=true 自动按 ctrlrange 截断

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

    # --- 9.3 每 100ms 日志 ---
    if data.time >= next_log:
        log_pose()
        log_mnn(last_infer_ms)
        log_pd(tau)
        next_log += LOG_INTERVAL

    viewer.sync()
print("仿真结束")

最终效果

第二章：T800足底调试检查

在进行机器人仿真时，往往会出现机器人运动和我们期望不一致的现象发生，在第一章我们给大家演示了失败的例子，并通过ai的帮助，成功让机器人正常直立行走。但是很多时候机器人走不好，往往不是AI脑子笨，而是我们给他们设定的”出场环境“太糟糕了。因此在进行机器人仿真前，必须先做好足底调试检查工作，确保机器人在初始状态下能正常站立。

大家会发现整个代码除了机器人初始高度修改了，其他地方没有发生变化，但是机器人无法做到直立行走。这就说明了给机器人进行足底检查的重要性了。那这个初始高度值是如何确定的，以及其他初始值如何确定。有两种方法，第一种是直接测量物理世界人形机器人真实高度来获取，第二种是通过代码扫描。

提示语：data.qpos[0:3] = [0.0, 0.0, 10.0]这个数组中，最后一个数是机器人的初始化高度，如果比较高，机器人会掉下来，如果比较低，机器人会陷在地面下，你帮我写一个扫描代码脚17_T800_scan.py，从0.5米到1.5mi之间，每次微调2cm，扫描机器人本体与大地接触点的数量。

从打印信息分析可知，机器人的初始高度应设置在1米

---

总结

本模块围绕 T800 人形机器人的仿真调试展开，核心思路是：当机器人无法稳定行走时，不要只盯着画面，而要回到数据中去定位问题。

第一章从获取机器人运行日志入手，通过给 15_T800_mnn.py 增加位姿、推理耗时和 PD 控制器日志，把“黑盒”的仿真过程变成可观测的数据流。当机器人瘫倒时，将日志交给 AI 分析，最终定位到观测空间构造、默认姿态、PD 增益和 action_scale 等部署参数未对齐官方配置的问题，修复后机器人恢复直立行走。

第二章则指出，很多时候机器人走不好并非策略本身的问题，而是“出场环境”没设置好。通过足底调试检查，我们验证了初始高度对站立稳定性的关键影响，并借助扫描脚本在 0.5 米到 1.5 米之间以 2cm 步长扫描接触点数量，最终确定初始高度应设置为 1 米。

回顾整个调试过程，可以提炼出三条经验：第一，日志是仿真调试的第一现场，数据能反映所有问题；第二，部署参数必须与训练侧严格对齐，任何一处不一致都会导致行为面目全非；第三，在怀疑策略之前，先确认机器人的初始状态是否合理。希望这套“先看数据、再调参数、最后才怀疑策略”的方法，能帮助你在后续的实战案例中少走弯路。