07 · ikpy 入门

这一章要解决什么问题：不想自己写 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 章示例代码运行完毕。")

本章学习目标

学完本章，你将能够：

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

7.1 ikpy 在仿真栈里的位置

回顾第 00 章的分层：

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

ikpy 只做一件事：运动学。

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

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

这意味着：

💡 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 是每一颗珍珠。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'
)

参数详解：

• 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 号）：

用旧写法会直接报错：

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 构建流程图：

#mermaid-svg-jLYbYdI7A7pceNVJ{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-jLYbYdI7A7pceNVJ .error-icon{fill:#552222;}#mermaid-svg-jLYbYdI7A7pceNVJ .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-jLYbYdI7A7pceNVJ .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-jLYbYdI7A7pceNVJ .marker{fill:#333333;stroke:#333333;}#mermaid-svg-jLYbYdI7A7pceNVJ .marker.cross{stroke:#333333;}#mermaid-svg-jLYbYdI7A7pceNVJ svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-jLYbYdI7A7pceNVJ p{margin:0;}#mermaid-svg-jLYbYdI7A7pceNVJ .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster-label text{fill:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster-label span{color:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster-label span p{background-color:transparent;}#mermaid-svg-jLYbYdI7A7pceNVJ .label text,#mermaid-svg-jLYbYdI7A7pceNVJ span{fill:#333;color:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ .node rect,#mermaid-svg-jLYbYdI7A7pceNVJ .node circle,#mermaid-svg-jLYbYdI7A7pceNVJ .node ellipse,#mermaid-svg-jLYbYdI7A7pceNVJ .node polygon,#mermaid-svg-jLYbYdI7A7pceNVJ .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-jLYbYdI7A7pceNVJ .rough-node .label text,#mermaid-svg-jLYbYdI7A7pceNVJ .node .label text,#mermaid-svg-jLYbYdI7A7pceNVJ .image-shape .label,#mermaid-svg-jLYbYdI7A7pceNVJ .icon-shape .label{text-anchor:middle;}#mermaid-svg-jLYbYdI7A7pceNVJ .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-jLYbYdI7A7pceNVJ .rough-node .label,#mermaid-svg-jLYbYdI7A7pceNVJ .node .label,#mermaid-svg-jLYbYdI7A7pceNVJ .image-shape .label,#mermaid-svg-jLYbYdI7A7pceNVJ .icon-shape .label{text-align:center;}#mermaid-svg-jLYbYdI7A7pceNVJ .node.clickable{cursor:pointer;}#mermaid-svg-jLYbYdI7A7pceNVJ .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-jLYbYdI7A7pceNVJ .arrowheadPath{fill:#333333;}#mermaid-svg-jLYbYdI7A7pceNVJ .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-jLYbYdI7A7pceNVJ .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-jLYbYdI7A7pceNVJ .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-jLYbYdI7A7pceNVJ .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-jLYbYdI7A7pceNVJ .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-jLYbYdI7A7pceNVJ .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster text{fill:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ .cluster span{color:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-jLYbYdI7A7pceNVJ .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-jLYbYdI7A7pceNVJ rect.text{fill:none;stroke-width:0;}#mermaid-svg-jLYbYdI7A7pceNVJ .icon-shape,#mermaid-svg-jLYbYdI7A7pceNVJ .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-jLYbYdI7A7pceNVJ .icon-shape p,#mermaid-svg-jLYbYdI7A7pceNVJ .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-jLYbYdI7A7pceNVJ .icon-shape .label rect,#mermaid-svg-jLYbYdI7A7pceNVJ .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-jLYbYdI7A7pceNVJ .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-jLYbYdI7A7pceNVJ .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-jLYbYdI7A7pceNVJ :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}

是

否

开始构建 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 的语义是：

推论（极其重要）：

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

如果最后一个 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 代码逐行解读

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

末端 = 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 参数的对应：

用它重建第 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：

本书实战用 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 的本质区别

7.8.4 ikpy 符号计算的深入理解

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

构建 Chain 时发生了什么：

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

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

符号矩阵的样子（简化的 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 代入符号表达式，得到数值结果。

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

符号计算的优势：

💡 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]))

逐段讲解：

常见错误排查：

错误 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 常见错误速查

7.10 动手练

练习参考答案与提示

练习 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", # ← 必须显式指定！
)

⚠️ 常见错误：移动关节只写了 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 变量。两者结果完全一致。

混合使用的注意事项

参考答案见 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 速查表

在本项目中的应用

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

扩展阅读方向

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

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