No.12 双摆 Lemniscate 数值逆运动学

本节把 No.6 的解析 Jacobian IK 升级为 NLopt 数值 IK，并把目标轨迹从圆换成 Lemniscate（∞ 字形曲线）。核心思想是：把仿真器当作「黑箱 FK」，扔给 COBYLA 优化器，让算法自己解 IK，不用手算 Jacobian。

核心对比：No.6 = 解析 IK（手算 J⁻¹）；No.12 = 数值 IK（NLopt 自动解）。各有取舍。

文件说明

mujoco/No_12/
├── manipulator.xml      # MuJoCo XML 模型文件
└── manipulator_ik.py    # 完整脚本：含 NLopt IK、Lemniscate 轨迹、matplotlib 可视化

No.12 没有最小脚本（no_12.py）。要看效果必须跑 manipulator_ik.py。

⚠️ 需要额外依赖：pip install nlopt matplotlib

一、manipulator.xml 详解（对比 No.6）

<mujoco>
    <option timestep="0.001" integrator="RK4" gravity="0 0 0">  <!-- 重力关 -->
        <flag energy="enable" contact="disable" />
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <body pos="0.5 0 1.25" euler="0 90 0">
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
            <geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>
            <body pos="0 0.1 1" euler="0 0 0">
                <joint name="pin2" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
                <geom type="cylinder" size="0.05 0.5" rgba=".9 .9 .9 1" mass="1"/>
                <site name="tip" pos="0 0 0.5" size="0.1" />
            </body>
        </body>
    </worldbody>

    <actuator>
        <position name="pservo1" joint="pin"  kp="100" />
        <velocity name="vservo1" joint="pin"  kv="10"  />
        <position name="pservo2" joint="pin2" kp="100" />
        <velocity name="vservo2" joint="pin2" kv="10"  />
    </actuator>

    <sensor>
        <framepos    objtype='site' objname='tip' />
        <framelinvel objtype='site' objname='tip' />
    </sensor>
</mujoco>

XML 跟 No.6 几乎一样

配置项	No.6	No.12
关节	`pin`, `pin2`	同
`euler`	`0 90 0`	同
末端 site	`<site name="endeff">`	`<site name="tip">`（改名）
重力	`gravity="0 0 0"`	同
actuator	2 position + 2 velocity servo	同
sensor	framepos + framelinvel	同
timestep	0.0001	0.001（粗 10 倍）

唯一明显差异：site 名字从 endeff 改成 tip，timestep 粗 10 倍。其他完全一致。

二、Lemniscate（∞ 字形）轨迹

2.1 参数方程

def get_lemniscate_ref(t):
    wt = omega * t
    denominator = 1 + np.sin(wt)**2

    x = center_x + (a * np.cos(wt)) / denominator
    z = center_z + (a * np.sin(wt) * np.cos(wt)) / denominator
    return np.array([x, z])

Lemniscate of Bernoulli 的标准参数化（极坐标版转笛卡尔）：

x(t) = a·cos(ωt) / (1 + sin²(ωt))
z(t) = a·sin(ωt)·cos(ωt) / (1 + sin²(ωt))

2.2 形状示意

       z
       ↑
   ╭───╮ ╭───╮
   │   │ │   │       ← ∞ 字
   ╰───┼─┼───╯
       └─┘───→ x
   ╭───╮ ╭───╮
   │   │ │   │
   ╰───╯ ╰───╯

2.3 参数含义

参数	值	含义
`a`	0.25	半轴长（控制 ∞ 字大小）
`omega`	0.4	角频率（rad/s）
周期	`2π/ω ≈ 15.7s`	画完一个 ∞ 需 15.7 秒

2.4 simend 设计

simend = 0.25 + 2 * np.pi / omega    # ≈ 15.95 秒

0.25 秒 启动 + 一个完整周期（2π/ω）。

2.5 圆心锚定

def init_controller(model, data):
    end_eff_pos = forward_kinematics([-0.5, 1.0])
    center_x = end_eff_pos[0] - 0.25       # 圆心 x = 当前末端 x - a
    center_z = end_eff_pos[1]              # 圆心 z = 当前末端 z

为什么要锚定？让 Lemniscate 从末端当前位置附近开始，自然过渡，而不是凭空出现。

三、核心：数值 IK（NLopt）

3.1 优化问题

        min_{q1, q2}      0
   subject to -π ≤ q1 ≤ π
              -π ≤ q2 ≤ π
              FK(q) = X_target

