MIT Drake 真实动力学模拟 Faze4 六轴机械臂（详细分册）

📌 本分册目标

站内的 Faze4 真实 3D 轨迹实验室 是运动学演示：它在浏览器里做 6 轴 IK 解算，算出「关节角要转到多少」，然后用 Three.js 把机械臂画出来。它会告诉你几何上到得了，但不会告诉你真机转不转得动 —— 因为那里没有重力、没有惯性、没有电机扭矩上限。

本分册补上这一层：用 MIT Drake 把同一台 Faze4 放进真实动力学里跑。URDF 的关节原点与官方 faze4_description 逐字段对齐，Drake 会真实计算重力矩、惯性耦合、科氏力与各轴扭矩，于是你能亲眼看到：同样一条轨迹，真机需要付出的跟踪滞后与扭矩饱和到底有多少。

Faze4 是全 3D 打印的六轴臂，每个关节里塞了一套摆线针轮减速箱 —— 这让它的「惯量分布」和 AR4 那种铝件臂完全不同，也带来了全篇最关键的一条结论：它的控制周期必须压到 0.2 ms（5 kHz），否则腕部会直接数值发散。这正是本分册要讲透的东西。

🧰 环境准备

• Python 3.9 ~ 3.11（Drake 的 wheel 只支持特定 Python 版本，越新的 Drake 要求越新的 Python）
• pydrake 与 numpy：python3 -m venv ~/drake-venv && source ~/drake-venv/bin/activate && pip install drake numpy
• 系统：Ubuntu 20.04 / 22.04 与 macOS 支持最好；Windows 请用 WSL2（Drake 无官方 Windows pip 包）
• Intel 版 macOS：官方较高版本已不再提供 x86_64 wheel，需装最后支持的版本 pip install drake==1.34.0
• 可选：浏览器（用于 Meshcat 的 3D 视图）

📦 第 1 步：四个文件，一个目录

• ⬇ faze4_arm.urdf 示意连杆模型：连杆外形用长方体近似，无需任何外部网格文件，Drake 可直接加载。关节原点 / 轴向 / 限位逐字段对应官方描述。先跑通这一个。
• ⬇ faze4_real.urdf 真实零件模型：7 个官方零件网格转出的 OBJ，真机外观（打印件 + 电机 + 减速箱）。用于第 5 步出 3D 动画。
• ⬇ arm_wave_drake_faze4.py 仿真主程序：建世界 + 5 kHz 数字控制器 + 增益自动整定 + 数据记录，支持回放网页导出的关节轨迹。
• ⬇ arm_meshcat_faze4.py 真实外观 3D：加载真实零件版 URDF，可在浏览器里看姿态，也可导出单文件动画 HTML；支持 --shape sine|circle|tri 在画板上画正弦曲线 / 圆形 / 三角形，并实时画出末端运动轨迹（见第 6 步）。

四个文件放在同一个目录下（脚本按自身位置找 URDF，不需要改路径）。若只想跑数值，前两个文件的下一个就够了。

🔧 第 2 步：读懂 URDF —— Faze4 的「身份证」

URDF 用一棵树描述机器人：节点是 link（连杆），边是 joint（关节）。下表就是 faze4_arm.urdf 的全部关节参数，逐字段取自官方 faze4_description —— Drake 就是靠这些数算真实的动力学。

全臂自重合计约 9.11 kg。官方给出的连杆长度是 L1 = 0.23682 m、L2 = 0.32 m、L3 = 0.0735 m、L4 = 0.2507 m、L5 = 0.057 m（L5 是末端法兰到 tool 参考点）—— 验算一下：sqrt(0.037114² + 0.22683² + 0.10011²) = 0.25070，正好等于 L4，说明表中的相对位置与官方口径完全自洽。

<!-- ① 必须有一个固定关节把底座焊在世界原点，否则整条臂会自由落体 -->
<link name="world"/>
<joint name="world_joint" type="fixed">
  <parent link="world"/><child link="base_link"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!-- ② 关节 = 父子关系 + 相对位置 + 轴向 + 限位 + 扭矩上限 + 阻尼 -->
<!--    注意 Faze4 的关节轴并不都是「竖直」的：J4 的轴是 (0.148, 0.905, 0.399) -->
<joint name="podlaktica" type="revolute">
  <parent link="lakat"/><child link="podlaktica"/>
  <origin xyz="0 -0.0735 0" rpy="0 0 0"/>
  <axis xyz="0.14804 0.90477 0.39934"/>
  <limit lower="-3.14159265" upper="3.14159265" effort="10" velocity="3.0"/>
  <dynamics damping="0.10"/>
