这一章要解决什么问题：把第 07 章学的 ikpy 用到真实机器人上——本书的 Epson CX4-A601C 六轴机械臂。

本章会完成一次三方交叉验证（ikpy / MuJoCo / 手写 FK），并暴露一个实战中最常见的问题：冗余自由度乱跳。

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

"""
第 08 章配套代码：ikpy FK/IK 实战（驱动本书的 6 轴机械臂）。


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

内容：
  1. 用 ikpy 为 6 轴机械臂建链（参数直接抄 MJCF）
  2. FK 与 MuJoCo 对拍（三方验证：ikpy / MuJoCo / 手写）
  3. IK 求解取/放料点，精度评估
  4. 关节限位验证
  5. 三维可视化（matplotlib，无显示环境下自动保存为 PNG）
  6. 与第 06 章自研 DLS 求解器对比
  7. 动手练答案
"""
from pathlib import Path
import os
import warnings
import numpy as np
import mujoco

warnings.filterwarnings("ignore")     # 屏蔽 ikpy 的 fixed-link 警告
np.set_printoptions(precision=6, suppress=True)
PROJECT_ROOT = Path(__file__).resolve().parent.parent.parent


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

# ============================================================
# 8.1 为 6 轴机械臂建 ikpy 链
# ============================================================
print("=" * 70)
print("8.1  为 6 轴机械臂建 ikpy 链")
print("=" * 70)

MJCF = PROJECT_ROOT / "models/cx4_a601c_simulation.xml"

# 关节限位（弧度）—— 与 MJCF 的 <joint range="..."> 完全一致
BOUNDS = [
    (-3.141593, 3.141593),      # J1
    (-2.705260, 1.169371),      # J2
    (-1.099557, 3.368485),      # J3
    (-4.712389, 4.712389),      # J4
    (-2.356194, 2.356194),      # J5
    (-9.424778, 9.424778),      # J6
]

# (名称, 相对父的平移, 关节轴) —— 直接抄 MJCF 的 body pos / joint axis
# Z-up 约定（MuJoCo 默认）：J1 绕竖直 +Z；俯仰关节 J2/J3/J5 绕 +Y；
# 滚转关节 J4/J6 沿小臂方向 +X。零位时手臂沿 -X 平伸（臂平面 = XZ，所有偏移 y=0）
# 注意：按第 07 章的约定，最后必须有一个"末端偏移"固定 link
ARM_LINKS = [
    ("j1", [0.000, 0.000, 0.176], [0, 0, 1]),
    ("j2", [-0.06, 0.000, 0.144], [0, 1, 0]),
    ("j3", [0.000, 0.000, 0.260], [0, 1, 0]),
    ("j4", [-0.28, 0.000, 0.030], [1, 0, 0]),
    ("j5", [0.000, 0.000, 0.000], [0, 1, 0]),
    ("j6", [0.000, 0.000, 0.000], [1, 0, 0]),
]
# 末端偏移 = gripper_mount + end_effector（两段都沿工具轴 -X，可直接相加）
#   gripper_mount: (-0.0725, 0.0, 0.002535)   # 法兰面中心
#   end_effector : (-0.048, 0.0, 0.0)         # 夹爪 TCP
TOOL_OFFSET = [-0.1205, 0.0, 0.002535]


def build_arm_chain():
    """构建本书 6 轴机械臂的 ikpy Chain。"""
    links = [OriginLink()]
    for (name, trans, axis), bnd in zip(ARM_LINKS, BOUNDS):
        links.append(URDFLink(name=name,
                              origin_translation=trans,
                              origin_orientation=[0, 0, 0],
                              rotation=axis,
                              bounds=bnd))
    # 末端固定偏移（第 07 章的关键约定：没有它，J6 的旋转完全不起作用）
    links.append(URDFLink(name="ee",
                          origin_translation=TOOL_OFFSET,
                          origin_orientation=[0, 0, 0],
                          rotation=None, joint_type="fixed"))
    return Chain(name="cx4_a601c", links=links,
                 active_links_mask=[False] + [True] * 6 + [False])


chain = build_arm_chain()
print("links:", [l.name for l in chain.links])
print("active_links_mask:", chain.active_links_mask)
print("末端偏移 link:", chain.links[-1].name, "->", TOOL_OFFSET)
print("\n>> 注意最后那个 ee 固定 link：没有它，J6 转多少度末端都不会动！")

# ============================================================
# 8.2 FK 三方对拍：ikpy / MuJoCo / 第 05 章手写
# ============================================================
print("\n" + "=" * 70)
print("8.2  FK 三方对拍：ikpy / MuJoCo / 手写")
print("=" * 70)

model = mujoco.MjModel.from_xml_path(str(MJCF))
data = mujoco.MjData(model)
ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")

from scipy.spatial.transform import Rotation as Rot


def T_trans(t):
    T = np.eye(4)
    T[:3, 3] = t
    return T


def T_rot(axis, ang):
    T = np.eye(4)
    T[:3, :3] = Rot.from_rotvec(np.array(axis, float) * ang).as_matrix()
    return T


def fk_manual(q):
    """第 05 章手写的 FK（直接连乘法）。"""
    T = np.eye(4)
    for (_, trans, axis), qi in zip(ARM_LINKS, q):
        T = T @ T_trans(trans) @ T_rot(axis, qi)
    # gripper_mount + end_effector（两段分开加，与 MJCF 一致）
    T = T @ T_trans([-0.0725, 0.0, 0.002535]) @ T_trans([-0.048, 0.0, 0.0])
    return T


rng = np.random.default_rng(42)
print("  q(度)                        ikpy            MuJoCo          手写FK        最大偏差")
max_all = 0.0
for _ in range(5):
    q = rng.uniform(-0.5, 0.5, 6)
    q_full = np.concatenate([[0.0], q, [0.0]])      # base + 6 关节 + 末端

    p_ikpy = chain.forward_kinematics(q_full)[:3, 3]
    data.qpos[:6] = q
    mujoco.mj_forward(model, data)
    p_mj = data.site_xpos[ee_id].copy()
    p_manual = fk_manual(q)[:3, 3]

    dev = max(np.linalg.norm(p_ikpy - p_mj), np.linalg.norm(p_ikpy - p_manual))
    max_all = max(max_all, dev)
    print(f"  {np.round(np.degrees(q), 1)}  {np.round(p_ikpy, 5)}  {np.round(p_mj, 5)}  "
          f"{np.round(p_manual, 5)}  {dev:.2e}")

