2. 正运动学

上一章我们只控一个关节。这一章把关节串起来——给出 n 个关节角，求末端执行器在世界坐标系下的 6-DoF 位姿。正运动学是逆运动学、步态规划、视觉-动作对齐的前提。

本章目标

• 能写出齐次变换矩阵 T = [R p; 0 1]
• 能从 URDF / MJCF 描述里读出每个关节的父坐标系与旋转轴
• 能用 Python + NumPy 实现一个 3-DoF 机械臂的 FK，并在 MuJoCo 里验证末端位置误差 < 1e-6
• 能画出雅可比矩阵（为第 3 章逆解做准备）

前置阅读

• 第 1 章 执行器与 PD 控制
• 想更直观理解，可以点击 交互模式 学习

2.1 机械臂组成

在推导出正运动学公式之前，我们先铺垫一些概念，首先是机械臂的运动组成。机械臂的运动由两个关键元素组成：关节(Joint)和连杆(Link)。此外还有一个特殊的连杆叫末端执行器(End Effector)，它是机械臂的"手"，在拓扑上是最后一个连杆，但功能特殊，所以单独命名。

• 关节(Joint)：连接相邻两个连杆，提供一个或多个自由度(DoF)。最常见的是旋转关节(revolute joint)，提供一个绕固定轴的转角 ；另一类是移动关节(prismatic joint)，提供一个沿固定轴的位移 。一个 -DoF 机械臂的状态由所有关节变量构成的向量 完全决定。
• 连杆(Link)：相邻两关节之间的刚体段，自身位姿仅由父关节决定，与子关节的状态无关。FK 计算中，连杆的作用是给出"父关节到子关节"的固定齐次变换，与几何外形（长度、惯量、碰撞体）无关。

此外，还有两个特殊的连杆：末端执行器(End Effector)和基座(Base)。末端执行器（简称末端）是机械臂的"手"，在拓扑上是最后一个连杆，但功能特殊，所以单独命名；基座是机械臂的根部，通常固定在地面或工作台上，也是一个特殊的连杆，虽然它没有父关节。三者在串联链条中的关系如 图 1 所示。

关节、连杆与末端执行器的串联关系。

通常我们关心末端的运动状态（位置、姿态、位姿），而末端的状态完全由所有关节的状态决定。正运动学(Forward Kinematics, FK)就是这样一个函数：输入是 个关节变量 ，输出是末端执行器相对于基座或世界坐标系的位姿 ，具体下文再展开说明。

2.2 坐标系

要表达"末端在哪、朝哪"，必须先有参照物——坐标系就是这个参照物。机械臂里常见的坐标系有四种，按"由全局到局部"的顺序排列：

• 世界坐标系(world frame)：通常记为 ，是整个仿真或现实场景的全局参考系，地板、桌面、目标物体的位置都在这套坐标系里给。
• 基座坐标系(base frame)：通常记为 ，是机械臂根部所在的坐标系。它要么与世界坐标系重合（机械臂固定在原点），要么与世界坐标系之间有一个已知的固定变换（机械臂装在移动底盘或工作台上）。
• 连杆坐标系(link frame)：每个连杆都有一个，记为 ，通常贴在该连杆的父关节上。它会随着前序关节转动而在世界里移动，是 FK 链式乘法的中间产物。
• 末端坐标系(end-effector frame)：记为 ，贴在末端执行器上，是我们真正关心的输出坐标系。

这些坐标系会沿运动链逐级传递，整体关系如 图 2 所示。

四种坐标系与 FK 的链式变换。

正运动学的核心就是把末端坐标系 中表示的几何量（典型如原点 ）搬到基座坐标系 或世界坐标系 中，具体下文再展开说明。

2.3 位姿

有了坐标系，我们就可以描述末端执行器的状态了。末端的状态通常用位置(position)和姿态(orientation)来刻画，其中位置刻画末端原点的坐标，即表示末端"在哪"，姿态刻画末端坐标轴的朝向，即表示末端"朝哪"。把两者合并到同一个变换中，就得到了位姿(pose)，是刚体状态的完整描述。

位置通常用一个三维向量 表示，如式 所示：

姿态描述末端坐标系相对参考坐标系的旋转状态，相比位置，表达方式更多样、也更复杂，常见有三种：