</joint>

<!-- ③ 连杆 = 外观 + 碰撞 + 惯性（Drake 靠 mass 与 inertia 算真实受力） -->
<link name="nadlaktica">
  <visual><origin xyz="0 -0.18 0"/><geometry><box size="0.10 0.36 0.10"/></geometry></visual>
  <inertial>
    <origin xyz="0 -0.18 0"/>
    <mass value="3.097"/>
    <inertia ixx="0.033450" ixy="0" ixz="0" iyy="0.005300" iyz="0" izz="0.033450"/>
  </inertial>
</link>

▶️ 第 3 步：跑起来

[faze4] plant=6 dof · gravity=-9.81 m/s²
[faze4] 扭矩上限(N·m) = [7.9 8.5 4.7 3.7 3.5 6. ]
[faze4] 有效惯量(kg·m²) = [5.78e-02 3.99e-01 4.36e-02 1.37e-03 2.60e-04 6.36e-06]
[faze4] 整定 kp = [158.  170.   94.   74.   70.   63.6]
[faze4] 整定 kd = [ 4.84 13.18  3.24  0.51  0.22  0.02]
[faze4] 初始位姿(度) = [ 0.0e+00  2.2e+01  2.2e+01  3.1e-15 -2.2e+01 -2.2e+01]
t=0.0s 实测(rad):    0.000    0.378    0.378    0.000   -0.378   -0.378  末端XYZ(m): 0.079 -0.521 0.395
t=3.0s 实测(rad):    0.435    0.244   -0.191   -0.435   -0.244    0.191  末端XYZ(m): 0.220 -0.495 0.600
t=5.0s 实测(rad):    0.261   -0.173   -0.433   -0.261    0.172    0.433  末端XYZ(m): 0.105 -0.326 0.745
t=7.0s 实测(rad):   -0.153   -0.431   -0.277    0.153    0.430    0.277  末端XYZ(m): 0.072 -0.243 0.713
[summary] 最大跟踪误差(rad): 0.000 0.000 0.000 0.000 0.000 0.000
[summary] 扭矩饱和占比(%):  0 0 0 0 0 0
[summary] 末端轨迹范围(m): x[0.066, 0.226] y[-0.545, -0.241] z[0.395, 0.749]
[done] 轨迹已保存 -> faze4_trail.csv

PD 增益是怎么来的

不需要手调增益 —— 程序会先用动能法测出每个关节的有效转动惯量（令该关节速度为 1，则动能 T = ½I），再按下面的判据整定：

kp = min( effort / 0.05 ,  0.4 · I / Δt² )
     #  前项：0.05 rad 误差内电机不进入饱和（线性工作区）
     #  后项：离散 PD 的稳定刚度上限（无量纲刚度 b = Δt²·kp/I 必须 < 0.5）
kd = min( 1.6 · sqrt( kp · I ) ,  I / (2Δt) )
     #  前项：略过阻尼，抑制摆动超调
     #  后项：微分时间常数 τ = I/kd 必须 ≥ 2Δt（腕部抖动的解药）

注意后项不是 AR4 那册里的带宽约束 0.5·(1.5/Δt)²·I，而是按 Faze4 实测重新标定的 0.4·I/Δt²。数值差在哪、为什么必须改，就是下一节的内容 —— 这是本分册最值钱的一段。想手动覆盖用 --kp / --kd；想看每次的力矩细节，加 --debug 会额外打印命令、误差、实际角速度与重力矩。

怎么读 summary

• 最大跟踪误差：真实动力学必然有滞后，关键看量级是否可接受（本模型实测：默认参数下收敛到 0，关掉重力前馈后 J2 约 0.041 rad ≈ 2.3°）。若是几十度，多半是增益或惯量设置有误。
• 扭矩饱和占比：某个关节饱和占比高，就说明这个动作在真机上最容易堵转丢步 —— 要么加大减速比，要么降低速度 / 幅值。
• 末端轨迹范围：与站内 3D 实验室画出的轨迹做对比，验证「解出来的角度」与「真跑出来的路径」差多少。
• ⚠️ 时间步长：Faze4 上它不是一个可选参数，而是能不能跑的前提，见下方专节。

⚠️ 惯量鸿沟：为什么 Faze4 必须跑 5 kHz