维度	No.6 解析 IK	No.12 数值 IK
决策变量	Δq（关节修正）	q1, q2（绝对关节角）
求解方法	`J⁻¹ · Δx`	NLopt COBYLA
目标	误差最小	可行性（cost=0）
约束	无	2 个等式约束（末端 = 目标）
求解时间	O(1) 微秒级	O(N) ms 级（N=迭代次数）

3.2 完整 IK 函数

def inverse_kinematics(x):
    opt = nlopt.opt(nlopt.LN_COBYLA, 2)
    opt.set_lower_bounds([-np.pi, -np.pi])
    opt.set_upper_bounds([np.pi, np.pi])
    opt.set_min_objective(cost_func)
    tol = [1e-4, 1e-4]
    opt.add_equality_mconstraint(equality_constraints, tol)
    opt.set_xtol_rel(1e-4)
    sol = opt.optimize(x)
    return sol

完全没用到任何关于这个机械臂的 Jacobian 或 DH 参数 —— 纯靠仿真器的 FK 当黑箱。

3.3 约束函数 = 仿真器当 FK

def equality_constraints(result, x, grad):
    end_eff_pos = forward_kinematics(x)        # 调仿真器算 FK
    result[0] = end_eff_pos[0] - X_target[0]  # x 误差
    result[1] = end_eff_pos[1] - X_target[1]  # z 误差

跟 No.11 的 simulator 思路一致：仿真器 = 约束求值器。

四、forward_kinematics：单独 `data_sim` 的妙用

4.1 关键设计

# 主代码里（第 259 行）
data_sim = mj.MjData(model)     # 独立的 FK 仿真数据

为什么需要独立的 data_sim？

用途	用什么 data
主仿真（动画显示）	`data`
IK 计算（每步调一次）	`data_sim`

好处：

IK 计算不污染主仿真的状态
IK 调 mj_forward 不会反过来影响主仿真
主仿真跑 1000 步，IK 跑 N 次（不同 q 试探），互不干扰

4.2 实现

def forward_kinematics(q):
    data_sim.qpos[0] = q[0]
    data_sim.qpos[1] = q[1]
    data_sim.ctrl[0] = data_sim.qpos[0]     # ← 多余，见 FAQ
    data_sim.ctrl[2] = data_sim.qpos[1]

    mj.mj_forward(model, data_sim)

    end_eff_pos = np.array([
        data_sim.sensordata[0],   # tip x
        data_sim.sensordata[2]    # tip z
    ])
    return end_eff_pos

关键：mj_forward 不消耗时间步，只算一次前向运动学。

五、controller 详解

def controller(model, data):
    global X_target

    # 1. 算当前时刻的 Lemniscate 目标
    X_target = get_lemniscate_ref(data.time)

    # 2. 当前关节角作为初始猜测
    qpos = np.array([data.qpos[0], data.qpos[1]])

    # 3. 调 NLopt 解 IK
    sol = inverse_kinematics(qpos)

    # 4. 写 position servo 目标
    data.ctrl[0] = sol[0]
    data.ctrl[2] = sol[1]

5.1 每步都重新解 IK

是的，每步都调 NLopt。这是 No.12 的最大开销。

5.2 `X_target` 全局变量

X_target 在 controller 里被赋值，但在 equality_constraints 里被读。这俩函数不直接传参，靠 global 共享。

def equality_constraints(result, x, grad):
    global X_target    # 读全局
    ...

设计选择：用全局变量避免在 opt.add_equality_mconstraint 的 callback 里传参。能用但丑陋。

5.3 `set_mjcb_control` 没注册

# mj.set_mjcb_control(controller)    ← 注释掉

为什么注释掉？因为主循环里手动调 controller：

while (data.time - simstart < 0.1):
    controller(model, data)         # 手动调
    mj.mj_step(model, data)

好处：

每 0.1 秒才调一次 controller（不是每 mj_step）
避免每步 1000Hz 跑 NLopt（太慢）

坏处：

不是标准用法
跟 mj_step 解耦，可能漏调（如果 data.time - simstart >= 0.1 的判断出问题）

六、init_controller 详解

def init_controller(model, data):
    global center_x, center_z, X_target

    # 1. 用某个 q 算末端位置，确定 Lemniscate 中心
    end_eff_pos = forward_kinematics([-0.5, 1.0])
    center_x = end_eff_pos[0] - 0.25
    center_z = end_eff_pos[1]

    # 2. 解 t=0 时的 IK，得到初始关节角
    q_guess = np.array([-0.5, 1.0])
    X_target = get_lemniscate_ref(0.0)
    q_pos = inverse_kinematics(q_guess)

    # 3. 写初始关节角
    data.qpos[0] = q_pos[0]
    data.qpos[1] = q_pos[1]