• 旋转矩阵(Rotation Matrix)：记为 ，是满足 与 的正交矩阵。它的三列对应末端坐标系 三个坐标轴在参考坐标系 下的单位向量，因此 既给出旋转的代数表示，又能直接读出"末端朝哪"。优点是合成只需矩阵乘、与齐次变换天然兼容；缺点是用 9 个数表示 3 个自由度，参数冗余。
• 欧拉角(Euler Angles)：用绕三个轴依次旋转的角度组合表示姿态，最常见的是 Roll（翻滚）、Pitch（俯仰）、Yaw（偏航）三个角。只需 3 个数、对人类直观；但当两个旋转轴对齐时会发生万向节死锁(gimbal lock)，瞬时丢失一个自由度，因此不适合做积分或插值。
• 四元数(Quaternion)：记为 且 ，用 4 个实数表示旋转。表达不直观，但全空间无奇异、数值稳定、插值平滑，是仿真器（MuJoCo / Isaac）和控制器内部最常用的姿态表示。

把位置 与姿态 并入同一个矩阵，便得到本章后续反复出现的齐次变换矩阵(homogeneous transformation matrix)，如式 所示：

其中 给出位置， 给出姿态。该矩阵的几何意义如 图 3 所示，即把末端坐标系 中表示的点变换到参考坐标系 中。

位置、姿态与位姿的几何关系。

正运动学(Forward Kinematics, FK)就是这样一个函数，如式 所示：

其中 是第 个关节的旋转角度， 是末端执行器相对于基座坐标系的位姿。正运动学的计算内容就是把每个关节的旋转叠加起来，得到末端的整体位姿。

2.4 齐次变换矩阵

本节我们详细展开一下齐次变换矩阵，即式 里那个"把末端坐标系 中表示的点变换到参考坐标系 中"的过程。

上一节末尾要把相邻坐标系之间的变换写成代数。这一节先用 图 4 建立直觉，再用几段代码验证：旋转做什么、平移做什么、合起来又是什么——读完会自然得到 §2.2 里那个 的形式。

旋转、平移与组合变换的几何直觉。旋转改变方向，平移改变位置，组合变换先把局部坐标转到参考坐标方向，再整体搬到参考坐标中的位置。

旋转：让向量绕原点转。 二维平面里一个点 绕原点逆时针转 度，直接用旋转矩阵 乘上去：

import numpy as np

def rot2(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s], [s, c]])

p = np.array([1.0, 0.0])
print(rot2(np.deg2rad(90))  @ p)   # [0., 1.]   转到 y 轴
print(rot2(np.deg2rad(180)) @ p)   # [-1., 0.]  转到 -x 轴

旋转只改变方向，不改变长度，原点也保持不动——单凭旋转没办法把 搬到 这种"原点都换了位置"的目标。

平移：整体搬一段距离。 平移在代数上就是加一个向量 ：

t = np.array([2.0, 1.0])
print(p + t)   # [3., 1.]

特征反过来：只改变位置，不改变方向。两件事合起来，足以描述刚体在平面里能做的全部运动——绕一个点转 + 整体挪一段。

组合：先旋转再平移。 写成代数就是课本里常见的形式：

也就是：坐标系 里的点 先用 旋到 的方向，再加上 原点在 里的位置 。代码验证：

R = rot2(np.deg2rad(90))    # B 相对 A 转了 90°
t = np.array([2.0, 1.0])    # B 原点在 A 里的位置
p_B = np.array([1.0, 0.0])  # 这个点在 B 里的坐标

p_A = R @ p_B + t
print(p_A)   # [2., 2.]

问题。 旋转是矩阵乘法，平移是向量加法，两件事拼不到同一个运算里。一旦串起来三个连杆，就会写成 这种嵌套——能写，但对人和代码都不友好。

齐次形式：把两件事压成一个矩阵乘。 给点补一维，把二维点 写成 ；把 和 拼成一个 3×3 矩阵：

def make_T(R, t):
    T = np.eye(3)
    T[:2, :2] = R
    T[:2,  2] = t
    return T

T = make_T(rot2(np.deg2rad(90)), np.array([2.0, 1.0]))
p_B_h = np.array([1.0, 0.0, 1.0])    # 末尾的 1 是齐次坐标
p_A_h = T @ p_B_h
print(p_A_h)                         # [2., 2., 1.]  和上面手算一致

