MuJoCo教程-04 机器人正运动学

MuJoCo教程-04 机器人正运动学

编程进阶社

正运动学（Forward Kinematics, FK）是机器人学最基础的概念：给定各关节角度，计算末端执行器在世界坐标系中的位置和姿态。我们不需要求解复杂的 DH 参数。MuJoCo 已经帮我们做了这些计算，但理解其原理和学会使用相关 API 是掌握机器人仿真的关键。

正运动学

但是实际计算的时候，也可以采用正向动力学函数mj_forward来计算正运动学。

正向动力学

所谓正向动力学，是指根据力矩计算加速度，然后对加速度积分计算得到速度和位置信息。

仔细看下mj_forward代码，是通过mj_forwardSkip函数来计算的。

正向动力学源码

而再仔细阅读下mj_fwdPosition函数，发现它实际上是通过mj_forward函数来计算的。

正向动力学源码

mj_fwdPosition函数则是调用函数mj_forward来计算的。

正向动力学源码

最后总结下，就是也可以通过函数mj_forward来计算正运动学。

给定 qpos (关节角度) & qvel (关节速度)
│
├──► 1. mj_fwdPosition ───【这里就在计算正向运动学！】
│ └─ 递归计算所有 body、geom、site 在 3D 空间中的位置和姿态 (xpos, xmat, site_xpos)
│ └─ 进行碰撞检测 (Collision detection)
│
├──► 2. mj_fwdVelocity ───【速度运动学】
│ └─ 计算空间线速度、角速度以及雅可比矩阵 (Jacobian)
│
├──► 3. mj_fwdActuation ───【执行器与力计算】
│ └─ 计算电机输出力矩、阻尼力、重力项
│
└──► 4. mj_fwdAcceleration ──【正向动力学核心】
└─ 求解约束 (Contacts/Joint Limits)，最终算出关节加速度 qacc

在 MuJoCo 中，每个 body（刚体）和 site（附着点）都有自己的局部坐标系。通过mj_forward计算后，MuJoCo 会填充data中的全局位姿信息：

字段

含义

形状

data.body_xpos[body_id]

刚体在世界坐标系中的位置 (x, y, z)

(3,)

data.body_xmat[body_id]

刚体的旋转矩阵（列主序, 3x3）

(9,)

data.body_xquat[body_id]

刚体的四元数 (w, x, y, z)

(4,)

data.site_xpos[site_id]

site 在世界坐标系中的位置

(3,)

data.site_xmat[site_id]

site 的旋转矩阵

(9,)

这些变量名的命名规则：

• •xpos=xformposition（变换后的位置）

• •xmat=xformmatrix（变换后的旋转矩阵）

• •xquat=xformquaternion（变换后的四元数）

import mujoco
import numpy as np
加载模型
model = mujoco.MjModel.from_xml_path(&;scene.xml&;)
data = mujoco.MjData(model)
设置关节角度并计算正运动学
data.qpos[:] = model.key(&;home&;).qpos
mujoco.mj_forward(model, data)
获取 body 和 site 在世界坐标系中的位置
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, &;shoulder_link&;)
print(f&;shoulder_link 世界位置: {data.body_xpos[body_id]}&;)
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, &;attachment_site&;)
print(f&;末端 site 世界位置: {data.site_xpos[site_id]}&;)

MuJoCo 使用四元数表示刚体的姿态，优点是避免万向锁且计算高效。四元数格式为(w, x, y, z)。

将四元数转换为旋转矩阵：

 获取某个 body 的四元数
quat = data.body_xquat[body_id]  (w, x, y, z)
转换为 3x3 旋转矩阵
mat = np.zeros(9)
mujoco.mju_quat2Mat(mat, quat)
mat = mat.reshape(3, 3)  列主序 (column-major)
print(f&;旋转矩阵:\n{mat}&;)

将旋转矩阵转换为欧拉角（roll, pitch, yaw）：

def mat2euler(mat_flat):
&;&;&;将 MuJoCo 列主序 3x3 旋转矩阵转换为欧拉角 (roll, pitch, yaw) 弧度&;&;&;
R = mat_flat.reshape(3, 3)
 提取 RPY (ZYX 欧拉角，对应 MuJoCo 内部的约定)
 R = Rz(yaw) * Ry(pitch) * Rx(roll)
