这一章要解决什么问题：不想自己写 DH、不想自己实现 DLS，有没有现成的库能用？

有，就是 ikpy。本章把它拆开讲清楚：三个类、一套参数、以及 4.0 版本改了哪些 API（网上大量教程已过时）。

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

"""
第 07 章配套代码：ikpy 入门。

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

内容：
  1. 环境检查（ikpy + sympy）
  2. 第一个程序：2 连杆平面臂（FK + IK）
  3. active_links_mask 的三种设置方式（实测行为）
  4. 关节限位 bounds 验证
  5. 固定连杆必须写 joint_type="fixed"（错误演示）
  6. DHLink 与第 05 章手写 DH 对照
  7. 动手练答案
"""
import warnings
import numpy as np

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

# ============================================================
# 7.2 环境检查
# ============================================================
print("=" * 66)
print("7.2  环境检查")
print("=" * 66)

import importlib

for pkg in ("ikpy", "sympy", "numpy", "scipy"):
    try:
        mod = importlib.import_module(pkg)
        print(f"  [OK]      {pkg:<8} {getattr(mod, '__version__', '?')}")
    except ImportError as e:
        print(f"  [MISSING] {pkg:<8} {e}")
        if pkg == "sympy":
            print("            -> ikpy 依赖 sympy 但不会自动安装，请执行: pip install sympy")

import ikpy
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink, DHLink

print(f"\nikpy 版本: {ikpy.__version__}  (本书按 4.0 实测编写)")


# ============================================================
# 7.4 第一个程序：2 连杆平面臂
# ============================================================
print("\n" + "=" * 66)
print("7.4  第一个程序：2 连杆平面臂")
print("=" * 66)


def make_two_link():
    """创建一条 2 连杆平面臂（在 XZ 平面内摆动，与本书 Z-up 坐标系一致）。

    ikpy 最关键的建模约定（极其容易理解错）：

        link[i] 的 origin_translation        = 从父关节到本关节的偏移
        最后一个 link 的 origin_translation  = 从最后关节到末端的距离

    只有"关节后面还有偏移"，这个关节的旋转才会改变末端位置。
    所以一台 2 连杆臂需要 4 个元素：
        OriginLink + 关节1 + 关节2 + 末端偏移(固定)
    若只写 OriginLink + 2 个关节，第二个关节后面没有偏移，它的旋转
    完全不影响末端位置 —— IK 会解出任意角度却"看起来正确"，极具迷惑性。
    """
    return Chain(
        name="two_link_arm",
        links=[
            OriginLink(),                                     # 地基
            URDFLink(name="shoulder",                         # 关节 1（基座在原点）
                     origin_translation=[0, 0, 0],
                     origin_orientation=[0, 0, 0],
                     rotation=[0, 1, 0]),                       # 绕 +Y 转（XZ 平面内俯仰）
            URDFLink(name="elbow",                            # 关节 2（距关节 1 为 L1）
                     origin_translation=[-0.30, 0, 0],
                     origin_orientation=[0, 0, 0],
                     rotation=[0, 1, 0]),
            URDFLink(name="tip",                              # 末端偏移（距关节 2 为 L2）
                     origin_translation=[0, 0, 0.30],
                     origin_orientation=[0, 0, 0],
                     rotation=None, joint_type="fixed"),
        ],
        active_links_mask=[False, True, True, False],          # 地基与末端不参与优化
    )


