具身智能机器人实战教程——模块2（3）
·
模块 2：认识 T800 机器人仿真模型
第一章：两个核心坐标系
第二章：结合T800人形机器人理解坐标系
文章目录
前言
做过人形机器人仿真开发的朋友，大概率踩过坐标系的坑：明明写的控制代码逻辑没问题，机器人肢体却乱飞、姿态错乱、碰撞检测异常。很多时候不是动力学算法出错，而是分不清相对坐标（局部坐标）和绝对坐标（世界坐标）。
本课时以 T800 人形机器人模型为载体，讲解 MuJoCo 仿真环境下两套坐标系的含义、API 读取方式、适用场景，以及二者之间的转换逻辑，是后续 PD 控制、AI 策略生成、人形行走任务的前置基础。
第一章：两个核心坐标系
1.1 绝对坐标（世界坐标系 World Frame）
世界坐标系是 MuJoCo 仿真环境里固定不变的全局参考框架，原点一般在仿真地面，Z 轴竖直向上，X 向前，Y 向左，遵循右手坐标系规则。
- 含义：所有 link/body 相对于仿真场景地面的位置姿态，不受机器人自身转动、关节摆动影响。
- 作用：描述机器人整体在环境中的位置、双脚落地位置、机器人和场景物体（木块、障碍物）之间的空间关系。
- MuJoCo 数据接口：
# body的世界坐标位置
body_world_pos = data.xpos[body_id]
# body的世界旋转矩阵
body_world_mat = data.xmat[body_id].reshape(3,3)
# body的世界四元数姿态
body_world_quat = data.xquat[body_id]
xpos/xmat/xquat存储的都是绝对世界坐标系下的位姿，仿真每一步主循环会自动更新这些数值
1.2相对坐标（局部坐标系 Local Frame）
相对坐标是以 ** 父 body（父连杆）** 作为参考基准，每个连杆、关节都拥有自己的局部坐标系。
- 含义：子部件相对于父连杆的偏移、旋转。T800 机器人是树形运动学树，躯干是根节点，大腿相对于躯干、小腿相对于大腿，全部使用相对坐标描述。
- 作用：机器人模型 MJCF 文件定义时，body 的
pos属性默认就是相对父 body 的局部坐标；关节运动输出、PD 控制目标位置，大多是在局部坐标系下表达。 - MuJoCo 数据接口：
# qpos里面关节角度，是关节自身局部坐标系的转动量
joint_qpos = data.qpos[joint_id]
⚠️重点区分：MJCF 配置文件写的 pos 是相对父物体；仿真运行时 data.xpos 返回的是世界绝对坐标，二者不要混淆MuJoCo。
一张表快速对比
| 维度 | 绝对坐标（世界坐标） | 相对坐标（局部坐标） |
|---|---|---|
| 参考基准 | 仿真环境地面，固定不变 | 父连杆 body，随父部件运动变化 |
| 获取接口 | data.xpos / data.xquat | qpos、模型 mjcf 中的 pos 属性 |
| 典型用途 | 判断机器人是否摔倒、与环境物体交互 | 关节控制、PD 控制器目标设置、模型建模 |
| 变化特性 | 机器人全身移动就会改变 | 父不动，子部件才会改变 |
第二章：结合 T800 人形机器人理解坐标系
2.1T800机器人的坐标系
T800 人形机器人拥有大量 link（连杆）与 joint（关节），整棵运动学树从躯干根节点向下分支：躯干→大腿→小腿→足部；躯干→大臂→小臂→手部。
- 躯干的世界坐标：读取躯干 body 的
xpos，可以直接拿到 T800 机器人整个人在仿真地面上的三维位置。机器人向前走，躯干的 x 轴世界坐标就持续变大。 - 大腿相对躯干坐标：大腿连杆的位置，是相对于躯干的局部坐标系。当机器人躯干发生倾斜旋转，大腿的局部坐标基准也跟着旋转。
- 足部的两套坐标
- 绝对坐标：脚在地面上的落点，用来做落地检测、判断步长；
- 相对坐标：脚相对于躯干的偏移，用于逆运动学求解、规划腿部摆动轨迹。
利用代码先来查看一下mujoco仿真平台下T800各个关节的坐标。
提示语：帮我编写一个脚本叫07_T800_body_pos.py用于获取机器人每个body的body pos，并且把他们打印出来
"""
07_T800_body_pos.py
获取并打印 T800 机器人每个 body 的位置 (x, y, z)
- body_pos: XML 中定义的相对父 body 的偏移
- xpos: 仿真计算后的世界坐标 (需 mj_forward 后才有值)
运行: D:/miniconda3/envs/t800/python.exe 07_T800_body_pos.py
"""
import mujoco
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
mujoco.mj_forward(model, data) # 计算运动学, 填充 xpos
print(f"body 总数: {model.nbody}")
print("-" * 78)
print(f"{'#':>3} {'名称':<28} {'相对父body pos (x,y,z)':<28} {'世界坐标 (x,y,z)':<24}")
print("-" * 78)
for i in range(model.nbody):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i) or "(world)"
rel = model.body_pos[i] # 相对父 body 的偏移
world = data.xpos[i] # 世界坐标系下的位置
print(f"{i:>3} {name:<28} "
f"({rel[0]:7.3f},{rel[1]:7.3f},{rel[2]:7.3f}) "
f"({world[0]:7.3f},{world[1]:7.3f},{world[2]:7.3f})")
###
在Anaconda Prompt软件中执行这些命令
###
(base) C:\Users\qx>conda activate t800 ##激活虚拟环境
(t800) C:\Users\qx>cd Desktop\T800\code
(t800) C:\Users\qx\Desktop\T800\code>cd ../
(t800) C:\Users\qx\Desktop\T800>cd code2 ##定位到代码存放位置
(t800) C:\Users\qx\Desktop\T800\code2>python 07_T800_body_pos.py ##执行代码
2.2坐标系使用情况
实际开发中，经常需要互相转换：
- 拿到世界坐标系下目标点，需要转换到机器人局部坐标系，再给到关节控制器；
- 读取关节局部运动数据，需要转换为世界坐标，判断肢体有没有碰撞地面。
MuJoCo 内部已经提供完整变换矩阵，通过data.xmat旋转矩阵，就可以完成局部↔世界坐标的矢量变换。
本课时只建立概念，具体矩阵运算会在后续课时的仿真调试、二次开发环节实战演示。
✅使用绝对坐标场景
- 判断机器人是否摔倒：读取躯干、头部的世界坐标高度；
- 环境交互任务：T800 踢木块、手持物体走路，计算机器人身体和场景物体之间距离；
- 观测空间构建（后面 AI 强化学习会用到）：获取机器人各个部位在全局环境中的位置作为观测输入。
✅使用相对坐标场景
- PD 控制写关节目标，关节驱动都是基于局部坐标系；
- MJCF 模型编写、修改连杆位置；
- 机器人本体内部运动规划，肢体之间的相对摆动。
我们可以结合2.1中获取的机器人坐标生成机器人骨架
-----------------------------------------------------------------------------
0 world ( 0.000, 0.000, 0.000) ( 0.000, 0.000, 0.000)
1 LINK_BASE ( 0.000, 0.000, 1.030) ( 0.000, 0.000, 1.030)
2 LINK_HIP_PITCH_L ( 0.000, 0.079, 0.000) ( 0.000, 0.079, 1.030)
3 LINK_HIP_ROLL_L ( 0.005, 0.040, -0.108) ( 0.005, 0.119, 0.922)
4 LINK_HIP_YAW_L ( 0.009, 0.000, -0.081) ( 0.014, 0.119, 0.842)
5 LINK_KNEE_PITCH_L ( -0.038, 0.000, -0.288) ( -0.024, 0.119, 0.554)
6 LINK_ANKLE_PITCH_L ( -0.003, 0.000, -0.439) ( -0.026, 0.119, 0.115)
7 LINK_ANKLE_ROLL_L ( 0.001, 0.000, -0.038) ( -0.025, 0.119, 0.077)
8 LINK_FOOT_L ( 0.000, 0.000, 0.000) ( -0.025, 0.119, 0.077)
9 LINK_HIP_PITCH_R ( 0.000, -0.079, 0.000) ( 0.000, -0.079, 1.030)
10 LINK_HIP_ROLL_R ( 0.005, -0.040, -0.108) ( 0.005, -0.119, 0.922)
11 LINK_HIP_YAW_R ( 0.009, 0.000, -0.081) ( 0.014, -0.119, 0.842)
12 LINK_KNEE_PITCH_R ( -0.038, 0.000, -0.288) ( -0.024, -0.119, 0.554)
13 LINK_ANKLE_PITCH_R ( -0.003, 0.000, -0.439) ( -0.026, -0.119, 0.115)
14 LINK_ANKLE_ROLL_R ( 0.001, 0.000, -0.038) ( -0.025, -0.119, 0.077)
15 LINK_FOOT_R ( 0.000, 0.000, 0.000) ( -0.025, -0.119, 0.077)
16 LINK_WAIST_YAW ( 0.000, 0.000, 0.104) ( 0.000, 0.000, 1.134)
17 LINK_SHOULDER_PITCH_L ( -0.011, 0.163, 0.279) ( -0.011, 0.163, 1.413)
18 LINK_SHOULDER_ROLL_L ( 0.000, 0.063, -0.015) ( -0.011, 0.226, 1.397)
19 LINK_SHOULDER_YAW_L ( 0.000, 0.009, -0.083) ( -0.011, 0.234, 1.315)
20 LINK_ELBOW_PITCH_L ( 0.002, 0.014, -0.151) ( -0.009, 0.248, 1.164)
21 LINK_ELBOW_YAW_L ( -0.002, 0.009, -0.132) ( -0.011, 0.257, 1.032)
22 LINK_WRIST_END_L ( 0.029, 0.012, -0.132) ( 0.018, 0.270, 0.900)
23 LINK_SHOULDER_PITCH_R ( -0.011, -0.163, 0.279) ( -0.011, -0.163, 1.413)
24 LINK_SHOULDER_ROLL_R ( 0.000, -0.063, -0.015) ( -0.011, -0.226, 1.397)
25 LINK_SHOULDER_YAW_R ( 0.000, -0.009, -0.083) ( -0.011, -0.234, 1.315)
26 LINK_ELBOW_PITCH_R ( 0.002, -0.014, -0.151) ( -0.009, -0.248, 1.164)
27 LINK_ELBOW_YAW_R ( -0.002, -0.009, -0.132) ( -0.011, -0.257, 1.032)
28 LINK_WRIST_END_R ( 0.029, -0.012, -0.132) ( 0.018, -0.270, 0.900)
29 LINK_HEAD_PITCH ( -0.011, 0.000, 0.386) ( -0.011, 0.000, 1.520)
30 LINK_HEAD_YAW ( 0.000, 0.000, 0.040) ( -0.011, 0.000, 1.560)提示语：基于上面的世界坐标信息，帮我写一个matplotlib的代码叫08_T800_body_xyz.py，展示机器人的骨架
这个命令执行需要安装一个包，名为matplotlib，可以执行下面这个命令安装，或者让trae给你安装
###
在Anaconda Prompt软件中执行这些命令
###
conda activate t800
pip install matplotlib"""
08_T800_body_xyz.py
用 matplotlib 3D 可视化 T800 机器人的骨架 (基于 body 世界坐标 + 父子连线)
运行: D:/miniconda3/envs/t800/python.exe 08_T800_body_xyz.py
"""
import mujoco
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D # noqa: F401
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
# 收集每个 body 的世界坐标
n = model.nbody
pos = np.zeros((n, 3))
names = []
for i in range(n):
pos[i] = data.xpos[i]
names.append(mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i) or "world")
# 父子关系构成骨架线
edges = []
for i in range(1, n): # 跳过 world(0)
pid = int(model.body_parentid[i])
edges.append((pid, i))
# 按部位分组着色
LEG_L = {2, 3, 4, 5, 6, 7, 8}
LEG_R = {9, 10, 11, 12, 13, 14, 15}
ARM_L = {17, 18, 19, 20, 21, 22}
ARM_R = {23, 24, 25, 26, 27, 28}
HEAD = {29, 30}
TORSO = {1, 16}
def color_of(i):
if i in LEG_L:
return "blue"
if i in LEG_R:
return "orange"
if i in ARM_L:
return "green"
if i in ARM_R:
return "red"
if i in HEAD:
return "purple"
return "black" # torso / base / world
fig = plt.figure(figsize=(6, 8))
ax = fig.add_subplot(111, projection="3d")
# 画骨架连线
for a, b in edges:
color = color_of(b) # 以子节点的部位上色
ax.plot([pos[a, 0], pos[b, 0]],
[pos[a, 1], pos[b, 1]],
[pos[a, 2], pos[b, 2]],
color=color, linewidth=2.0)
# 画关节点
for i in range(1, n): # 跳过 world
ax.scatter(pos[i, 0], pos[i, 1], pos[i, 2],
color=color_of(i), s=40, edgecolors="black", linewidths=0.5, depthshade=True)
# 标注关键关节名
labels = {1: "BASE", 16: "WAIST", 5: "KNEE_L", 12: "KNEE_R",
17: "SHOULDER_L", 23: "SHOULDER_R", 22: "WRIST_L", 28: "WRIST_R",
30: "HEAD"}
for idx, label in labels.items():
ax.text(pos[idx, 0], pos[idx, 1], pos[idx, 2] + 0.03,
label, fontsize=7, color="black")
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_zlabel("Z (m)")
ax.set_title("T800 Robot Skeleton (Body Positions)")
# 等比例 + 朝向
ax.set_box_aspect((1, 1, 1.5))
all_pts = pos[1:] # 不含 world
ax.set_xlim(all_pts[:, 0].min() - 0.3, all_pts[:, 0].max() + 0.3)
ax.set_ylim(all_pts[:, 1].min() - 0.3, all_pts[:, 1].max() + 0.3)
ax.set_zlim(0, all_pts[:, 2].max() + 0.1)
ax.view_init(elev=15, azim=-75)
plt.tight_layout()
plt.show()
总结
本节课是从看懂模型走向写运动控制代码的关键分水岭。后面 PD 控制、强化学习观测空间、踢木块、手持物体行走任务，全部依赖这套坐标系知识。
- 绝对坐标：以仿真世界地面为基准，做环境交互、全局状态观测；
- 相对坐标：以父连杆为基准，做关节控制、机器人本体运动规划；
- MuJoCo 中
data.xpos/xquat获取世界坐标；qpos、MJCF pos 字段代表局部相对坐标； - 人形仿真大量 bug 根源，都是坐标系混用，调试时优先打印坐标做校验。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
6
0- 0
已为社区贡献8条内容
所有评论(0)