3D 同样处理：点变成 ，矩阵变成 4×4，这就是 §2.2 提前预告过的齐次变换矩阵：

它把"位置 + 姿态 "合并成单一对象，运算上只剩矩阵乘。

两个关键性质：

• 可以一路乘下去。 个关节串起来的末端位姿就是 ，这一乘法正是 FK 的全部计算内容。
• 可逆且解析可写。，把世界坐标系下的目标搬到末端坐标系时会反复用到。

2.5 运动链

把基座、各连杆、末端执行器按"父→子"顺序连成一棵树（机械臂是单链，腿 / 手是分叉），就得到运动链(kinematic chain)。运动链上每条边对应一个关节，每个关节贡献一个齐次变换；FK 就是沿着这条链把所有齐次变换乘起来，如式 所示：

剩下的问题只有一个：怎么写出 ？目前主流有两种约定，分别是 DH 参数和 URDF / MJCF 父-子链式约定。

2.5.1 DH 参数

DH 参数（Denavit-Hartenberg parameters）是一种给串联机械臂选取连杆坐标系的约定。它的目标不是描述连杆的几何外形，而是把相邻两个连杆坐标系之间的刚体变换压缩成四个标量参数。给定第 个关节附近的坐标系后，Craig 教材常用的 modified DH 写法为式 ：

这里的 表示从坐标系 到坐标系 的变换。需要注意：DH 有 standard DH 和 modified DH 等不同记法，矩阵相乘顺序会不同；阅读论文、教材或代码时，必须先确认作者采用的是哪一种约定。本文只用上式说明 Craig modified DH 的思路，后续代码不依赖 DH 表。

这四个参数的几何含义如 图 5 所示。

Craig modified DH 四个参数的几何含义示意。图中只强调轴、距离和角度的定义关系，不代表具体机械臂的真实比例。

对应到参数表，可以写成：

参数	几何含义	旋转关节中的角色
	沿 方向，从 轴到 轴的距离	常与连杆长度相关，但不一定等于机械结构的外形长度
	绕 轴，从 轴转到 轴的角度	描述相邻关节轴之间的夹角
	沿 轴，从 轴到 轴的距离	旋转关节中通常为常量；移动关节中通常是变量
	绕 轴，从 轴转到 轴的角度	旋转关节的关节变量

Craig modified DH 参数含义

从计算角度看，DH 的价值在于：一行参数就能生成一个固定形式的 4×4 矩阵，整条机械臂仍然按 相乘。对于教材中的串联机械臂、解析 IK 推导和手工计算，这种表格化表达很高效。

但 DH 不是机器人模型文件的通用格式。URDF / MJCF 中的关节坐标系通常已经由建模工具、CAD 导出流程或仿真器约定好；这些坐标系不一定满足 DH 选系规则。把一个真实模型改写成 DH 表，往往需要重新放置中间坐标系，并逐个关节核对轴向、偏置和正方向。因此，本章后面的实现采用 URDF / MJCF 更自然的父-子链式写法。

2.5.2 URDF / MJCF 链

URDF / MJCF 不要求我们重新按 DH 规则摆坐标系。它采用更直接的父-子链式约定：机器人是一棵树，link 是树上的节点，joint 是连接两个节点的边。沿着 parent → child 的方向把每条边的变换乘起来，就得到末端位姿。

world → base → link1 → link2 → link3 → ... → end_effector
         joint1   joint2   joint3

读一个 <joint> 时，可以先把它拆成四个问题：

• parent：这条边从哪个连杆出发？
• child：这条边连到哪个连杆？
• origin：在父连杆坐标系里，关节原点放在哪里、朝向如何？
• axis：如果这是旋转关节或移动关节，它沿哪根轴运动？

URDF / MJCF 父-子链中这四个字段的关系如 图 6 所示。关键是：origin 是模型加载时就确定的固定偏置；axis 是运行时关节变量 作用的方向。

URDF / MJCF 父-子链式约定。一个 joint 连接一个 parent link 和一个 child link；origin 给出固定偏置，axis 给出关节运动方向。

仍然用 joint2 举例：