print(f"\n三方最大偏差 = {max_all:.2e}")
print(">> ikpy 的 Chain 与 MuJoCo / 手写 FK 完全一致，说明建链参数正确。")

# ============================================================
# 8.3 IK 求解取/放料点
# ============================================================
print("\n" + "=" * 70)
print("8.3  IK 求解取/放料点")
print("=" * 70)

# Z-up 世界坐标：取/放料点 = 地面上物块的中心（边长 0.04 -> z = 0.02）
PICK_POS = np.array([-0.18, -0.30, 0.02])
PLACE_POS = np.array([0.30, -0.12, 0.02])


def ik_solve_and_check(target, q_seed=None, **kw):
    """用 ikpy 求解并回代验证。"""
    init = np.zeros(len(chain.links)) if q_seed is None else \
        np.concatenate([[0.0], np.asarray(q_seed, float), [0.0]])
    q_full = chain.inverse_kinematics(target_position=target,
                                      initial_position=init, **kw)
    p = chain.forward_kinematics(q_full)[:3, 3]
    err = float(np.linalg.norm(p - target))
    return q_full[1:7], err, p            # 只返回 6 个关节角


for name, tgt in [("取料点 PICK", PICK_POS), ("放料点 PLACE", PLACE_POS)]:
    q_sol, err, reached = ik_solve_and_check(tgt)
    # MuJoCo 独立验证
    data.qpos[:6] = q_sol
    mujoco.mj_forward(model, data)
    p_mj = data.site_xpos[ee_id].copy()
    in_bounds = bool(np.all(q_sol >= np.array([b[0] for b in BOUNDS]) - 1e-6) and
                     np.all(q_sol <= np.array([b[1] for b in BOUNDS]) + 1e-6))
    print(f"\n  {name} = {tgt}")
    print(f"    ikpy  IK 解(度) = {np.round(np.degrees(q_sol), 2)}")
    print(f"    ikpy  回代末端  = {np.round(reached, 5)}   误差 = {err:.2e}")
    print(f"    MuJoCo 验证末端 = {np.round(p_mj, 5)}   误差 = {np.linalg.norm(p_mj - tgt):.2e}")
    print(f"    关节限位内      = {in_bounds}")

# ============================================================
# 8.4 精度统计（多个目标点）
# ============================================================
print("\n" + "=" * 70)
print("8.4  IK 精度统计")
print("=" * 70)

# Z-up：x 沿零位伸展方向（-X），y 横向，z 高度（全部离地 0.08~0.18 m）
targets = []
for x in (-0.35, -0.25, -0.15):
    for y in (-0.20, 0.0, 0.20):
        for z in (0.08, 0.18):
            targets.append(np.array([x, y, z]))

errs = []
for tgt in targets:
    _, err, _ = ik_solve_and_check(tgt)
    errs.append(err)
errs = np.array(errs)
print(f"  目标点数        : {len(targets)}")
print(f"  误差中位数      : {np.median(errs):.2e} m")
print(f"  误差最大        : {errs.max():.2e} m")
print(f"  误差 < 1mm 占比 : {(errs < 1e-3).mean() * 100:.0f}%")
print(f"  误差 < 1cm 占比 : {(errs < 1e-2).mean() * 100:.0f}%")

# ============================================================
# 8.6 三维可视化
# ============================================================
print("\n" + "=" * 70)
print("8.6  三维可视化")
print("=" * 70)

import matplotlib
matplotlib.use("Agg")          # 无显示环境下使用非交互后端
import matplotlib.pyplot as plt

OUT_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "outputs")
os.makedirs(OUT_DIR, exist_ok=True)

fig = plt.figure(figsize=(9, 7))
ax = fig.add_subplot(111, projection="3d")

q_show = np.concatenate([[0.0], ik_solve_and_check(PICK_POS)[0], [0.0]])
chain.plot(q_show, ax, target=PICK_POS)
ax.set_title("ikpy Chain: CX4-A601C reaching PICK_POS", fontsize=12)
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_zlabel("Z (m)")
ax.view_init(elev=20, azim=-90)
fig.tight_layout()
png_path = os.path.join(OUT_DIR, "ch08_ikpy_chain.png")
fig.savefig(png_path, dpi=110)
plt.close(fig)
print(f"  已保存: {os.path.abspath(png_path)}")

# ============================================================
# 8.7 与第 06 章自研 DLS 对比
# ============================================================
print("\n" + "=" * 70)
print("8.7  ikpy vs 第 06 章自研 DLS")
print("=" * 70)

import time

Q_MIN = model.jnt_range[:6, 0].copy()
Q_MAX = model.jnt_range[:6, 1].copy()


def numeric_jacobian(fk_func, q, eps=1e-6):
    """数值雅可比（第 06 章的实现）。"""
    n = len(q)
    p0 = fk_func(q)[:3, 3]
    J = np.zeros((3, n))
    for i in range(n):
        qp = q.copy()
        qp[i] += eps
        J[:, i] = (fk_func(qp)[:3, 3] - p0) / eps
    return J


def ik_dls(fk_func, target, q_init, q_min=None, q_max=None,
           max_iter=300, tol=1e-4, lam=0.02, alpha=0.6):
    """自研 DLS：数值雅可比版（第 06 章）。"""
    q = np.array(q_init, dtype=float).copy()
    target = np.asarray(target, dtype=float)
    err = np.inf
    for _ in range(max_iter):
        e = target - fk_func(q)[:3, 3]
        err = float(np.linalg.norm(e))
        if err < tol:
            return q, True, err
        J = numeric_jacobian(fk_func, q)
        JJT = J @ J.T + lam ** 2 * np.eye(3)
        dq = J.T @ np.linalg.solve(JJT, e) * alpha
        q = q + dq
        if q_min is not None:
            q = np.clip(q, q_min, q_max)
    return q, False, err


