这一章要解决什么问题：在看代码之前，先把"行话"搞懂。
什么是自由度？什么是位姿？什么是工作空间和奇异位形？为什么 6 轴机械臂这么常见？

配套代码：[code/ch04_robotics_concepts.py]

"""
第 04 章配套代码：机器人学核心概念。

运行：
    D:\\Environment\\dm_control_env\\python.exe ch04_robotics_concepts.py

演示：
  · 工作空间判定（可达 / 不可达）
  · 2 连杆正运动学手算
  · 数值雅可比 vs MuJoCo 解析雅可比（mj_jacSite）对比
  · 奇异位形：伪逆会"爆炸"，阻尼最小二乘(DLS)稳住

本章代码自带一个 3 连杆平面臂模型（XML 写在文件里），不依赖项目其他文件。
"""
import numpy as np
import mujoco

np.set_printoptions(precision=4, suppress=True)

# ============================================================
# 4.6 工作空间判定
# ============================================================
print("=" * 62)
print("4.6  工作空间判定（2 连杆平面臂）")
print("=" * 62)

L1, L2 = 0.3, 0.25
max_reach = L1 + L2          # 完全伸直
min_reach = abs(L1 - L2)     # 完全折叠

print(f"臂长 L1={L1}, L2={L2}")
print(f"最大可达半径 = {max_reach:.3f} m")
print(f"最小可达半径 = {min_reach:.3f} m   # 工作空间是个圆环")


def reachable(x, z):
    """判断 (x, z) 是否在 2 连杆臂的工作空间内（Z-up，臂在 XZ 平面运动）。"""
    r = np.hypot(x, z)
    return min_reach <= r <= max_reach


for pt in [(0.4, 0.2), (0.6, 0.0), (0.1, 0.05), (0.0, 0.0)]:
    ok = reachable(*pt)
    r = np.hypot(*pt)
    print(f"  点 ({pt[0]:>4}, {pt[1]:>4})  半径 {r:.3f}  ->  "
          f"{'可达' if ok else '不可达'}")

# ============================================================
# 4.5.1 正运动学 FK 手算（2 连杆）
# ============================================================
print("\n" + "=" * 62)
print("4.5.1  正运动学 FK（2 连杆，三角函数法）")
print("=" * 62)


def fk_2link(q1, q2, l1=0.3, l2=0.3):
    """2 连杆平面臂 FK。q1/q2 为弧度，返回末端 (x, z)。"""
    t1 = q1
    t2 = q1 + q2
    x = l1 * np.sin(t1) + l2 * np.sin(t2)
    z = l1 * np.cos(t1) + l2 * np.cos(t2)
    return np.array([x, z])


q1, q2 = np.radians(45), np.radians(-30)
end = fk_2link(q1, q2)
print(f"关节角 [{np.degrees(q1):.0f}°, {np.degrees(q2):.0f}°] -> 末端 (x, z) = {np.round(end, 4)}")

# 用齐次矩阵交叉验证（第 02 章的 link_transform 写法）
def rot_y(th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])


def make_transform(rot, trans):
    T = np.eye(4)
    T[:3, :3], T[:3, 3] = rot, trans
    return T


def link_transform(rot, local_offset):
    """先沿本地方向平移，再整体旋转（第 02 章 2.5.3 的写法 B）。"""
    return make_transform(rot, np.zeros(3)) @ make_transform(np.eye(3), local_offset)


T = (link_transform(rot_y(q1), np.array([0, 0, 0.3]))
     @ link_transform(rot_y(q2), np.array([0, 0, 0.3])))
end_mat = (T @ np.array([0, 0, 0, 1]))[:3]
print(f"矩阵法验证                      -> 末端 (x, y, z) = {np.round(end_mat, 4)}")
print(f"两法一致: {np.allclose([end[0], end[1]], [end_mat[0], end_mat[2]])}")

# ============================================================
# 4.7 雅可比：数值法 vs MuJoCo 解析法
# ============================================================
print("\n" + "=" * 62)
print("4.7  雅可比：数值微分 vs mujoco.mj_jacSite")
print("=" * 62)