<!-- joint2 是 link1 -> link2 这条边；axis 表示运行时绕关节 z 轴旋转 -->
<joint name="joint2" type="revolute" axis="0 0 1">
  <!-- 固定偏置：从 link1 原点沿 x 走 0.3 m，姿态不变，到达 joint2 原点 -->
  <origin xyz="0.3 0 0" rpy="0 0 0"/>

  <!-- 这条边从 link1 出发 -->
  <parent link="link1"/>

  <!-- 绕 axis 转过当前关节角后，进入 link2 坐标系 -->
  <child  link="link2"/>
</joint>

将上述 XML 转换为齐次矩阵时，可以将其分解为固定变换和关节变换两部分：

1. 固定变换由 origin 给出，描述关节坐标系相对于父连杆坐标系的静态位姿。这里 xyz="0.3 0 0" 且 rpy="0 0 0"，因此固定变换为 。
2. 关节变换由 type、axis 和当前关节变量共同决定。这里 type="revolute" 且 axis="0 0 1"，因此关节角 对应绕 轴的旋转 。

因此，对于该 joint2，从 link1 坐标系到 link2 坐标系的变换为：

对第 个关节作同样的分解，可得到式 ：

其中 由 origin 决定，在模型加载后保持不变； 由关节类型、关节轴和当前关节变量决定。对于旋转关节，它表示绕 axis 的旋转；对于移动关节，它表示沿 axis 的平移。

本章后续实现采用这一表示方式。其优点是 FK 代码可以直接读取 URDF / MJCF 中的参数，无需先将模型手动转换为 DH 表。两种约定的对照见表 1。

维度	DH 参数	URDF / MJCF 父-子链
每个关节的描述	四个参数	一个 <origin> + 一个 axis + 一个
坐标系位置	由 DH 规则强制（常与机械结构错位）	自由放置（一般贴在父关节）
与仿真器衔接	需要手算转换	加载即用
适合场景	教材推导、解析式 IK	仿真、真机、可视化

DH 参数与 URDF / MJCF 父-子链式约定的对照

2.6 平面 3-DoF 手推

作为热身，先考虑最干净的平面 3-DoF 臂：三个连杆长度为 ，三个关节都绕 轴旋转，关节角分别为 。该几何模型如 图 7 所示。

平面 3-DoF 机械臂的几何分解。每一段连杆的方向角都是前面关节角的累计和，末端坐标由三段连杆在 x、y 方向上的投影相加得到。

推导时先确定每段连杆相对世界坐标系的方向角：

• 第 1 段方向角是 。
• 第 2 段的方向由前两个关节共同决定，所以方向角是 。
• 第 3 段的方向由三个关节共同决定，所以方向角是 。

一段长度为 、方向角为 的连杆，对末端 坐标的贡献是 ，对 坐标的贡献是 。把三段贡献相加，就得到式 ：

如果把末端姿态也一起写出来，这个平面例子其实还有一个很干净的结果：

也就是说，这个 3-DoF 平面臂的完整 FK 输出可以理解成 。下一章解析 IK 时，这个 会作为"末端朝向"重新出现。

注意这里关节角是逐级累加的——这是下一章解析 IK 能那么干净收敛的关键。

把它写成齐次矩阵形式，方便后面推广到 3D：

接下来需要说明的是：为什么链式矩阵相乘后，末端位置可以直接从结果矩阵中读取。

原因在于齐次变换矩阵始终保持如下结构：

右上角的 就是子坐标系原点在父坐标系中的位置。对这个平面 3-DoF 例子来说，连乘得到的 表示"末端坐标系相对于基座坐标系的位姿"，因此：

所以末端位置不是额外算出来的，而是已经存放在 的平移列中。代码里读 T[:3, 3]，读的正是这一列的前三个分量；平面问题里 ，所以前两项就是式 中的 。

2.7 NumPy 与 MuJoCo

先定义两个最基础的齐次变换构造器，后面所有 FK 代码都靠它们拼积木：

import numpy as np

def rot_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    T = np.eye(4)
    # 齐次矩阵左上角 3x3 存旋转；这里是绕 z 轴的二维平面旋转。
    T[:3, :3] = [[c, -s, 0],
                 [s,  c, 0],
                 [0,  0, 1]]
    return T

def trans(x, y, z):
    T = np.eye(4)
    # 齐次矩阵第 4 列前三项存平移向量，也就是子坐标系原点的位置。
    T[:3, 3] = [x, y, z]
    return T