为什么这里也解一次 IK？因为 get_lemniscate_ref(0.0) 给的初始目标位置可能不是当前 FK 位置，先解一次让仿真正确起步。

七、graph() 可视化（运行结束后）

def graph():
    # 收集的 end_eff_pos
    end_eff_pos_arr = np.concatenate(end_eff_pos, axis=1)

    # 参考 Lemniscate
    wt = omega * np.linspace(0.0, simend, 500)
    denominator = 1 + np.sin(wt)**2
    leminiscate_x = center_x + (a * np.cos(wt)) / denominator
    leminiscate_z = center_z + (a * np.sin(wt) * np.cos(wt)) / denominator

    # 画图
    fig, ax = plt.subplots(1, 1, figsize=(8, 5))
    ax.plot(end_eff_pos_arr[0, :], end_eff_pos_arr[1, :], color="cornflowerblue", ...)
    ax.plot(leminiscate_x, leminiscate_z, color="darkorange", ...)
    ax.set_aspect("equal")
    plt.show(block=False)
    plt.pause(5)
    plt.close()

在主循环结束后画 matplotlib 对比图：实测末端轨迹（蓝）vs 参考 Lemniscate（橙）。

依赖：需要 matplotlib + LaTeX（mpl.rcParams['text.usetex'] = True）。没装 LaTeX 会报错。

八、跟 No.6 解析 IK 的对比

维度	No.6 解析 IK	No.12 数值 IK
核心公式	`Δq = J⁻¹ · Δx`	NLopt COBYLA
需要 Jacobian 吗	✅ 必须手算	❌ 完全不用
需要 FK 闭式解吗	❌	❌（用仿真器当 FK）
需要系统参数吗	✅（杆长 L1, L2）	❌
每步求解时间	O(1) ≈ 10 μs	O(N) ≈ 1-10 ms
精度	精确（一阶近似）	近似（靠 tol 控制）
奇异点	需手动处理	自动避开（靠约束）
适用性	仅简单机械臂	任何系统（黑箱）
可扩展到 N 关节吗	难（手写 J）	✅ 直接通用

数值 IK 的核心优势

你完全不需要知道系统的 Jacobian、DH 参数、连杆长度。只要有仿真器，任何机器人都能用这套方法。

这就是 model-free IK 的吸引力 —— 系统变了就换仿真器，算法代码完全不用改。

数值 IK 的核心劣势

慢：每步要解 50-200 次 NLopt
不稳定：可能卡在局部最优（COBYLA 是局部算法）
依赖初值：初始猜测 x0 选得不好可能不收敛

九、整体控制流程图

启动 ───────────────────────────────────────────
  │
  ├─ 创建 data (主仿真) + data_sim (FK 用)
  ├─ init_controller:
  │    ├─ 锚定 Lemniscate 中心
  │    ├─ 解 t=0 的 IK
  │    └─ 设 data.qpos 为解出来的关节角
  └─ 注册 controller（实际**手动**调）

