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

lagelangri666

93人浏览 · 2026-09-08 17:13:45

lagelangri666 · 2026-09-08 17:13:45 发布

模块 3：底层控制原理

第一章：电机力矩范围获取

第二章：力矩控制案例

第三章：手动控制T800

提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言

我们用 PD 控制把"期望关节角度"翻译成了"关节力矩"，并把 tau 写进 data.ctrl 交给仿真主循环。但一个容易被忽略的事实是：PD 公式算出来的数值可能远大于真实电机所能提供的力矩。如果放任这个数值直接下发，仿真中的机器人会表现出"违背物理"的爆发力，训练出来的策略迁移到真机时必然失效。这一集我们聚焦力矩范围（ctrlrange / forcerange），它是把理想控制量约束回物理可行域的关键一环，也是仿真与真机之间不可绕过的一致性保障。

第一章：电机力矩范围获取

真实机器人的每个关节都由电机、减速器和驱动器组成，它们能输出的力矩存在一个天然上限：电流受限于驱动器，扭矩受限于电机与减速比。当 PD 控制器因为误差过大或增益过高而算出一个巨大的 tau 时，硬件并不会真的兑现这个力矩，而是被驱动器的电流保护钳制在最大值。MuJoCo 通过力矩范围把这一物理事实建模进来：一旦下发的控制量越界，引擎会自动将其截断到边界值。这样仿真的动力学响应就与真机保持一致，避免了仿真中"轻松完成"、真机中"力矩不足摔倒"的落差。可以说，力矩范围是让控制策略具备可迁移性的第一道保险。

提示语：帮我写一个脚本叫12_T800_iterate_motors.py，遍历t800身上的每个电机，然后打印他的ctrlrange。

"""
12_T800_iterate_motors.py
遍历 T800 机器人的所有电机(actuator), 打印名称、对应关节、gear、ctrlrange
运行: D:/miniconda3/envs/t800/python.exe 12_T800_iterate_motors.py
"""
import mujoco
model = mujoco.MjModel.from_xml_path("t800.xml")
print(f"电机(actuator)总数: {model.nu}")
print("-" * 80)
print(f"{'#':>3}  {'电机名称':<30} {'关节':<26} {'gear':>6} {'ctrlrange':>16}")
print("-" * 80)
for i in range(model.nu):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
jid = int(model.actuator_trnid[i, 0])
jname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, jid)
gear = float(model.actuator_gear[i, 0])
lo = float(model.actuator_ctrlrange[i, 0])
hi = float(model.actuator_ctrlrange[i, 1])
print(f"{i:>3}  {name:<30} {jname:<26} {gear:>6.1f} [{lo:>6.1f}, {hi:>6.1f}]")

从打印的信息来看，有的电机力矩范围大，说明功率大，有的力矩范围小，说明功率小。当我们去组装机器人时，就可以根据力矩范围选择电机。

第二章：力矩控制案例

提示语：帮我写一个新的脚本叫13_T800_ctrl_clamp.py，给每个电机输入力矩的控制值为10000，打印真实的控制值，来观察clamp函数是否生效。