把式 的链式乘法直接照抄成代码，就是 3-DoF 平面臂的 FK：

def fk_planar(thetas, L=(0.3, 0.25, 0.15)):
    t1, t2, t3 = thetas
    # 每一组 rot_z(theta_i) @ trans(L_i, 0, 0)
    # 对应一节“先绕关节转，再沿当前连杆 x 方向走 L_i”。
    T = rot_z(t1) @ trans(L[0], 0, 0) \
        @ rot_z(t2) @ trans(L[1], 0, 0) \
        @ rot_z(t3) @ trans(L[2], 0, 0)
    return T     # 末端位置直接读 T[:3, 3]；末端姿态在 T[:3, :3] 里。

然后用 MuJoCo 的 mj_forward 作为"标准答案"交叉校验——给 100 组随机关节角，比较末端位置的最大误差。

先写一份与 fk_planar 默认参数对齐的最小 MJCF，存成 planar_3dof.xml：

planar_3dof.xml

<mujoco model="planar_3dof">
  <!-- 本例只校验几何位姿；设为零重力，让模型文件保持最小配置 -->
  <option gravity="0 0 0"/>

  <worldbody>
    <body name="link1" pos="0 0 0">
      <!-- 三个关节都绕 z 轴旋转，对齐 fk_planar 里的 rot_z(theta_i) -->
      <joint name="j1" type="hinge" axis="0 0 1"/>
      <geom type="capsule" fromto="0 0 0 0.3 0 0" size="0.02"/>

      <!-- pos 是 link2 相对 link1 的固定偏置，对应 trans(L1, 0, 0) -->
      <body name="link2" pos="0.3 0 0">
        <joint name="j2" type="hinge" axis="0 0 1"/>
        <geom type="capsule" fromto="0 0 0 0.25 0 0" size="0.018"/>

        <!-- pos 是 link3 相对 link2 的固定偏置，对应 trans(L2, 0, 0) -->
        <body name="link3" pos="0.25 0 0">
          <joint name="j3" type="hinge" axis="0 0 1"/>
          <geom type="capsule" fromto="0 0 0 0.15 0 0" size="0.015"/>
          <!-- site 放在第三节末端，对应 trans(L3, 0, 0) 后的末端点 -->
          <site name="end_site" pos="0.15 0 0" size="0.01"/>
        </body>
      </body>
    </body>
  </worldbody>
</mujoco>

三个 hinge 关节都绕 轴；每节连杆的 pos 就是父关节到子关节的固定偏置 ，对应 §2.5.2 父-子链式约定里那一段固定变换；end_site 标在第三节末端，正是 Python 代码里 mj_name2id(..., 'end_site') 要查询的 site。

import mujoco

model = mujoco.MjModel.from_xml_path('planar_3dof.xml')
data  = mujoco.MjData(model)
# 后面比较的是 end_site 的世界坐标，所以先拿到它在 MuJoCo 模型里的 id。
end_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, 'end_site')

max_err = 0.0
for _ in range(100):
    # 随机采样一组关节角，覆盖不同姿态，而不是只测零位。
    q = np.random.uniform(-np.pi, np.pi, size=3)
    data.qpos[:3] = q
    mujoco.mj_forward(model, data)           # 让 MuJoCo 按 kinematic tree 更新所有位姿
    p_mj = data.site_xpos[end_id]            # MuJoCo 算出的 end_site 世界坐标

    p_ours = fk_planar(q)[:3, 3]             # 自己的 FK 矩阵第 4 列前三项

    max_err = max(max_err, np.linalg.norm(p_mj - p_ours))

print(f'max |p_ours - p_mj| = {max_err:.2e}')
# 期望量级在 1e-14 ~ 1e-12;超过 1e-6 基本是参数/轴向写错了

FK 是后面 IK、雅可比、控制器的共同基石——差 0.5 mm，在第 3 章末端画圆时就是一圈肉眼可见的毛刺，所以这一步要对齐到 1e-12 量级再往下走。

2.8 雅可比矩阵

FK 给出的是"给定关节角，末端在哪里"。在控制和 IK 中，还需要回答另一个问题：关节角发生一个很小的变化时，末端位置会如何变化。若只考虑位置部分，可以把 FK 写成：

对 关于关节变量求偏导，就得到雅可比矩阵：