sy = np.sqrt(R[0, 0]**2 + R[1, 0]**2)
if sy > 1e-6:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0.0
return np.array([roll, pitch, yaw])
使用
quat = data.site_xquat[site_id]  或 data.body_xquat[...]
mat = np.zeros(9)
mujoco.mju_quat2Mat(mat, quat)
rpy = mat2euler(mat)
print(f&;欧拉角 (RPY): {np.degrees(rpy)} 度&;)

data.body_xmat和data.site_xmat存储的是3x3 旋转矩阵，按列主序 (column-major) 展平为一维数组（9个元素）。

 xmat 的索引方式（列主序）:
 数组: [m0, m1, m2, m3, m4, m5, m6, m7, m8]
 矩阵:
 m0 m3 m6 第0列 第1列 第2列
 m1 m4 m7
 m2 m5 m8
每一列是 body/site 局部坐标系 X, Y, Z 轴在世界坐标系中的方向向量
mat = data.site_xmat[site_id].reshape(3, 3, order=&;F&;)
 mat[:, 0] → 局部 X 轴在世界坐标系中的方向
 mat[:, 1] → 局部 Y 轴在世界坐标系中的方向
 mat[:, 2] → 局部 Z 轴在世界坐标系中的方向
print(f&;X轴方向: {mat[:, 0]}&;)
print(f&;Y轴方向: {mat[:, 1]}&;)
print(f&;Z轴方向: {mat[:, 2]}&;)

旋转矩阵是正交矩阵，满足R^T = R^{-1}，det(R) = 1。

使用 MuJoCo API 计算正运动学

正运动学最直接的方式就是利用 MuJoCo 内置的mj_forward：

import mujoco
import numpy as np
model = mujoco.MjModel.from_xml_path(&;scene.xml&;)
data = mujoco.MjData(model)
设置关节角度
data.qpos[:] = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]
执行正运动学计算
mujoco.mj_forward(model, data)
获取末端执行器位置
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, &;attachment_site&;)
ee_pos = data.site_xpos[site_id].copy
print(f&;末端位置: {ee_pos}&;)
遍历所有刚体，获取每个刚体的世界位置
for i in range(model.nbody):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
pos = data.body_xpos[i]
print(f&;{name}: x={pos[0]:.3f}, y={pos[1]:.3f}, z={pos[2]:.3f}&;)

核心流程：

1. 1. 设置data.qpos（关节位置）

2. 2. 调用mujoco.mj_forward(model, data)完成正运动学计算

3. 3. 从data中读取各刚体和 site 的全局位姿

理解正运动学的底层原理：沿运动链从基座逐级累积变换矩阵。每个刚体的位姿 = 父刚体位姿 * 局部关节变换。

def manual_forward_kinematics(model, data):
&;&;&;手动沿运动链递推计算各 body 的位姿（演示原理）&;&;&;
from scipy.spatial.transform import Rotation as R
body_positions = {}
body_orientations = {}
def traverse(body_id, parent_pos, parent_quat):
 获取 body 在父坐标系中的位置
local_pos = model.body_pos[body_id].copy
local_quat = model.body_quat[body_id].copy
如果有对应的关节，叠加关节旋转
jnt_id = model.body_jntadr[body_id]
if jnt_id >=0 and model.jnt_type[jnt_id] == mujoco.mjtJoint.mjJNT_HINGE:
q = data.qpos[model.jnt_qposadr[jnt_id]]
axis = model.jnt_axis[jnt_id]
 绕关节轴的旋转四元数
half = q / 2.0
jnt_rot = np.array([np.cos(half), axis[0]*np.sin(half),
axis[1]*np.sin(half), axis[2]*np.sin(half)])
 叠加：先局部旋转，再局部平移
local_quat = mujoco_mul_quat(local_quat, jnt_rot)
累积变换到世界坐标系
world_quat = mujoco_mul_quat(parent_quat, local_quat)
 旋转父坐标系中的位置偏移
world_pos = parent_pos + quat_rotate(parent_quat, local_pos)
body_positions[body_id] = world_pos
body_orientations[body_id] = world_quat
递归子刚体
for child_id in range(model.nbody):
if model.body_parentid[child_id] == body_id:
traverse(child_id, world_pos, world_quat)
从根 body (world, id=0) 开始
for child_id in range(model.nbody):
if model.body_parentid[child_id] == 0:
traverse(child_id, np.zeros(3), np.array([1.0, 0.0, 0.0, 0.0]))
return body_positions, body_orientations