def ik_dls_mujoco(mdl, dat, site_id, target, q_init,
                  max_iter=300, tol=1e-4, lam=0.02, alpha=0.6):
    """自研 DLS：MuJoCo 解析雅可比版（第 06 章项目实战版，最快）。"""
    # 保存原始状态，避免 IK 副作用污染仿真数据
    saved_qpos = dat.qpos[:6].copy()
    jp = np.zeros((3, mdl.nv))
    jr = np.zeros((3, mdl.nv))
    q = np.array(q_init, dtype=float).copy()
    err = np.inf
    for _ in range(max_iter):
        dat.qpos[:6] = q
        mujoco.mj_forward(mdl, dat)
        e = target - dat.site_xpos[site_id]
        err = float(np.linalg.norm(e))
        if err < tol:
            # 恢复原始仿真状态
            dat.qpos[:6] = saved_qpos
            mujoco.mj_forward(mdl, dat)
            return q, True, err
        mujoco.mj_jacSite(mdl, dat, jp, jr, site_id)
        J = jp[:, :6]
        JJT = J @ J.T + lam ** 2 * np.eye(3)
        dq = J.T @ np.linalg.solve(JJT, e) * alpha
        q = np.clip(q + dq, mdl.jnt_range[:6, 0], mdl.jnt_range[:6, 1])
    # 恢复原始仿真状态
    dat.qpos[:6] = saved_qpos
    mujoco.mj_forward(mdl, dat)
    return q, False, err


N = 10
t0 = time.perf_counter()
for _ in range(N):
    chain.inverse_kinematics(target_position=PICK_POS)
t_ikpy = (time.perf_counter() - t0) / N * 1000

t0 = time.perf_counter()
for _ in range(N):
    ik_dls(fk_manual, PICK_POS, np.zeros(6), Q_MIN, Q_MAX)
t_dls_num = (time.perf_counter() - t0) / N * 1000

t0 = time.perf_counter()
for _ in range(N):
    ik_dls_mujoco(model, data, ee_id, PICK_POS, np.zeros(6))
t_dls_an = (time.perf_counter() - t0) / N * 1000

q_ikpy, e_ikpy, _ = ik_solve_and_check(PICK_POS)
q_dls, _, e_dls = ik_dls(fk_manual, PICK_POS, np.zeros(6), Q_MIN, Q_MAX)
q_dls2, _, e_dls2 = ik_dls_mujoco(model, data, ee_id, PICK_POS, np.zeros(6))

print(f"  {'方法':<28}{'耗时':>12}{'末端误差':>16}")
print(f"  {'-' * 56}")
print(f"  {'ikpy (scipy 优化)':<28}{t_ikpy:>9.2f} ms{e_ikpy:>16.2e}")
print(f"  {'自研 DLS（数值雅可比）':<26}{t_dls_num:>9.2f} ms{e_dls:>16.2e}")
print(f"  {'自研 DLS（解析雅可比）':<26}{t_dls_an:>9.2f} ms{e_dls2:>16.2e}")

print("\n  >> 自研 DLS 用【解析雅可比】(mj_jacSite) 时最快，比 ikpy 快约 "
      f"{t_ikpy / t_dls_an:.0f} 倍。")
print("     用数值雅可比时反而比 ikpy 慢 —— 印证第 06 章的结论：雅可比要选对。")

print("\n  三组解（度）:")
print(f"    ikpy        = {np.round(np.degrees(q_ikpy), 2)}")
print(f"    DLS 数值    = {np.round(np.degrees(q_dls), 2)}")
print(f"    DLS 解析    = {np.round(np.degrees(q_dls2), 2)}")
print("  >> 三组解各不相同（多解），但末端都到达目标。")

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

# 练习 2：验证 J6 是否影响末端（对比有无末端偏移 link）
links_no_ee = [OriginLink()]
for (name, trans, axis), bnd in zip(ARM_LINKS, BOUNDS):
    links_no_ee.append(URDFLink(name=name, origin_translation=trans,
                                origin_orientation=[0, 0, 0],
                                rotation=axis, bounds=bnd))
chain_no_ee = Chain(name="no_ee", links=links_no_ee,
                    active_links_mask=[False] + [True] * 6)

q_a = np.concatenate([[0.0], np.zeros(6), []])
q_b = q_a.copy()
q_b[6] = 1.0                       # 只改 J6
p_a = chain_no_ee.forward_kinematics(q_a)[:3, 3]
p_b = chain_no_ee.forward_kinematics(q_b)[:3, 3]
print(f"练习2 无末端偏移时，J6 从 0 转到 1 rad:")
print(f"   末端 {np.round(p_a, 6)} -> {np.round(p_b, 6)}  "
      f"位移 = {np.linalg.norm(p_b - p_a):.2e}  <- J6 完全不起作用！")

# 有末端偏移时
q_a2 = np.concatenate([[0.0], np.zeros(6), [0.0]])
q_b2 = q_a2.copy()
q_b2[6] = 1.0
p_a2 = chain.forward_kinematics(q_a2)[:3, 3]
p_b2 = chain.forward_kinematics(q_b2)[:3, 3]
print(f"   加上末端偏移后: 位移 = {np.linalg.norm(p_b2 - p_a2):.4f} m  <- J6 生效了")

# 练习 3：用上一个解做种子，观察解的连续性
print("\n练习3 用上一次的解做种子（轨迹连续性）:")
print("  沿 Y 从 0.30 走到 0.12（向基座靠近），共 5 个点：\n")
path = np.linspace(0.30, 0.12, 5)
prev = np.zeros(6)
jumps = []
for i, y in enumerate(path):
    tgt = np.array([-0.18, y, 0.04])
    q_s, e_s, _ = ik_solve_and_check(tgt, q_seed=prev)
    if i == 0:
        print(f"  点{i}: q(度)={np.round(np.degrees(q_s), 1)}   (起点)")
    else:
        dq = np.linalg.norm(q_s - prev)
        jumps.append(np.degrees(dq))
        print(f"  点{i}: q(度)={np.round(np.degrees(q_s), 1)}  "
              f"相对上一点变化 = {np.degrees(dq):5.1f}°   末端误差={e_s:.1e}")
    prev = q_s