其中 的每一列对应一个关节的影响。第 列可以理解为：只让第 个关节以单位速度运动，其余关节不动时，末端会产生的瞬时速度方向和大小。

对平面 3-DoF 臂，末端位置是二维量 ，关节变量有 3 个，因此位置雅可比是 ：

令 ，，。由式 直接求导可得：

这个矩阵的结构也反映了运动链的因果关系： 会影响三段连杆，所以第 1 列包含 ； 只影响第 2、3 段，所以第 2 列不含 ； 只影响末端最后一段，所以第 3 列只含 。

在 3D 机械臂中，如果同时描述末端线速度和角速度，常用的空间雅可比为 ：前 3 行对应线速度，后 3 行对应角速度。

有两种常用算法：

1. 解析求导：先写出 FK 的显式表达式，再对每个 求偏导。它适合结构简单、公式可以手动推导的机械臂；优点是运行时计算直接，缺点是公式与具体机构强绑定。
2. 几何雅可比：不展开完整 FK 公式，而是直接利用每个关节轴在世界坐标系下的位置和方向。对旋转关节，第 列通常写成

其中 是第 个关节轴上一点的位置， 是该关节轴在世界坐标系下的单位方向， 是末端位置。上半部分给出该关节对末端线速度的贡献，下半部分给出该关节对末端角速度的贡献。对于移动关节，线速度部分是关节轴方向，角速度部分为零。

雅可比在后续章节会反复出现：第 3 章数值 IK 用它把末端位置误差转成关节角修正量；第 5 章接触力分析会用到雅可比转置；第 6 章奖励设计中也会用它描述末端速度。因此，本节只需要先建立两个结论：雅可比是 FK 对关节变量的局部线性化；它的每一列描述一个关节对末端瞬时运动的贡献。

2.9 工作空间散点

工作空间(reachable workspace) 是末端能到达的所有点的集合。用 FK 做一次 Monte Carlo 就能把它画出来：

import matplotlib.pyplot as plt

pts = []
for _ in range(20000):
    q = np.random.uniform(-np.pi, np.pi, size=3)
    pts.append(fk_planar(q)[:2, 3])
pts = np.array(pts)

plt.figure(figsize=(5, 5))
plt.scatter(pts[:, 0], pts[:, 1], s=1, alpha=0.3)
plt.axis('equal'); plt.title('3-DoF planar arm workspace')
plt.show()

运行结果应接近 图 8 所示的点云。由于本例的三段长度满足 ，机械臂可以折叠到原点附近，因此工作空间近似为一个圆盘；如果最长连杆长度大于其余连杆长度之和，中心会出现不可达区域，点云会更接近环形。

平面 3-DoF 机械臂工作空间散点图的实际生成结果。采样点表示末端可达位置，外边界对应三段连杆近似伸直时的最大半径。

这张图在第 3 章选 IK 目标时会派上用场：目标点落在工作空间外就是典型的"无解"，这时再好的 IK 算法也无法得到满足约束的关节角。

2.10 末端叠加可视化

MuJoCo 支持在 viewer 里叠加"调试几何"，非常适合把 FK 的中间结果可视化出来——比如末端位置、每一节连杆坐标轴、目标点。

import mujoco.viewer

with mujoco.viewer.launch_passive(model, data) as v:
    while v.is_running():
        # 假设我们在 Python 里用自己的 FK 算末端位置
        p = fk_planar(data.qpos[:3])[:3, 3]

        # 清空旧的调试几何再画
        v.user_scn.ngeom = 0
        mujoco.mjv_initGeom(
            v.user_scn.geoms[0],
            type=mujoco.mjtGeom.mjGEOM_SPHERE,
            size=[0.02, 0, 0], pos=p, mat=np.eye(3).flatten(),
            rgba=[1, 0.3, 0.3, 1],
        )
        v.user_scn.ngeom = 1

        mujoco.mj_step(model, data)
        v.sync()

预期效果如 图 9 所示。若 FK 链式乘法、body pos 和 joint axis 均一致，两者应重合在第三节连杆末端。

MuJoCo 末端位置叠加校验的实际生成结果。红色点表示 Python FK 计算出的末端位置，蓝色点表示 MuJoCo 中的 end_site。

