No.11 抛射体轨迹优化（NLopt 非线性规划）

本节介绍一个完全不同的控制范式 —— 离线轨迹优化（trajectory optimization）。前 9 节都是「在线」控制器：每步读状态、算控制量。No.11 是「离线」求解：先用 NLopt 找最优初始速度（v, θ），然后开环播放让小球飞向目标。

核心思想：把仿真器当作「约束求值器」，扔给优化器，让算法自己找参数。

文件说明

mujoco/No_11/
├── ball.xml             # MuJoCo XML 模型文件（地面 + 圆柱 + 球 + 目标盒）
└── projectile_opt.py    # 完整脚本：含 NLopt 优化、simulator、开环播放

No.11 没有最小脚本（no_11.py）。要看效果必须跑 projectile_opt.py。

一、ball.xml 详解

<mujoco>
    <visual>
        <headlight ambient="0.5 0.5 0.5"/>
    </visual>

    <worldbody>
        <geom type="plane" size="100 1 0.1" rgba=".9 0 0 1"/>

        <!-- 【No.11 关键】视觉参考柱 -->
        <geom type="cylinder" pos="5 0 1" size="0.2 1" rgba="0.9 0.9 0.9 1" />

        <!--
            抛射体（球）：
            pos="0 0 0.1"（地面附近）
            <joint type="free"/>：6 DOF 自由关节
        -->
        <body pos="0 0 .1">
            <joint type="free"/>
            <geom type="sphere" size=".1" rgba="0 .9 0 1" mass="1"/>
        </body>

        <!--
            目标（盒）：
            pos="5 0 2.1"（圆柱顶上）
            同样 free 关节
        -->
        <body pos="5 0 2.1">
            <joint type="free"/>
            <geom type="box" size="0.1 0.1 0.1" rgba="0.9 0.9 0 1" mass="0.1"/>
        </body>
    </worldbody>
</mujoco>

场景布局

                  ┌──┐  ← 目标盒 (5, 0, 2.1)
                  │  │
                  │  │
       ┌──────────┴──┴──────────┐
       │  cylinder (5, 0, 1)    │   ← 视觉参考柱
       │  size="0.2 1"          │
       └──────────┬──────────────┘
                  │
══════════════════╪═══════════════  ← 地面
       ●                              ← 抛射体 (0, 0, 0.1)
       ball (mass=1)

物体	位置	作用
地面	z=0	球碰到就停
圆柱	(5, 0, 1)，半径 0.2，高 2	视觉参考柱（无质量）
球	(0, 0, 0.1)，mass=1	抛射体，free 关节
盒	(5, 0, 2.1)，mass=0.1	目标，需要被击中

关键设计：圆柱只是视觉（无质量），目标是盒（不是柱顶）。这个反直觉设计让优化目标精确（不用算柱顶坐标）。

二、核心：弹道优化（NLopt）

2.1 优化问题定义

        min_X      0
   subject to 0.1 ≤ v   ≤ 10000
              0.1 ≤ θ   ≤ π/2 - 0.1
              0.1 ≤ T   ≤ 10000
              x(T) = 5.0      ← 落点 x
              z(T) = 2.1      ← 落点 z

3 个决策变量：X = [v, θ, T]（初速度大小、发射角、飞行时间）

目标函数：恒为 0（可行性问题，找满足约束的参数即可）

2 个等式约束：飞行 T 秒后，位置 (x, z) 必须等于目标 (5.0, 2.1)

2.2 为什么需要优化？