print(f"\n  最大跳变 = {max(jumps):.1f}°，平均跳变 = {np.mean(jumps):.1f}°")
print("  >> ⚠️ 即使每次都用上一点的解做种子，相邻两步的关节角仍会跳变十几度！")
print("     原因在于：只约束了【位置】(3 个)，而机械臂有 6 个自由度，")
print("     剩下 3 个冗余自由度（主要是 J4/J6 腕部）没有任何约束，会乱跳。")
print("     解决办法见第 09 章：加姿态约束锁住冗余自由度（正则化效果很差，慎用）。")

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

---

本章学习目标

学完本章，你将能够：

1. 从 MJCF 模型文件提取运动学参数，并翻译成 ikpy 的 URDFLink 列表
2. 构建本书 6 轴机械臂的完整 ikpy Chain，包含 6 个关节 + 末端固定偏移
3. 完成 FK 三方交叉验证（ikpy / MuJoCo / 手写 FK），误差达到机器精度级别
4. 用 ikpy 求解取料点和放料点的 IK，理解 initial_position 的完整长度要求
5. 批量统计 IK 精度，理解 ikpy 在可达目标上的高精度表现
6. 理解冗余自由度乱跳问题——只约束位置时腕部关节的无规律跳变
7. 用 chain.plot() 可视化运动链，理解无显示环境下的 Agg 后端设置
8. 在 ikpy 和自研 DLS 之间做出正确选型

前置知识：本章需要第 05 章的正运动学实现、第 06 章的逆运动学概念、第 07 章的 ikpy 基础。本章是前三章知识的综合实战。

---

8.1 建模：从 MJCF 抄参数到 ikpy

第 05 章我们已经从 cx4_a601c_simulation.xml 提取了运动学参数。现在把它翻译成 ikpy 的 Chain。

关节	相对父的平移 pos	关节轴 axis	限位（弧度）
j1	(0, 0, 0.176)	(0, 0, 1) 绕 +Z（腰转，竖直轴）	±3.1416
j2	(-0.06, 0, 0.144)	(0, 1, 0) 绕 +Y（肩部俯仰）	-2.705 ~ 1.169
j3	(0, 0, 0.260)	(0, 1, 0) 绕 +Y（肘部俯仰）	-1.100 ~ 3.368
j4	(-0.28, 0, 0.030)	(1, 0, 0) 绕 +X（腕部滚转）	±4.712
j5	(0, 0, 0)	(0, 1, 0) 绕 +Y（腕部俯仰）	±2.356
j6	(0, 0, 0)	(1, 0, 0) 绕 +X（法兰滚转）	±9.425

💡 从 MJCF 到 ikpy 的映射规则：

• MJCF 的 <body pos="x y z"> → ikpy 的 origin_translation=[x, y, z]
• MJCF 的 <joint axis="x y z"> → ikpy 的 rotation=[x, y, z]
• MJCF 的 <joint range="lo hi"> → ikpy 的 bounds=(lo, hi)
• 直接照抄即可，不需要转换（因为两者都是"相对父连杆"的描述）

末端偏移（第 07 章的关键约定！）：

gripper_mount : (-0.0725, 0.0, 0.002535)   # 法兰面中心
end_effector  : (-0.048,  0.0, 0.0)        # 夹爪 TCP
─────────────────────────────────────────────
合并（mount 无旋转，可直接相加）:
                 (-0.1205, 0.0, 0.002535)  # 两段都沿工具轴 -X

💡 为什么可以直接相加：因为 gripper_mount 和 end_effector 都是固定偏移（没有旋转），两个平移向量可以直接相加。如果中间有旋转，就需要用矩阵乘法而不是向量加法。

完整建链代码

BOUNDS = [(-3.141593, 3.141593), (-2.705260, 1.169371), (-1.099557, 3.368485),
          (-4.712389, 4.712389), (-2.356194, 2.356194), (-9.424778, 9.424778)]

# Z-up 约定（MuJoCo 默认）：J1 绕竖直 +Z；俯仰关节 J2/J3/J5 绕 +Y；
# 滚转关节 J4/J6 沿小臂方向 +X。零位时手臂沿 -X 平伸（臂平面 = XZ，所有偏移 y=0）
ARM_LINKS = [
    ("j1", [ 0.000, 0.000, 0.176], [0, 0, 1]),
    ("j2", [-0.060, 0.000, 0.144], [0, 1, 0]),
    ("j3", [ 0.000, 0.000, 0.260], [0, 1, 0]),
    ("j4", [-0.280, 0.000, 0.030], [1, 0, 0]),
    ("j5", [ 0.000, 0.000, 0.000], [0, 1, 0]),
    ("j6", [ 0.000, 0.000, 0.000], [1, 0, 0]),
]
TOOL_OFFSET = [-0.1205, 0.0, 0.002535]

def build_arm_chain():
    """构建本书 6 轴机械臂的 ikpy Chain。
    返回：Chain 对象，包含 OriginLink + 6 个关节 + 1 个末端固定偏移
    """
    links = [OriginLink()]
    # 循环创建 6 个关节连杆
    for (name, trans, axis), bnd in zip(ARM_LINKS, BOUNDS):
        links.append(URDFLink(name=name,
                              origin_translation=trans,       # 相对父的平移
                              origin_orientation=[0, 0, 0],   # 无额外旋转
                              rotation=axis,                    # 旋转轴
                              bounds=bnd))                      # 关节限位
    # 末端固定偏移 —— 没它 J6 完全不起作用
    links.append(URDFLink(name="ee",
                          origin_translation=TOOL_OFFSET,
                          origin_orientation=[0, 0, 0],
                          rotation=None, joint_type="fixed"))
    # active_links_mask: OriginLink=False, 6个关节=True, 末端固定=False
    return Chain(name="cx4_a601c", links=links,
                 active_links_mask=[False] + [True] * 6 + [False])