"""
13_T800_ctrl_clamp.py
参考 11_T800_pd.py 的 PD 结构, 但去掉手动 np.clip,
验证 MuJoCo 内置 ctrllimited 是否自动 clamp
- 故意放大 PD 增益, 让输出的 tau 远超 ctrlrange
- 对比 data.ctrl (写入值) vs data.actuator_force (MuJoCo 实际输出)
运行: D:/miniconda3/envs/t800/python.exe 13_T800_ctrl_clamp.py
"""
import numpy as np
import mujoco
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
nu = model.nu
收集每个 actuator 对应的关节 qpos/qvel 地址 (同 11_T800_pd.py)
qposadr = np.zeros(nu, dtype=int)
dofadr = np.zeros(nu, dtype=int)
for i in range(nu):
jid = int(model.actuator_trnid[i, 0])
qposadr[i] = int(model.jnt_qposadr[jid])
dofadr[i] = int(model.jnt_dofadr[jid])
故意放大增益, 让 PD 输出远超 ctrlrange (例如 10000+)
Kp = np.full(nu, 100000.0)
Kd = np.full(nu, 1000.0)
q_des = np.zeros(nu)
dq_des = np.zeros(nu)
ctrl_range = model.actuator_ctrlrange.copy()
给一个初始扰动, 让 PD 产生大力矩
data.qpos[qposadr] = 0.5  # 0.5 rad 偏差
mujoco.mj_forward(model, data)
print("验证 MuJoCo 内置 clamp (ctrllimited) 是否生效")
print(f"故意放大增益: Kp={Kp[0]}, Kd={Kd[0]}, 初始扰动=0.5 rad")
print(f"未使用 np.clip (手动 clamp), 直接写入 data.ctrl")
print(f"电机总数: {nu}")
print("-" * 95)
print(f"{'#':>3}  {'电机名称':<28} {'ctrlrange':>16} {'写入ctrl':>12} {'实际force':>12} {'被clamp':>8}")
print("-" * 95)
单步仿真, 触发 PD 输出
q = data.qpos[qposadr]
dq = data.qvel[dofadr]
tau = Kp * (q_des - q) + Kd * (dq_des - dq)  # 无 np.clip!
data.ctrl[:] = tau
mujoco.mj_step(model, data)
all_clamped = True
for i in range(nu):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
lo = float(ctrl_range[i, 0])
hi = float(ctrl_range[i, 1])
ctrl_in = float(data.ctrl[i])
force = float(data.actuator_force[i])
clamped = abs(force) < abs(ctrl_in)
if not clamped:
all_clamped = False
mark = "✓" if clamped else "✗"
print(f"{i:>3}  {name:<28} [{lo:>6.0f},{hi:>6.0f}] "
f"{ctrl_in:>12.1f} {force:>12.1f} {mark:>8}")
print("-" * 95)
print(f"MuJoCo 内置 clamp 是否全部生效: {'是' if all_clamped else '否'}")
print()
print("结论: 即使去掉 11_T800_pd.py 中的 np.clip,")
print("      MuJoCo 仍会按 ctrlrange 自动限制 actuator_force,")
print("      因此 11_T800_pd.py 里的 np.clip 是冗余的安全双保险。")

即使填入的力矩参数超过了电机的最大范围，电机也会将其截断至输出范围内。后续我们在调试人形机器人，ctrlrange是保证硬件不损坏的前提。如果代码中不做ctrlrange限制，在仿真环境中机器人跑跳效果很好，但是如果部署在真实环境中，机器人硬件可能会瞬间损坏，原地爆炸。

第三章：手动控制T800

提示语：帮我写一个脚本叫14_T800_manual.py，我需要手动拖动，机器人的20多个关节，来控制机器人运动。给我提供最简单的教学代码

import mujoco
import mujoco.viewer
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
关闭重力，机器人不会倒，方便手动拖动关节摆姿势
model.opt.gravity[:] = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
# 不覆盖 data.ctrl，用户可通过两种方式手动控制：
# 方式1：双击机器人某个部位，用鼠标拖动（会施加弹簧力拉动机器人）
# 方式2：在 Viewer 右侧 Control 面板拖动 25 个滑块（每个对应一个关节的力矩）
mujoco.mj_step(model, data)
viewer.sync()

总结

本文围绕 T800 人形机器人的底层控制原理，系统梳理了电机力矩范围（ctrlrange）的获取、约束与验证机制。核心要点如下：

力矩范围的物理意义：真实电机的输出力矩受驱动器电流和电机减速比限制，存在天然上限。MuJoCo 通过 ctrlrange 建模这一物理约束，避免仿真中出现"违背物理"的爆发力，保证仿真动力学响应与真机一致。
获取力矩范围：通过 12_T800_iterate_motors.py 遍历所有电机，打印名称、对应关节、gear 和 ctrlrange，可据此了解各关节的功率大小，为电机选型与机器人组装提供依据。
内置 clamp 验证：13_T800_ctrl_clamp.py 故意放大 PD 增益并去掉手动 np.clip，验证 MuJoCo 内置的 ctrllimited 会自动将越界的控制量截断到 ctrlrange 范围内，说明手动 np.clip 是冗余的安全双保险。
手动控制实践：14_T800_manual.py 通过关闭重力并借助 Viewer 的鼠标拖动或 Control 面板滑块，实现对 20 多个关节的直观手动控制，便于调试与摆姿。
工程启示：ctrlrange 是保证硬件不损坏的前提。若仿真中不做限制，策略迁移到真机时可能导致硬件瞬间损坏，因此务必在控制链路中保留这一约束。

掌握力矩范围的获取与自动截断机制，是让控制策略从仿真平滑迁移到真机的关键一步，也是后续调试人形机器人的重要基础。建议读者动手运行三个脚本，亲身体验从"获取范围"到"验证截断"再到"手动控制"的完整闭环。