这是本分册的核心。Faze4 是全 3D 打印件，腕部 J6 只带动一个约 20 g 的夹爪，关节侧有效惯量只有 6.36e-06 kg·m²；而 J2 肩上扛着整条大臂，是 3.99e-01 kg·m² —— 两者相差 6.3 万倍（AR4 那册只有三个量级）。

离散 PD 控制的稳定性有个无量纲判据：b = Δt² · kp / I，b 越过约 0.5 就会出现步频极限环（关节相邻两个控制周期正负来回，力矩长期顶在上限）。同一套整定式，把 Δt 从 0.2 ms 放大到 1 ms，只有惯量最小的 J6 会瞬间撞上这个天花板：

规律很清楚：J1~J5 的 kp 由电机扭矩决定，J6 的 kp 由数值稳定性决定。在 5 kHz 下 J6 的稳定上限是 63.6，刚好落在合理区间；而在 1 kHz 下它被压到 2.5 —— 位置环软到几乎没劲。实测结果：

三组对照实验（实测数据）

「关重力场」一行看起来什么都没变，是因为 5 kHz 下这套增益的力矩裕度很足 —— 想知道重力到底占多少，要靠 --debug 打印的重力矩。实测在 t = 1.0 s 时 tau_g = [0.000, 3.524, 1.769, -0.010, 0.017, 0.000]：J2 需要 3.52 N·m、J3 需要 1.77 N·m 来对抗自重，而这两轴的扭矩上限是 8.5 / 4.7 N·m —— 也就是说单是「把手臂端住」就吃掉了 J2 四成、J3 近四成的力矩额度。这就是为什么加了前馈之后 J2 的跟踪误差从 2.3° 直接归零。

🔁 第 4 步：回放网页导出的轨迹（本分册的重头戏）

这是把「网页运动学」和「真实动力学」接起来的关键一步。

🎬 第 5 步：真实外观 3D 与离线动画

• 看实时 3D：python3 arm_meshcat_faze4.py --hold --html none —— 浏览器打开终端打印的 http://localhost:7000，由 7 个官方零件组成的 Faze4 姿态被真实动力学驱动。一定要加 --hold：否则脚本跑完就退出，meshcat 服务随之关闭，画面会一闪而过。
• 导出可离线播放的动画：python3 arm_meshcat_faze4.py --html arm_real_3d_faze4.html —— 生成的单个 HTML 文件自带全部几何与控制，双击即可播放，方便分享或存档（代价是要把 7 个网格内嵌成 base64，比较慢，所以加了 --html none 可跳过）。
• 只出数值、不开 3D：python3 arm_meshcat_faze4.py --no-meshcat —— 适合服务器或无图形界面环境。
• 回放网页轨迹：python3 arm_meshcat_faze4.py --q-traj q_traj.npy —— 用真实零件看那条轨迹在动力学下的表现。
• 在画板上画正弦 / 圆 / 三角形：python3 arm_meshcat_faze4.py --shape circle --hold --html none —— 场景里会实时画出末端运动轨迹，详见第 6 步。

⚠️ 两个必踩的坑（脚本已内建处理，这里说明原因）

坑一：碰撞体会把前臂关节「锁死」。Faze4 的 collision 是给碰撞检测用的粗略长方体，相邻连杆的盒子在关节处本来就会互相咬合。Drake 的接触求解器于是施加巨大的穿透修正力，把末端整个「按住」：实测跑圆形轨迹时 J4 饱和占比 99%、J3 88%，J4 跟踪误差 1.931 rad（约 111°），末端直接偏离参考路径最多 147 mm、y 坐标从板面 −0.480 m 漂到 −0.470 m 附近来回撞。把自碰撞整体排除后，立刻恢复到 0% / 0.001 rad、y 恒为 −0.480 m。脚本默认排除（要保留用 --self-collisions）。

坑二：起步姿态不能随便给。Faze4 腕部惯量极小（J6 仅 6.36e-06 kg·m²），如果让机械臂从全零姿态起步，而命令轨迹的起点并不是全零（摆动模式起点是 [0°, 22°, 22°, 0°, −22°, −22°]），第一个控制周期就会吃到满幅误差 —— 腕部瞬间甩出去，J2 的力矩也会顶到上限。实测：起步姿态取全零时 J2 启动瞬态饱和 4%；改成从命令轨迹的起点起步后降为 0%。脚本两种模式都自动从命令起点起步，这也是为什么你在第 3 步会看到 初始位姿(度) 那一行不是全零。