经典抛物线公式的解析解存在（v = sqrt(g·R² / (2·cos²θ·(tanθ - H/R))），但：

仿真器有空气阻力、接触、积分误差等复杂因素
想在「真实仿真」里命中目标，解析解不准确
用仿真器本身作为约束求值器，可以自动处理这些细节

2.3 优化算法：COBYLA

opt = nlopt.opt(nlopt.LN_COBYLA, 3)

属性	值	含义
算法	`LN_COBYLA`	无导数、约束优化算法
维度	3	决策变量数 = 3

为什么 COBYLA：

不需要梯度（仿真器的约束没法解析求导）
支持等式/不等式约束
对小规模、低维问题效率够用

2.4 完整代码

def optimize_ic(x):
    opt = nlopt.opt(nlopt.LN_COBYLA, 3)
    opt.set_lower_bounds([0.1, 0.1, 0.1])
    opt.set_upper_bounds([10000.0, np.pi/2 - 0.1, 10000.0])
    opt.set_min_objective(cost_func)             # cost = 0
    opt.add_equality_mconstraint(equality_constraints, tol=[1e-4, 1e-4])
    opt.set_xtol_rel(1e-4)
    sol = opt.optimize(x)
    return sol

def cost_func(x, grad):
    return 0.0                                   # 可行性问题，cost 恒为 0

def equality_constraints(result, x, grad):
    pos = simulator(x)                          # 仿真 → 终态位置
    result[0] = pos[0] - 5.0                    # x 误差
    result[1] = pos[1] - 2.1                    # z 误差

simulator(x) 是核心：把决策变量 (v, θ, T) 转成「仿真 T 秒后」的位置。这个函数是优化器的「黑箱」。

三、simulator 函数详解：仿真器当作约束求值器

def simulator(x):
    v, theta, time_of_flight = x[0], x[1], x[2]

    # 1. 设置初速度
    data.qvel[0] = v * np.cos(theta)    # x 方向
    data.qvel[2] = v * np.sin(theta)    # z 方向

    # 2. 仿真 T 秒
    while data.time < time_of_flight:
        mj.mj_step(model, data)

    # 3. 读终态位置
    pos = np.array([data.qpos[0], data.qpos[2]])

    # 4. 重置 data（为下一次调用准备）
    mj.mj_resetData(model, data)

    return pos

3.1 输入 → 输出

输入	含义
`x[0]` = v	速度大小
`x[1]` = θ	发射角（与水平面夹角）
`x[2]` = T	飞行时间

输出	含义
`pos[0]`	T 秒后的 x 坐标
`pos[1]`	T 秒后的 z 坐标

3.2 为什么用 `data.time` 作为循环条件？

while data.time < time_of_flight:
    mj.mj_step(model, data)

data.time 由 MuJoCo 内部维护，每一步 mj_step 增加 model.opt.timestep。当 data.time 达到 time_of_flight 时停止。

3.3 为什么 `mj_resetData` 放在最后？

每次 simulator 调用都会修改全局 data（设初速度、跑仿真）。如果不重置，下一次调用会从「上一次终态」开始 → 结果完全错。

mj_resetData 把 data.qpos 和 data.qvel 复位到 XML 里的初始值。

潜在 bug：simulator 假设初始 qpos 和 qvel 就是 XML 默认值。如果你在调用 simulator 之前先动了 data，状态会污染。

3.4 `qvel[0]`、`qvel[2]` 的索引

<joint type="free"/> 给 body 6 个 DOF（3 平移 + 3 旋转）：

`qvel` 索引	含义	No.11 是否用到
0	x 方向线速度	✅
1	y 方向线速度	❌
2	z 方向线速度	✅
3	绕 x 角速度	❌
4	绕 y 角速度	❌
5	绕 z 角速度	❌

所以 qvel[0] = v·cos(θ)、qvel[2] = v·sin(θ)。

四、init_controller：找最优 v, θ, T

def init_controller(model, data):
    # 初始猜测
    v = 10.0
    theta = np.pi / 4
    time_of_flight = 2.0

    if NLOPT_IMPORTED:
        sol = optimize_ic(np.array([v, theta, time_of_flight]))
    else:
        sol = np.array([9.398687489285555, 1.2184054599970882, 1.5654456340479144])

    v_sol, theta_sol = sol[0], sol[1]
    simend = sol[2] + 2  # 仿真时间稍长于飞行时间

    data.qvel[0] = v_sol * np.cos(theta_sol)
    data.qvel[2] = v_sol * np.sin(theta_sol)

4.1 Fallback 机制

try:
    import nlopt
except ImportError:
    print("nlopt not imported, switching to pre-computed solution")
    NLOPT_IMPORTED = False

鲁棒性设计：如果没装 nlopt，用预计算解 sol = [9.40, 1.22, 1.57]。

预计算解 = 优化器对初始猜测 [10, π/4, 2] 跑出来的解。

4.2 初始猜测的物理意义

变量	初始值	物理含义
v	10.0 m/s	较快速度（够得着 5m 远）
θ	π/4 ≈ 45°	经典最优抛射角（无空气阻力时）
T	2.0 s	估计飞行时间

4.3 simend 的设计

simend = sol[2] + 2    # 仿真时间比飞行时间长 2 秒

飞行 T 秒后球已经落地。多给 2 秒让球在地面滚/停，方便看效果。

五、controller 详解：空函数

def controller(model, data):
    pass

这是 No.11 最大的特点：

之前 No.4-9	No.11
controller 每步算 `u`	controller 什么都不做

为什么？因为：

优化器已经算好了 v 和 θ
init_controller 已经把 qvel[0] 和 qvel[2] 设成最优值
之后开环播放，让物理引擎自己把球送过去
不需要任何反馈

这就是「开环控制」的极致形式 —— 控制器不存在。

六、主循环：开环播放

init_controller(model, data)        # 离线找最优 v, θ
mj.set_mjcb_control(controller)     # 注册空 controller

while not glfw.window_should_close(window):
    simstart = data.time

    while (data.time - simstart < 1.0/60.0):
        mj.mj_step(model, data)      # 物理步进，**没有控制输入**

    if (data.time >= simend):
        break

    # 相机跟随球
    cam.lookat[0] = data.qpos[0]
    mj.mjv_updateScene(...)
    mj.mjr_render(...)
    glfw.swap_buffers(window)
    glfw.poll_events()

整个主循环没有读状态、算控制量、写 actuator —— 纯粹的物理仿真 + 渲染。

七、跟 No.4-9 的本质区别

维度	No.4-9 在线控制	No.11 离线优化
决策时机	每步	一次性（init_controller）
反馈	必须有	不需要（开环）
控制律	`u = K(x)`, `Δq = J⁻¹·Δx` 等	没有控制律
目标	跟踪/镇定/轨迹	一次性命中目标
方法	PD、IK、LQR、FSM	非线性规划（NLopt）
仿真器角色	物理引擎	约束求值器
失败恢复	可以反馈纠错	不行（开环）
计算成本	每步 O(n²) ~ O(n³)	离线 O(N) 次仿真
鲁棒性	中（取决于控制器）	差（参数不准就 miss）

控制思想对比

No.4-9:  「**在线**」
  每步: 读 x → 算 u → 写 actuator
  优点: 鲁棒于扰动和参数误差
  缺点: 需要设计控制器、调参

No.11:  「**离线**」
  一次性: 找参数 (v, θ, T) → 设初速度 → 开环播放
  优点: 不需要控制器，理论最优
  缺点: 完全开环，扰动即失败

No.11 是「模型预测 + 开环执行」的最简形式。

八、整体流程图

启动 ───────────────────────────────────────────
  │
  ├─ 加载 ball.xml
  ├─ 创建 model, data
  │
  ├─ init_controller:
  │    ├─ 设初始猜测 (v=10, θ=π/4, T=2)
  │    │
  │    └─ 调 optimize_ic:
  │         │
  │         └─ 循环（COBYLA 内部）:
  │              │
  │              ├─ 提议新参数 (v', θ', T')
  │              │
  │              ├─ 调 simulator(v', θ', T'):
  │              │     ├─ 设 qvel
  │              │     ├─ while data.time < T': mj_step
  │              │     ├─ 读 (qpos[0], qpos[2])
  │              │     └─ mj_resetData
  │              │
  │              └─ 算约束违反: (pos - target)
  │
  ├─ 解: v_sol, θ_sol, T_sol
  ├─ data.qvel[0] = v_sol * cos(θ_sol)
  ├─ data.qvel[2] = v_sol * sin(θ_sol)
  └─ simend = T_sol + 2

主循环 ───────────────────────────────────────────
  每帧 (60Hz):
    内层 1000Hz: mj_step（**无控制**）
    外层: 渲染 + 相机跟随

  最终: 球**精准**落在目标盒 (5, 0, 2.1) 附近

九、运行方法

# 1. 安装依赖
pip install nlopt numpy

# 2. 运行
cd mujoco/No_11/
mjpython projectile_opt.py

预期效果：

球从 (0, 0, 0.1) 出发
按优化器算的 (v, θ) 抛射
精准落在 (5, 0, 2.1) 的目标盒上
相机自动跟随

⚠️ 第一次启动会卡 1-3 秒 —— NLopt 在跑优化（大约 50-200 次 simulator 调用）。这是离线代价。

十、调参 / 玩转

1. 改目标位置

# 改 XML
<body pos="5 0 2.1">  →  <body pos="7 0 3.0">

# 改约束
result[0] = pos[0] - 5.0  →  result[0] = pos[0] - 7.0
result[1] = pos[1] - 2.1  →  result[1] = pos[1] - 3.0

2. 加快优化速度

opt.set_xtol_rel(1e-4)  →  opt.set_xtol_rel(1e-2)   # 粗糙一点
tol = [1e-4, 1e-4]      →  tol = [1e-2, 1e-2]        # 约束放宽

3. 改用更快的算法

opt = nlopt.opt(nlopt.LN_COBYLA, 3)         # 慢但稳
opt = nlopt.opt(nlopt.LN_NELDERMEAD, 3)    # 单纯形，无约束
opt = nlopt.opt(nlopt.GN_AGS, 3)           # 全局优化（慢但能找到全局最优）

4. 加成本函数

def cost_func(x, grad):
    # 最小化发射能量
    return 0.5 * x[0]**2

# 注意：这样 min 不再是 0，是 0.5·v²
# 优化器会找「**最省力**」的命中方式

十一、常见问题

1. `ImportError: No module named nlopt`

解决：

# macOS
brew install nlopt
pip install nlopt

# 或 conda
conda install -c conda-forge nlopt

装不上就用预计算解（代码里已经 fallback）。

2. 球飞出去没命中目标

可能原因：

数值精度不够（tol=[1e-4, 1e-4] 太松）
初始猜测离真实解太远，COBYLA 陷入局部
仿真器本身有 bug（mj_resetData 顺序错）

调试：

def simulator(x):
    print(f"  try v={x[0]:.2f} θ={x[1]:.2f} T={x[2]:.2f}")
    v, theta, time_of_flight = x[0], x[1], x[2]
    data.qvel[0] = v * np.cos(theta)
    data.qvel[2] = v * np.sin(theta)
    while data.time < time_of_flight:
        mj.mj_step(model, data)
    pos = np.array([data.qpos[0], data.qpos[2]])
    mj.mj_resetData(model, data)
    print(f"    → pos=({pos[0]:.2f}, {pos[1]:.2f})")
    return pos

3. 优化要好几秒

原因：COBYLA 是无梯度算法，需要很多次 simulator 调用（每次 ~1000 步仿真）。

加速：

用有限差分给 grad 参数填值，配合 LD_MMA（需要梯度）
用更小的 tol
减少决策变量数

4. 球飞完砸穿了地面

原因：积分器在大 v 下不稳定。

解决：

减小 timestep（XML 里的）
增大阻尼（无中生有……这个例子没有）

5. `simulator` 重置 data 之后，初始条件变了？

原因：mj_resetData 把 qpos 恢复到 XML 里的值。如果之后再调 simulator，初始 v 又是基于这个新 qpos。

验证：

print(data.qpos)  # 调用 simulator 后
# 应该是 [0, 0, 0.1, 1, 0, 0, 0] (位置+四元数)

6. 为什么目标盒是 free 关节？

因为目标盒也要被重力影响（mass=0.1）。如果用固定关节，撞上去会刚性弹开。free 关节让它能跟着被撞飞，物理上更真实。

7. 跟 MPC 什么关系？

	No.11 离线优化	MPC（在线）
求解时机	一次性	每个控制步
计算预算	多	少
反应扰动	❌	✅
实现复杂度	简单	复杂

No.11 是 MPC 的「最简离线版」。

8. `data.qvel[0]` 是线速度还是广义速度？

对 free joint，是线速度（单位 m/s）。对 hinge joint，是角速度（rad/s）。这是约定。

9. 怎么改成「加空气阻力」？

XML 里加：

<option density="1.225"/>  <!-- 空气密度 -->

MuJoCo 会自动算空气阻力。约束函数会相应改变（解析公式不准确了，必须靠优化器）。

10. 优化器解不稳定（每次结果不一样）

原因：COBYLA 是确定性的，但浮点累积误差可能让结果差 1e-3。

解决：

np.random.seed(0) （如果有随机性）
用 set_xtol_rel(1e-6) 更紧的容差
多目标：用全局算法（GN_AGS）

十二、整体公式对应

──────── 优化问题（数学）───────
min_{v, θ, T}      0
subject to:
    0.1 ≤ v ≤ 10000
    0.1 ≤ θ ≤ π/2 - 0.1
    0.1 ≤ T ≤ 10000
    x_simulator(v, θ, T) = 5.0
    z_simulator(v, θ, T) = 2.1

──────── 仿真器（约束求值）───────
simulator(v, θ, T):
    qvel[0] = v·cos(θ)
    qvel[2] = v·sin(θ)
    while time < T: mj_step
    return (qpos[0], qpos[2])

──────── 优化器（COBYLA）───────
sol = nlopt.LN_COBYLA.optimize([v, θ, T])
~ 50-200 次 simulator 调用

──────── 开环播放（主循环）───────
mj_step(model, data)  # 无控制输入
球从初速度自然飞到目标

十三、一句话总结

No.11 = 「离线轨迹优化 + 开环播放」。把仿真器当作约束求值器扔给 NLopt，让 COBYLA 算法自动找最优 (v, θ, T) 让球命中目标。没有控制律、没有反馈、controller 是空函数 —— 这是「模型预测 + 开环执行」的最简形式，也是 No.4-9 在线控制范式的对照面。