逐段讲解：

代码段	说明
BOUNDS	6 个关节的限位元组列表，从 MJCF 的 jnt_range 抄来
ARM_LINKS	6 个关节的 (名称, 平移, 旋转轴) 元组列表，从 MJCF 抄来
TOOL_OFFSET	末端固定偏移，合并了 gripper_mount 和 end_effector 两段
links = [OriginLink()]	链的起点，必须是 OriginLink
for ... in zip(ARM_LINKS, BOUNDS)	循环创建 6 个关节连杆，同时遍历参数和限位
URDFLink(..., rotation=axis, bounds=bnd)	旋转关节：设置 rotation 轴和 bounds 限位
URDFLink(..., rotation=None, joint_type="fixed")	末端固定偏移：必须显式写 joint_type=“fixed”
active_links_mask=[False] + [True]*6 + [False]	8 个元素：基座(False) + 6关节(True) + 末端(False)

实测结构：

links: ['Base link', 'j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'ee']
active_links_mask: [False  True  True  True  True  True  True False]

6 轴臂 Chain 结构示意图

OriginLink (基座, 固定)
    │
    ├─ j1: origin_translation=[0, 0, 0.176], rotation=[0,0,1]
    │   (腰转, 绕+Z轴, 竖直轴)
    │   │
    │   ├─ j2: origin_translation=[-0.06, 0, 0.144], rotation=[0,1,0]
    │   │   (肩部俯仰, 绕+Y轴)
    │   │   │
    │   │   ├─ j3: origin_translation=[0, 0, 0.260], rotation=[0,1,0]
    │   │   │   (肘部俯仰, 绕+Y轴)
    │   │   │   │
    │   │   │   ├─ j4: origin_translation=[-0.28, 0, 0.030], rotation=[1,0,0]
    │   │   │   │   (腕部滚转, 绕+X轴, 沿小臂方向)
    │   │   │   │   │
    │   │   │   │   ├─ j5: origin_translation=[0,0,0], rotation=[0,1,0]
    │   │   │   │   │   (腕部俯仰, 绕+Y轴)  ← 球型腕，三轴交于一点
    │   │   │   │   │   │
    │   │   │   │   │   ├─ j6: origin_translation=[0,0,0], rotation=[1,0,0]
    │   │   │   │   │   │   (法兰滚转, 绕+X轴, 沿工具轴)
    │   │   │   │   │   │   │
    │   │   │   │   │   │   └─ ee: origin_translation=[-0.1205, 0, 0.002535], fixed
    │   │   │   │   │   │       (末端执行器/TCP, 固定偏移)
    │   │   │   │   │   │
    │   │   │   │   │   └─ 注意: j6 后面必须有 ee 偏移，否则 J6 不起作用!

⚠️ 再次强调末端偏移的必要性

实测对比（其他关节角全为 0，只把 J6 从 0 转到 1 rad）：

无末端偏移 link : 末端 [-0.34  0.    0.61] -> [-0.34  0.    0.61]   位移 = 0.00e+00
加上末端偏移后  : 位移 = 0.0024 m

没有末端偏移，J6 这个关节等于不存在。 这个错误不会报错、不会警告，只会让你的机械臂"莫名其妙少一个自由度"。

💡 位移 0.0024 m 是怎么来的：末端偏移 (-0.1205, 0, 0.002535) 几乎全部沿工具轴（-X，正好与 J6 的转轴 +X 平行），J6 转 1 rad 时只有垂直于转轴的分量（半径 0.002535 m）画圆弧，弦长 = 2 × 0.002535 × sin(0.5) ≈ 0.0024 m。若偏移完全沿工具轴，J6 滚转将丝毫不改变末端位置——这正是"滚转轴沿工具方向"的含义（J6 影响的是末端姿态，而非位置）。

---

8.2 FK 三方对拍

用 5 组随机位姿，对比三种实现：

方法	说明
ikpy chain.forward_kinematics	符号矩阵 + 数值代入
MuJoCo data.site_xpos	物理引擎内部
手写 fk_arm（第 05 章）	直接连乘法

FK 三方对拍流程：

实测输出：

  q(度)                                    ikpy              MuJoCo            手写FK        最大偏差
  [ 15.7  -3.5  20.5  11.3 -23.3  27.3]  [-0.43979 -0.1155   0.6801 ]  ...  2.52e-16
  [ 15.0  16.4 -21.3  -2.8  -7.4  24.5]  [-0.37286 -0.10141  0.55193]  ...  1.59e-16
  [  8.2  18.5  -3.2 -15.6   3.1 -25.0]  [-0.35041 -0.04731  0.70875]  ...  2.22e-16
  ...

三方最大偏差 = 2.52e-16

✅ 三方完全一致（机器精度级别），证明我们从 MJCF 抄来的参数、以及 ikpy 建链方式都正确。

2.52e-16 是什么概念：双精度浮点数的机器精度（machine epsilon）约为 2.22e-16。2.52e-16 与机器精度同量级，说明三种方法在数学上完全等价，差异仅来自浮点运算的舍入误差和运算顺序的不同。

forward_kinematics 的输入长度

q_full = np.concatenate([[0.0], q6, [0.0]])    # base + 6 关节 + 末端 = 8 个
T = chain.forward_kinematics(q_full)

长度必须等于 len(chain.links)（这里是 8）。固定 link 的位置填 0 即可。

💡 为什么需要完整长度数组：ikpy 的 forward_kinematics 内部会遍历 chain.links，对每个 link 取出对应的关节角。如果数组长度不够，会出现索引越界或使用错误的关节角。这是 ikpy 的设计选择——它不区分"活动关节"和"固定连杆"，统一用一个数组表示所有 link 的状态。

快捷转换：ikpy 提供了 chain.active_to_full(x, full) 和 chain.active_from_full(q) 两个方法，可以在"活动关节数组"和"完整数组"之间转换。但直接用 np.concatenate 更直观。

---

8.3 IK 求解取/放料点

# Z-up 世界坐标：取/放料点 = 地面上物块的中心（边长 0.04 -> z = 0.02）
PICK_POS  = np.array([-0.18, -0.30, 0.02])
PLACE_POS = np.array([0.30, -0.12, 0.02])

def ik_solve_and_check(target, q_seed=None):
    """用 ikpy 求解 IK 并验证结果。
    参数：
        target: 目标末端位置 (3,)
        q_seed: 初始关节角 (6,)，None 表示用全零
    返回：
        (q_sol, error, p_actual): 解(6,)、末端误差、实际末端位置
    """
    # 构造完整长度的初始位置数组：base(0) + 6关节 + 末端(0)
    init = np.zeros(len(chain.links)) if q_seed is None else \
        np.concatenate([[0.0], np.asarray(q_seed, float), [0.0]])
    # 调用 ikpy 的 IK 求解器
    q_full = chain.inverse_kinematics(target_position=target,
                                      initial_position=init)
    # 回代 FK 验证
    p = chain.forward_kinematics(q_full)[:3, 3]
    return q_full[1:7], float(np.linalg.norm(p - target)), p

逐行讲解：

代码	说明
init = np.zeros(len(chain.links)) if q_seed is None else ...	构造完整长度的初始位置。如果没有种子，用全零；否则用 base(0) + 6关节 + 末端(0)
np.concatenate([[0.0], np.asarray(q_seed, float), [0.0]])	把 6 个关节角包装成 8 个元素的完整数组
chain.inverse_kinematics(target_position=target, initial_position=init)	调用 ikpy 的 IK 求解器，返回完整长度的解数组
p = chain.forward_kinematics(q_full)[:3, 3]	把解回代 FK，计算实际末端位置（验证用）
return q_full[1:7], ...	只返回 6 个活动关节的角度（去掉 base 和末端的 0）

实测输出：

取料点 PICK = [-0.18 -0.3   0.02]
  ikpy  IK 解(度) = [  55.51  -70.01  -11.8   -27.79  -21.36 -538.41]
  ikpy  回代末端  = [-0.18 -0.3   0.02]   误差 = 3.40e-08
  MuJoCo 验证末端 = [-0.18 -0.3   0.02]   误差 = 3.40e-08
  关节限位内      = True

放料点 PLACE = [ 0.3  -0.12  0.02]
  ikpy  IK 解(度) = [148.81 -71.77 -15.22 -52.64 -33.61 400.35]
  ikpy  回代末端  = [ 0.3  -0.12  0.02]   误差 = 4.19e-09
  MuJoCo 验证末端 = [ 0.3  -0.12  0.02]   误差 = 4.19e-09
  关节限位内      = True

✅ 误差在 1e-9 ~ 1e-8 米 量级——ikpy 的 least_squares 优化器精度非常高。
相比之下，第 06 章自研 DLS 的误差是 8.42e-05（0.08 mm）。两者都远超工程需求，但 ikpy 更精确。

为什么 ikpy 比自研 DLS 更精确：

• ikpy 用 scipy 的 least_squares，这是一个成熟的非线性最小二乘求解器，使用信任区域算法，会自动调整步长和收敛判据
• 自研 DLS 用固定步长（alpha=0.6）和固定容差（tol=1e-4），一旦满足容差就停止，不会继续优化
• 如果把自研 DLS 的 tol 设得更小（如 1e-10），也能达到类似精度，但需要更多迭代次数

注意 initial_position 的用法：它必须是完整长度（含 base 和固定 link），不是 6 个关节角。

⚠️ 常见错误：把 6 个关节角直接传给 initial_position，会导致 ikpy 报错或使用错误的初始值。必须用 np.concatenate([[0.0], q6, [0.0]]) 包装成完整长度。

---

8.4 精度统计

对 18 个工作空间内的目标点批量求解：

目标点数        : 18
误差中位数      : 1.03e-08 m
误差最大        : 6.06e-08 m
误差 < 1mm 占比 : 100%
误差 < 1cm 占比 : 100%

📌 结论：在可达且未撞限位的目标上，ikpy 的精度极高且稳定。

误差分布解读：

• 中位数 1.03e-08 m（约 10 纳米）：一半的目标点误差小于 10 纳米
• 最大值 6.06e-08 m（约 61 纳米）：最差的目标点也只有 61 纳米误差
• 100% < 1mm：所有目标点都远低于 1mm 的工程需求

这个精度远超实际机械臂的重复定位精度（通常 0.01~0.1mm），说明 ikpy 的计算精度不是瓶颈——机械臂本身的机械精度才是。

⚠️ 但这不代表 IK 永远成功。ikpy 对不可达目标会"尽力而为"且不报错——每次求解后必须自己检查末端误差。

精度统计的代码模式：

errors = []
for target in test_targets:
    q_full = chain.inverse_kinematics(target_position=target)
    p = chain.forward_kinematics(q_full)[:3, 3]
    errors.append(np.linalg.norm(p - target))

print(f"误差中位数: {np.median(errors):.2e}")
print(f"误差最大:   {np.max(errors):.2e}")
print(f"<1mm 占比:  {np.mean(np.array(errors) < 1e-3) * 100:.0f}%")

---

8.5 关节限位

限位通过 URDFLink(bounds=(lo, hi)) 设置，ikpy 会把它传给 scipy.optimize.least_squares 的 bounds。

实测中两组解都在限位内（关节限位内 = True）。

自查代码：

q_min = np.array([b[0] for b in BOUNDS])
q_max = np.array([b[1] for b in BOUNDS])
in_bounds = np.all(q_sol >= q_min - 1e-6) and np.all(q_sol <= q_max + 1e-6)

💡 为什么用 1e-6 的容差：scipy 的 least_squares 在边界处可能有微小的数值溢出（如 1.0000000001），直接用 >= 和 <= 可能误判。加一个 1e-6 的容差可以避免这种假阳性。1e-6 rad 约等于 0.00006°，完全可以忽略。

关节限位的实际效果：

当目标需要关节超出限位时：
  ikpy 不会报错
  解会被钳在限位边界上
  末端会有残差（几厘米到几十厘米）
  必须自己检查 ‖FK(q) - target‖

---

8.6 三维可视化

ikpy 自带 chain.plot()，可以画出运动链的骨架：

import matplotlib
matplotlib.use("Agg")          # 无显示环境（服务器/SSH）必须加这一行
import matplotlib.pyplot as plt

fig = plt.figure(figsize=(9, 7))
ax = fig.add_subplot(111, projection="3d")

chain.plot(q_full, ax, target=PICK_POS)      # target 会画一个目标点标记
ax.set_xlabel("X (m)"); ax.set_ylabel("Y (m)"); ax.set_zlabel("Z (m)")
ax.view_init(elev=20, azim=-90)
fig.savefig("ch08_ikpy_chain.png", dpi=110)

生成的图保存在 outputs/ch08_ikpy_chain.png：展示机械臂伸向取料点的姿态。

⚠️ matplotlib.use("Agg") 很重要：在没有图形界面的环境（远程服务器、CI、Docker）里，不设置会报错。Agg 是"纯保存文件"的非交互后端。

常见报错：

ImportError: Cannot load backend 'TkAgg' which requires the 'tk' interactive framework

解决：在 import matplotlib.pyplot 之前设置 matplotlib.use("Agg")。

chain.plot 的参数（ikpy 4.0.0 实测签名：plot(joints, ax, target=None, show=False)）：

参数	说明
joints（第 1 个）	完整关节角数组
ax	matplotlib 的 3D axes
target	可选，画出目标点标记
show	可选，是否直接弹出窗口显示（默认 False；脚本/无显示环境保持默认即可）

💡 chain.plot() 的局限性：

• 只画连杆的骨架（线段），不画机械臂的 3D 模型
• 不支持碰撞体、视觉体的显示
• 适合快速验证运动学，不适合做最终的可视化

要做真实的 3D 可视化，应该用 MuJoCo 的渲染器或 dm_control 的 viewer。

---

8.7 ikpy vs 自研 DLS：怎么选

实测对比（求解 PICK_POS，各跑 10 次取平均）：

方法	耗时	末端误差
ikpy（scipy 优化）	17.61 ms	3.40e-08
自研 DLS（数值雅可比）	9.10 ms	8.42e-05
自研 DLS（解析雅可比 mj_jacSite）	0.96 ms	8.42e-05

💡 自研 DLS 用解析雅可比时比 ikpy 快约 18 倍；但用数值雅可比反而更慢。
这印证了第 06 章的结论：雅可比的实现方式是性能关键。
（性能数据随 CPU 负载浮动，量级是"解析雅可比比 ikpy 快一个数量级"。）

性能差异的原因：

方法	为什么快/慢
ikpy	用 sympy 符号矩阵，每次 FK 都有 Python 层的符号计算开销；scipy 优化器需要多次函数评估
自研 DLS + 数值雅可比	数值雅可比需要 7 次 FK（n+1 次），每次 FK 都用 scipy 的 from_rotvec，Python 开销大
自研 DLS + 解析雅可比	mj_jacSite 是 C 实现，一次前向传播就算完，比数值差分快约一个数量级（实测约 9.5 倍）

选型建议：

场景	推荐
离线规划、快速验证、需要姿态控制	ikpy（接口友好）
嵌在 50 Hz 仿真循环里实时跑	自研 DLS + mj_jacSite（快约 18 倍）
需要加自定义约束（避障、能量最优）	ikpy + 正则项，或自己写优化

两组解不同但都正确：

ikpy     = [  55.51  -70.01  -11.8   -27.79  -21.36 -538.41] 度
DLS 解析 = [  59.58  -67.73  -23.36  -7.16   11.7     0.56] 度

注意 j1/j2 接近（决定末端位置），而 j4/j6 差异很大（冗余自由度，j6 的 -538.41° 与 0.56° 模 360° 后相差约 179°）。

💡 为什么两组解都正确但关节角不同：因为只约束了位置（3个方程），而机械臂有 6 个自由度。多出的 3 个冗余自由度可以自由取值，只要不影响末端位置。ikpy 和自研 DLS 使用不同的优化算法和初始值，所以收敛到了不同的冗余自由度取值。这正是第 06 章讲的"多解"问题，也是第 09 章要解决的核心问题。

---

8.8 ⚠️ 实战中最常见的问题：冗余自由度乱跳

沿 Y 方向从 [-0.18, 0.30, 0.04] 走到 [-0.18, 0.12, 0.04]（向基座靠近），每次都以上一点的解作为种子：

点0: q(度)=[ -56.7  -65.3  -15.1   19.3  -20.6 -191.7]   (起点)
点1: q(度)=[ -52.1  -66.   -17.9   14.7  -28.  -189.1]  相对上一点变化 =  10.6°   ← 突变！
点2: q(度)=[ -47.2  -67.5  -20.1    8.9  -34.4 -187.3]  变化 =  10.4°
点3: q(度)=[ -42.   -69.8  -22.     1.9  -39.4 -187.1]  变化 =  10.5°
点4: q(度)=[ -35.9  -72.5  -23.4   -5.3  -43.6 -195.7]  变化 =  13.7°   ← 最大突变！

最大跳变 = 13.7°，平均跳变 = 11.3°

即使每一步都用上一点的解做种子，相邻两步的关节角仍会跳变十几度。

为什么会这样

我们只约束了位置（3 个方程），而机械臂有 6 个自由度——多出的 3 个冗余自由度完全没有被约束。

优化器在满足位置约束的所有解里，随便挑了一个离种子近的。但"离种子近"是 6 维关节空间的距离，冗余方向上的任何漂移都不影响末端误差，于是腕部（j4/j6）就会在等价解之间乱跳。

冗余自由度乱跳的示意图：

末端轨迹（平滑直线）:
  ●────●────●────●────●
  点0   点1   点2   点3   点4

关节角轨迹（j4, 乱跳）:
  │     ╱╲
  │    ╱  ╲        ╱╲
  │   ╱    ╲      ╱  ╲
  │  ╱      ╲────╱    ╲
  │ ╱                  ╲
  └──────────────────────  点
   0   1   2   3   4

  点0→点1: 关节角整体跳变 10.6°  ← 末端只移动了 4.5cm，关节却跳了 10° 以上!

后果：末端走得很平滑，但机械臂的姿态会突然"抽一下"——在真实机器人上这非常危险。

🔥 在真实机器人上的危害：

• 关节突然高速运动，可能超过电机的速度/加速度限制
• 产生巨大的惯性力，可能导致机械臂振动或损坏
• 如果周围有人或障碍物，突然的姿态变化可能导致碰撞
• 在工业场景中，这种"不可预测的运动"是绝对不允许的

怎么办

第 09 章会给出三种解法：

1. regularization_parameter（正则化）——惩罚偏离种子的幅度，最直接。
2. 姿态约束（orientation_mode）——把冗余自由度也约束住。
3. 零空间投影——在自研求解器里显式控制冗余方向。

💡 这也是本项目 pick_and_place.py 用自研 DLS 而非 ikpy 的原因之一：自研求解器可以显式加"关节中心偏置"和"限位排斥"，把冗余自由度往期望的方向引导。

但"姿态约束"具体怎么加大有讲究——第 09 章的实测会给出一个反直觉的答案：随手锁一个单轴方向可能反而更糟，锁完整姿态才是正解（悬念留到 9.4 揭晓）。

---

8.9 动手练

1. 建链练习：照抄 MJCF 参数，自己建一次 6 轴臂的 chain，并验证 FK 与 MuJoCo 一致（偏差应 < 1e-10）。

2. 末端偏移验证：去掉最后的 ee link，把 J6 从 0 转到 1 rad，观察末端位移（应为 0）。加回 ee 再试（应约 0.0024 m——末端偏移几乎沿工具轴，只有很小的径向分量）。

3. 轨迹连续性：沿一条直线采样 5 个点，每点用上一点的解做种子，统计最大跳变。思考：为什么会跳？

4. 精度统计：随机生成 20 个工作空间内的目标点，统计 IK 误差分布。

练习参考答案与提示

练习 1 提示：参考 8.1 节的 build_arm_chain() 函数。验证时用 5 组随机位姿，对比 chain.forward_kinematics(q_full)[:3,3] 和 data.site_xpos[ee_id]，偏差应 < 1e-15。

练习 2 答案：

• 无末端偏移时：J6 从 0 转到 1 rad，末端位移 = 0.00e+00（J6 不起作用）
• 有末端偏移时：末端位移 ≈ 0.0024 m（J6 绕 +X 轴滚转，末端偏移 (-0.1205, 0, 0.002535) 几乎全沿工具轴，只有 0.002535 m 的径向分量画圆弧）

练习 3 提示：参考 8.8 节的实验。最大跳变通常在 10~15° 之间。原因是只约束了位置（3个方程），冗余自由度（3个）没有被约束，优化器在等价解之间跳跃。

练习 4 提示：

• 随机生成目标点时，要确保在工作空间内（Z-up 下 x 沿零位伸展方向（-X）、y 横向、z 为离地高度，如 x ∈ [-0.4, 0], y ∈ [-0.2, 0.2], z ∈ [0.02, 0.2]）
• 统计误差的中位数、最大值、<1mm 占比
• 预期结果：中位数 < 1e-8 m，100% < 1mm

参考答案见 code/ch08_ikpy_fk_ik.py。

---

8.10 小结

核心要点回顾

• 从 MJCF 建 ikpy 链：逐关节抄 pos / axis / range，最后补一个末端固定偏移。
• 🔥 末端偏移 link 必不可少——没有它，最后一个关节完全不起作用（实测位移 0.00e+00）。
• FK 三方对拍（ikpy / MuJoCo / 手写）偏差 2.52e-16，参数提取正确。
• IK 精度极高：误差 ~1e-08 m，18 个测试点全部 < 1 mm。
• initial_position 必须是完整长度数组（含 base 和固定 link）。
• 可视化用 chain.plot(q, ax, target=...)；无显示环境记得 matplotlib.use("Agg")。
• 性能：自研 DLS + 解析雅可比 比 ikpy 快约 18 倍（随 CPU 负载浮动）。
• ⚠️ 只约束位置会导致冗余自由度乱跳（实测最大跳变 13.7°）——第 09 章解决。

关键代码模式

场景	代码模式
构建 Chain	Chain(links=[OriginLink(), ..., URDFLink(..., joint_type="fixed")], active_links_mask=[False]+[True]*n+[False])
FK	q_full = np.concatenate([[0.0], q6, [0.0]]); T = chain.forward_kinematics(q_full)
IK	q_full = chain.inverse_kinematics(target_position=target, initial_position=init)
验证	p = chain.forward_kinematics(q_full)[:3,3]; error = np.linalg.norm(p - target)
可视化	chain.plot(q_full, ax, target=target)

在本项目中的应用

• 第 09 章继续用这个 Chain 做姿态控制和冗余自由度管理
• 第 19 章用 ikpy 做交叉验证（与自研 DLS 对比）
• pick_and_place.py 主要用自研 DLS（实时性更好），但 ikpy 用于离线验证
• 快速原型验证时，ikpy 比自研 DLS 更方便（不需要 MuJoCo 模型也能跑）

扩展阅读方向

1. ikpy 的 inverse_kinematics_frame：支持完整位姿（位置+姿态）的 IK，下一章会详细讲
2. ikpy 的 regularization_parameter：正则化参数，用于抑制冗余自由度跳变（下一章会证明它效果不好）
3. scipy.optimize.least_squares 文档：理解信任区域算法、bounds 处理、收敛判据
4. 轨迹规划中的 IK 调用模式：连续轨迹时如何用前一帧解做种子，如何处理跳变
5. 其他运动学库对比：PyKDL（ROS 标准）、trac_ik（更快的 IK）、Robotics Toolbox（MATLAB/Python）

---

上一章：07 · ikpy 入门 ｜ 下一章：09 · ikpy 进阶