这一步的目的，是把 Python 代码中的数学模型和 MuJoCo 模型中的运动链对齐。如果红色点偏离蓝色点，优先检查坐标系定义、body pos 参数、joint axis 方向以及矩阵乘法顺序。到第 3 章做 IK 时，目标点和当前末端都会用这种方式实时叠加到场景中。

小结

• 机械臂的组成：关节(Joint) 定义自由度，连杆(Link) 是关节之间的刚体；末端执行器是最后一个连杆，它的状态由位置 + 姿态合成的位姿(pose) 描述。
• FK 是什么：FK 把关节角映射到末端位姿 ，位姿用坐标系来描述——常见的有世界、基座、连杆、末端四种，FK 的链式乘法本质就是在它们之间逐级变换。
• 运动链与齐次变换：把基座、各连杆、末端按"父→子"串起来就得到运动链；每个关节对应一个"固定偏置 + 轴向旋转"的变换，齐次变换矩阵 把位置和姿态合在一起，可以一路乘下去得到末端位姿。
• 两种描述约定：DH 参数（教材推导）和 URDF / MJCF 父-子链式约定（仿真 / 真机）是描述运动链的两条路；后者更贴近实际机器人描述，本章代码全部按这一约定写。
• 从 FK 到雅可比：齐次矩阵描述末端"在哪、朝哪"，雅可比 描述"关节角怎么动，末端怎么动"——它是运动学和动力学之间的桥梁，也是下一章数值 IK 的核心工具。

思考

1. §2.5.2 提到 URDF 的 <origin> 是常量 、axis + 是变量 。如果某个关节的 <origin rpy="0.5 0 0">（父连杆预先绕 x 轴转了 0.5 rad），这 0.5 rad 应该出现在 FK 链式乘法的哪一项？关节角取 时末端位置会和 rpy="0 0 0" 时不同吗？

2. 把 §2.6 平面 3-DoF 的第三个关节换成移动关节（沿 link2 末端 轴平移 ），末端位置 与朝向 该怎么写？为什么这种情况下 仍然只取决于前两个旋转关节？

3. §2.7 自写 FK 与 mj_forward 的误差通常在 1e-12 ~ 1e-14 量级。如果把 MJCF 里某个 <body pos> 写成 0.299 0 0（少 1 mm），误差会跳到什么量级？三个关节里这种"参数对不齐"出在哪一节最严重？

4. §2.9 用 Monte Carlo 采样画出工作空间。给定 ，能不能解析写出工作空间外边界和内边界的半径？什么条件下三连杆可以折叠到原点（内边界半径为 0）？

5. §2.8 几何雅可比第 列依赖 。如果机械臂处于"完全伸直"姿态（所有 ），这个雅可比的行列式会接近零——这个现象叫什么名字、几何意义是什么、对下一章的数值 IK 会带来什么麻烦？

动手任务

§2.10 那个红球只是把"Python FK"和"MuJoCo FK"在一条平面 3-DoF 臂上对齐了一次。本章动手任务把同一招升级到 Pupper 真腿——三个关节不再都是 ，而是 HAA → HFE → KFE 这种混合轴结构（按 §2.5 的 URDF / MJCF 父-子链式约定）。FK chain 写错一节，红球立刻飞出脚面——这是 Lab 自带的"自动出错检测"。

脚本扫频版：HAA / HFE / KFE 依次在各自限位区间里单独扫一遍，红球按你写的 fk_leg(θ) 实时叠在 foot site 上。这是作品集 GIF；Lab 还自带一组 tkinter 滑杆 teleop 模式（uv run python lab_2_fk_teleop/starter.py --viewer），可以手动拖三个关节实时验证 FK——这才是 §2.10 那个红球的真腿版。

要做的四件事：

• 在 Ch1 的 scene.xml 上删掉 <equality> 整块，让 HAA / HFE / KFE 都能动
• 写 fk_leg(θ) 的四段齐次变换链；100 组随机关节角对 mj_forward 误差 m
• tkinter 三滑杆 + mujoco.viewer.launch_passive，红球实时叠在 foot 上
• Monte Carlo 采 2 万点，画 Pupper 工作空间"葫芦"散点（§2.9 平面甜甜圈的 3D 版）

完整 starter / 测试 / 交付清单见 exercises/lab_2_fk_teleop/。

参考资料

• CS123 Lecture 3: Forward Kinematics
• 《机器人学导论》（Craig）第 3 章