主循环 (60Hz) ────────────────────────────────────
  内层 (0.1s 跑 100 步):
    │
    ├─ 手动调 controller:
    │    ├─ 读 data.time
    │    ├─ X_target = get_lemniscate_ref(t)  ← 算参考目标
    │    ├─ 用当前 qpos 当初始猜测
    │    ├─ inverse_kinematics(qpos):
    │    │    └─ COBYLA 内部:
    │    │         └─ equality_constraints:
    │    │              └─ forward_kinematics(q'):
    │    │                   ├─ data_sim.qpos = q'
    │    │                   ├─ mj_forward
    │    │                   └─ return (data_sim.sensordata[0], [2])
    │    └─ data.ctrl[0] = sol[0]  (pin)
    │    └─ data.ctrl[2] = sol[1]  (pin2)
    │
    └─ mj_step（用 ctrl 推进物理）
  
  外层: 渲染 + 收集 end_eff_pos
  结束: graph() 画 matplotlib 对比图

十、运行方法

pip install nlopt matplotlib
cd mujoco/No_12/
mjpython manipulator_ik.py

预期效果：

双摆末端在 (x, z) 平面画 ∞ 字
持续约 15.7 秒（一个完整周期）
结束后弹 matplotlib 窗口，对比实测 vs 参考轨迹
两条曲线应该重合（数值 IK 精度足够）

⚠️ 没装 LaTeX 会报错（text.usetex = True 那行）。可以注释掉 mpl.rcParams['text.usetex'] = True。

十一、常见问题 / Bugs

1. ⚠️ 重复的 `forward_kinematics` 函数（dead code）

# 第 45-58 行：错误的版本
def forward_kinematics(self, q):    # ← 多了个 self
    ...

# 第 78-91 行：正确的版本
def forward_kinematics(q):
    ...

第一个永远不会被调用（Python 用的是第二个）。可以删掉。

2. ⚠️ `forward_kinematics` 里的 `ctrl` 设置是死代码

data_sim.ctrl[0] = data_sim.qpos[0]
data_sim.ctrl[2] = data_sim.qpos[1]

mj_forward 不会应用 actuator dynamics。这两行对 FK 结果没影响。可以删。

3. ⚠️ `set_mjcb_control` 注释掉

# mj.set_mjcb_control(controller)

靠手动调 controller：

while (data.time - simstart < 0.1):
    controller(model, data)
    mj.mj_step(model, data)

能用但不规范。如果想标准用法，恢复 set_mjcb_control 并删除主循环里的手动调用。

4. matplotlib LaTeX 报错

! LaTeX Error: File `amsmath.sty' not found.

解决：

# 注释掉
# mpl.rcParams['text.usetex'] = True
# mpl.rcParams['text.latex.preamble'] = r'\usepackage{amsmath}'

或装 MacTeX（macOS）/ texlive-full（Linux）。

5. NLopt 每步都跑，太慢

观察：每个 0.1s 的控制步要解 1 次 NLopt ≈ 1-10ms。加上 mj_step 100 步 ≈ 0.1s，单帧总耗时 0.1-0.2s → 实际跑 5-10x 实时速度。

加速：

降低 NLopt 容差（tol=[1e-3, 1e-3]）
减少迭代次数（xtol_rel=1e-3）
用解析 IK（No.6）代替

6. 末端轨迹不完全画 ∞ 字

可能原因：

NLopt 容差太大
奇异点附近 IK 解不稳
仿真器自身数值误差

调试：在 controller 里加：

print(f"X_target={X_target}, sol={sol}, FK(sol)={forward_kinematics(sol)}")

7. `X_target` 全局变量的隐患

def controller(model, data):
    global X_target
    X_target = get_lemniscate_ref(data.time)   # 写

def equality_constraints(result, x, grad):
    global X_target                          # 读
    ...

隐患：如果 equality_constraints 在 controller 之前被调（理论上不会），会读到上一步的 X_target。

更好的做法：inverse_kinematics(x_target) 显式传参，不用全局变量。

8. 跟 No.6 比速度差异

任务	No.6 解析	No.12 数值
单步 IK	≈ 10 μs	≈ 1-10 ms
1000 步仿真	≈ 10 ms	≈ 1-10 s
实时性	1000+ Hz	1-10 Hz

No.12 不能用于实时高频控制。适合离线规划 + 重放（MPC 风格）。

9. 跟 No.11 抛射体优化的区别

	No.11 抛射体	No.12 Lemniscate IK
优化变量	v, θ, T（3 个）	q1, q2（2 个）
目标	命中目标点	跟踪连续轨迹
调用频率	1 次（启动时）	每 0.1s
控制器	空	写 `data.ctrl`

No.12 是 No.11 的「在线版」 —— 每步都做一次小优化。

10. 用更高效的优化器？

算法	速度	适用
`LN_COBYLA`	慢	当前（无梯度、鲁棒）
`LD_MMA`	快	需要梯度（手动有限差分）
`GN_AGS`	很慢	全局最优

十二、整体公式对应

─────── 参考轨迹（任务空间）──────
X*(t) = (x_lemniscate(t), z_lemniscate(t))   ← Lemniscate 参数化

─────── IK 优化（每 0.1s 一次）──────
min_{q1, q2}  0
s.t.  FK(q) = X*(t)
      -π ≤ q ≤ π

─────── 仿真器作 FK（黑箱）──────
data_sim.qpos = q
mj_forward
return (data_sim.sensordata[0], data_sim.sensordata[2])

─────── 实时控制（每 0.1s 一次）──────
data.ctrl[0] = sol[0]
data.ctrl[2] = sol[1]
position servo (kp=100) 内部做 PD 到位

─────── 物理仿真（每 0.0001s 一次）──────
mj_step 用 ctrl 推进
末端到达 sol 对应的位置

十三、一句话总结

No.12 = 「Lemniscate 目标 + NLopt 数值 IK + 独立 data_sim FK + matplotlib 对比可视化」。跟 No.6 的解析 IK 相比，牺牲了速度换来了通用性 —— 不需要推导 Jacobian 也能让任意系统跟踪任意轨迹。核心代价是每步 1-10ms 的 NLopt 求解开销，所以只能 10Hz 控制频率。