用 Pinocchio 给 Franka Panda 做逆运动学

Pinocchio 是目前最快的刚体动力学库之一，C++ 内核 + Python 绑定，被 Crocoddyl、TSID、HPP 等大量项目作为底层。本文讲清楚两件事：

1. 雅可比迭代 IK 的原理（用 numpy 在浏览器内试一遍 2-link 平面臂）
2. 用 Pinocchio 跑真实 Panda 7-DOF IK（在 JupyterLab 中运行）

1. 雅可比迭代 IK 是什么

逆运动学（IK）：给定目标位姿 T_target，求关节角 q 使末端到达目标。最经典的算法是雅可比迭代法：

1. 算当前 FK：当前末端位姿 T_cur
2. 算位姿误差 err = T_target − T_cur
3. 算末端雅可比 J（末端位姿对关节角的偏导）
4. 求关节增量：dq = J⁻¹ · err（用阻尼最小二乘防奇异）
5. q ← q + α · dq，回到 1，直到收敛

下面用 numpy 实现一个最简版本（2-link 平面臂），可以在浏览器内直接跑。

2. 在浏览器内试一下：2-link 平面臂 IK

下面这段代码用纯 numpy 实现 2-link 平面臂的 FK + 数值雅可比 IK，并把迭代轨迹画出来。点 Run 执行（首次加载 Pyodide runtime ~15MB，之后页内缓存）。

怎么玩：

• 改 target = np.array([1.5, 0.5]) 里的目标点，看 IK 怎么收敛
• 改 q_init 看不同初始猜测的影响
• 把 lr=0.5 改成 lr=0.05 看慢收敛
• 当目标超出 [0, L1+L2] 半径时（np.sqrt(target[0]**2 + target[1]**2) > 2.0），雅可比奇异，会震荡

3. 在 JupyterLab 中跑 Pinocchio + 真实 Panda

上面的 numpy 版本只是教学。真实场景用 Pinocchio（C++ 内核，比 numpy 快几十倍），并且支持任意 URDF、SE3 误差（位置 + 朝向）、关节限位。

完整 notebook（包含：加载 Panda → 6D 误差 IK → meshcat 可视化）放在 JupyterLab 中：

notebook 内容要点（仅供参考，要在 JupyterLab 内才能跑）：

import pinocchio as pin
import numpy as np
from example_robot_data import load
 
robot = load('panda')
model = robot.model
data = model.createData()
frame_id = model.getFrameId('panda_hand')
 
q0 = pin.neutral(model)
q0[:7] = np.array([0.0, -0.5, 0.0, -1.8, 0.0, 1.5, 0.8])
 
def ik_solve(model, data, frame_id, q_init, T_target,
             max_iter=200, tol=1e-4, damping=1e-3, alpha=0.5):
    q = q_init.copy()
    for i in range(max_iter):
        pin.forwardKinematics(model, data, q)
        pin.updateFramePlacement(model, data, frame_id)
        T_cur = data.oMf[frame_id]
        err = pin.log6(T_target.actInv(T_cur)).vector  # 6D error
        if np.linalg.norm(err) < tol:
            return q
        J = pin.computeFrameJacobian(
            model, data, q, frame_id,
            pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
        )
        JJt = J @ J.T + (damping ** 2) * np.eye(6)
        dq = J.T @ np.linalg.solve(JJt, err)
        dq_full = np.zeros_like(q)
        dq_full[:7] = dq[:7]
        q = pin.integrate(model, q, alpha * dq_full)
    return q
 
T_target = pin.SE3(np.eye(3), np.array([0.4, 0.2, 0.6]))
q_sol = ik_solve(model, data, frame_id, q0, T_target)
print(f"Final EE: {data.oMf[frame_id].translation}")

关键 API：

• pin.forwardKinematics — 算 FK
• pin.computeFrameJacobian — 算雅可比
• pin.log6(...).vector — SE3 误差（6 维：3 平动 + 3 旋转）
• pin.integrate — 流形上的加法（避免直接相加违反关节空间约束）

4. 小结

• 原理（numpy + 浏览器）：理解 FK / 雅可比 / 阻尼最小二乘
• 实战（Pinocchio + JupyterLab）：任意 URDF、SE3 误差、生产可用

下一步可以挑战：

• 关节限位：每步迭代后 clip 到 model.lowerPositionLimit / upperPositionLimit
• 冗余求解：7-DOF Panda 是冗余的，IK 时可同时优化避奇异 secondary task
• 全身动力学（TSID）：把 IK 换成动态平衡 + 力矩约束

Pinocchio 官方 tutorials 看完前 5 个就够入门。