# 一个 3 连杆平面臂（全部绕 Y 轴转，在 XZ 平面内运动，Z-up 坐标系）
ARM_XML = """
<mujoco>
  <worldbody>
    <body name="base" pos="0 0 0">
      <joint name="j1" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
      <geom type="cylinder" size="0.05 0.03" rgba="0.5 0.5 0.5 1"/>
      <body name="link1" pos="0 0 0.3">
        <joint name="j2" type="hinge" axis="0 1 0" range="-2.7 1.17"/>
        <geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.025" rgba="0.8 0.8 0.85 1"/>
        <body name="link2" pos="0 0 0.3">
          <joint name="j3" type="hinge" axis="0 1 0" range="-1.1 3.36"/>
          <geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.025" rgba="0.8 0.8 0.85 1"/>
          <site name="tip" pos="0 0 0.3" size="0.02" rgba="1 0.3 0.3 1"/>
        </body>
      </body>
    </body>
  </worldbody>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(ARM_XML)
data = mujoco.MjData(model)
tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")

# 设一个非奇异的姿态
data.qpos[:3] = [0.4, -0.6, 0.3]
mujoco.mj_forward(model, data)
print(f"当前关节角 q = {data.qpos[:3]}")
print(f"末端位置      = {np.round(data.site_xpos[tip_id], 4)}")


def numeric_jacobian(mdl, dat, site_id, eps=1e-6):
    """有限差分法数值求雅可比：微扰每个关节，看末端位置怎么变。"""
    nv = mdl.nv
    q0 = dat.qpos.copy()
    p0 = dat.site_xpos[site_id].copy()
    J = np.zeros((3, nv))
    for i in range(nv):
        dat.qpos[i] = q0[i] + eps
        mujoco.mj_forward(mdl, dat)
        J[:, i] = (dat.site_xpos[site_id] - p0) / eps
        dat.qpos[i] = q0[i]
    mujoco.mj_forward(mdl, dat)
    return J


J_num = numeric_jacobian(model, data, tip_id)

# MuJoCo 的解析雅可比
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, tip_id)

jac_match = np.allclose(J_num, jacp, atol=1e-5)   # 先存下来，后面 jacp 会被覆盖

print("\n数值雅可比 J_num (3x3):")
print(np.round(J_num, 4))
print("\nMuJoCo 解析雅可比 jacp:")
print(np.round(jacp, 4))
print(f"\n两者是否一致: {jac_match}")

# 验证 Δx = J · Δq
dq_test = np.array([0.01, -0.02, 0.005])
p_before = data.site_xpos[tip_id].copy()
data.qpos[:3] += dq_test
mujoco.mj_forward(model, data)
dx_actual = data.site_xpos[tip_id] - p_before
dx_pred = jacp @ dq_test
print(f"\nΔx 实际 = {np.round(dx_actual, 5)}")
print(f"J·Δq 预测 = {np.round(dx_pred, 5)}   # 小位移下吻合")

# ============================================================
# 4.8 奇异位形：伪逆爆炸 vs DLS 稳定
# ============================================================
print("\n" + "=" * 62)
print("4.8  奇异位形：pinv 爆炸 vs 阻尼最小二乘(DLS)")
print("=" * 62)

# 让手臂【接近】完全伸直：这叫"接近奇异"，是实际工程中最危险的情况
# （注意：完全伸直时伪逆反而返回 0，因为目标方向已完全不可达；
#   真正会"爆炸"的是"差一点点就伸直"的情况）
LAM = 0.02


def solve_dls(J, dx, lam=LAM):
    """阻尼最小二乘：Δq = Jᵀ·(J·Jᵀ + λ²I)⁻¹·Δx"""
    return J.T @ np.linalg.solve(J @ J.T + lam ** 2 * np.eye(J.shape[0]), dx)


EPS = 0.01                                  # 离完全伸直还差 0.01 rad
data.qpos[:3] = [EPS, -EPS, EPS * 0.5]
mujoco.mj_forward(model, data)
print(f"接近奇异姿态 q = {np.round(data.qpos[:3], 4)}  "
      f"末端 = {np.round(data.site_xpos[tip_id], 4)}")
print("手臂几乎完全伸直，沿 Z 方向【几乎无法再伸长】—— 边界奇异")

mujoco.mj_jacSite(model, data, jacp, jacr, tip_id)
J = jacp.copy()
sigma = np.linalg.svd(J, compute_uv=False)
print(f"\n雅可比 J =\n{np.round(J, 4)}")
print(f"奇异值 σ = {np.round(sigma, 6)}   # 第二个奇异值极小 -> 接近秩亏")

# 要求末端沿 Z 往外再伸 1 cm（物理上几乎做不到）
dx = np.array([0.0, 0.0, 0.01])

dq_pinv = np.linalg.pinv(J) @ dx            # 标准伪逆
dq_dls = solve_dls(J, dx)                   # 阻尼最小二乘

print(f"\npinv 解出的 dq = {np.round(dq_pinv, 4)}")
print(f"  ||dq|| = {np.linalg.norm(dq_pinv):.3f} rad "
      f"(= {np.degrees(np.linalg.norm(dq_pinv)):.0f}°)   <- 关节要转好几圈！会抖飞")
print(f"\nDLS 解出的 dq  = {np.round(dq_dls, 4)}   (λ={LAM})")
print(f"  ||dq|| = {np.linalg.norm(dq_dls):.4f} rad "
      f"(= {np.degrees(np.linalg.norm(dq_dls)):.1f}°)   <- 温和，安全")

# 扫描：越接近奇异，伪逆放大得越离谱，而 DLS 始终稳定
print("\n  离奇异的距离 ε  |  最小奇异值  |  ||dq|| pinv  |  ||dq|| DLS")
print("  " + "-" * 62)
for eps in [0.3, 0.1, 0.03, 0.01, 0.003, 0.001]:
    data.qpos[:3] = [eps, -eps, eps * 0.5]
    mujoco.mj_forward(model, data)
    mujoco.mj_jacSite(model, data, jacp, jacr, tip_id)
    Js = jacp.copy()
    sv = np.linalg.svd(Js, compute_uv=False)
    p = np.linalg.norm(np.linalg.pinv(Js) @ dx)
    d = np.linalg.norm(solve_dls(Js, dx))
    print(f"  {eps:<15} |  {sv[1]:<10.5f} |  {p:<11.3f} |  {d:.4f}")
print("\n  >> 越接近奇异，pinv 放得越大（ε=0.001 时要转 78 rad ≈ 12 圈）；")
print("     DLS 始终把 ||dq|| 压在很小的范围——这就是它的价值。")

# ============================================================
# 4.12 动手练 参考答案
# ============================================================
print("\n" + "=" * 62)
print("4.12 动手练 参考答案")
print("=" * 62)

print("练习1 工作空间判定:")
for pt in [(0.4, 0.2), (0.6, 0.0), (0.1, 0.05)]:
    print(f"  ({pt[0]}, {pt[1]}) -> {'可达' if reachable(*pt) else '不可达'}")

print(f"\n练习2 FK: q=[45°, -30°] -> 末端 (x,z) = {np.round(fk_2link(*np.radians([45, -30])), 4)}")

print(f"\n练习3 雅可比对比: 数值法与 mj_jacSite 一致 = {jac_match}")

print(f"\n练习4 奇异性(ε=0.01 接近伸直):")
print(f"  ||dq|| pinv = {np.linalg.norm(dq_pinv):.3f} rad  "
      f"(= {np.degrees(np.linalg.norm(dq_pinv)):.0f}°)  <- 爆炸")
print(f"  ||dq|| DLS  = {np.linalg.norm(dq_dls):.4f} rad  "
      f"(= {np.degrees(np.linalg.norm(dq_dls)):.1f}°)  <- 稳定")

print("\n第 04 章示例代码运行完毕。")

本章学习目标

读完本章，你将能够：

  1. 说清机械臂的基本构造：连杆和关节的区别，旋转关节 vs 移动关节，6 个关节各自的作用。
  2. 理解自由度的概念：解释为什么 6 轴机械臂能"以任意姿态到达任意点"，5 轴和 7 轴各有什么局限/优势。
  3. 区分三个"位置"：关节角 q、末端位置、目标位置——这是新手最常混淆的概念。
  4. 理解正运动学 FK 和逆运动学 IK：FK 是"关节→末端"（唯一、简单），IK 是"末端→关节"（可能无解/多解、需要迭代）。
  5. 计算工作空间：对 2 连杆臂能判断一个点是否可达，理解实际 6 轴臂工作空间更复杂的原因。
  6. 理解雅可比矩阵：它描述"关节微动→末端微动"的线性关系，是 IK 的核心工具。
  7. 识别和处理奇异位形：知道三种典型奇异（边界、腕部、肩部），理解为什么"接近奇异"比"完全奇异"更危险，会用阻尼最小二乘（DLS）稳定求解。
  8. 理解冗余自由度：知道如何利用零空间偏置让机械臂远离关节限位。

📌 前置知识：本章需要第 02 章的 3D 数学（向量、矩阵、齐次变换）和第 03 章的 NumPy 基础（伪逆、矩阵乘法）。

4.1 机械臂的解剖：连杆与关节

一台机械臂由两种东西交替串联而成：

[基座] ─关节1─ [连杆1] ─关节2─ [连杆2] ─关节3─ ... ─关节6─ [末端/工具]
  • 连杆（Link）：刚性的"骨头"，有固定长度和形状。
  • 关节（Joint）：连接两根连杆的"关节"，提供相对运动。

机械臂解剖图（对应本项目的 Epson CX4-A601C）：

                    大臂连杆(L1)         小臂连杆(L2)          腕部
                   ┌──────────┐        ┌──────────┐        ┌────┐
                   │          │        │          │        │    │
  基座 ──J1──●────●    J2    ●────●────●    J3    ●────●────● J4 ● J5 ● J6 ──▶ 法兰/夹爪
 (固定)  旋转    │          │        │          │        │    │
                 └──────────┘        └──────────┘        └────┘
                  0.26m                0.28m               腕部连杆
                  
  关节序列：J1(基座旋转) → J2(大臂俯仰) → J3(小臂俯仰) → J4(腕翻转) → J5(腕俯仰) → J6(法兰旋转)

💡 连杆长度是"魔法数字"：在模型文件里，每个 <body pos="x y z"> 的 pos 属性就是连杆长度（相对于父 body 的偏移）。比如教学示例里大臂连杆的 pos="0 0 0.3" 表示它在父 body 的 Z 方向 0.3 米处（Z-up：竖直向上）。本项目模型里上图标注的 0.26 m（肩→肘）/ 0.28 m（肘→腕）就来自真实机械臂的 CAD 模型。

关节的两种基本类型

类型英文运动变量例子
旋转关节revolute绕一个轴转动角度 θ（弧度）人的肘关节
移动关节prismatic沿一个轴平移距离 d（米）液压缸、直线模组

工业机械臂几乎全是旋转关节。本书的 6 轴臂 6 个都是旋转关节。

在 MJCF（MuJoCo 的模型格式）里，关节这样写：

<joint name="j1" type="hinge" axis="0 0 1" range="-3.14159 3.14159"/>
<!--  名字      类型=旋转   绕 Z 轴转    角度限位（弧度）        -->

MJCF 关节属性详解：

属性含义本项目的值说明
name关节名字"joint_j1" ~ "joint_j6"代码里用 physics.named.data.qpos["joint_j1"] 访问
type关节类型"hinge"（旋转）/ "slide"（移动）hinge = 铰链，slide = 滑轨
axis旋转/平移轴"0 1 0"（Y轴）等单位向量，决定关节绕哪个轴转
range限位范围"-3.14159 3.14159" 等下限 上限，单位是弧度（因为 <compiler angle="radian"/>）
<joint name="joint_left_finger" type="slide" axis="0 0 1" range="0 0.03"/>
<!--  类型=滑动（移动关节），沿 Z 轴平移，行程 0~0.03 米          -->

📌 MuJoCo 里 hinge = 旋转关节，slide = 移动关节（名字来自"铰链"和"滑轨"）。

本项目的夹爪用的是 slide 关节：两个手指沿 Z 轴对向滑动（一个沿 +Z、一个沿 −Z），行程 0~0.03 米（3 厘米）。这是移动关节在本项目中的唯一应用。

MJCF 模型的完整结构

关节不是孤立存在的，它嵌在 body（连杆） 的层级结构里。一个完整的机械臂 MJCF 模型由三层元素嵌套组成：

<mujoco>
  <option gravity="0 0 -9.81"/>          <!-- 全局选项：重力、时间步等 -->
  <worldbody>                              <!-- 世界根节点 -->
    <body name="base" pos="0 0 0">       <!-- 基座 body（固定在地面） -->
      <joint name="j1" type="hinge" axis="0 0 1" range="-3.14 3.14"/>
      <geom type="cylinder" size="0.05 0.03" rgba="0.5 0.5 0.5 1"/>
      <body name="link1" pos="0 0 0.3">  <!-- 大臂 body（相对基座沿 +Z 偏移 0.3m） -->
        <joint name="j2" type="hinge" axis="0 1 0" range="-2.7 1.17"/>
        <geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.025" rgba="0.8 0.8 0.85 1"/>
        <body name="link2" pos="0 0 0.3"><!-- 小臂 body -->
          <joint name="j3" type="hinge" axis="0 1 0" range="-1.1 3.36"/>
          <geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.025"/>
          <site name="tip" pos="0 0 0.3" size="0.02" rgba="1 0.3 0.3 1"/>
        </body>
      </body>
    </body>
  </worldbody>
</mujoco>

📌 示例与真值的关系：这个示例的几何是教学简化（body 偏移简化为沿 +Z 的直链，真实模型的连杆偏移见第 05 章参数表）；但 j2/j3 的限位数值（-2.7 1.17、-1.1 3.36）借用了本项目 CX4 模型的真实限位（-155°+67°、-63°+193°）。

三种核心元素的作用：

元素作用关键属性
<body>刚体连杆，定义层级关系和相对位置name、pos（相对父 body 的偏移）
<joint>关节，定义 body 之间的运动方式name、type、axis、range
<geom>几何形状，用于碰撞检测和渲染type（sphere/capsule/cylinder/box）、size、rgba
<site>参考点，不参与碰撞，用于标记末端位置等name、pos、size

body 的嵌套 = 连杆的串联。子 body 的 pos 是相对于父 body 的偏移，这就是连杆长度。关节写在哪个 body 里，就控制哪个 body 相对于父 body 的运动。

geom 的常见类型：

类型形状size 含义用途
sphere球体r（半径）小球、末端标记
capsule胶囊（圆柱+半球）r（半径），用 fromto 指定两端连杆（最常用，碰撞计算高效）
cylinder圆柱r h（半径、半高）基座
box长方体x y z（半尺寸）桌面、障碍物

💡 为什么连杆用 capsule 而不是 box？ 胶囊的碰撞检测是凸体之间最快的，而且不会有"棱角卡住"的问题。工业机械臂的连杆在 MuJoCo 里几乎都用 capsule 近似。

site 的作用：site 是一个"虚拟参考点"，没有质量、不参与碰撞，但可以在代码里通过 mj_name2id 找到它，然后读取它的位置 data.site_xpos[id] 和旋转 data.site_xmat[id]。机械臂的末端执行器中心点（TCP）通常就定义为一个 site。

球面腕（Spherical Wrist）：三轴交汇的腕部设计

直观理解：就像人的手腕

人的手腕可以上下摆、左右摆、旋转——三个自由度的轴线交汇于一点（腕心）。
工业机械臂的末端腕部（J4/J5/J6）也是同样的设计：三个关节的轴线交于同一点，称为球面腕。

为什么叫"球面"腕？

因为三个关节轴线交汇于一点后，末端法兰盘的中心到腕心的距离是固定的。
当 J4/J5/J6 转动时，法兰盘中心始终在以腕心为球心、固定半径的球面上运动——所以叫"球面腕"。

        J4轴 (X)
         │
    ┌────┴────┐
    │  腕心    │  ← J4/J5/J6 三轴交汇点
    │  (0,0,0) │
    └────┬────┘
         │
        J5轴 (Y)
         │
        J6轴 (X)  ← 与J4平行，但偏移了法兰盘长度
         │
    ┌────┴────┐
    │ 法兰盘中心 │  ← 始终在以腕心为球心的球面上
    └─────────┘
球面腕的运动学意义
特性说明
位置解耦J1~J3 决定腕心位置，J4~J6 决定末端姿态，两者独立求解
解析IK存在Pieper 解法：先解 J1~J3 位置，再解 J4~J6 姿态，比迭代法快几个数量级
奇异点J5=0° 时 J4 和 J6 轴线重合，产生奇异（无法确定 J4/J6 各自角度）
本项目配置J4轴=+X, J5轴=+Y, J6轴=+X；j4 腕心偏移 (-0.28, 0, 0.03)，j5/j6 与 j4 同心（pos [0,0,0]）
本项目模型中的球面腕

在 cx4_a601c_simulation.xml 中：

<body name="j4" pos="-0.280000 0.000000 0.030000">
  <joint name="joint_j4" axis="1.0 0.0 0.0" .../>  <!-- +X轴 -->
  <body name="j5" pos="0 0.000000 0.000000">  <!-- 与j4同心 -->
    <joint name="joint_j5" axis="0.0 1.0 0.0" .../>  <!-- +Y轴 -->
    <body name="j6" pos="0 0.000000 0.000000">  <!-- 与j5同心 -->
      <joint name="joint_j6" axis="1.0 0.0 0.0" .../>  <!-- +X轴 -->

注意 j5 和 j6 的 pos 都是 [0,0,0]——这意味着它们与父 body 同心，三个关节轴线交汇于 j4 的原点（腕心）。

💡 球面腕是 6 轴机械臂的标准设计。几乎所有工业 6 轴机器人（ABB、KUKA、FANUC、Epson）都采用球面腕，
因为它让逆运动学有解析解，控制速度快、精度高。非球面腕（如 SCARA）只有 4 轴，无法实现任意姿态。

4.2 自由度（DOF）

自由度 = 独立控制的数量，通常等于可动关节数。

机器人自由度能做什么
2 轴平面臂2在平面内到达任意 (x, y)，无法控制朝向
3 轴（如 SCARA）3平面内任意位置 + 一个转动
6 轴工业臂6空间任意位置 + 任意朝向（完全灵活）
7 轴协作臂76 + 1 个冗余自由度（可避障）

为什么是 6？

在三维空间里完整描述一个刚体的位姿需要 6 个数：

位姿 = 位置 (x, y, z)  +  朝向 (rx, ry, rz)
       └──── 3 个 ────┘    └──── 3 个 ────┘

所以 6 个自由度刚好能"以任意姿态到达空间任意点"——这是工业机械臂用 6 轴的数学原因。

末端执行器（End-Effector）：机械臂最末端干活的部分——夹爪、吸盘、焊枪、喷头。它的位置叫 TCP（Tool Center Point，工具中心点）。

本书项目中，末端是一个两指平行夹爪，TCP 定义在两个手指中间的 end_effector site 上。

4.3 位姿（Pose）

位姿 = 位置 + 朝向，是机器人学最核心的概念。

# 一个位姿可以这样表示（本书常用）
pose = {
    "position": [0.3, 0.1, 0.5],          # x, y, z（米）
    "orientation": [0.7071, 0, 0.7071, 0],  # 四元数 (w,x,y,z)
}

也可以用一个 4×4 齐次变换矩阵完整表达（见第 02 章）。

三个必须区分的"位置"：

名称含义MuJoCo 里怎么取
关节角 q6 个关节各转了多少data.qpos[:6]
末端位置夹爪在世界坐标系的哪里data.site_xpos[ee_id]
目标位置你想让夹爪去哪里你自己给定

⚠️ 新手最常混淆的就是这三个。记住：
关节角是机器人"感受到"的，末端位置是"算出来"的，目标位置是"你想要"的。

4.4 坐标系：三个必须分清的"系"

世界坐标系 (world frame)
  └─ 固定在地面，原点通常在机器人基座中心
      │
      ├─ 基座坐标系 (base frame)
      │    └─ 固连在机器人底座上
      │
      └─ 关节坐标系 (joint frames)
           └─ 每个关节一个，随关节转动
                └─ 工具坐标系 (tool frame)
                     └─ 固连在末端执行器上（TCP 就在这里）
  • 正运动学：从最内层（关节）往外算到最外层（工具）——从内到外。
  • 逆运动学：从最外层（目标）往内推到最内层（关节）——从外到内。

在 MuJoCo 里读取：

# 世界坐标系下某个 site 的位置
ee_pos = data.site_xpos[ee_id]              # shape (3,)

# 世界坐标系下某个 body 的位姿
body_pos = data.xpos[body_id]               # 位置 (3,)
body_mat = data.xmat[body_id].reshape(3,3)  # 旋转矩阵 3x3
body_quat = data.xquat[body_id]             # 四元数 (w,x,y,z)

坐标系变换实战

在实际项目中，经常需要在不同坐标系之间转换数据。以下是最常见的三种场景：

场景 1：世界坐标 → 基座局部坐标

# 已知末端在世界坐标系的位置，求它在基座坐标系下的位置
T_base = ...  # 基座在世界系的变换矩阵（通常是单位矩阵，因为基座固定在地面）
p_world = data.site_xpos[ee_id]           # 世界系位置
p_base = transform_point(invert_transform(T_base), p_world)  # 基座系位置

场景 2：工具坐标 → 世界坐标

# 已知工具尖端在工具坐标系下的偏移 (0, 0, 0.05)，求它在世界系的位置
T_ee = ...  # 末端在世界系的变换矩阵（4x4）
p_tool_local = np.array([0, 0, 0.05])     # 工具尖端在工具系下的位置
p_world = transform_point(T_ee, p_tool_local)  # 转换到世界系

场景 3：目标点从基座系 → 世界系

# 取料点通常定义在基座坐标系下，需要转成世界系才能和 MuJoCo 数据比较
PICK_POS_BASE = np.array([-0.18, -0.30, 0.02])  # 基座系下的取料点（Z-up：物块中心在地面上方 0.02 m）
T_base = np.eye(4)  # 基座固定在世界原点，变换矩阵是单位矩阵
pick_pos_world = transform_point(T_base, PICK_POS_BASE)
# 如果基座不在原点，就用实际的 T_base

💡 记忆口诀：从 A 坐标系转到 B 坐标系，用 T_B_to_world @ invert(T_A_to_world) @ p_A。或者更简单：目标坐标系的变换矩阵 × 源坐标系的逆变换 × 点。

4.5 正运动学 FK 与逆运动学 IK

4.5.1 正运动学（Forward Kinematics, FK）

输入：6 个关节角 q = [q1, q2, ..., q6]
输出：末端位姿 T（4×4 矩阵）

FK:  q ──▶ T

特点：

  • ✅ 一定有解（只要关节角合法，末端一定在某个地方）
  • ✅ 解唯一（同一组关节角，末端位置不可能有两个）
  • ✅ 计算快、稳定（就是一串矩阵乘法）
# ikpy 里的 FK
T = chain.forward_kinematics(q)      # 4x4 矩阵
position = T[:3, 3]                   # 取位置部分

4.5.2 逆运动学（Inverse Kinematics, IK）

输入：末端目标位姿 T_target
输出：6 个关节角 q

IK:  T_target ──▶ q

特点（每条都很重要）：

特性说明后果
可能无解目标点在工作空间之外求解失败，需要检测
可能多解同一位置，手肘可朝上/朝下需要选择"最好"的那组
不保证唯一6 轴臂同一位姿最多有 8 组解选解策略很重要
需要迭代通常没有闭式解，只能数值迭代有收敛速度/精度问题
有奇异性某些位形下会"卡住"需要阻尼处理

💡 回想第 00 章那个例子：输入关节角 [0, 0.6, -0.9]，ikpy 解出 [0, 0.6, 0]——两组不同的关节角，末端位置相同。这就是多解。

4.5.3 一句话对比

FKIK
方向关节 → 末端末端 → 关节
解的数量唯一0 个、1 个或多个
计算直接矩阵连乘数值迭代
难度简单难（第 06 章整章讲它）

什么时候用 FK，什么时候用 IK？

是

否

是

否

你知道什么？

知道关节角 q?

用 FK 算末端位姿
q → T

知道末端目标 T_target?

用 IK 算关节角
T_target → q

需要先确定目标
（轨迹规划/任务规划）

典型应用场景：

  • 仿真监控：已知关节角（从传感器读），用 FK 算末端在哪 → 显示末端位置。
  • 轨迹跟踪：已知末端目标轨迹（一系列 T_target），用 IK 算每个时刻的关节角 → 发给控制器。
  • 碰撞检测：用 FK 算出每个连杆的位置 → 检查是否和障碍物碰撞。
  • 工作空间分析：遍历所有关节角组合，用 FK 算出所有末端位置 → 绘制工作空间。

IK 的多解问题与选解策略

6 轴机械臂同一位姿最多有 8 组解（对应"手肘朝上/朝下"、“肩部前/后”、"腕部翻转/不翻转"的 2×2×2 组合）。

同一位姿 T_target 的 8 组解：
  解1: 手肘朝上 + 肩部朝前 + 腕部正转
  解2: 手肘朝上 + 肩部朝前 + 腕部翻转
  解3: 手肘朝上 + 肩部朝后 + 腕部正转
  ...
  解8: 手肘朝下 + 肩部朝后 + 腕部翻转

选解策略（实际项目中必须考虑）：

  1. 最近解：选离当前关节角最近的那组（||q - q_current|| 最小）→ 运动最平滑。
  2. 无碰撞解：排除会导致自碰撞或环境碰撞的解。
  3. 限位内解：排除超出关节限位的解。
  4. 能量最小解：选关节运动量最小的那组。

本项目的 IKSolver.solve() 用的是雅可比迭代法，从当前关节角出发迭代收敛——这天然倾向于"最近解"，因为迭代起点是当前位形。

4.6 工作空间（Workspace）

工作空间 = 末端执行器能够到达的所有位置的集合。

2 连杆平面臂的工作空间可视化：

            Z ↑
              │
        ┌─────┼─────┐
       ╱  可达  │  可达  ╲
      ╱   区域  │  区域   ╲
     ╱          │          ╲
    ●  min_reach│  max_reach ●
    ╲  (内环)   │   (外环)  ╱
     ╲          │          ╱
      ╲  不可达  │  不可达  ╱
        └─────┼─────┘
              │
  ────────────┼────────────→ X
              原点
             (基座)

  工作空间 = 圆环（annulus）
  内半径 = |L1 - L2|（完全折叠时末端离基座的距离）
  外半径 = L1 + L2（完全伸直时末端离基座的距离）
  
  在内环以内：够不到（手臂折叠到最短也够不着那么近）
  在外环以外：够不到（手臂伸直了也够不着那么远）
  在圆环内：可达（但某些点可能姿态受限）

对于一台 2 连杆平面臂（臂长 L1、L2）：

  • 最大半径：L1 + L2（完全伸直）
  • 最小半径：|L1 - L2|（完全折叠）
  • 工作空间是一个圆环（annulus）
import numpy as np

L1, L2 = 0.3, 0.25
max_reach = L1 + L2        # 0.55
min_reach = abs(L1 - L2)   # 0.05

def reachable(x, z):
    r = np.hypot(x, z)
    return min_reach <= r <= max_reach

print(reachable(0.4, 0.2))   # True
print(reachable(0.6, 0.0))   # False（太远）

实际机械臂的工作空间更复杂，因为：

  • 每个关节都有角度限位（不是 360° 随便转）
  • 连杆之间可能自碰撞
  • 有些位置虽然能到，但姿态受限制（够得到但转不过来）

6 轴机械臂的工作空间（概念图）：

  6 轴臂的工作空间不是完美的圆环/球体，而是一个"变形的土豆"：
  
        ┌─────────────┐
       ╱    可达区域    ╲
      ╱  ┌─────────┐  ╲
     ╱  ╱  内部空洞   ╲  ╲  ← 某些角度因为关节限位
    │  │   (够不到)    │  │    导致内部有"洞"
     ╲  ╲             ╱  ╱
      ╲  └─────────┘  ╱
       ╲               ╱
        └─────────────┘
     ← 边界是"锯齿状"的
       因为不同关节限位组合
       导致外边界不光滑

💡 调试经验：如果 IK 老是解不出来，第一件事就是检查目标点是否在工作空间内。这是最常见的原因。

快速检查方法：计算目标点到基座的距离 r = np.linalg.norm(target_pos - base_pos)，如果 r > L1+L2+L3（所有连杆长度之和），那肯定够不到。这是一个粗略但快速的判断。

4.7 雅可比矩阵（Jacobian）——IK 的关键工具

雅可比回答一个问题：“如果关节 i 稍微动一点，末端会往哪个方向移动多少？”

雅可比矩阵的直观解释：

想象机械臂的末端是一个"小球"，6 个关节是 6 只"手"在推它。每只手推一下，小球都会往某个方向动一点。

雅可比矩阵的每一列，就是"第 i 只手推一下（关节 i 动 1 弧度），小球往哪个方向动多少"：

J = [ j1  j2  j3  j4  j5  j6 ]
     │   │   │   │   │   │
     │   │   │   │   │   └─ 第6关节动1弧度，末端的位移方向和大小
     │   │   │   │   └───── 第5关节动1弧度，末端的位移方向和大小
     │   │   │   └───────── 第4关节动1弧度，末端的位移方向和大小
     │   │   └───────────── 第3关节动1弧度，末端的位移方向和大小
     │   └───────────────── 第2关节动1弧度，末端的位移方向和大小
     └───────────────────── 第1关节动1弧度，末端的位移方向和大小

每一列 j_i 是一个 3 维向量 (dx, dy, dz)，表示"关节 i 动 1 弧度时末端的位移"

数学上，它是末端位置对关节角的偏导数矩阵：

J = ∂x/∂q     形状：(3, 6)  —— 3 行（x,y,z），6 列（6 个关节）

于是有核心关系式：

Δx = J · Δq
  • Δq（6 维）：关节的微小变化
  • Δx（3 维）：末端的微小位移

这个关系的流程图：

乘以雅可比 J

J 是线性映射
小位移下近似成立

求伪逆 J⁺

Δq = J⁺ · Δx

关节微动 Δq
(6维)

末端微动 Δx
(3维)

Δx = J · Δq

目标末端位移 Δx

关节增量 Δq

最小二乘最优解

反过来求 IK：

Δq = J⁺ · Δx       （J⁺ 是伪逆，见 3.8 节）

这就是雅可比迭代法的核心，第 06 章会完整实现它。

雅可比迭代法的完整流程：

给定目标位置 x_target，当前关节角 q_current：

1. 用 FK 计算当前末端位置 x_current = FK(q_current)
2. 计算误差 Δx = x_target - x_current
3. 如果 ||Δx|| < 阈值（比如 1mm），收敛，退出
4. 计算当前位形下的雅可比 J(q_current)
5. 计算关节增量 Δq = J⁺ · Δx（或 DLS 版本）
6. 更新关节角 q_new = q_current + α · Δq（α 是步长，通常 0.1~0.6）
7. 回到步骤 1，重复直到收敛或达到最大迭代次数

💡 为什么需要步长 α？ 因为 Δx = J · Δq 只在微小位移下成立（线性近似）。如果 Δx 很大，直接用 Δq = J⁺·Δx 会超调。所以乘以一个小步长 α（比如 0.6），每次只走一部分，多迭代几次。这就是"梯度下降"的思想。

MuJoCo 里怎么算雅可比

MuJoCo 内置了高效的雅可比计算：

jacp = np.zeros((3, model.nv))      # 平移雅可比 (3, nv)
jacr = np.zeros((3, model.nv))      # 旋转雅可比 (3, nv)

mujoco.mj_jacSite(model, data, jacp, jacr, site_id)
# jacp[:, :6] 就是 6 个关节对末端位置的雅可比

mj_jacSite 参数详解：

参数含义形状
modelMuJoCo 模型MjModel
dataMuJoCo 数据（当前状态）MjData
jacp输出：平移雅可比（位置对关节的偏导）(3, nv)
jacr输出：旋转雅可比（姿态对关节的偏导）(3, nv)
site_id要计算雅可比的 site（末端点）的 IDint

model.nv 是速度变量数。本项目的机械臂有 8 个自由度（6 个关节 + 2 个夹爪手指，占 qpos 的前 8 位）；加载的完整模型还包含目标物的 freejoint（7 个位置自由度 + 6 个速度自由度），因此完整模型 model.nq = 15、model.nv = 14。
取 jacp[:, :6] 只保留机械臂的 6 个关节。

为什么有两个雅可比？ 因为末端有 6 个自由度（位置 3 + 朝向 3），所以完整的雅可比是 (6, 6)。jacp 是前 3 行（位置），jacr 是后 3 行（朝向）。做"只控制位置"的 IK 时，只用 jacp。

从名字到 ID：mj_name2id

MuJoCo 内部用整数 ID 索引 body、joint、geom、site 等对象，但代码里通常只知道名字。用 mj_name2id 把名字转成 ID：

import mujoco

# 查找叫 "tip" 的 site 的 ID
tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")

# 查找叫 "link1" 的 body 的 ID
link1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "link1")

# 查找叫 "j1" 的 joint 的 ID
j1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "j1")

# 用 ID 读取数据
ee_pos = data.site_xpos[tip_id]       # 末端位置
ee_mat = data.site_xmat[tip_id]        # 末端旋转矩阵（9维扁平）

mj_name2id 的三个参数：

参数含义常用取值
modelMuJoCo 模型MjModel 对象
type对象类型枚举mjtObj.mjOBJ_BODY / mjOBJ_JOINT / mjOBJ_GEOM / mjOBJ_SITE
name对象的名字XML 里 name="..." 的值

⚠️ mujoco 3.x 的枚举写法变了。旧版写 mujoco.mjOBJ_SITE，3.x 必须写 mujoco.mjtObj.mjOBJ_SITE。如果报 AttributeError: module 'mujoco' has no attribute 'mjOBJ_SITE'，就是用了旧写法。

如果名字不存在，mj_name2id 返回 -1。好的代码应该检查返回值，避免用 -1 去索引数组（会取到最后一个元素，导致难以排查的 bug）。

计算运动学：mj_forward

在读取 data.site_xpos、data.qpos 派生量之前，必须先调用 mujoco.mj_forward(model, data)。它根据当前的关节角 qpos，计算所有 body 和 site 的位置、速度、加速度——也就是做一次正运动学计算。

# 设置关节角
data.qpos[:3] = [0.4, -0.6, 0.3]

# 必须先 forward，否则 site_xpos 还是上一次的旧值！
mujoco.mj_forward(model, data)

# 现在可以安全读取
print(data.site_xpos[tip_id])    # 正确的末端位置

典型流程（每次需要读取末端状态时）：

1. 设置 data.qpos（关节角）
2. 调用 mujoco.mj_forward(model, data)  ← 不能省！
3. 读取 data.site_xpos / site_xmat / xquat 等

⚠️ 忘记 mj_forward 是新手最常见的 bug 之一。表现为：改了关节角，但末端位置没变，或者雅可比是过期值。mj_jacSite 只读取 data 中已有的运动学结果——改 qpos 后必须先 mj_forward，否则得到的是过期雅可比（第 06 章的 DLS 求解器每次迭代都会先 mj_forward 再取雅可比）。

4.8 奇异位形（Singularity）

奇异位形 = 机械臂"卡住"的姿势，此时它在某个方向上无法移动末端，或者需要无限大的关节速度。

奇异位形的生活类比：

想象你站在一个房间里，手臂完全伸直去够远处的东西。这时候：

  • 你想再往远够 1 厘米——做不到，因为手臂已经伸到最长了。
  • 你的肘关节"锁死"了，无法再通过弯肘来调整手的位置。
  • 这就是"边界奇异"——手臂在工作空间的边缘，某个方向的自由度丢失了。

三种典型奇异

1. 边界奇异（Outstretched）
   手臂完全伸直，到达工作空间最外缘
   ┌───────────●  ← 无法再往外伸
   大臂和小臂成一条直线，肘关节"锁死"

2. 腕部奇异（Wrist singularity）
   第 4 轴和第 6 轴共线
   ────●──○──   两个旋转轴重合，失去一个自由度
   就像你的手腕转到某个角度时，"翻转"和"旋转"变成了同一个动作

3. 肩部奇异（Shoulder singularity）
   腕部中心正好落在第 1 轴的旋转轴上
   此时 J1 旋转不会改变末端位置（因为末端在旋转轴上）
   就像你站在原地转圈圈，但手一直指着正上方——手的位置没变

奇异检测与处理流程图：

否（远离奇异）

是（接近奇异）

否

是

当前关节角 q

计算雅可比 J(q)

SVD 分解求奇异值
σ₁ ≥ σ₂ ≥ σ₃

最小奇异值
σ_min < 阈值?

用标准伪逆
Δq = J⁺·Δx

用阻尼最小二乘 DLS
Δq = Jᵀ(JJᵀ+λ²I)⁻¹Δx

更新关节角
q = q + α·Δq

收敛?

完成

数学上发生了什么

看雅可比的奇异值分解（SVD）。若 J 的奇异值是 σ₁ ≥ σ₂ ≥ ... ，则伪逆会按 1/σᵢ 放大对应方向：

  • 远离奇异：所有 σᵢ 都够大 → 1/σᵢ 温和 → 解正常。
  • 接近奇异：某个 σᵢ → 0 → 1/σᵢ → ∞ → 关节增量爆炸。
  • 完全奇异：σᵢ = 0 → 伪逆把该方向直接置零（SVD 的约定），反而"安静"。

用代码计算奇异值：

import numpy as np

# 只需要奇异值时，加 compute_uv=False（性能优化，不计算 U 和 V 矩阵）
sigma = np.linalg.svd(J, compute_uv=False)
print(f"奇异值 σ = {sigma}")
print(f"最小奇异值 σ_min = {sigma.min()}")

# 判断是否接近奇异
if sigma.min() < 1e-4:
    print("警告：接近奇异位形，建议使用 DLS 而不是纯伪逆")

💡 compute_uv=False 是重要的性能优化。完整的 SVD 会返回 U、Σ、V 三个矩阵，其中 U 和 V 在只需要奇异值时完全用不上。加了 compute_uv=False 后，NumPy 会跳过 U 和 V 的计算，速度快好几倍。在 IK 迭代中每步都要检查奇异性时，这个优化很关键。

⚠️ 重要细节：真正会炸的是"接近奇异"，不是"完全奇异"。
完全奇异时目标方向彻底不可达，伪逆干脆给 0；
而"差一点点就伸直"时，伪逆为了够到那个方向，会命令关节转好几圈——这才是机械臂突然抽搐的根源。

实测：伪逆爆炸 vs DLS

用本章配套的 3 连杆平面臂实测（手臂"几乎伸直"，要求末端沿径向再伸 1 cm）：

离奇异的距离 ε最小奇异值 σ₂‖Δq‖ 伪逆‖Δq‖ DLS
0.30.03790.262 rad0.205 rad
0.10.01270.788 rad0.226 rad
0.030.00382.63 rad0.092 rad
0.010.00137.89 rad（452°）0.032 rad
0.0030.000426.3 rad0.010 rad
0.0010.000178.9 rad（≈12 圈）0.003 rad

结论：越接近奇异，伪逆放得越离谱（要转 12 圈！），而 DLS 始终把 ‖Δq‖ 压在很小的范围。

如何解决：阻尼最小二乘（DLS）

标准伪逆在奇异附近会爆炸。阻尼最小二乘加一个小阻尼项：

# 标准伪逆（接近奇异时会爆炸）
dq = np.linalg.pinv(J) @ dx

# 阻尼最小二乘 DLS（稳定）
λ = 0.02                                  # 阻尼系数
JJT = J @ J.T                             # (3, 3)
damped = JJT + λ**2 * np.eye(3)
dq = J.T @ np.linalg.solve(damped, dx)    # (6,)

数学上，DLS 把 1/σ 换成了 σ/(σ² + λ²)：

  • σ 很大时 → ≈ 1/σ（和伪逆一样精确）
  • σ → 0 时 → ≈ σ/λ² → 0（自动抑制，不会爆炸）

阻尼的作用：牺牲一点精度，换取数值稳定。λ 越大越稳定但越迟钝，通常取 0.01 ~ 0.1。

📌 本项目 pick_and_place.py 用的就是 DLS，阻尼系数 IK_DAMPING = 0.02。第 06 章会详解。

4.9 冗余（Redundancy）

自由度 > 任务所需时称为冗余。

  • 6 轴臂做「只控制位置」的任务（3 个约束）：有 3 个冗余自由度
  • 7 轴臂（如 KUKA LBR）：有 1 个冗余自由度

冗余的好处：同一末端位置有无穷多组关节解，你可以从中挑选满足额外目标的那一组：

  • 远离关节限位
  • 避开障碍物
  • 最小化能量/运动量

如何利用冗余：在 IK 的零空间（null space）里加一个偏置项。

# 在解出 dq 后，加上"朝关节中心靠拢"的偏置（示意写法）
center_bias = 0.05 * (joint_centers - q_current)
dq += center_bias

这样机械臂在满足末端位置的同时，会自动避开极限位姿。

⚠️ 上面是简化示意：严格实现应先把偏置投影到雅可比零空间 N = I − J⁺J 再叠加（dq += N @ center_bias）。直接相加会让偏置与主任务"拔河"——本项目实测末端会停滞在离目标约 6.5 cm 处。投影版的做法与实测数据见第 06 章。

📌 本项目 IKSolver.solve() 里就用了这个技巧（IK_CENTER_BIAS = 0.05，偏置经零空间投影后叠加）。

4.10 常见机械臂构型速览

构型特点代表
关节型（Articulated）6 个旋转关节，最通用本书的 Epson CX4、KUKA、ABB、Fanuc
SCARA水平面内快、垂直方向刚装配、拾放
Delta（并联）极快、轻载食品分拣
直角坐标（Gantry）三个直线轴，简单精确3D 打印、CNC
协作臂（Cobot）通常 7 轴，力控安全UR、Franka

本书的 Epson CX4-A601C 属于关节型 6 轴，是最经典的工业构型。

4.11 DH 参数简介（第 5 章预备知识）

DH 参数（Denavit-Hartenberg parameters）是描述机械臂连杆和关节的标准方法。它用 4 个参数描述每个连杆，然后通过齐次变换矩阵串联起来。

DH 的四个参数

参数符号含义本项目中的变化
连杆长度a_i相邻两关节轴之间的距离固定（连杆的物理长度）
连杆扭转α_i相邻两关节轴之间的角度固定（连杆的物理结构）
关节偏移d_i沿关节轴的平移量固定（旋转关节）/ 变化（移动关节）
关节角θ_i绕关节轴的旋转角度变化（旋转关节的控制量）

对于旋转关节（本项目全部 6 个都是），θ_i 是变量，a_i、α_i、d_i 是常量。

DH 参数表的样子

一个典型的 6 轴机械臂 DH 参数表：

关节 ia_i (m)α_i (°)d_i (m)θ_i (变量)
1090d1θ1
2a200θ2
3a300θ3
40-90d4θ4
50900θ5
600d6θ6

💡 DH 参数的"魔法数字"：a_i、d_i 来自机械臂的 CAD 模型（连杆长度），α_i 来自关节轴的相对朝向（比如 J1 是 Z 轴、J2 是 Y 轴，它们的夹角就是 90°）。这些数字在建模时确定，之后不变。

第 05 章会用 DH 参数推导 Epson CX4-A601C 的完整正运动学，这里先有个概念。

DH 变换矩阵

每个关节 i 对应一个 4×4 齐次变换矩阵 T_i，由 4 个 DH 参数计算：

T_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

然后总的正运动学就是把它们乘起来：

T_total = T_1 · T_2 · T_3 · T_4 · T_5 · T_6

这就是第 02 章讲的"串联多个关节 = 矩阵依次相乘"的具体应用。

4.12 动手练

  1. 工作空间判定：2 连杆臂 L1=0.3, L2=0.25，判断 (0.4, 0.2)、(0.6, 0)、(0.1, 0.05) 是否可达。

  2. FK 手算：2 连杆平面臂，L1=L2=0.3，关节角 [45°, -30°]，求末端 (x, z)。

  3. 雅可比数值验证：用有限差分法数值计算雅可比（微扰每个关节，看末端位置怎么变），与 mujoco.mj_jacSite 的解析结果对比。

  4. 奇异性观察：让 2 连杆臂"完全伸直"（关节角都接近 0），观察伪逆解出的 dq 有多大。

参考答案与解析

练习 1：工作空间判定

L1, L2 = 0.3, 0.25
max_reach = L1 + L2    # 0.55
min_reach = abs(L1 - L2)  # 0.05

def reachable(x, z):
    r = np.hypot(x, z)
    return min_reach <= r <= max_reach

for pt in [(0.4, 0.2), (0.6, 0.0), (0.1, 0.05)]:
    r = np.hypot(*pt)
    print(f"({pt[0]}, {pt[1]}): r={r:.3f}, {'可达' if reachable(*pt) else '不可达'}")

结果：

  • (0.4, 0.2): r=0.447，在 [0.05, 0.55] 内 → 可达
  • (0.6, 0.0): r=0.600 > 0.55 → 不可达（太远，超过最大伸展）
  • (0.1, 0.05): r=0.112，在 [0.05, 0.55] 内 → 可达

练习 2：FK 手算

L1 = L2 = 0.3m, q1 = 45°, q2 = -30°

大臂末端（相对基座）：
  x1 = L1·sin(q1) = 0.3 × sin(45°) = 0.3 × 0.7071 = 0.2121
  z1 = L1·cos(q1) = 0.3 × cos(45°) = 0.3 × 0.7071 = 0.2121

小臂相对大臂的朝向 = q1 + q2 = 45° + (-30°) = 15°
小臂贡献：
  x2 = L2·sin(15°) = 0.3 × 0.2588 = 0.0776
  z2 = L2·cos(15°) = 0.3 × 0.9659 = 0.2898

末端 = 大臂末端 + 小臂贡献：
  x = 0.2121 + 0.0776 = 0.2897
  z = 0.2121 + 0.2898 = 0.5019

末端 (x, z) ≈ (0.290, 0.502)

代码验证：

def fk_2link(q1, q2, l1=0.3, l2=0.3):
    t1, t2 = q1, q1 + q2
    x = l1*np.sin(t1) + l2*np.sin(t2)
    z = l1*np.cos(t1) + l2*np.cos(t2)
    return np.array([x, z])

print(fk_2link(np.radians(45), np.radians(-30)))
# [0.2898, 0.5019]  ✓ 与手算一致

练习 3：雅可比数值验证

数值雅可比（有限差分法）的核心思想：

J[:, i] ≈ [FK(q + ε·e_i) - FK(q)] / ε

其中 e_i 是第 i 个单位向量（只有第 i 个元素为 1，其余为 0）
ε 是一个很小的数（比如 1e-6）

def numeric_jacobian(fk_func, q, eps=1e-6):
    n = len(q)
    J = np.zeros((3, n))
    for i in range(n):
        q_plus = q.copy()
        q_plus[i] += eps
        J[:, i] = (fk_func(q_plus) - fk_func(q)) / eps
    return J

与 mujoco.mj_jacSite 的解析结果对比，误差应该在 1e-5 量级（有限差分的截断误差 + 浮点误差）。

完整可运行代码见 code/ch04_robotics_concepts.py，里面用一个 3 连杆平面臂做了完整对比。

练习 4：奇异性观察

当 2 连杆臂接近完全伸直时（q1≈0, q2≈0），雅可比的最小奇异值趋近于 0，伪逆解出的 ||dq|| 会爆炸。

以 ε=0.01（离完全伸直还差 0.01 rad）为例：

  • 最小奇异值 σ₂ ≈ 0.0013
  • 伪逆解出的 ||dq|| ≈ 7.89 rad（约 452°！关节要转一圈半）
  • DLS 解出的 ||dq|| ≈ 0.032 rad（约 1.8°，温和安全）

这就是为什么实际项目中必须用 DLS 而不是纯伪逆——纯伪逆在奇异附近会让机械臂"抽搐"甚至损坏。

完整的扫描数据（ε 从 0.3 到 0.001）见 code/ch04_robotics_concepts.py 的输出。

4.13 概念地图（背下来这张表）

概念一句话代码里的对应
连杆 Link刚性的"骨头"<body>
关节 Joint提供相对运动<joint type="hinge">
自由度 DOF可独立控制的数量model.nv
位姿 Pose位置 + 朝向4×4 矩阵 / site_xpos + xmat
TCP末端工具中心点end_effector site
工作空间能到达的所有位置由臂长和限位决定
FK关节角 → 末端位姿chain.forward_kinematics(q)
IK末端位姿 → 关节角chain.inverse_kinematics(...)
雅可比 J关节速度 → 末端速度mujoco.mj_jacSite(...)
奇异卡住的位形det(J·Jᵀ) ≈ 0
冗余自由度多于约束零空间偏置

4.14 常见错误与排查

症状原因解决方法
IK 求解不收敛，关节角乱跳目标点在工作空间外先检查 np.linalg.norm(target - base) < L1+L2+L3
机械臂突然"抽搐"，关节转好几圈接近奇异位形，纯伪逆爆炸改用 DLS（阻尼最小二乘），加阻尼项 λ=0.02
IK 解出的关节角超过限位没有做限位裁剪每次迭代后 q = np.clip(q, q_min, q_max)
末端到达位置但姿态不对只用了位置雅可比 jacp，没考虑朝向需要同时用 jacp 和 jacr，构造 6×6 完整雅可比
mj_jacSite 返回的雅可比全是 0忘了调用 mj_forward 先更新状态计算雅可比前必须 mujoco.mj_forward(model, data)
多解问题：IK 每次解出不同的关节角6 轴臂同一位姿最多 8 组解加初始值偏置（从当前位形出发迭代），或加零空间偏置
关节速度指令过大导致仿真发散IK 步长 α 太大减小步长（α=0.1~0.3），或加关节速度限幅

💡 IK 调试流程：

  1. 目标点在工作空间内吗？（距离检查）
  2. 当前位形接近奇异吗？（SVD 检查最小奇异值）
  3. 用 DLS 了吗？（纯伪逆在奇异附近会炸）
  4. 步长 α 合适吗？（太大会超调，太小收敛慢）
  5. 限位裁剪了吗？（解可能超出关节范围）

4.15 扩展阅读方向

  • 《Robotics: Modelling, Planning and Control》(Siciliano et al.)：机器人学经典教材，第 2-3 章讲运动学和雅可比，第 3.3 节讲奇异位形。
  • 《Modern Robotics》(Lynch & Park)：免费在线教材，http://hades.mech.northwestern.edu/index.php/Modern_Robotics ，第 5 章讲速度运动学（雅可比）和奇异。
  • A Note on the Damped Least-Squares Method for Inverse Kinematics：https://mathweb.ucsd.edu/~mrosenba/teaching/damped_least_squares.pdf （DLS 方法的原始论文解读）
  • MuJoCo 运动学文档：https://mujoco.readthedocs.io/en/stable/modeling.html （MJCF 建模和运动学计算）

4.16 小结

  • 机械臂 = 连杆 + 关节；6 个旋转关节 → 6 自由度 → 能以任意姿态到达任意点。
  • 连杆是刚性的"骨头"（MJCF 里的 <body>），关节提供相对运动（<joint type="hinge"> 旋转 / type="slide" 移动）。
  • 自由度 = 独立控制的数量：6 轴刚好满足"位置 3 + 朝向 3"，7 轴有冗余可避障，5 轴姿态受限。
  • 位姿 = 位置 + 朝向，用 4×4 齐次矩阵表达。
  • 三个"位置"要分清：关节角 q（机器人感受到的）、末端位置（算出来的）、目标位置（你想要的）。
  • FK 唯一且简单（矩阵连乘，q → T），IK 可能无解/多解（需要迭代，T_target → q）。
  • 工作空间 = 末端能到达的所有位置；2 连杆臂是圆环，6 轴臂是"变形的土豆"（有关节限位和自碰撞）。
  • 雅可比 J (3×6) 把"关节变化"映射到"末端位移"（Δx = J·Δq），是 IK 的核心工具；每一列是"关节 i 动 1 弧度时末端的位移方向"。
  • 雅可比迭代法：反复计算误差 → 雅可比 → 关节增量 → 更新关节角，直到收敛。
  • 奇异位形下 J 秩亏，需用**阻尼最小二乘（DLS）**稳定求解；"接近奇异"比"完全奇异"更危险（伪逆会爆炸）。
  • 三种典型奇异：边界奇异（手臂伸直）、腕部奇异（J4/J6 共线）、肩部奇异（腕部在 J1 轴上）。
  • 冗余自由度可通过零空间偏置利用（如远离限位），本项目用 IK_CENTER_BIAS = 0.05。
  • 本项目 pick_and_place.py 用 DLS，阻尼系数 IK_DAMPING = 0.02。

下一章开始动手：自己实现正运动学。

上一章：03 · NumPy 实战 ｜ 下一章：05 · 正运动学 FK

# python # 开发语言
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

AICC呼叫中心系统：让排队更聪明，让坐席更会“协作”

avatar DAMO开发者矩阵

2026小鹏VLA2.0落地复盘：算力效率碾压峰值算力，解锁智驾时空认知新赛道

2026年小鹏第二代VLA模型的全面升级，彻底打破了智驾行业“堆峰值算力、拼帧级识别”的传统内卷模式，核心突破是让车载AI具备完整时空认知能力，通过30秒长时序记忆、流式并行推理、高利用率自研芯片三大核心技术，解决了传统智驾碎片化感知、响应滞后、数据成本高昂的行业痛点。同时小鹏依托模型蒸馏分层落地、技术对外输出、跨场景AI复用，完成从车企向物理AI科技平台的转型，但端侧模型能力损耗、技术收入波动、

avatar DAMO开发者矩阵

2026 HandEdit实操指南：人类视角转机器人灵巧手编辑全流程与避坑要点

HandEdit的开源落地，彻底补齐了机器人灵巧手视觉编辑领域的数据与评测短板，解决了传统方案数据成本高、构型不兼容、评测不规范、落地无效的核心痛点。其2亿+规模化样本、26类全构型覆盖、六步标准化生成流程、三维度评测体系，为具身智能视觉模型的训练、对比、迭代提供了统一行业基准。相较于传统数据采集方案，HandEdit可将机器人视觉模型训练成本降低60%以上，同时大幅提升模型的实操适配性。对于研发

avatar DAMO开发者矩阵