🎯 第 6 步：在画板上画 正弦 / 圆形 / 三角形（运动轨迹可视化）

站内 3D 轨迹实验室 里的三种末端轨迹，现在可以直接在 Drake 里跑 —— 用的是同一块画板、同一组目标点，差别只在于：网页是瞬时的运动学解，这里是 5 kHz 扭矩控制下的真实动力学响应。

# 三种形状同时看：各起一个进程、端口分开，浏览器分别打开对照。
# 场景里会自动画出：蓝线 = 参考轨迹   黄线 = 末端实际轨迹   红球 = 当前目标点
python3 arm_meshcat_faze4.py --shape circle --port 7000 --hold --html none   # → localhost:7000
python3 arm_meshcat_faze4.py --shape sine   --port 7001 --hold --html none   # → localhost:7001
python3 arm_meshcat_faze4.py --shape tri    --port 7002 --hold --html none   # → localhost:7002

# 只跑一种：端口默认就是 7000，不用写 --port
python3 arm_meshcat_faze4.py --shape sine --hold --html none

# 导出可离线播放 / 转发分享的单文件动画
python3 arm_meshcat_faze4.py --shape sine --html arm_sine_3d.html

# 只算数值，并把解出的关节轨迹存下来，喂给数值脚本做定量分析
python3 arm_meshcat_faze4.py --shape tri --no-meshcat --save-q q_tri.npy
python3 arm_wave_drake_faze4.py --q-traj q_tri.npy

画板在 Drake 世界系里的位置

网页用 Y-up 显示，Drake 的世界系是 Z-up，中间差一次 Rx(-90°)。换算后网页坐标 (x, YC, ZP) 对应 Drake 的 (x, −ZP, YC)：画板竖直立在正前方 y = −0.48 m、板中心横向 x = 0.08 m、中心高 z = 0.55 m，板面 0.50 × 0.44 m。三种形状与网页的 targetAt() 逐字段一致，都是 5 s 一个周期的闭合路径，可以一直循环。

末端轨迹怎么变成关节角：用 Drake 的逆运动学

和网页一样，只解 J1 / J2 / J3 / J5，J4 与 J6 全程锁 0（末端姿态不变，笔尖始终垂直板面）。区别是这里用 Drake 自带的 InverseKinematics，约束直接写在 URDF 的真实运动学上。采样点数用 --shape-points 调（默认 240 点/圈）。

BOARD_ZP, BOARD_XC, BOARD_YC, BOARD_R = 0.48, 0.08, 0.55, 0.16  # 前伸/横向中心/高度/圆半径
SINE_L, SINE_A = 0.19, 0.12                                     # 正弦：半长 / 幅值
SHAPE_PERIOD = 5.0                                              # 每圈（每程）5 s，与网页一致
Q_SEED = np.array([0.0104, 0.1795, -0.5049, 0.0, -0.0938, 0.0])  # 与网页同一个初值


def shape_points(shape, n=240):
    """画板上的末端目标点，返回 n×3（Drake 世界系 Z-up）。
    三种形状与网页 arm_traj_3d.html 的 targetAt() 完全一致。"""
    ts = np.linspace(0.0, 1.0, n, endpoint=False)
    if shape == "circle":
        a = 2.0 * np.pi * ts
        pts[:, 0] = BOARD_XC + BOARD_R * np.cos(a)
        pts[:, 2] = BOARD_YC + BOARD_R * np.sin(a)
    elif shape == "tri":
        ...                                    # 三条边等分一周，起点在正上方顶点
    else:                                      # sine
        u = np.where(ts < 0.5, -SINE_L + 4.0 * SINE_L * ts,
                     SINE_L - 4.0 * SINE_L * (ts - 0.5))
        pts[:, 0] = BOARD_XC + u
        pts[:, 2] = BOARD_YC + SINE_A * np.sin(SINE_W * u)
    pts[:, 1] = -BOARD_ZP                      # 全部落在画板平面内
    return pts