虽然实际开发中我们总是使用mj_forward，但理解手动递推能帮助你：调试模型问题、在无 MuJoCo 环境下做离线计算、设计自己的运动学求解器。

正弦波关节运动

让 UR5e 所有关节以不同频率和幅度做正弦运动，观察末端轨迹：

import mujoco
import numpy as np
import csv
model = mujoco.MjModel.from_xml_path(&;scene.xml&;)
data = mujoco.MjData(model)
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, &;attachment_site&;)
dt = 0.01
duration = 10.0
steps = int(duration / dt)
trajectory =   [(time, x, y, z), ...]
for step in range(steps):
t = step * dt
各关节以不同频率和幅度做正弦运动
data.qpos[0] = 0.5 * np.sin(2 * np.pi * 0.2 * t)  shoulder_pan
data.qpos[1] = -1.5 + 0.3 * np.sin(2 * np.pi * 0.3 * t)  shoulder_lift
data.qpos[2] = 1.5 + 0.3 * np.sin(2 * np.pi * 0.25 * t)  elbow
data.qpos[3] = -1.5 + 0.2 * np.sin(2 * np.pi * 0.35 * t)  wrist_1
data.qpos[4] = -1.5 + 0.2 * np.sin(2 * np.pi * 0.4 * t)  wrist_2
data.qpos[5] = 0.5 * np.sin(2 * np.pi * 0.15 * t)  wrist_3
计算正运动学
mujoco.mj_forward(model, data)
记录末端位置
pos = data.site_xpos[site_id].copy
trajectory.append((t, pos[0], pos[1], pos[2]))
if step % 100 == 0:
print(f&;步骤 {step}/{steps}, 末端位置: ({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})&;)

将记录的末端轨迹可视化：

import numpy as np
import matplotlib.pyplot as plt
读取轨迹数据
data_array = np.array(trajectory)
t = data_array[:, 0]
x = data_array[:, 1]
y = data_array[:, 2]
z = data_array[:, 3]
fig = plt.figure(figsize=(12, 5))
子图1: 3D轨迹
ax1 = fig.add_subplot(1, 2, 1, projection=&;3d&;)
ax1.plot(x, y, z, linewidth=0.8, color=&;steelblue&;)
ax1.scatter(x[0], y[0], z[0], color=&;green&;, s=50, label=&;起点&;)
ax1.scatter(x[-1], y[-1], z[-1], color=&;red&;, s=50, label=&;终点&;)
ax1.set_xlabel(&;X (m)&;)
ax1.set_ylabel(&;Y (m)&;)
ax1.set_zlabel(&;Z (m)&;)
ax1.set_title(&;末端执行器 3D 轨迹&;)
ax1.legend
子图2: XYZ 时间序列
ax2 = fig.add_subplot(1, 2, 2)
ax2.plot(t, x, label=&;X&;, linewidth=0.8)
ax2.plot(t, y, label=&;Y&;, linewidth=0.8)
ax2.plot(t, z, label=&;Z&;, linewidth=0.8)
ax2.set_xlabel(&;时间 (s)&;)
ax2.set_ylabel(&;位置 (m)&;)
ax2.set_title(&;末端位置随时间变化&;)
ax2.legend
ax2.grid(True, alpha=0.3)
plt.tight_layout
plt.savefig(&;trajectory_plot.png&;, dpi=150)
plt.show

通过改变各关节的正弦波参数（频率、振幅、偏移），可以生成不同的工作空间轨迹。这直观地展示了正运动学的核心思想：关节空间 → 笛卡尔空间的映射关系。

实践任务

运行提供的三个 Python 脚本，理解正运动学计算流程：

1. 1.01_forward_kinematics.py— 输出各刚体和末端 site 在世界坐标系中的位姿，理解 MuJoCo API

2. 2.02_sine_motion_trajectory.py— 驱动关节做正弦运动，记录末端轨迹到 CSV

3. 3.03_plot_trajectory.py— 将轨迹可视化为 3D 图

以上代码都是由AI生成，自我测试。

python 01_forward_kinematics.py --view

python 02_sine_motion_trajectory.py --view

python 03_plot_trajectory.py --view

以上就是个人的理解，如有错误，多多指教！

大概如此，仅供参考！