# ============ 两个反面教材 ============
with warnings.catch_warnings():
    warnings.simplefilter("ignore")

    # 反面教材 A：连杆沿 X、却绕 X 转 —— 连杆方向与旋转轴平行，旋转不产生位移
    chain_bad_axis = Chain(name="bad_axis", links=[
        OriginLink(),
        URDFLink(name="l1", origin_translation=[-0.30, 0, 0],
                 origin_orientation=[0, 0, 0], rotation=[-1, 0, 0]),
        URDFLink(name="l2", origin_translation=[-0.30, 0, 0],
                 origin_orientation=[0, 0, 0], rotation=[-1, 0, 0]),
    ], active_links_mask=[False, True, True])
    T_badA = chain_bad_axis.forward_kinematics([0, np.radians(30), np.radians(-45)])
    print(f"反面教材A 连杆沿-X + 绕-X转: q=[0,30deg,-45deg] -> 末端 {np.round(T_badA[:3, 3], 4)}")
    print("          无论关节转多少，末端都停在 (-0.6,0,0) —— 旋转没起作用！\n")

    # 反面教材 B：少了末端偏移 —— 最后一个关节不影响末端位置
    chain_no_tip = Chain(name="no_tip", links=[
        OriginLink(),
        URDFLink(name="j1", origin_translation=[0, 0, 0.00],
                 origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
        URDFLink(name="j2", origin_translation=[0, 0, 0.30],
                 origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
    ], active_links_mask=[False, True, True])
    T_B1 = chain_no_tip.forward_kinematics([0, np.radians(30), np.radians(-45)])
    T_B2 = chain_no_tip.forward_kinematics([0, np.radians(30), np.radians(0)])
    print(f"反面教材B 缺末端偏移: q2=-45deg -> {np.round(T_B1[:3, 3], 5)}")
    print(f"                     q2=   0deg -> {np.round(T_B2[:3, 3], 5)}")
    print("          两个完全不同的关节角，末端位置却一样 —— 因为 j2 后面没有偏移！\n")


with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    chain = make_two_link()

print("links:", [l.name for l in chain.links])
print("active_links_mask:", chain.active_links_mask)

# 正运动学 FK
# 注意：forward_kinematics 需要【完整】数组，长度 = len(chain.links)
# 这里 = base(1) + shoulder(1) + elbow(1) + tip(1) = 4 个
q = [0.0, np.radians(30), np.radians(-45), 0.0]
T = chain.forward_kinematics(q)
print(f"\nFK: 关节角 [30°, -45°] -> 末端 {T[:3, 3]}")

# 逆运动学 IK
q_sol = chain.inverse_kinematics(target_position=T[:3, 3])
print(f"IK: 解(完整数组) = {np.round(q_sol, 6)}")
print(f"    关节角 = {np.round(np.degrees(q_sol[1:3]), 3)}°   # 应回到 30° / -45°")

# 往返验证
T_back = chain.forward_kinematics(q_sol)
print(f"往返误差 = {np.linalg.norm(T_back[:3, 3] - T[:3, 3]):.2e}")

# ============================================================
# 7.5 active_links_mask 的实测行为
# ============================================================
print("\n" + "=" * 66)
print("7.5  active_links_mask 实测行为")
print("=" * 66)

links_2 = [
    OriginLink(),
    URDFLink(name="shoulder", origin_translation=[0, 0, 0],
             origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
    URDFLink(name="elbow", origin_translation=[-0.30, 0, 0],
             origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
    URDFLink(name="tip", origin_translation=[-0.30, 0, 0],
             origin_orientation=[0, 0, 0], rotation=None, joint_type="fixed"),
]

print("  --- 情况 1：不传 mask（会警告）---")
with warnings.catch_warnings(record=True) as w:
    warnings.simplefilter("always")
    c1 = Chain(links=links_2)
    print(f"  mask = {c1.active_links_mask}")
    for ww in w:
        print(f"  Warning: {str(ww.message)[:80]}...")

print("\n  --- 情况 2：传 [False, True, True, False]（推荐写法）---")
c2 = Chain(links=links_2, active_links_mask=[False, True, True, False])
print(f"  mask = {c2.active_links_mask}   # 原样保留，无警告")

print("\n  --- 情况 3：传 [False, True, False, False]（锁死关节 2）---")
c3 = Chain(links=links_2, active_links_mask=[False, True, False, False])
print(f"  mask = {c3.active_links_mask}")
q_locked = c3.inverse_kinematics(target_position=[0.0, 0.0, 0.42])
print(f"  IK 解 = {np.round(q_locked, 4)}   # 第 2 个关节保持 0，被锁死")

print("\n  规律: OriginLink -> False；真实关节 -> True；固定偏移连杆 -> False")

# ============================================================
# 7.6 关节限位 bounds
# ============================================================
print("\n" + "=" * 66)
print("7.6  关节限位 bounds")
print("=" * 66)

with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    chain_lim = Chain(
        name="limited_arm",
        links=[
            OriginLink(),
            URDFLink(name="j1", origin_translation=[-0.30, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=[0, 1, 0],
                     bounds=(-3.14, 3.14)),
            URDFLink(name="j2", origin_translation=[-0.30, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=[0, 1, 0],
                     bounds=(-1.5, 1.5)),          # ← 限位收紧到 ±1.5 rad
            URDFLink(name="ee", origin_translation=[-0.25, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=None,
                     joint_type="fixed"),          # 末端固定偏移（小臂）
        ],
        active_links_mask=[False, True, True, False],
    )

print("各关节 bounds:", [l.bounds for l in chain_lim.links])
print("(连杆沿 -X，绕 Y 转 -> 在 XZ 平面运动；总臂长 0.30+0.25 = 0.55)\n")

for tgt in ([-0.20, 0.0, 0.45], [-0.10, 0.0, 0.50], [-0.35, 0.0, -0.20]):
    qq = chain_lim.inverse_kinematics(target_position=tgt)
    TT = chain_lim.forward_kinematics(qq)
    err = np.linalg.norm(TT[:3, 3] - np.array(tgt))
    hit = " ★撞限位" if abs(abs(qq[2]) - 1.5) < 1e-6 else ""
    print(f"  目标 {tgt} -> 解={np.round(qq[1:3], 4)}  "
          f"末端={np.round(TT[:3, 3], 5)}  误差={err:.2e}{hit}")

print("\n  >> 若目标需要超出限位才能到达，j2 会被钳在 ±1.5，末端因此有残差。")
print("     ikpy 不会报错，而是返回'尽力而为'的解。")

# ============================================================
# 7.5.4 固定连杆必须写 joint_type="fixed"（错误演示）
# ============================================================
print("\n" + "=" * 66)
print("7.5.4  固定连杆的坑（错误 vs 正确写法）")
print("=" * 66)

# 错误写法
try:
    URDFLink(name="bad_ee", origin_translation=[0, 0, 0.045],
             origin_orientation=[0, 0, 0], rotation=None)      # 缺 joint_type
except ValueError as e:
    print(f"  ❌ 错误写法报错:\n     {type(e).__name__}: {e}")

# 正确写法
good = URDFLink(name="good_ee", origin_translation=[0, 0, 0.045],
                origin_orientation=[0, 0, 0], rotation=None,
                joint_type="fixed")
print(f"  ✅ 正确写法 OK: name={good.name}, joint_type={good.joint_type}")

# ikpy 3.x 的旧参数名（会报错）
print("\n  旧版(ikpy 3.x)写法在 4.0 下会报错:")
try:
    URDFLink(name="x", translation_vector=[0, 0, 0.3],
             orientation=[0, 0, 0], rotation=[0, -1, 0])
except TypeError as e:
    print(f"     TypeError: {e}")

# ============================================================
# 7.7 DHLink 与第 05 章手写 DH 对照
# ============================================================
print("\n" + "=" * 66)
print("7.7  DHLink vs 第 05 章手写 DH")
print("=" * 66)


def dh_transform(a, alpha, d, theta):
    """第 05 章手写的标准 DH 变换矩阵。"""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct, -st * ca,  st * sa, a * ct],
        [st,  ct * ca, -ct * sa, a * st],
        [0.0,      sa,       ca,      d],
        [0.0,     0.0,      0.0,    1.0],
    ])


with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    chain_dh = Chain(
        name="dh_arm",
        links=[
            OriginLink(),
            DHLink(name="j1", d=0.20, a=0.30, alpha=0.0, theta=0.0),
            DHLink(name="j2", d=0.00, a=0.25, alpha=0.0, theta=0.0),
        ],
        active_links_mask=[False, True, True],
    )

q1, q2 = np.radians(30), np.radians(45)
T_dh = chain_dh.forward_kinematics([0.0, q1, q2])
T_manual = dh_transform(0.30, 0.0, 0.20, q1) @ dh_transform(0.25, 0.0, 0.0, q2)

print(f"  ikpy DHLink    = {np.round(T_dh[:3, 3], 6)}")
print(f"  第05章手写 DH  = {np.round(T_manual[:3, 3], 6)}")
print(f"  一致: {np.allclose(T_dh[:3, 3], T_manual[:3, 3])}")
print("\n  >> 证明第 05 章手写的 DH 变换矩阵与 ikpy 的实现完全等价。")

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

# 练习 1：2 连杆 FK
with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    ch1 = make_two_link()
T1 = ch1.forward_kinematics([0.0, np.radians(30), np.radians(-45), 0.0])
print(f"练习1 末端位置 = {np.round(T1[:3, 3], 6)}")
# 手算交叉验证: 末端 = Ry(q1) @ (t2 + Ry(q2) @ t3)
#   t2=(-0.3,0,0) (elbow偏移), t3=(0,0,0.3) (tip偏移) —— 与 make_two_link() 一致
_th1, _th2 = np.radians(30), np.radians(-45)


def _ry(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])


_manual = _ry(_th1) @ (np.array([-0.3, 0, 0]) + _ry(_th2) @ np.array([0, 0, 0.3]))
print(f"练习1 手算验证 = {np.round(_manual, 6)}   一致: {np.allclose(T1[:3, 3], _manual)}")

# 练习 2：mask 对比
print("练习2 见 7.5 节：不传 mask -> 全 True + 警告；传 [F,T,T] -> 原样保留")

# 练习 3：bounds=(-1.0, 1.0)
with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    ch3 = Chain(
        name="tight", links=[
            OriginLink(),
            URDFLink(name="j1", origin_translation=[-0.30, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=[0, 1, 0],
                     bounds=(-3.14, 3.14)),
            URDFLink(name="j2", origin_translation=[-0.30, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=[0, 1, 0],
                     bounds=(-1.0, 1.0)),          # ← 更紧的限位
            URDFLink(name="ee", origin_translation=[-0.25, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=None,
                     joint_type="fixed"),
        ], active_links_mask=[False, True, True, False])
# 给一个需要大幅弯曲才能到达的目标（靠近基座）
q_tight = ch3.inverse_kinematics(target_position=[-0.10, 0.0, 0.10])
print(f"练习3 解 = {np.round(q_tight[1:3], 4)}  "
      f"j2 是否被钳在 ±1.0: {abs(abs(q_tight[2]) - 1.0) < 1e-3}")

# 练习 4：DHLink 重建 3 自由度臂（含移动关节）
print("\n练习4 3 自由度臂（2 转 + 1 移）:")
with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    ch4 = Chain(
        name="dh_3dof", links=[
            OriginLink(),
            DHLink(name="j1", d=0.20, a=0.30, alpha=0.0, theta=0.0),
            DHLink(name="j2", d=0.00, a=0.25, alpha=0.0, theta=0.0),
        ], active_links_mask=[False, True, True])
# 注意：ikpy 的 DHLink 不直接支持"移动关节变量 d"，
# 移动关节需要用 URDFLink(translation=...) 实现
with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    ch4b = Chain(
        name="urdf_3dof", links=[
            OriginLink(),
            # 与 DH 参数对应：j1=d=0.20(基座高)+a=0.30(臂长), 绕Z转
            URDFLink(name="j1", origin_translation=[0, 0, 0],
                     origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
            URDFLink(name="j2", origin_translation=[0.30, 0, 0.20],
                     origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
            URDFLink(name="j3", origin_translation=[0.25, 0, 0],
                     origin_orientation=[0, 0, 0],
                     translation=[0, 0, 1],          # 移动关节沿Z（对应DH的d变量）
                     joint_type="prismatic"),        # ← 必须显式指定！
        ], active_links_mask=[False, True, True, True])
q4 = [0.0, np.radians(30), np.radians(45), 0.1]
T4 = ch4b.forward_kinematics(q4)
T4_dh = (dh_transform(0.30, 0, 0.20, np.radians(30))
         @ dh_transform(0.25, 0, 0.0, np.radians(45))
         @ dh_transform(0.0, 0, 0.1, 0.0))
print(f"  URDFLink 版 (含移动关节) = {np.round(T4[:3, 3], 6)}")
print(f"  手写 DH 版               = {np.round(T4_dh[:3, 3], 6)}")
print(f"  一致: {np.allclose(T4[:3, 3], T4_dh[:3, 3])}")
print("  >> 移动关节用 URDFLink(translation=轴方向) 实现，对应 DH 里的 d 变量。")

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

本章学习目标

学完本章，你将能够：

  1. 理解 ikpy 在仿真栈中的定位——它只做运动学，与 MuJoCo 互补
  2. 掌握 ikpy 的三个核心类：Chain、OriginLink、URDFLink（及 DHLink）
  3. 正确构建一条运动链——理解 origin_translation 的语义，避免"最后一个关节不起作用"的坑
  4. 理解 active_links_mask 的作用——哪些关节参与 IK 优化
  5. 设置关节限位 bounds——理解 ikpy 的"尽力而为"行为
  6. 用 DHLink 建链——与第 05 章的手写 DH 交叉验证
  7. 理解 ikpy 内部机制——符号矩阵 FK + scipy 优化 IK
  8. 排查 ikpy 常见错误——API 变更、固定连杆类型、mask 长度等

前置知识：本章需要第 05 章的正运动学概念和第 06 章的逆运动学基本概念。ikpy 是这两章理论的"现成实现"，理解理论后再看 ikpy 会非常顺畅。

7.1 ikpy 在仿真栈里的位置

回顾第 00 章的分层：

第 3 层  任务与算法   →  dm_control
第 2 层  物理仿真     →  MuJoCo
第 1 层  运动学       →  ikpy        ← 本章
第 0 层  数学         →  NumPy

ikpy 只做一件事：运动学。

它是纯几何计算，不涉及质量、重力、碰撞、摩擦。给它一组关节角，它算出末端在哪（FK）；给它一个末端目标，它算出关节角（IK）。仅此而已。

💡 生活类比：ikpy 就像计算器里的"几何计算"功能——你输入边长和角度，它算出对角线长度。它不关心这个物体是什么材质、会不会摔碎、有没有撞到别的东西。那些是物理仿真器（MuJoCo）的事。

这意味着：

ikpy 能做的ikpy 不能做的
✅ 正/逆运动学❌ 物理仿真（会不会倒、会不会撞）
✅ 关节限位约束❌ 动力学（力矩、能耗）
✅ 姿态控制❌ 碰撞检测
✅ 可视化运动链❌ 渲染真实场景

💡 ikpy 与 MuJoCo 是互补的：ikpy 告诉你"关节应该转到这里"，MuJoCo 告诉你"实际转过去会发生什么"。本项目两者都用。

7.2 安装

pip install ikpy

💡 关于依赖 sympy：ikpy 4.0 已在包元数据中声明依赖 sympy，正常 pip install ikpy 会自动把它带上；只有在 --no-deps、离线安装 wheel 等场景下才可能缺失。如果你看到：

ModuleNotFoundError: No module named 'sympy'

执行 pip install sympy 即可。ikpy 用 sympy 做符号化的变换矩阵推导，这是它的核心机制（也是它比纯数值实现更灵活的原因）。

验证安装：

import ikpy
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink, DHLink
print(ikpy.__version__)      # 本书实测：4.0.0

💡 为什么 ikpy 用 sympy（符号计算）：ikpy 在构建 Chain 时，用 sympy 为每个连杆推导出符号化的变换矩阵（包含符号变量 θ1, θ2, …）。然后在 FK/IK 时，把具体数值代入符号表达式。这样做的好处是：

  • 推导一次，反复使用（构建链时推导，计算时代入）
  • 支持任意轴、任意欧拉角表示（符号计算不关心具体数值）
  • 可以自动计算雅可比（对符号表达式求导）

代价是比纯数值实现慢——这也是为什么第 06 章的自研 DLS 用 MuJoCo 的解析雅可比会快约一个数量级（见 8.7 实测）。

7.3 三个核心类

7.3.1 类的层次

Link（抽象基类）
 ├── OriginLink    链的起点，固定不动，不提供变换
 ├── URDFLink      URDF 风格的连杆（最常用）
 └── DHLink        DH 参数风格的连杆（呼应第 05 章）
类用途什么时候用
Chain把一堆 Link 串成一条链，提供 FK/IK永远是入口
OriginLink链的"地基"每条链的第一个元素
URDFLink一个关节 + 一段偏移描述大多数机器人
DHLink用 DH 四参数描述一个关节你手上有 DH 表时

💡 类比：Chain 是一串珍珠项链，Link 是每一颗珍珠。OriginLink 是项链的搭扣（固定的起点），URDFLink 是普通珍珠（每个都有自己的位置和旋转轴），DHLink 是用 DH 参数描述的特殊珍珠。

7.3.2 URDFLink 的参数（最重要）

ikpy 4.0 的实测签名：

URDFLink(
    name: str,
    origin_translation: np.ndarray,     # ← 注意！不是 translation_vector
    origin_orientation: np.ndarray,
    rotation: np.ndarray = None,        # 旋转关节的轴
    translation: np.ndarray = None,     # 移动关节的轴
    bounds: tuple = None,               # 关节限位 (lower, upper)
    angle_representation: str = 'rpy',
    use_symbolic_matrix: bool = True,
    joint_type: str = 'revolute',       # 'revolute' | 'prismatic' | 'fixed'
)
参数对应 URDF 的什么说明
origin_translation<origin xyz="..."/>相对父连杆的平移
origin_orientation<origin rpy="..."/>相对父连杆的旋转（欧拉角）
rotation<axis xyz="..."/>旋转关节的转轴
translation<axis xyz="..."/>移动关节的移动方向
bounds<limit lower upper/>关节限位 (lower, upper)
joint_type<joint type="..."/>revolute / prismatic / fixed

参数详解：

  • origin_translation：这是最重要的参数。它表示"这个连杆的关节原点，相对于父连杆关节原点的偏移"。注意：不是"连杆长度"，而是"关节到关节的距离"。详见 7.4 节。
  • origin_orientation：连杆坐标系相对于父坐标系的旋转，用 RPY（Roll-Pitch-Yaw）欧拉角表示。大多数情况下是 [0,0,0]。
  • rotation：旋转关节的转轴，单位向量。如 [0,1,0] 表示绕 Y 轴旋转。旋转关节必须设置这个参数。
  • translation：移动关节的移动方向，单位向量。如 [0,0,1] 表示沿 Z 轴移动。移动关节必须设置这个参数，且 joint_type 必须为 'prismatic'。
  • bounds：关节限位 (lower, upper) 元组，单位是弧度（旋转关节）或米（移动关节）。ikpy 会把它传给 scipy 优化器的 bounds 参数。
  • joint_type：关节类型。'revolute'（旋转，默认）、'prismatic'（移动）、'fixed'（固定）。固定连杆必须显式写 joint_type="fixed"。

🔥 本书实测踩坑（第 1 号）：

网上旧教程（ikpy 3.x）ikpy 4.0 正确写法
URDFLink(translation_vector=[...])URDFLink(origin_translation=[...])

用旧写法会直接报错：

TypeError: URDFLink.__init__() got an unexpected keyword argument 'translation_vector'

本书所有代码都按 4.0 实测编写。

7.3.3 Chain 的构造

Chain(
    links,                    # Link 列表
    active_links_mask=None,   # 哪些关节参与 IK 优化
    name='chain',
    urdf_metadata=None,
    jax_precompile=True,
)

Chain 构建流程图：

是

否

开始构建 Chain

创建 OriginLink() 作为链的起点

对每个关节创建 URDFLink

设置 origin_translation (相对父的偏移)

设置 rotation (旋转轴) 或 translation (移动轴)

设置 bounds (关节限位)

设置 joint_type (revolute/prismatic/fixed)

还有关节?

创建末端固定偏移 URDFLink (joint_type=fixed)

设置 active_links_mask = [False] + [True]*n + [False]

Chain(name=..., links=..., active_links_mask=...)

完成!

7.4 第一个程序：2 连杆平面臂

7.4.1 ⚠️ 先搞懂一个致命约定：origin_translation 到底量的是什么

这是 ikpy 最容易理解错、且错了之后极难自查的地方。 请务必先读这一小节。

一条 ikpy 链的整体变换是：

T_total = T₁ · T₂ · ... · Tₙ
末端位置 = T_total 作用于原点 (0,0,0)

展开后你会发现一个关键事实：

末端 = t₁ + R₁·( t₂ + R₂·( t₃ + ... ) )
       └ 关节1 前的偏移      └ 关节1→关节2   └ 关节2→末端

所以 origin_translation 的语义是：

位置origin_translation 的含义
links[1]基座 → 关节 1 的偏移
links[i]关节 i-1 → 关节 i 的偏移（第 i-1 段连杆长度）
最后一个 link最后关节 → 末端（TCP） 的距离

推论（极其重要）：

只有"关节后面还有偏移"，这个关节的旋转才会改变末端位置。

如果最后一个 link 就是你的最后一个关节，那它后面没有任何偏移——这个关节转多少度，末端都纹丝不动！

💡 生活类比：想象你手里拿着一根棍子。棍子的一端在你手里（关节），另一端是棍子的末端。如果你转动手腕（关节旋转），棍子的末端会画一个圆弧——因为棍子有长度（关节后面有偏移）。

但如果你手里什么都没拿（关节后面没有偏移），你转动手腕，"末端"就是你的手腕本身——它不会因为转动而移动位置。这就是"最后一个关节不起作用"的原因。

7.4.2 两个反面教材（实测）

反面教材 A：转轴与连杆同向

# 连杆沿 -X 方向伸展，却绕 -X 轴转 —— 旋转对连杆不产生位移
Chain(links=[
    OriginLink(),
    URDFLink(name="l1", origin_translation=[-0.30, 0, 0],
             origin_orientation=[0, 0, 0], rotation=[-1, 0, 0]),
    URDFLink(name="l2", origin_translation=[-0.30, 0, 0],
             origin_orientation=[0, 0, 0], rotation=[-1, 0, 0]),
], active_links_mask=[False, True, True])

实测（q=[0, 30°, -45°]）：

反面教材A 连杆沿-X + 绕-X转: q=[0,30deg,-45deg] -> 末端 [-0.6  0.  0.]
          无论关节转多少，末端都停在 (-0.6,0,0) —— 旋转没起作用！

💡 为什么转轴与连杆同向后旋转不产生位移：如果连杆沿 -X 轴伸展，关节也绕 X 轴旋转，那么旋转就是"绕着连杆自身的轴转"——就像转动螺丝刀，螺丝刀的尖端位置不变，只是在原地转。要让旋转产生位移，转轴必须与连杆方向垂直（或至少有垂直分量）。

反面教材 B：少了末端偏移

# 只有 OriginLink + 2 个关节，第二个关节后面没有偏移
Chain(links=[
    OriginLink(),
    URDFLink(name="j1", origin_translation=[0, 0, 0.00],
             origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
    URDFLink(name="j2", origin_translation=[0, 0, 0.30],
             origin_orientation=[0, 0, 0], rotation=[0, 1, 0]),
], active_links_mask=[False, True, True])

实测：

q2 = -45°  ->  末端 [0.15  0.  0.25981]
q2 =   0°  ->  末端 [0.15  0.  0.25981]     ← 完全一样！

两个截然不同的关节角，末端位置相同 —— 因为 j2 后面没有偏移，它的旋转被"浪费"掉了。

🔥 这个错误极具迷惑性：IK 照样能跑、照样"收敛"、误差照样接近 0，但你的机械臂少了一个有效自由度。如果你发现某个关节怎么动都不影响末端，先检查它后面有没有偏移。

7.4.3 正确的 2 连杆臂

一台真正的 2 连杆臂需要 4 个元素：

import numpy as np
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink

chain = Chain(
    name="two_link_arm",
    links=[
        OriginLink(),                                     # ① 地基
        URDFLink(name="shoulder",                         # ② 关节 1（基座在原点）
                 origin_translation=[0, 0, 0.00],
                 origin_orientation=[0, 0, 0],
                 rotation=[0, 1, 0]),                      #    绕 Y 转
        URDFLink(name="elbow",                            # ③ 关节 2（距关节 1 为 L1=0.30，沿 -X）
                 origin_translation=[-0.30, 0, 0],
                 origin_orientation=[0, 0, 0],
                 rotation=[0, 1, 0]),
        URDFLink(name="tip",                              # ④ 末端偏移（距关节 2 为 L2=0.30，沿 +Z）
                 origin_translation=[0, 0, 0.30],
                 origin_orientation=[0, 0, 0],
                 rotation=None, joint_type="fixed"),
    ],
    active_links_mask=[False, True, True, False],          # ⑤ 地基与末端不参与优化
)

# 正运动学：关节角 → 末端位姿
# 注意：forward_kinematics 需要【完整】数组，长度 = len(chain.links) = 4
q = [0.0, np.radians(30), np.radians(-45), 0.0]
T = chain.forward_kinematics(q)
print("末端位置:", T[:3, 3])

# 逆运动学：末端位置 → 关节角
q_sol = chain.inverse_kinematics(target_position=T[:3, 3])

2 连杆臂结构示意图（零位姿态侧视图，X 向右、Z 向上；两个关节都绕 Y 转，在 XZ 平面内摆动）：

                                末端 (tip)
                                  ●
                                  │
                                  │ L2 = 0.30 (origin_translation of tip, 沿 +Z)
                                  │
  基座 ●── J1 (shoulder) ●←───────● J2 (elbow)   ← 都绕 Y 旋转
       (shoulder 的 origin_translation   │
        = 0, 基座在原点)                 │
                                 L1 = 0.30 (origin_translation of elbow, 沿 -X)

实测输出：

FK: 关节角 [30°, -45°] -> 末端 [-0.337453  0.        0.439778]
IK: 解(完整数组) = [ 0.        0.523599 -0.785398  0.      ]
    关节角 = [ 30. -45.]°        ← 正好回到我们输入的 30° / -45°
往返误差 = 5.08e-09

💡 IK 从全零种子出发，收敛回了输入的 [30°, -45°]（往返误差 5.08e-09）——注意逆解不唯一（第 06 章）并不意味着每次都会返回不同的解：优化器从种子出发"就近"收敛。换用其他种子，可能收敛到别的等价构型，也可能卡在不好的谷里——第 09 章 9.3 节会专门实测 initial_position 对收敛的影响。

7.4.4 代码逐行解读

行说明
① OriginLink()每条链必须以它开头，代表固定的基座
② 关节 1origin_translation = 基座到关节 1 的偏移（这里是 0）
③ 关节 2origin_translation = 第 1 段连杆长度 L₁ = 0.30
④ 末端 tiporigin_translation = 第 2 段连杆长度 L₂ = 0.30，必须 joint_type="fixed"
⑤ active_links_maskOriginLink 与固定末端都是 False，只有真实关节是 True

手算交叉验证（验证你真的理解了）：

末端 = Ry(q1) @ (t2 + Ry(q2) @ t3)
     = Ry(30°) @ ( [-0.3,0,0] + Ry(-45°) @ [0,0,0.3] )
     = [-0.337453, 0, 0.439778]        # 与 ikpy 完全一致 ✓

💡 手算公式的含义：

  • t2 = [-0.3,0,0] 是关节 1 到关节 2 的偏移（第一段连杆长度，沿 -X）
  • t3 = [0,0,0.3] 是关节 2 到末端的偏移（第二段连杆长度，沿 +Z）
  • Ry(q2) @ t3：第二段连杆先被关节 2 的旋转改变方向
  • t2 + Ry(q2) @ t3：两段偏移在关节 2 的坐标系中相加
  • Ry(q1) @ (...)：整体再被关节 1 的旋转改变方向

这就是"先平移后旋转"的语义，与第 05 章讲的 MuJoCo 语义一致。

7.5 ⚠️ active_links_mask：ikpy 最容易搞错的地方

这个参数决定哪些关节会被 IK 优化器改变。

7.5.1 实测行为

links = [OriginLink(), shoulder, elbow, tip]      # 4 个元素

Chain(links=links)                                       # 不传 mask
# → active_links_mask = [True, True, True, True]

Chain(links=links, active_links_mask=[False, True, True, False])
# → active_links_mask = [False, True, True, False]       # 原样保留

Chain(links=links, active_links_mask=[False, True, False, False])
# → active_links_mask = [False, True, False, False]      # 原样保留

规则很简单：

  • 不传 → 全部 True（包括 OriginLink 和固定末端）
  • 传了 → 原样保留，ikpy 不会帮你改

7.5.2 不传会怎样

fixed 类型的 link（OriginLink 和末端偏移）不提供任何变换，却被标记为"可优化"，ikpy 会对每一个发出警告：

UserWarning: Link Base link (index: 0) is of type 'fixed' but set as active
UserWarning: Link tip (index: 3) is of type 'fixed' but set as active

虽然只是警告、结果通常仍正确，但应当显式设置以避免歧义。

⚠️ 注意：mask 的长度必须与 links 一致，否则 ikpy 会报错。这是改链结构时最常见的低级错误。

7.5.3 正确的设置方法

# 最简单情形：除 OriginLink 外全是真实关节（最后一个 link 不是固定偏移）
n = len(links)
active_links_mask = [False] + [True] * (n - 1)

更精细的情况（比如末端有个固定的夹爪连杆）——判断依据是"这个 link 有没有关节"，而不是它的位置：

# OriginLink + 6 个关节 + 2 个固定连杆（mount / ee）
active_links_mask = [False] + [True] * 6 + [False, False]

📌 规律：有旋转/移动关节的 link 为 True，OriginLink 与末端固定 link 为 False。

  • OriginLink → 永远 False
  • 真实关节 → True
  • 固定偏移连杆（joint_type="fixed"）→ False

⚠️ 所以本章的 4 元素链（OriginLink + 2 关节 + 末端固定）应写 [False, True, True, False]——不能照搬上面的通式 [False] + [True] * (n - 1)，否则末端固定 link 会被误设为 True 并触发警告。

7.5.4 ⚠️ 固定连杆还必须写 joint_type="fixed"

本书实测踩坑（第 2 号）：如果你创建了一个既无 rotation 也无 translation 的连杆（纯偏移），必须显式写 joint_type="fixed"：

# ❌ 错误：默认 joint_type='revolute'，但没有 rotation 轴
URDFLink(name="ee", origin_translation=[0, 0, 0.045],
         origin_orientation=[0, 0, 0], rotation=None)
# ValueError: Joint type is 'revolute' but rotation axis = False and translation axis = False

# ✅ 正确
URDFLink(name="ee", origin_translation=[0, 0, 0.045],
         origin_orientation=[0, 0, 0], rotation=None, joint_type="fixed")

💡 为什么 ikpy 不自动检测：因为 rotation=None 也可能是"用户忘了设置"，而不是"这是一个固定连杆"。ikpy 选择报错而不是猜测，避免静默错误。所以你必须显式声明 joint_type="fixed"。

7.6 关节限位：bounds

bounds 是一个 (lower, upper) 元组，ikpy 会把它传给底层优化器（scipy.optimize.least_squares 的 bounds 参数），保证解一定在限位内。

URDFLink(name="j2",
         origin_translation=[-0.06, 0.0, 0.144],
         origin_orientation=[0, 0, 0],
         rotation=[0, 1, 0],
         bounds=(-2.705260, 1.169371))     # 本书 6 轴臂的 J2 限位（弧度）

bounds 的工作原理：

scipy.optimize.least_squares(
    fun=residual_function,   # 末端误差
    x0=initial_position,     # 初始关节角
    bounds=(q_min, q_max),   # ← ikpy 把 bounds 传这里
    ...
)

scipy 的 least_squares 会在优化过程中自动把解限制在 bounds 范围内，使用的是"信任区域反射算法"（Trust Region Reflective）。

实测验证（2 连杆：L₁=0.30、L₂=0.25，连杆沿 -X 伸展、绕 Y 转，在 XZ 平面运动；J2 限位 ±1.5）：

各关节 bounds: [(-inf, inf), (-3.14, 3.14), (-1.5, 1.5), (-inf, inf)]
(连杆沿 -X，绕 Y 转 -> 在 XZ 平面运动；总臂长 0.30+0.25 = 0.55)

  目标 [-0.2, 0.0, 0.45] -> 解=[1.2693 1.1593]  末端=[-0.2   0.    0.45]  误差=1.16e-08
  目标 [-0.1, 0.0, 0.5] -> 解=[1.7647 0.4111]  末端=[-0.1  0.   0.5]  误差=1.70e-09
  目标 [-0.35, 0.0, -0.2] -> 解=[-0.6603 -1.5   ]  末端=[-0.39797  0.      -0.39181]  误差=1.98e-01 ★撞限位

前两个目标在限位内可达，误差在 1e-8 量级；第 3 个目标的 j2 被死死钳在 -1.5（限位值），末端因此留下了明显残差（19.8 cm）。

💡 限位 vs 可达性：如果目标需要超出限位才能到达，ikpy 不会报错，而是返回一个尽力而为的解（贴在限位上，末端有残差）。这和第 06 章自研 DLS 的行为一致。

工程做法：调用完 IK 后，一定要自己检查 ‖FK(q) - target‖。ikpy 不会告诉你它失败了。

7.7 DHLink：直接用 DH 参数建链

第 05 章我们手写了 DH 变换矩阵。ikpy 内置了 DHLink，参数就是 DH 四参数：

DHLink(name=None, d=0, a=0, alpha=0, theta=0, bounds=None,
       use_symbolic_matrix=True, length=0)

DHLink 参数与第 05 章 DH 参数的对应：

DHLink 参数第 05 章 DH 参数含义
ddᵢ连杆偏移（移动关节的变量）
aaᵢ连杆长度
alphaαᵢ连杆扭角
thetaθᵢ关节角（旋转关节的变量）

用它重建第 05 章那台 3 自由度臂的前两节：

chain = Chain(name="dh_arm", links=[
    OriginLink(),
    DHLink(name="j1", d=0.20, a=0.30, alpha=0.0, theta=0.0),
    DHLink(name="j2", d=0.00, a=0.25, alpha=0.0, theta=0.0),
], active_links_mask=[False, True, True])

T = chain.forward_kinematics([0, np.radians(30), np.radians(45)])

实测对比（q₁=30°, q₂=45°）：

ikpy DHLink      = [0.324512  0.391481  0.200000]
第 05 章手写 DH   = [0.324512  0.391481  0.200000]
一致: True

✅ 这证明了第 05 章我们手写的 DH 变换矩阵与 ikpy 的实现完全等价。

什么时候用 DHLink vs URDFLink：

场景用哪个
手上有 DH 参数表（论文、老手册）DHLink
手上有 URDF / MJCF 模型URDFLink（参数可直接抄）
机械臂轴系混合多样（如本书 J1 绕 Z、J2/J3 绕 Y、J4/J6 绕 X）URDFLink（更直观）

本书实战用 URDFLink，因为参数能从 MJCF 直接抄。

⚠️ 注意：DHLink 不支持移动关节。如果你的机械臂有移动关节（prismatic），需要用 URDFLink(translation=轴方向, joint_type="prismatic")。这是因为 DH 参数法中移动关节的变量是 d，但 DHLink 的实现把 theta 作为唯一变量（旋转关节），不支持把 d 作为变量。

7.8 ikpy 的"黑箱"里是什么

知道 ikpy 内部怎么做，才能调好它。

7.8.1 FK：符号矩阵 + 数值代入

ikpy 用 sympy 为每个连杆推导出符号化的变换矩阵，然后在 forward_kinematics 时把具体角度代进去。

  • 优点：推导一次，反复使用；支持任意轴、任意欧拉角表示。
  • 缺点：比纯数值慢（这也是为什么第 06 章自研 DLS 用 MuJoCo 的 mj_jacSite 会快约一个数量级，见 8.7 实测）。

FK 内部流程：

构建 Chain 时：
  对每个 Link：
    用 sympy 推导符号变换矩阵 T_i(θ_i)
    保存符号表达式

调用 forward_kinematics(q) 时：
  把 q 的数值代入符号表达式
  计算 T_total = T_1(θ1) · T_2(θ2) · ... · T_n(θn)
  返回 T_total

7.8.2 IK：scipy 数值优化

ikpy 的 IK 本质是最小化末端误差：

minimize  ‖FK(q) - target‖²
   q
s.t.      q_min ≤ q ≤ q_max

用的是 scipy.optimize.least_squares（默认）或 scipy.optimize.minimize。

两种优化器：

chain.inverse_kinematics(target_position=..., optimizer="least_squares")  # 默认
chain.inverse_kinematics(target_position=..., optimizer="scalar")         # 旧版默认

least_squares 通常更快更稳，保持默认即可。

💡 least_squares vs scalar 的区别：

  • least_squares：专门针对最小二乘问题优化，使用信任区域算法，收敛更快
  • scalar：通用的 scipy.optimize.minimize，使用 BFGS 或其他通用算法，更慢但更通用

ikpy 4.0 默认用 least_squares，这是正确的选择——IK 本质就是最小二乘问题。

7.8.3 与自研 DLS 的本质区别

自研 DLS（第 06 章）ikpy
方法沿雅可比迭代逼近全局优化（最小化误差）
每步用雅可比信息，方向明确数值试探，靠 scipy 收敛
速度快（可用解析雅可比）较慢
灵活性固定公式可加正则项、姿态约束等
多解由初始值决定由初始值 + 正则项决定

7.8.4 ikpy 符号计算的深入理解

ikpy 的核心竞争力在于它的符号化变换矩阵推导。让我们深入理解这是如何工作的。

构建 Chain 时发生了什么：

chain = Chain(name="arm", links=[OriginLink(), URDFLink(...), ...])

当这行代码执行时，ikpy 内部会：

  1. 为每个连杆创建符号变量：θ1, θ2, ..., θn（sympy 的 Symbol 对象）
  2. 推导每个连杆的符号变换矩阵：用 origin_translation、origin_orientation、rotation 等参数，构造出包含符号变量的 4×4 矩阵
  3. 连乘得到总变换矩阵：T_total = T1(θ1) · T2(θ2) · ... · Tn(θn)，结果是一个包含所有符号变量的 4×4 矩阵
  4. 编译为数值函数（可选）：用 sympy.lambdify 把符号表达式编译为高效的数值函数

符号矩阵的样子（简化的 2 连杆示例）：

T_total = [
    [cos(θ1+θ2), -sin(θ1+θ2), 0, L1·cos(θ1) + L2·cos(θ1+θ2)],
    [sin(θ1+θ2),  cos(θ1+θ2), 0, L1·sin(θ1) + L2·sin(θ1+θ2)],
    [0,           0,            1, 0],
    [0,           0,            0, 1]
]

这是一个符号表达式——θ1 和 θ2 不是数字，而是 sympy 的符号变量。当调用 forward_kinematics([0, 0.5, 0.3, 0]) 时，ikpy 把 θ1=0.5, θ2=0.3 代入符号表达式，得到数值结果。

为什么符号计算比纯数值慢：

操作纯数值（NumPy）符号计算（sympy）
矩阵乘法C 实现，微秒级Python 符号运算，毫秒级
三角函数直接调用 np.cos符号化简后再代入
内存存储数值数组存储符号表达式树
优点快灵活、可求导、可化简

符号计算的优势：

  1. 自动求雅可比：对符号矩阵求偏导，可以得到精确的解析雅可比，不需要数值差分
  2. 支持任意轴/欧拉角：符号计算不关心具体数值，任何旋转表示都能处理
  3. 可化简：sympy 可以自动化简复杂的三角表达式，减少计算量
  4. 可导出：符号表达式可以导出为 C 代码或 MATLAB 代码，用于嵌入式系统

💡 ikpy 的 use_symbolic_matrix 参数：URDFLink 有一个 use_symbolic_matrix=True 参数。如果设为 False，ikpy 会用纯数值计算（更快，但灵活性降低）。默认是 True，因为符号计算的灵活性更重要。

在性能敏感的场景下，可以尝试设为 False，但要注意某些功能（如自动雅可比）可能不可用。

7.8.5 从零构建 Chain 的完整示例

让我们通过一个完整的示例，理解构建 Chain 的每一步。

目标：构建一个 3 自由度机械臂——基座旋转（J1，绕 Z）、大臂俯仰（J2，绕 Y）、末端偏移（固定）。

第 1 步：确定运动学参数

基座 → J1: 偏移 (0, 0, 0.1)，绕 Z 旋转
J1 → J2:   偏移 (0, 0, 0.3)，绕 Y 旋转
J2 → 末端: 偏移 (0, 0, 0.25)，固定

第 2 步：确定关节限位

J1: ±180° (±π)
J2: -90° ~ +90° (-π/2 ~ π/2)

第 3 步：编写代码

import numpy as np
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink

# 构建 Chain：4 个元素（OriginLink + 2关节 + 末端固定）
chain = Chain(
    name="3dof_arm",
    links=[
        # ① 基座（固定）
        OriginLink(),
        # ② J1：基座旋转，绕 Z 轴
        URDFLink(
            name="j1",
            origin_translation=[0, 0, 0.1],   # 基座高 0.1m
            origin_orientation=[0, 0, 0],
            rotation=[0, 0, 1],                 # 绕 Z 轴
            bounds=(-np.pi, np.pi),             # ±180°
        ),
        # ③ J2：大臂俯仰，绕 Y 轴
        URDFLink(
            name="j2",
            origin_translation=[0, 0, 0.3],   # 大臂长 0.3m
            origin_orientation=[0, 0, 0],
            rotation=[0, 1, 0],                 # 绕 Y 轴
            bounds=(-np.pi/2, np.pi/2),        # ±90°
        ),
        # ④ 末端固定偏移
        URDFLink(
            name="tip",
            origin_translation=[0, 0, 0.25],  # 末端长 0.25m
            origin_orientation=[0, 0, 0],
            rotation=None,
            joint_type="fixed",                 # 必须显式声明！
        ),
    ],
    # active_links_mask：基座=False, J1=True, J2=True, 末端=False
    active_links_mask=[False, True, True, False],
)

# 验证结构
print("links:", [l.name for l in chain.links])
print("active:", chain.active_links_mask)

# FK 测试：J1=30°, J2=45°
q_full = [0, np.radians(30), np.radians(45), 0]
T = chain.forward_kinematics(q_full)
print("末端位置:", T[:3, 3])

# IK 测试：求解目标位置 [0.3, 0, 0.4]
q_sol = chain.inverse_kinematics(target_position=[0.3, 0, 0.4])
print("IK 解(度):", np.degrees(q_sol[1:3]))

逐段讲解：

步骤说明注意事项
① OriginLink()链的起点，代表固定基座必须是第一个元素
② J1 URDFLink第一个关节：基座旋转origin_translation 是基座到 J1 的偏移，rotation 是旋转轴
③ J2 URDFLink第二个关节：大臂俯仰origin_translation 是 J1 到 J2 的偏移（即大臂长度）
④ tip URDFLink末端固定偏移必须写 joint_type="fixed"，否则报错
active_links_mask标记哪些关节参与 IK长度必须等于 len(links)，基座和固定末端为 False
q_fullFK 的输入数组长度必须等于 len(links)，固定 link 填 0

常见错误排查：

错误 1: ValueError: Joint type is 'revolute' but rotation axis = False
原因: 末端固定连杆没写 joint_type="fixed"
解决: 添加 joint_type="fixed"

错误 2: UserWarning: Link Base link (index: 0) is of type 'fixed' but set as active
原因: 没设 active_links_mask，或设错了
解决: active_links_mask=[False, True, True, False]

错误 3: 末端位置不随 J2 变化
原因: J2 后面没有末端偏移（缺 tip link）
解决: 添加末端固定偏移 URDFLink

错误 4: IK 结果离目标很远
原因: 目标不可达，或撞到关节限位
解决: 检查目标是否在工作空间内；回代 FK 验证末端误差

7.9 常见错误速查

报错原因解决
No module named 'sympy'缺依赖pip install sympy
unexpected keyword argument 'translation_vector'用了 3.x 的 API改成 origin_translation
Joint type is 'revolute' but rotation axis = False固定连杆没写类型加 joint_type="fixed"
UserWarning: Link Base link ... is of type 'fixed'没设 active_links_mask设 [False] + [True]*(n-1)
IK 结果离目标很远目标超出工作空间，或撞到关节限位检查目标半径与 bounds；ikpy 会"尽力而为"不报错
某个关节怎么动，末端都不变该关节后面没有偏移（缺末端 link），或转轴与连杆同向见 7.4.1 / 7.4.2，补末端偏移或改转轴
ValueError: Your target must be a 4x4用了 inverse_kinematics_frame 但目标不是 4×4用 inverse_kinematics 传 3 维位置
mask 长度报错active_links_mask 长度与 links 不一致两者长度必须相等
Initial guess is outside of provided boundsinitial_position 超出 bounds先 np.clip 到限位内

7.10 动手练

  1. 最小链：用 OriginLink + 2 个关节 + 1 个末端偏移（L1=L2=0.3）建一条 2 连杆臂，计算 q=[30°, -45°] 的末端位置，并用 7.4.4 的手算公式交叉验证。
    提示：如果只写 2 个 URDFLink 不加末端偏移，你会发现第二个关节转多少度末端都不动。

  2. active_links_mask 实验：分别用"不传 mask"和"传 [False,True,True,False]"建同一条链，观察警告数量与 FK 结果是否一致。

  3. 限位验证：给第 2 个关节设 bounds=(-1.0, 1.0)，求一个需要大幅弯曲才能到达的目标（如 [0.10, 0, 0.10]），验证解确实被钳在限位内。

  4. DHLink 对照：用 DHLink 重建第 05 章的 3 自由度臂（含移动关节），与手写 DH 对比。
    提示：移动关节需要用 URDFLink(translation=轴方向, joint_type="prismatic")，DHLink 不支持。

练习参考答案与提示

练习 1 答案：

# 建链（4个元素：OriginLink + 2关节 + 末端偏移）
chain = Chain(name="two_link", links=[
    OriginLink(),
    URDFLink(name="j1", origin_translation=[0,0,0], origin_orientation=[0,0,0], rotation=[0,1,0]),
    URDFLink(name="j2", origin_translation=[-0.3,0,0], origin_orientation=[0,0,0], rotation=[0,1,0]),
    URDFLink(name="tip", origin_translation=[0,0,0.3], origin_orientation=[0,0,0], rotation=None, joint_type="fixed"),
], active_links_mask=[False, True, True, False])

q = [0, np.radians(30), np.radians(-45), 0]
T = chain.forward_kinematics(q)
print(T[:3, 3])  # 实测得到 [-0.337453, 0, 0.439778]

手算验证：Ry(30°) @ ([-0.3,0,0] + Ry(-45°) @ [0,0,0.3]) = [-0.337453, 0, 0.439778] ✓（与 7.4.4 的手算公式一致）

练习 2 提示：不传 mask 时，ikpy 会对 OriginLink 和末端固定连杆各发一条 UserWarning（共 2 条），但 FK 结果与传 mask 时完全一致。这说明警告不影响计算结果，但应该显式设置 mask 以保持代码清晰。

练习 3 提示：设置 bounds=(-1.0, 1.0) 后，求解需要大幅弯曲的目标时，j2 会被钳在 ±1.0 rad（约 ±57°），末端会有残差。检查 np.abs(q_sol[2]) <= 1.0 + 1e-6 验证限位生效。

练习 4 完整参考答案：用 DHLink 重建第 05 章的 3 自由度臂（2 转 + 1 移），与手写 DH 对比。

关键知识点：移动关节不能用 DHLink

第 05 章的 3 自由度臂有一个移动关节（J3，沿 Z 轴伸缩）。DHLink 的实现把 theta 作为唯一变量（旋转关节），不支持把 d 作为变量（移动关节）。所以移动关节必须用 URDFLink 实现：

URDFLink(
    name="j3",
    origin_translation=[0.25, 0, 0],     # 父关节到本关节的偏移
    origin_orientation=[0, 0, 0],
    translation=[0, 0, 1],                 # ← 移动方向：沿 Z 轴
    joint_type="prismatic",                # ← 必须显式指定！
)
参数旋转关节移动关节
运动轴rotation=[x, y, z]translation=[x, y, z]
joint_type"revolute"（默认）"prismatic"（必须显式写）
变量含义关节角（弧度）移动量（米）

⚠️ 常见错误：移动关节只写了 translation 但忘了写 joint_type="prismatic"。ikpy 默认 joint_type="revolute"，会导致参数校验失败或行为异常。

完整代码：混合使用 DHLink 和 URDFLink
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink, DHLink
import numpy as np

# 方案 A：前两个旋转关节用 DHLink，第三个移动关节用 URDFLink
chain_dh = Chain(
    name="dh_3dof",
    links=[
        OriginLink(),
        DHLink(name="j1", d=0.20, a=0.30, alpha=0.0, theta=0.0),
        DHLink(name="j2", d=0.00, a=0.25, alpha=0.0, theta=0.0),
    ],
    active_links_mask=[False, True, True],
)

# 方案 B：全部用 URDFLink（含移动关节）
chain_full = Chain(
    name="urdf_3dof",
    links=[
        OriginLink(),
        # 与 DH 参数对应：j1 = d=0.20(基座高) + a=0.30(臂长)，绕 Z 转
        URDFLink(name="j1", origin_translation=[0, 0, 0],
                 origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
        URDFLink(name="j2", origin_translation=[0.30, 0, 0.20],
                 origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
        URDFLink(name="j3", origin_translation=[0.25, 0, 0],
                 origin_orientation=[0, 0, 0],
                 translation=[0, 0, 1],          # 移动方向：沿 Z 轴
                 joint_type="prismatic"),         # 必须显式指定！
    ],
    active_links_mask=[False, True, True, True],
)

# 测试：q = [J1=30°, J2=45°, J3=0.1m]
q = [0.0, np.radians(30), np.radians(45), 0.1]
T_urdf = chain_full.forward_kinematics(q)

# 手写 DH 验证（第 05 章的 dh_transform）
T_dh_manual = (
    dh_transform(0.30, 0, 0.20, np.radians(30))
    @ dh_transform(0.25, 0, 0.0, np.radians(45))
    @ dh_transform(0.0, 0, 0.1, 0.0)      # 移动关节：d=0.1
)
实测输出
URDFLink 版 (含移动关节) = [0.324512 0.391481 0.3     ]
手写 DH 版               = [0.324512 0.391481 0.3     ]
一致: True

✅ 移动关节用 URDFLink(translation=轴方向, joint_type="prismatic") 实现，对应 DH 里的 d 变量。两者结果完全一致。

混合使用的注意事项

  1. 坐标系一致性：DHLink 用标准 DH 约定（Rz(θ)·Tz(d)·Tx(a)·Rx(α)），而 URDFLink 用"先平移后旋转"约定。两者在同一条链中混用时，必须确保每个 link 的 origin_translation 与 DH 参数的几何含义一致。最简单的做法是：要么全用 DHLink（只有旋转关节时），要么全用 URDFLink（有移动关节时）。

  2. active_links_mask 长度：必须与 links 列表长度一致。方案 A 有 3 个元素（OriginLink + 2 DHLink），方案 B 有 4 个元素（OriginLink + 3 URDFLink）。

  3. 移动关节的 bounds：单位是米（不是弧度）。例如 bounds=(0.0, 0.5) 表示移动范围 0~0.5m。

参考答案见 code/ch07_ikpy_intro.py。

7.11 小结

核心要点回顾

  • ikpy 只做运动学（FK/IK），不做物理——与 MuJoCo 互补。
  • 四个类：Chain（入口）、OriginLink（地基）、URDFLink（关节连杆）、DHLink（DH 参数连杆）。
  • 🔥 origin_translation 量的是"关节到关节"的距离，末端必须单独加一个固定偏移 link——否则最后一个关节完全不起作用（7.4.1，最难自查的坑）。
  • 🔥 转轴不能与连杆同向，否则旋转不产生位移（7.4.2 反面教材 A）。
  • 🔥 ikpy 4.0 用 origin_translation，不是 translation_vector。
  • 🔥 固定连杆必须写 joint_type="fixed"；移动关节必须写 joint_type="prismatic"。
  • active_links_mask：不传则全 True（会对每个 fixed link 发警告），传了则原样保留；OriginLink 和固定连杆应为 False。
  • 关节限位用 bounds=(lo, hi)，ikpy 保证解在限位内；超出可达范围时返回"贴在限位上"的尽力解，且不报错——必须自己检查末端误差。
  • IK 本质是 scipy.optimize.least_squares 最小化末端误差；解可能不是你输入的那组（多解，见第 09 章）。
  • DHLink 的 FK 与第 05 章手写 DH 完全一致（实测 [0.324512, 0.391481, 0.2]）。

关键 API 速查表

API作用
Chain(links=..., active_links_mask=...)构建运动链
OriginLink()链的起点（基座）
URDFLink(name, origin_translation, rotation, bounds, joint_type)URDF 风格连杆
DHLink(name, d, a, alpha, theta)DH 参数风格连杆
chain.forward_kinematics(q_full)正运动学（输入完整长度数组）
chain.inverse_kinematics(target_position=..., initial_position=...)逆运动学
chain.plot(q, ax, target=...)三维可视化

在本项目中的应用

  • 第 08 章用 ikpy 构建本书 6 轴机械臂的完整 Chain
  • 第 09 章用 ikpy 做姿态控制、冗余自由度管理、避障
  • 第 19 章用 ikpy 做交叉验证（与自研 DLS 对比）
  • pick_and_place.py 主要用自研 DLS（实时性更好），但 ikpy 用于离线验证和快速原型

扩展阅读方向

  1. ikpy 官方文档：https://github.com/Phylliade/ikpy ，查看最新 API 和示例
  2. URDF 规范：理解 URDF 的 <joint>、<link>、<origin>、<axis>、<limit> 等元素，有助于理解 ikpy 的参数设计
  3. sympy 符号计算：ikpy 的核心机制，了解 sympy 的 Matrix、Symbol、lambdify 等功能
  4. scipy.optimize.least_squares：ikpy IK 的底层优化器，了解信任区域算法和 bounds 处理
  5. PyKDL / trac_ik：其他主流运动学库，与 ikpy 对比学习。PyKDL 是 ROS 生态的标准，trac_ik 是更快速的 IK 求解器

下一章用 ikpy 完整驱动本书的 6 轴机械臂。

上一章：06 · 逆运动学 IK ｜ 下一章：08 · ikpy FK/IK 实战

# python
Logo
DAMO开发者矩阵

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

更多推荐

【信息融合与状态估计】时滞系统的协方差交叉融合估计研究（Matlab代码实现）

时滞系统广泛存在于工业过程、通信网络、机器人控制等领域，其状态估计面临信息异步性、估计偏差和相关性未知等挑战。协方差交叉（Covariance Intersection, CI）融合估计作为一种有效的信息融合方法，在处理具有未知相关性的多源信息时展现出独特优势。本文针对时滞系统，研究了协方差交叉融合估计方法，通过时间对齐、时滞补偿等技术，解决了时滞数据的处理和多源异步信息的融合问题，提高了状态估计

avatar DAMO开发者矩阵
cover

TVA具身智能系统（7）：与传统工业智能系统的代际差异解析

avatar DAMO开发者矩阵

【信息融合与状态估计】时滞系统的协方差交叉融合估计研究（Matlab代码实现）

时滞系统广泛存在于工业过程、通信网络、机器人控制等领域，其状态估计面临信息异步性、估计偏差和相关性未知等挑战。协方差交叉（Covariance Intersection, CI）融合估计作为一种有效的信息融合方法，在处理具有未知相关性的多源信息时展现出独特优势。本文针对时滞系统，研究了协方差交叉融合估计方法，通过时间对齐、时滞补偿等技术，解决了时滞数据的处理和多源异步信息的融合问题，提高了状态估计

avatar DAMO开发者矩阵