def solve_ik_path(plant, frame, pts, seed=None):
    """沿画板路径逐点解 IK，返回 N×6 关节角。J4、J6 锁 0。"""
    q = (Q_SEED if seed is None else np.asarray(seed, dtype=float)).copy()
    solve_idx = [0, 1, 2, 4]                   # 只让 J1/J2/J3/J5 去解位置
    for k, p in enumerate(pts):
        ik = InverseKinematics(plant)
        qv, prog = ik.q(), ik.prog()
        ik.AddPositionConstraint(frame, [0, 0, 0], plant.world_frame(),
                                 p - 2e-4, p + 2e-4)
        prog.AddQuadraticErrorCost(1e-3 * (W.T @ W), q, qv)   # 贴着上一个解
        prog.AddBoundingBoxConstraint(q[3], q[3], qv[3:4])    # J4 锁 0
        prog.AddBoundingBoxConstraint(q[5], q[5], qv[5:6])    # J6 锁 0
        prog.SetInitialGuess(qv, q)
        res = Solve(prog)
        if res.is_success():
            q = np.array(res.GetSolution())
        Q[k] = q
    return Q

把运动轨迹画进 3D 场景

# 开跑前：画板里放上参考轨迹（蓝线）与当前目标点（红球）
meshcat.SetLine("/board/ref", np.asfortranarray(ref_pts.T), line_width=3.0,
                rgba=Rgba(0.42, 0.55, 1.0, 0.85))
meshcat.SetObject("/board/target", Sphere(0.005), rgba=Rgba(1.0, 0.36, 0.45, 1.0))

# 主循环里：每 0.02 s 采一次 tool_link 的世界坐标，连成黄线
if t >= next_trail:
    next_trail += args.publish_period
    trail.append(plant.EvalBodyPoseInWorld(ctx, tool).translation())
    meshcat.SetLine("/trail", np.asfortranarray(np.array(trail).T),
                    line_width=3.0, rgba=Rgba(1.0, 0.84, 0.3, 0.95))
    meshcat.SetTransform("/board/target", RigidTransform(ref_now))

实测结果（macOS · Python 3.12 · Drake 1.34 · 12 s · 240 点/圈）

末端范围与画板设计逐位吻合（y 恒为 −0.480 m，说明末端始终贴着板面，实测范围略小于设计包络是因为离散采样点没取到顶点），IK 几何误差 ≤ 0.35 mm。三种轨迹的扭矩饱和都只有 0~1%，这与第 3 步里关节空间摆动时的表现相比还要更宽松 —— 原因很直接：沿画板走时末端速度只有约 0.07 m/s，比关节空间摆动温和得多。换句话说，同样一台臂，「画图」这种任务对电机是最友好的。

正弦那一路的偏差（5.4 mm）比圆形和三角形大，也正是因为它末端速度最快、往返换向时加速度最大 —— 这恰好是「跟踪滞后」最直观的体现：同样放松的增益下，轨迹越急，末端越跟不上。

❓ 常见问题

🧭 与站内其他内容的关系

• Faze4 真实 3D 轨迹实验室（运动学）：浏览器里看结构、玩 IK、导出 q_traj.npy —— 本分册的上游。
• MIT Drake 真实模拟 SmallRobotArm（项目 02 分册）：同样的方法论也能用在这台臂上，可对照着读。
• MIT Drake 真实模拟 Annin AR4（姊妹分册）：铝件臂 + 1 kHz 的版本，读完那册再读本册的「惯量鸿沟」一节，对比会非常明显。
• 做自己的机器人 · 项目库：Faze4 的官方资料、URDF 描述文件与打印件清单入口都在这里。

💡 提示与避坑

• URDF 里的关节轴方向与符号请照抄官方，别自己「纠正」—— 改了会出现仿真能动、真机反向。Faze4 的 J4 轴是斜的（0.148, 0.905, 0.399），看起来「不正」但它就是对的。
• 缺少 world 固定关节会让机械臂整体自由落体；惯量张量不满足三角不等式（ixx + iyy ≥ izz）Drake 会直接拒绝加载 —— 别靠折算转子惯量去「补」腕部惯量。
• 先跑示意模型 faze4_arm.urdf 把流程走通，再换 faze4_real.urdf 看外观 —— 网格只影响视觉，不影响控制结论。
• 关节阻尼（dynamics damping）别设太大，否则会掩盖控制器的真实表现。
• Faze4 的碰撞体只适合做碰撞检测，别拿来当动力学里的接触模型：相邻连杆的盒子本来就互相咬合，留着会让前臂被「按住」（实测 J4 饱和 99%）。
• 本页模型为站内原创教学模型：关节原点 / 轴向 / 限位 / 连杆长度取自官方公开描述文件，质量与惯量为同量级估算，扭矩上限为按 NEMA17 + 摆线针轮减速箱的合理估算；用于演示动力学与验证控制逻辑，真机标定请按实物替换参数。