前言

• 最近 VLA（Vision-Language-Action，视觉-语言-动作）具身智能发展迅速，从谷歌的 RT-2、OpenVLA 到 π0，大模型开始直接输出机器人动作，而这一切的物理载体正是机械臂
• 回顾往期内容，我们已经从传统机械臂完成了基础入门，学习了 MoveIt2 作为底层运动框架，结合 Gazebo 仿真实现了传统点云 PCA 加 OBB 的夹取方法，同时也部署了基于深度学习的 GraspNet，至此我们已经具备了相对基础的机械臂学习经验
  • 第一期：
    • 【玩转VLA具身智能机械臂】（一）：机械臂基础（上篇）——建模：结构、运动学与动力学
    • 【玩转VLA具身智能机械臂】（一）：机械臂基础（下篇）——规划、控制与全链路
  • 第二期： 【玩转VLA具身智能机械臂】（二）：MoveIt2 运动规划——架构、配置与 C++ 上手
  • 第三期：【玩转VLA具身智能机械臂】（三）：视觉避障——从 RGB-D 点云到 MoveIt octomap 的完整落地
  • 第四期：【玩转VLA具身智能机械臂】（四）：点云分割与 OBB 夹取——从 PCA 失效到主动扫描
  • 第五期：【玩转VLA具身智能机械臂】（五）：GraspNet 6-DoF 抓取——从网络提议到筛选执行
• 作为正式开始 VLA 学习的前置，今天这一期我们来讲讲传统机械臂的收尾：底层运控控制库 Pinocchio 与全新的仿真器 MuJoCo，最终实现使用 Pinocchio 在 MuJoCo 里对 Panda Franka 机械臂做底层操控画圆

观前提示：本文调用的 Pinocchio 库，核心是机械臂底层的数学原理与驱动，大部分概念都是应用数学和正逆运动学求解，公式难免枯燥难懂（特别是第三章）。对于 VLA，我们并不会涉及这么底层的数学计算与理解，所以如果看起来不舒服的朋友们，可以选择性观看第三章

---

---

1 Mujoco

1-1 介绍

• MuJoCo 的全称是 Multi-Joint dynamics with Contact，含义是"带接触的多关节动力学"

• 它最初由 Emo Todorov 等人开发，2021 年随 DeepMind 开源，目前由 Google DeepMind 持续维护，仓库地址是 https://github.com/google-deepmind/mujoco

• 它要解决的核心问题只有一个：把机器人与环境之间的接触、摩擦、约束，统一成一套能快速求解的数学形式

  • 传统物理引擎（ODE、Bullet 这一类）在接触上采用启发式冲量求解，参数不当会引起抖动，接触点增多时求解结果还会互相冲突
  • MuJoCo 把接触力写成带约束的凸优化问题，每步求解一次，所得接触力天然满足互补条件，稳定性明显优于冲量法
  • 另一项优势是动力学精度：它内部用广义坐标（关节角）描述系统，而非"笛卡尔坐标加约束"。因此同一个模型在 MuJoCo 中算出的动力学，与本期使用的 Pinocchio 以及刚体动力学教科书的公式一致

• 本系列使用的版本是 3.13.0，Python 与 C++ 共用同一个库

• 需要说明的是，MuJoCo 引擎用 C 编写，对外同时提供 C 接口与 Python 接口。以往使用 MuJoCo 时调 Python 接口即可；本文需要与 C++ 的 Pinocchio 对接，因此走 C 接口

  • 这两套接口指向同一个 libmujoco.so，Python 绑定只是其上的一层封装。这一点可以直接验证：该 .so 导出约 700 个 mj* 形式的 C 符号，C++ 修饰符号仅十余个；头文件也全部是 C 源码，靠 extern "C" 供 C++ 直接 include

说人话：MuJoCo 是一个专门为机器人做的物理引擎。它的接触算得又稳又快，而且动力学公式跟正经的刚体动力学教科书是一套，所以在仿真里调好的控制器，搬到真机上不会完全失效。

1-2 Mujoco 的应用价值与优势

• 它的能力范围见下表：

能力	说明
刚体动力学仿真	关节、连杆、闭环、软约束，一份 MJCF 描述
接触与摩擦	凸优化求解，用 solref / solimp 调软硬
传感器仿真	关节、力/力矩、IMU、触摸，以及相机（自带渲染器）
执行器建模	位置、速度、力矩、肌肉、通用，五类都支持
可视化与离屏渲染	自带 OpenGL 渲染器，可以开窗，也可以离屏出图

• 选择它的理由，在于与 Gazebo、PyBullet 的对比：

维度	MuJoCo	Gazebo	PyBullet
接触求解	凸优化，稳定	冲量法，参数敏感	冲量法
单步开销	小	大，要起一整套 ROS 节点	中等
模型描述	MJCF	SDF / URDF	URDF / MJCF
学习类生态	事实标准	弱	中等
典型用途	强化学习、模仿学习、控制研究	整机联调、系统集成	快速原型

• 对本系列而言，选择它有两项具体理由：

  • 第一，前五期一直在 Gazebo 中做 MoveIt2。Gazebo 强在整机联调，但每次启动需要拉起十几个 ROS 节点，采集一条演示轨迹的开销过大；而 VLA 训练需要几百上千条轨迹，仿真器必须能连续运行几万步
  • 第二，它能与 Pinocchio 读取同一份模型文件。这是本期方案成立的前提，3-2 节将展开

• 生态中还有两个容易混淆的项目：

  • MuJoCo MJX 与 MuJoCo Warp：两者是 MuJoCo 物理引擎的 GPU 重写版，分别基于 JAX 与 NVIDIA Warp；它们是独立项目，而非同一引擎的编译选项
  • MuJoCo Playground（https://github.com/google-deepmind/mujoco_playground）：建立在 MJX 与 Warp 之上的 GPU 加速环境集，任务覆盖经典控制、四足与双足运动，以及包括 PandaPickCube 在内的操作任务，并提供批量渲染器以支持视觉输入
  • 其定位是成千上万个环境并行运行强化学习，与本期要做的模仿学习路径不同。本期不使用，此处一并说明

• 后续 VLA 学习所用的 ACT、SmolVLA 这类模型，需要"相机图像加关节状态"配对的演示数据。

  • MuJoCo 两者都提供：状态是 mjData 中的数组，图像由其自带渲染器输出。这是本期先搭建环境的直接动机

1-3 安装

• MuJoCo 的安装过程简单

# 1. 先写一个约束文件，把 numpy 钉死在 1.26.4

printf 'numpy==1.26.4\n' > /tmp/mj_constraints.txt

# 2. 装 MuJoCo，用 -c 指定约束文件

pip3 install --user -c /tmp/mj_constraints.txt mujoco==3.13.0

# 3. 验证版本

python3 -c "import mujoco, numpy; print(mujoco.__version__, numpy.__version__)"
# 期望输出：3.13.0 1.26.4

1-4 Mujoco 初次运行

1-4-1 默认打开

• MuJoCo 的查看器不自带场景，默认打开的是空会话：没有模型、地面与光源，屏幕呈纯色，无法分辨任何内容

python3 -m mujoco.viewer

1-4-2 空白场景

• 这一点与 Gazebo 不同：Gazebo 启动即带一个 empty world，模型拖入即可使用；MuJoCo 的查看器只是一个查看模型的窗口，场景需要由我们提供
• 因此第一次运行时，我们先临时写一个最小场景 empty-scene.xml：一块带网格的地面、一盏平行光、一张天空盒，作用相当于 Gazebo 的 empty world

<mujoco model="empty-scene">
  <visual>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="120" elevation="-20"/>
  </visual>

  <asset>
    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0"
             width="512" height="3072"/>
    <texture type="2d" name="groundplane" builtin="checker" mark="edge"
             rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
             markrgb="0.8 0.8 0.8" width="300" height="300"/>
    <material name="groundplane" texture="groundplane" texuniform="true"
              texrepeat="5 5" reflectance="0.2"/>
  </asset>

  <worldbody>
    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
  </worldbody>
</mujoco>

• 文件很短，但后面反复用到的三块结构都已包含其中，1-5 节将逐个说明：
  • <visual> 控制渲染风格，<global> 中的 azimuth 与 elevation 决定查看器初始的观察角度
  • <asset> 管理贴图，地面上那层网格来自 builtin="checker" 的棋盘贴图，白色网格线由 markrgb 给出
  • <worldbody> 是世界的根节点，<light> 为光源，type="plane" 的那个 <geom> 即地面
• 随后将文件路径传给查看器：

python3 -m mujoco.viewer --mjcf="empty-scene.xml"

• 操作依旧是滚轮、鼠标左键和右键（这不废话吗?!!）

1-4-3 面板

• 查看器的界面分为三栏：
  • 左边是仿真控制，管理仿真的运行与显示
  • 中间是渲染视图，也就是 3D 画面本身
  • 右边是模型面板，按模型中实际存在的元素自动生成。空场景没有关节、执行器与约束，因此该栏为空
• 左边这一栏从上到下依次是：
  • File：存取模型与状态、打印模型和数据、截图、退出
  • Option：查看器自身的设置，辅助窗口开关、暂停刷新、全屏、垂直同步，以及界面间距与字体
  • Simulation：仿真的启停与状态管理，Pause/Run、Reset、Reload、关键帧存取、噪声、历史记录
  • Watch：监视 mjData 中某个数组的指定下标，并实时显示其值
  • Physics：物理引擎参数，包括求解器选择、Timestep、Iterations 等算法参数，以及按项禁用的 Disable Flags
  • Rendering：渲染选项，选相机、标签与坐标架，以及各类模型元素的可视化开关
  • Visualization：画面本身的参数，头灯、自由相机的视场角与初始观察角度、阴影强度
  • Group enable：按 group 批量控制显隐，Geom、Site、Joint 各六组
  • Logging：日志输出到 Console 还是 File，以及要记录哪些计时信息
• 右侧一栏从上到下依次是：
  • Joint：模型里每个关节的当前值
  • Control：每个执行器的 ctrl 输入
  • Equality：模型里定义的等式约束

1-5 MuJoCo 的核心概念

• 前两节完成了安装与初次运行，这一节说明 MuJoCo 的数据模型。第三章的 C++ 代码有相当一部分在读写下面这些结构体与数组，概念不清楚会直接影响下标映射的编写

1-5-1 两个核心结构体

• MuJoCo 的C-API 围绕两个结构体组织：mjModel 与 mjData

	mjModel	mjData
内容	机器人结构、惯量、执行器、传感器等静态信息	当前仿真状态
何时确定	mj_loadXML / from_xml_string 编译时定死	由 mj_step 每步更新
能否修改	建好之后原则上不再改（改了要 mj_forward 重建派生量）	每步都在变
生命周期	一份，可被多个 mjData 共享	每个线程、每个并行环境一份

说人话：mjModel 描述机器人本身，mjData 记录它当前的状态。因此并行运行 1000 个环境时，模型只需构建一次，数据需要 1000 份。

1-5-2 常用数组与 nq / nv / nu

• mjData 中最常用的几个数组：

字段	长度	含义
qpos	nq（广义坐标个数）	广义坐标（位置）
qvel	nv（自由度个数）	广义速度（自由度速度）
qacc	nv	广义加速度
ctrl	nu（执行器个数）	执行器控制输入
qfrc_bias	nv	偏置力，等于 $C(q,\dot{q})\dot{q}+g(q)$
xpos / xmat	3*nbody / 9*nbody（nbody 是 body 个数）	各 body 在世界系下的位置与旋转矩阵
sensordata	nsensordata（传感器数据个数）	传感器读数

• 这几个长度都是 mjModel 的字段，命名规则全库统一：n 加上数组名的首字母，nbody、ngeom、njnt 同理

• 数组长度直接读 mj_model->nq 即可，不要写死——长度随模型变化，硬编码会出错

• 需要特别说明的是，qpos 的长度是 nq，而 qvel 的长度是 nv，两者并不总是相等

  • 原因是广义坐标数与自由度数不是同一个概念
  • 最典型的是自由关节（<freejoint/>）：一个可在空间中自由运动的物体，nq = 7 而 nv = 6。它的位置用位置加四元数表示，3 个平移加 4 个四元数分量共 7 个；而自由度只有 3 个平移与 3 个转动，共 6 个
    • 球关节同理，qpos 占 4 个（四元数），qvel 只占 3 个（角速度）
    • 铰链关节不存在这一问题：它的 qpos 与 qvel 都只占一个数。本期使用的 Panda 七个臂关节全部是铰链，因此 nq = nv = 9
  • 代码中不能假设 nq == nv。这是后文建立映射表时分别维护 qidx 与 vidx 两套下标的原因

1-5-3 其余核心概念与 MJCF

• 除这两个结构体外，还需了解以下概念：

概念	是什么	在 MJCF 里的写法
body	连杆，有质量和惯量，构成一棵树	<body>
joint	关节，定义父子 body 之间怎么相对运动	<joint>
geom	几何体，负责碰撞和显示	<geom>
site	标记点，只用于定位，不参与碰撞	<site>
actuator	执行器，把 ctrl 变成力或力矩	<actuator> 下的子标签
sensor	传感器，把状态变成 sensordata	<sensor>
tendon	腱，把多个关节耦合起来（夹爪常用）	<tendon>

• MJCF 是描述上述内容的 XML 格式，表达能力强于 URDF：支持 <default> 类继承与 <equality> 约束，也可直接在 XML 中定义关键帧与传感器
• 一个典型的 MJCF 结构如下，这也是后文使用的 Panda 模型骨架：

<mujoco model="panda">
  <compiler angle="radian" meshdir="assets"/>

  <default>
    <default class="panda">
      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
      <general dyntype="none" biastype="affine"
               ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
    </default>
  </default>

  <worldbody>
    <body name="link0" childclass="panda">
      <body name="link1" pos="0 0 0.333">
        <joint name="joint1"/>
        <geom type="mesh" mesh="link1"/>
        <body name="link2" quat="1 -1 0 0">
          <joint name="joint2" range="-1.7628 1.7628"/>
          <!-- 中间省略 link3 到 link6 -->
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <general class="panda" name="actuator1" joint="joint1"
             gainprm="4500" biasprm="0 -4500 -450"/>
    <!-- actuator2 到 actuator7 同理 -->
    <general class="panda" name="actuator8" tendon="split"
             forcerange="-100 100" ctrlrange="0 255"/>
  </actuator>

  <keyframe>
    <key name="home" qpos="0 -0.569 0 -2.810 0 3.037 0 0.04 0.04"/>
  </keyframe>
</mujoco>

• 其中三处需要留意，它们在后文都会变成实际问题：
  • <default class="panda"> 中的 armature="0.1" damping="1" 是被动参数。它们不出现在刚体动力学方程 $M\ddot{q}+C\dot{q}+g=\tau$ 中，但在 MuJoCo 仿真中始终生效。Pinocchio 的 rnea() 不含这两项，3-8 节将专门处理
  • <keyframe name="home"> 提供一组预设位形。所有模式均以它为初始状态，以保证每次运行的结果可比
  • 执行器写的是 <general>，属于通用执行器，具体行为由 gainprm 与 biasprm 决定。Panda 在此配置出的是位置伺服而非力矩型，1-6 节将说明如何改为力矩型

1-5-4 模型从哪来：MuJoCo Menagerie

• 上面那份骨架并非手写，而是取自 MuJoCo Menagerie（https://github.com/google-deepmind/mujoco_menagerie）。这是 DeepMind 官方维护的模型集，收录约 60 个模型，覆盖人形、四足、双臂、机械臂、夹爪、无人机等类别
  • 每个模型一个目录，统一采用 MJCF 格式：<model>.xml 描述运动学树，scene.xml 再加入地面与光照，assets/ 存放 STL 或 OBJ 网格
• menagerie 全量约几 GB，而本期只需要 Panda 一个目录，因此采用稀疏克隆，只取 franka_emika_panda，体积仅几 MB。仓库本身保留，后续 VLA 换用其他机器人时执行一次 sparse-checkout add 即可

git clone --depth 1 --filter=blob:none --sparse \
    https://github.com/google-deepmind/mujoco_menagerie.git
cd mujoco_menagerie
git sparse-checkout set franka_emika_panda

# 这个目录后面要反复用，先存进变量

export PANDA_DIR="$PWD/franka_emika_panda"
ls "$PANDA_DIR"
# assets  panda.xml  scene.xml  panda.png  README.md

# 另有 hand.xml、panda_nohand.xml，以及一批 mjx_ 开头的文件（给 MJX 用的）

# 本期的主角只有两个：panda.xml 和 scene.xml

• 记下这个 PANDA_DIR，它是后面 C++ 侧的锚点：第三章中 MuJoCo 与 Pinocchio 都靠它定位模型文件
• 需要注意，它是 export 出的环境变量，只对当前终端会话有效。关闭终端、另开窗口或更换工作区后，它都可能失效；而查看器遇到无法打开的路径不会报错，只会退回空场景。该问题在下一节还会出现

1-5-5 两份 XML 的分工与加载验证

• menagerie 为每个模型提供两份 XML，这一分工在后文会反复出现：

  • panda.xml，即 1-5-3 节给出的那一份：只含机器人本体，包括连杆、关节、执行器与网格引用，不含地面、光照与天空盒
  • scene.xml：场景文件。其内容很少，首行即 <include file="panda.xml"/> 引入整个机器人，再补充 <visual> 的全局光照参数、一盏平行光，以及一块棋盘格纹理的地面

• scene.xml 篇幅很短，下面完整给出全文，不作省略。这 21 行即本项目实际使用的版本：

<mujoco model="panda scene">
  <include file="panda.xml"/>

  <visual>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="120" elevation="-20"/>
  </visual>

  <asset>
    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
      markrgb="0.8 0.8 0.8" width="300" height="300"/>
    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
  </asset>

  <worldbody>
    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
  </worldbody>
</mujoco>

• 对照 1-4 节的 empty-scene.xml 可以发现，两者的 <visual>、<asset>、<worldbody> 三块几乎完全一致，scene.xml 多出的只有首行 <include file="panda.xml"/>。即1-4 节的空场景就是官方场景去掉机械臂的结果

• 逐行来看：

  • <include file="panda.xml"/> 引入机器人。此处有一项硬约束：scene.xml 必须与 panda.xml 位于同一目录，否则 include 无法定位
  • <visual> 的三行控制渲染表现：头灯亮度、雾的颜色，以及默认相机的方位角与俯仰角
  • <asset> 定义两张纹理。skybox 是背景的渐变天空；groundplane 是画面中的网格地板，builtin="checker" 生成棋盘格，markrgb="0.8 0.8 0.8" 指定格线颜色，texrepeat="5 5" 使其平铺五遍
  • <worldbody> 中是一盏平行光，以及一块使用 groundplane 材质的 plane geom。地面、光照、天空全部集中在这几行，panda.xml 中一项都没有

• 这一分工是刻意的，两个引擎各取所需：

  • MuJoCo 使用 scene.xml。注意只加载 panda.xml 不会报错，模型也能正常显示，但画面中只有一台悬空的机械臂，难以判断它是否在运动。这类"能运行但结果不对"的问题，比直接报错更难排查
  • Pinocchio 使用 panda.xml。Pinocchio 只需要运动学树与惯量，地面与光照对它没有意义。3-2 节建立映射时，两个引擎正是按这一分工各读一份，这也是本期方案不会出现"两份描述对不齐"的根本原因

说人话：panda.xml 描述机器人本身，scene.xml 在此基础上加入地面与光照。仿真显示应加载后者。

• 下面把它加载进查看器验证：

python3 -m mujoco.viewer --mjcf="$PANDA_DIR/scene.xml"

• 加载后窗口标题变为 MuJoCo : panda scene（与 1-4 节一致，标题取自 xml 中的 model），Panda 立于网格地板之上
  • 此时右侧 Joint、Control 两个面板首次出现内容：9 个关节（joint1 到 joint7，加两个手指 finger_joint1、finger_joint2）、8 个执行器（actuator1 到 actuator8，前 7 个对应臂关节，第 8 个驱动夹爪）。对应上一节的记号即 nq = nv = 9，nu = 8。这两组数字在后文会反复出现

* 若画面仍是 1-4 节的空场景，问题通常不在模型，而是 `PANDA_DIR` 失效。可用一行命令确认：

ls "$PANDA_DIR/scene.xml"

• 若报 No such file or directory，说明变量有误，回到 1-5-4 节重新 export 后再试

1-6 MuJoCo 控制方式

• MuJoCo 的执行器有 motor、position、velocity、general 几类。前三类都是第四类的语法糖，最终均编译为 general 配合不同的 gainprm 与 biasprm

• 我们只关心最常见的三类，它们决定了对机械臂的控制层面：

控制方式	MJCF 写法	ctrl 的物理含义	特点
位置控制	<position kp="..."/>	目标关节角度	最稳，但只能管"走到哪"，管不了"用多大力"
速度控制	<velocity kv="..."/>	目标关节角速度	中间态，常用于移动底盘
力矩控制	<motor gear="1"/>	关节力矩	最底层，动力学补偿与阻抗控制只能在这一层做

• 这三者的差别可以用同一个公式表示。MuJoCo 的执行器标量力为：

$$f=\text{gainprm}[0]\cdot \text{ctrl}+\text{biasprm}[0]+\text{biasprm}[1]\cdot q+\text{biasprm}[2]\cdot \dot{q}$$

• 三类执行器只是这组参数的不同取值：

  • 位置伺服：gainprm[0] = kp，biasprm = [0, -kp, -kv]，展开为 $f=k_p(\text{ctrl}-q)-k_v\dot{q}$，即标准 PD 形式
  • 速度伺服：gainprm[0] = kv，biasprm = [0, 0, -kv]，得到 $f=k_v(\text{ctrl}-\dot{q})$
  • 力矩：gainprm[0] = 1，biasprm 全零，得到 $f=\text{ctrl}$

• 再乘上齿轮比 gear 换算到关节空间，即为真正作用在关节上的力矩：

$$\tau =\text{gear}\cdot f$$

• Panda 的 gear 是 1，所以 ctrl 就是关节力矩

• 问题在于：menagerie 中的 Panda 使用的是位置伺服

<general class="panda" name="actuator1" joint="joint1"
         gainprm="4500" biasprm="0 -4500 -450"/>

• 该行展开为 $f=4500(\text{ctrl}-q_1)-450{\dot{q}}_1$。若我们按力学公式算出力矩并直接写入 ctrl，MuJoCo 会将其当作目标角度

  • 后果是：算出的力矩一般在几牛米到几十牛米，被当作目标角度后，机械臂会以最大力冲向对应位形，表现为"控制器完全失灵"
  • 此外 ctrlrange 为 $\pm 2.8973$（弧度），力矩一旦超过该值就会被直接截断

• 因此必须在执行力矩控制之前把执行器改造成力矩型。改法有两种：

  • 改 XML：把 <general .../> 换成 <motor joint="joint1" gear="1" forcerange="-87 87"/>，重新加载
  • 运行时改 mjModel：模型加载之后、mj_step 之前，直接改写 gaintype / biastype / gainprm / biasprm

• 我们选择运行时改，这样做不需要维护第二份 XML，也就不会出现"改了 XML 而忘记同步"的问题：

// 把第 a 个执行器改造成纯力矩型：ctrl 直接就是力矩
mj_model->actuator_gaintype[a] = mjGAIN_FIXED;   // 增益固定为常数
mj_model->actuator_biastype[a] = mjBIAS_NONE;    // 去掉所有偏置项

// 注意 C API 里这两个数组是一维平数组，不是 Python 那样的 [a][k] 二维
for (int k = 0; k < mjNGAIN; ++k) mj_model->actuator_gainprm[a * mjNGAIN + k] = 0.0;
for (int k = 0; k < mjNBIAS; ++k) mj_model->actuator_biasprm[a * mjNBIAS + k] = 0.0;

mj_model->actuator_gainprm[a * mjNGAIN] = 1.0;   // 力矩 = 1.0 乘 ctrl
mj_model->actuator_ctrllimited[a] = 0;           // 解除 ctrl 限幅

• 这段代码中有两处易错点，均在实际调试中出现过：

  • 易错点一：C API 的数组是一维的。Python 绑定中 actuator_gainprm 是 [nu][mjNGAIN] 的二维数组，写作 gainprm[a][k]；C API 中它是 double* 一维数组，必须自行计算 a * mjNGAIN + k。
  • 易错点二：解除 ctrlrange 但保留 forcerange。位置模式下 ctrlrange 是弧度 $\pm 2.8973$，若保留它，算出的力矩会被限制在 $\pm 2.8973$ 牛米以内，大力矩会被全部削掉。而 forcerange（$\pm 87$ 与 $\pm 12$ 牛米）必须保留，它才是真实的力矩上限，也是保护仿真的那道闸

• 最后是夹爪：Panda 的第八个执行器驱动 split 腱，两个手指耦合在一起，属于指令型（ctrlrange 为 0 到 255，表示开口宽度），不参与我们的力矩控制

  • 因此改造循环中要按传输类型跳过它：只有 actuator_trntype[a] == mjTRN_JOINT 的执行器才改为力矩型
  • 它的力矩也不应出现在控制量中，3-2-2 节将说明如何将其从 tau 中清除

---

2 Pinocchio

2-1 介绍

• Pinocchio 是一个用 C++ 写的刚体动力学算法库，仓库地址是 https://github.com/stack-of-tasks/pinocchio
• 它由 Inria（法国国家信息与自动化研究所）主导开发，最早服务于人形机器人的行走控制，现在由 stack-of-tasks 组织维护
• 它的定位可以概括为：把刚体动力学中成熟的算法做成一个能在毫秒内跑完的库
  • 覆盖正运动学、雅可比、质量矩阵、逆动力学、正动力学与质心动力学
  • 这些算法均为解析递推而非数值近似，精度可达机器精度量级。3-2-3 节的交叉验证中，重力与科氏项能压到 $10^{-10}$ 以下即由此而来
• 它与 MoveIt2 不构成竞争关系，而是上下层关系：
  • MoveIt2 解决的是"从 A 点到 B 点如何绕开障碍物"，输出一条轨迹
  • Pinocchio 解决的是"给定一条轨迹，算出每个时刻应施加多大力"，输出力矩
  • 前五期完成了上面那层，本期补上下面这层

说人话：MoveIt2 负责"走哪条路"，Pinocchio 负责"每一步用多大力"。前者是规划，后者是控制。

• 本系列使用的版本为 4.0.0

2-2 应用场景

• Pinocchio 主要应用于以下几类工作：

场景	用到的能力	为什么选它
全身控制（WBC）	雅可比、动力学、质心动力学	要求单次求解在 1 ms 以内
模型预测控制（MPC）	动力学、自动微分	需要快速反复求导
逆运动学与逆动力学	FK、Jacobian、rnea	精度要求高
轨迹优化	动力学约束	需要解析梯度
强化学习	动力学随机化、状态估计	速度快，可嵌进 C++ 训练循环
力矩控制与阻抗控制	rnea 加雅可比	就是本期要做的事

• 与几个易混淆的库对比如下：

库	定位	与 Pinocchio 的差别
RBDL	刚体动力学	功能接近，但维护不活跃，生态弱
Drake	机器人工具箱	更全（含优化、仿真），也更重
KDL（MoveIt2 默认）	运动学	只有运动学，数值稳定性也不如 Pinocchio
MuJoCo	物理仿真器	它也能算动力学，但它是"仿真器"，Pinocchio 是"算法库"

• 最后一行直接关系到本期方案的合法性：即便 MuJoCo 本身可以计算动力学，仍需引入 Pinocchio。理由如下：
  • MuJoCo 的动力学为仿真服务，其输出的力需要喂给积分器，中间掺杂了约束求解、软接触与被动元件
  • Pinocchio 的动力学为控制服务，计算的是标准的 $M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau$，每一项都可以单独取出使用
  • 重力补偿需要 $g(q)$，阻抗控制需要雅可比 $J(q)$，计算力矩需要完整的 $M$、$C$ 与 $g$。MuJoCo 同样可以提供，但需要从 qfrc_bias、mj_fullM 这些仿真内部量中反推，并自行扣除被动元件
• 因此分工为：MuJoCo 充当"真机"，Pinocchio 充当"控制器"。这也是真机控制的标准做法：真机不会提供自身的动力学，必须由我们自行计算

2-3 安装

• 官方提供三条途径：

方式	命令	适用场景
ROS apt	sudo apt install ros-humble-pinocchio	已有 ROS 2，最省事
conda	conda install -c conda-forge pinocchio	用 conda 管环境
源码编译	cmake 加 make install，见官方 README	要定制，或要最新版

• 本机使用方法一，安装的是 4.0.0，位于 /opt/ros/humble 下。这里有一个必然遇到的问题：

# 必须先 source ROS 环境

source /opt/ros/humble/setup.bash
python3 -c "import pinocchio; print(pinocchio.__version__)"
# 4.0.0

• 因此我们将这一步骤收进 env.sh，每次打开终端先 source 它：

# env.sh 的关键两行

export MUJOCO_DIR="$(python3 -c 'import mujoco, os; print(os.path.dirname(mujoco.__file__))')"
source /opt/ros/humble/setup.bash

• 该 apt 构建在能力上存在若干边界，动手之前必须了解，否则会花费大量时间查找一个根本不存在的 API：

能力	本机构建	说明
MJCF 解析器	有	pinocchio/parsers/mjcf.hpp，本期方案的地基
URDF 解析器	有	pinocchio/parsers/urdf.hpp
hppfcl 碰撞检测	有	pinocchio::computeCollision 可用
InverseKinematics	没有	逆运动学得自己写，见 3-4 节
casadi 绑定	没有	import pinocchio.casadi 失败，不能做符号推导

• 尤其是没有 InverseKinematics 这一条，2-5-4 节的手写 DLS 即由此而来。实现约三十行，反而更容易看清迭代过程

2-4 核心数据结构、坐标系与 SE(3)

• 本节说明三件事：Pinocchio 用什么结构体承载模型、如何索引关节、以及如何表示位姿。这三项是读懂 2-5 节全部 API 的前提

2-4-1 Model 与 Data

• Pinocchio 的整个 API 同样只围绕两个结构体：Model 与 Data，思路与 MuJoCo 的 mjModel / mjData 完全一致

	pinocchio::Model	pinocchio::Data
内容	关节树、惯量、关节限位、摩擦、armature	算法中间结果与输出
何时确定	buildModel 时定死	每次调用算法时被更新
内存	一份	每个线程一份

• Pinocchio 的索引比 MuJoCo 多一层，这是初学阶段最容易混淆的地方：

索引	范围	含义
joint id	0 到 njoints-1	关节编号，0 是 universe（固定不动的世界）
idx_q	0 到 nq-1	该关节的位置在配置向量 $q$ 里的起始下标
idx_v	0 到 nv-1	该关节的速度在速度向量 $v$ 里的起始下标
frame id	0 到 nframes-1	坐标系编号

• 分开 idx_q 与 idx_v 的原因与 1-5 节相同：$nq$ 不一定等于 $nv$
  • 自由关节的位置需要 7 个数（3 个平移加 4 个四元数分量），速度只需要 6 个
  • Pinocchio 将这一区别显式表示为两个下标数组，取第 $i$ 个关节的位置即 q.segment(model.joints[i].idx_q, model.joints[i].nq)
  • 本期 Panda 全部为铰链关节，两者相等，但代码中不能作此假设

2-4-2 SE(3) 与 SO(3)

• Pinocchio 中的位姿均为 $SE(3)$ 的元素，即一个 $4\times 4$ 齐次变换矩阵：

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in SE(3),R\in SO(3),t\in {\mathbb{R}}^3$$

• $SO(3)$ 是三维旋转群，由所有行列式为 $+1$ 的正交矩阵构成：

S O ( 3 ) = {   R ∈ R 3 × 3    ∣    R T R = I ,    det ⁡ ( R ) = 1   } SO(3) = \{\, R \in \mathbb{R}^{3\times 3} \;\big|\; R^T R = I,\; \det(R) = 1 \,\} SO(3)={R∈R3×3 ​RTR=I,det(R)=1}

• 采用 $SO(3)$ 而非欧拉角的原因是欧拉角存在万向节死锁，且在奇异点附近，微小的姿态变化会引起角度剧烈跳变。旋转矩阵不存在该问题，代价是用 9 个数表示 3 个自由度，存在冗余

2-4-3 坐标系命名与世界系

• Pinocchio 中位姿的命名规则为 A_M_B，表示"B 在 A 坐标系下的位姿"。常见的有以下几个：

名字	含义
oMi	关节 $i$ 的坐标系在世界系（origin）下的位姿
oMf	frame $f$ 在世界系下的位姿
liMi	关节 $i$ 相对其父关节的位姿

• 这几个量都存放在 pinocchio::Data 中，oMi 与 oMf 均为 std::vector<SE3>。取末端位姿即 data.oMf[ee_frame]：

const pinocchio::SE3& oMf = pin_data.oMf[ee_frame];
Eigen::Vector3d p = oMf.translation();     // 位置 t
Eigen::Matrix3d R = oMf.rotation();        // 姿态 R

• 一个实操上的细节：世界系的选取会影响 $g(q)$ 的数值
  • Pinocchio 的重力存放在 model.gravity 中，默认为 (0, 0, -9.81)
  • MuJoCo 的重力存放在 mjModel.opt.gravity 中，MJCF 默认值同为 (0, 0, -9.81)
  • 两者一致，因此 $g(q)$ 可以直接比对。若更换场景（例如引入倾斜重力），必须先对齐该量，否则后续所有补偿量都会出错

---

2-5 核心内容

• 本节梳理 Pinocchio 的核心算法，每个算法对应一个最小的 C++ 片段。这些算法在第三章会逐个用到
• 先给出统一的开头，后续所有片段都在此基础上编写：

#include <pinocchio/parsers/urdf.hpp>

#include <pinocchio/algorithm/kinematics.hpp>

#include <pinocchio/algorithm/jacobian.hpp>

#include <pinocchio/algorithm/frames.hpp>

#include <pinocchio/algorithm/crba.hpp>

#include <pinocchio/algorithm/rnea.hpp>

#include <pinocchio/algorithm/aba.hpp>

// 从 URDF 建模型（本期实际用的是 MJCF，见 3-2 节）
pinocchio::Model model;
pinocchio::urdf::buildModel("panda.urdf", model);

// Data 必须由 Model 构造：内部缓冲区尺寸全按 model 定
pinocchio::Data data(model);

// 关节名到下标：用名字查，绝不写死数字
const pinocchio::JointIndex jid = model.getJointId("panda_joint1");
const pinocchio::FrameIndex fid = model.getFrameId("panda_hand");

// 配置向量。注意长度是 model.nq，不是 nv
Eigen::VectorXd q = pinocchio::neutral(model);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);

• 这里有两个必须记住的点，后续所有片段都依赖它们：
  • Data 必须由 Model 构造。若写成 pinocchio::Data data; 默认构造后再赋值，内部 Eigen 缓冲的尺寸全为 0，第一次调用算法就会越界
  • 所有下标都按名字查询：getJointId() 与 getFrameId()。写死下标在更换机器人、更换 Pinocchio 版本，甚至仅更换 URDF 导出方式时都会静默失效

2-5-1 正运动学 FK

• 正运动学解决的问题是：给定一组关节角，求末端的位置与姿态
• 其数学形式为从配置 $q$ 到末端位姿的映射：

$$T_{ee}(q)=T_{base}\prod _{i=1}^n{T}_{i-1,i}(q_i)$$

• 即沿运动学链从基座起，将各关节的变换依次乘到末端。每个 $T_{i-1,i}$ 只与一个关节变量有关，因此整条链的位姿是 $q$ 的解析函数
• Pinocchio 中需要分两步完成：
  • 第一步，对给定的 $q$，把模型 model 与当前所有关节的数据 data 一并传入，计算关节位姿
  • 第二步，单独计算 frame 的位姿。此处 frame 指末端法兰盘 hand

// 第一步：算所有关节的位姿，填 data.oMi
pinocchio::forwardKinematics(model, data, q);

// 第二步：把 frame 的位姿也算出来，填 data.oMf
// 关节位姿和 frame 位姿是两套东西，forwardKinematics 不会顺带算 frame
pinocchio::updateFramePlacements(model, data);

// 现在才能取末端位姿
const pinocchio::SE3& oMf = data.oMf[fid];
const Eigen::Vector3d p = oMf.translation();
const Eigen::Matrix3d R = oMf.rotation();

• 必须分两步的原因如下，这是高频出错点：
  • forwardKinematics 只填 data.oMi（关节位姿）
  • 末端执行器通常不是一个关节，而是一个 frame：它可能挂在某个关节上，相对该关节存在固定偏移（Panda 的 hand frame 位于 joint7 前方 0.107 m 处）
  • 若不调用 updateFramePlacements，读到的 data.oMf[fid] 是上一次调用留下的旧值（第一次调用时为单位阵）。程序不会报错，但结果是错的

---

2-5-2 雅可比矩阵 Jacobian

• 雅可比描述的是关节速度到末端速度的映射
• 对正运动学 $x=f(q)$ 求时间导数，由链式法则得到：

$$\dot{x}=J(q)\dot{q},J(q)=\frac{\partial f(q)}{\partial q}\in {\mathbb{R}}^{6\times n_v}$$

• $J$ 是 $6\times n_v$ 的：前 3 行是线速度，后 3 行是角速度：

$$J=\left[\begin{array}{c}{J}_p\\ J_{\omega }\end{array}\right],\left[\begin{array}{c}\dot{p}\\ \omega \end{array}\right]=\left[\begin{array}{c}{J}_p\\ J_{\omega }\end{array}\right]\dot{q}$$

• 它连接关节空间与笛卡尔空间。后续所有笛卡尔空间的控制律，都要通过 $J$ 与 $J^T$ 在这两个空间之间转换：
  • 关节空间：由 $q$ 张成的 $n_v$ 维空间，每一维对应一个关节变量。Panda 的 7 个臂关节为转动关节，单位是弧度；夹爪为移动关节，单位是米
  • 笛卡尔空间：末端位姿 $x$ 所在的空间，6 维，前 3 维为位置、后 3 维为姿态，单位统一为米与弧度。它与机械臂有几个关节无关，这也是同一个 $x$ 往往对应无数组 $q$ 的原因

方向	映射	用途
关节速度得到笛卡尔速度	$\dot{x}=J\dot{q}$	算当前末端的运动速度
笛卡尔力求得到关节力矩	$\tau =J^{T}F$	把笛卡尔空间算出的力搬回关节空间
笛卡尔位移得到关节位移	$\Delta q=J^{+}\Delta x$	逆运动学

• 中间一条来自虚功原理，是任务空间控制的核心：末端上的力 $F$ 对系统做的功必须等于关节力矩 $\tau$ 做的功：

$${\tau }^T\dot{q}=F^T\dot{x}=F^{T}J\dot{q}⟹\tau =J^{T}F$$

• 该式对任意 $\dot{q}$ 都成立，因此 $J^T$ 即为把笛卡尔空间的力映射回关节空间的矩阵。3-7 节与 3-9 节的任务空间控制律本质即此式

• 用法上 Pinocchio 提供两种写法，分别为先算后取与一步到位：

  • 这里的 nv 即 1-5-2 节的自由度个数（number of v，v 取自 qvel）。$J$ 的列数就取 nv，每列对应一个速度自由度，因此 Panda 的 $J$ 是 $6\times 9$
  • 9是因为9 = 7 个臂关节 + 2 个手指关节

// 写法一：先算所有关节的雅可比，再取 frame 的
pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);

Eigen::MatrixXd J(6, model.nv);
J.setZero();                        // 这一行不能省，原因见下
pinocchio::getFrameJacobian(model, data, fid,
                            pinocchio::LOCAL_WORLD_ALIGNED, J);

// 写法二：一步到位，内部会自己先算一遍
Eigen::MatrixXd J2(6, model.nv);
J2.setZero();
pinocchio::computeFrameJacobian(model, data, q, fid,
                                pinocchio::LOCAL_WORLD_ALIGNED, J2);

• 这里有两点必须说明：

• 第一，J.setZero() 不是可选项

  • computeFrameJacobian 只写运动链上各关节对应的列，其余列保持原值
  • 头文件中的注释写了：You must fill J with zero elements，其后紧接着 e.g. J.setZero()
  • 对 Panda 而言，hand frame 只挂在 7 个臂关节上，两个夹爪自由度对应的列不会被写入。若不清零，这两列就是未初始化的内存
  • 该问题的症状具有迷惑性，3-7 节会详细说明

• 第二，需要正确选择 LOCAL_WORLD_ALIGNED 参考系

参考系	原点	线速度的表达系	角速度的表达系
WORLD	世界系原点	世界系	世界系
LOCAL	末端	末端系	末端系
LOCAL_WORLD_ALIGNED	末端	世界系	世界系

• 我们要做笛卡尔空间控制，位置误差与速度均在世界系中表达，因此选择 LOCAL_WORLD_ALIGNED。它的原点为末端，三个轴与世界系平行，算出的 $\dot{x}$ 可以直接与世界系下的期望速度相减

2-5-3 速度与加速度

• 已知雅可比后，末端速度为：

$$\dot{x}=J(q)\dot{q}$$

• 再求一次导数得到加速度。$J$ 本身也是 $q$ 的函数，因此求导需使用乘积法则：

$$\ddot{x}=J(q)\ddot{q}+\dot{J}(q,\dot{q})\dot{q}$$

• 第二项 $\dot{J}\dot{q}$ 称为偏速度项（bias acceleration），表示在关节无角加速度时末端仍然产生的加速度分量。在高速运动场景中该项不可忽略

• Pinocchio 中可以直接获取该量：

// 带速度与加速度的重载，一次算完
pinocchio::forwardKinematics(model, data, q, v, a);

// 或者单独要雅可比的时间导数
pinocchio::computeJointJacobians(model, data, q);
pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);

• 本期没有用到 $\dot{J}$
  • 我们的圆轨迹末端线速度在 0.1 m/s 量级（半径 0.10 m、角频率 1.0 rad/s），偏速度项相对于阻抗控制中的 $K_{p}e$ 项小一个数量级以上
  • 阻抗控制本身不追求精确跟踪，其目标是使末端表现出弹簧阻尼系统的行为。$\dot{J}\dot{q}$ 对阻抗行为的影响，远小于我们所需的柔顺性
• 在高加速度的轨迹跟踪（例如高速拾放），以及把计算力矩控制实现在笛卡尔空间时，该项为必选项
• 这里的取舍原则为：先判断哪一项在当前量级下确实重要，再决定是否补偿。把所有项都加入控制器，通常带来的是更大的参数调试负担，而非更好的效果

2-5-4 逆运动学 IK

• 逆运动学是反问题：给定末端目标位姿，求关节角

$$q^{\ast }=\arg \underset{q}{\min }\Vert f(q)-x_{des}{\Vert }^2$$

• 它比正运动学困难得多，原因如下：

  • 非线性：除少数特殊构型（例如 6 轴球形手腕）外没有解析解
  • 可能多解：同一位置可能对应多种关节构型（肘上肘下、腕翻转）
  • 可能无解：目标点位于工作空间之外
  • 可能奇异：某些位形下雅可比降秩，逆解发散

• 工程上最常用的是迭代法：从当前 $q$ 出发，反复计算增量方向以逼近目标。核心公式来自对误差做一阶近似：

$$e=x_{des}-f(q)\approx J(q)\Delta q⟹\Delta q=J^{+}e$$

• 其中 $J^{+}$ 是伪逆。直接使用伪逆存在一个严重问题：在奇异位形附近 $J$ 接近降秩，$J^{+}$ 趋于无穷大，导致单步增量发散

• 解决办法是阻尼最小二乘（DLS，也称 Levenberg-Marquardt），即为伪逆添加阻尼项：

$$\Delta q=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}e$$

• 该式是本期逆运动学实际采用的形式，有以下几点需要说明：

  • $\lambda =0$ 时退化为 Moore-Penrose 伪逆 $J^{+}=J^T(J{J}^T{)}^{-1}$
  • $\lambda >0$ 时，$J{J}^T+{\lambda }^{2}I$ 一定可逆，与 $J$ 接近降秩的程度无关
  • 代价是收敛速度变慢、精度略有损失。$\lambda$ 即为"稳定"与"精度"之间的调节参数

• 本机的 Pinocchio 构建中没有 pinocchio::InverseKinematics（该模块未编译进去），因此这部分需要手写，3-4 节会给出完整实现。这里先给出骨架：

Eigen::VectorXd q = q_init;
for (int it = 0; it < max_iters; ++it) {
  pinocchio::forwardKinematics(model, data, q);
  pinocchio::updateFramePlacements(model, data);

  const Eigen::Vector3d e = target - data.oMf[fid].translation();
  if (e.norm() < tol) break;                       // 收敛

  Eigen::MatrixXd J(6, model.nv);
  J.setZero();
  pinocchio::computeFrameJacobian(model, data, q, fid,
                                  pinocchio::LOCAL_WORLD_ALIGNED, J);
  const Eigen::MatrixXd Jp = J.topRows(3);         // 只约束位置，取平移部分

  Eigen::Matrix3d JJt = Jp * Jp.transpose();
  JJt.diagonal().array() += lambda * lambda;       // 加阻尼
  const Eigen::Vector3d y = JJt.ldlt().solve(e);

  Eigen::VectorXd dq = Jp.transpose() * y;
  if (dq.norm() > 0.2) dq *= 0.2 / dq.norm();      // 限步长，防奇异位形跳太远
  q += dq;
}

• 较新的 Pinocchio 版本提供了 pinocchio::InverseKinematics 类，支持位置与姿态的加权目标，内部同样采用阻尼迭代。本机不存在该类，因此为手写实现；两条路径都能完成逆运动学求解

---

2-5-5 动力学

• 刚体动力学的运动方程为下式，它是整期文章中出现次数最多的一条公式：

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau$$

• 各项的物理含义：

项	名称	含义
$M(q)\ddot{q}$	惯性项	$M(q)$ 是质量矩阵，把角加速度变成力矩，且随位形变化
$C(q,\dot{q})\dot{q}$	科氏与离心项	关节之间的耦合：一个关节动了会"甩"到别的关节上
$g(q)$	重力项	撑住自重所需要的力矩
$\tau$	关节力矩	我们唯一能直接施加的量

• 这条方程解决的是力矩与运动之间的换算：给定机械臂当前的位形、速度与期望加速度，反推各关节需要出多大力矩
  • 输入为 $q$、$\dot{q}$、$\ddot{q}$ 三个量
  • 输出为关节力矩 $\tau$，这也是控制器唯一能写进 ctrl 的东西
  • 强调"反推"是因为控制器的输出只能落在 $\tau$ 上：我们想让机械臂做什么，最终都要换算成力矩。3-8 节选逆动力学而非正动力学，原因即在此

说人话：动力学方程就是"运动"和"力矩"之间的换算关系。给定期望的运动能反推出需要多大力矩，反过来给力矩也能算出会跑出什么加速度。

---

• Pinocchio 围绕该方程提供了一整套算法，最常用的有四个：

算法	函数	算什么	复杂度
逆动力学	rnea(model, data, q, v, a)	给定运动，求所需力矩 $\tau$	$O(n)$
正动力学	aba(model, data, q, v, tau)	给定力矩，求加速度 $\ddot{q}$	$O(n)$
质量矩阵	crba(model, data, q)	算 $M(q)$	$O(n^2)$
科氏矩阵	computeCoriolisMatrix(model, data, q, v)	算 $C(q,\dot{q})$	$O(n^2)$

---

• 逆动力学是本期的主力，力矩控制需要的是"给定目标运动，反推所需力矩"。它的展开式为：

$$\tau =M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)$$

说人话：逆动力学就是"把想让它怎么动，翻译成该出多大力矩"。我们给出每个关节的位置、速度与期望加速度，它反推各关节需要施加的力矩。至于遇到扰动会不会跑偏，它不管——那是叠加在上面的反馈项负责的事。

• 它将 $M$、$C$、$g$ 一次性算出，复杂度为 $O(n)$，明显快于分别计算 $M$、$C$ 与 $g$。这就是递归牛顿-欧拉算法（RNEA）的价值所在

// 完整逆动力学：给定期望加速度，算出需要的力矩
Eigen::VectorXd a_des = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau = pinocchio::rnea(model, data, q, v, a_des);

// 质量矩阵（注意：crba 只填上三角）
Eigen::MatrixXd M = pinocchio::crba(model, data, q);
M.triangularView<Eigen::StrictlyLower>() =
    M.transpose().triangularView<Eigen::StrictlyLower>();   // 补全对称部分

// 正动力学：给力矩，算加速度
Eigen::VectorXd a = pinocchio::aba(model, data, q, v, tau);

• 这里有三点容易被忽略、但必然导致问题：

• 第一，rnea(q, v, 0) 计算的是重力补偿

  • 将期望加速度置为零向量后，惯性项 $M\cdot 0$ 被消去，剩余项为：

$$\text{rnea}(q,v,0)=C(q,\dot{q})\dot{q}+g(q)$$

* 该项即为"使机械臂保持当前运动状态所需的额外力矩"。**3-3 节的重力补偿使用的就是它**
* 该量可以与 MuJoCo 对应：`mjData.qfrc_bias` 即为此项。两个引擎的该量是否一致，是 3-2-3 节交叉验证的第一条判据

• 第二，rnea 不包含关节阻尼与库仑摩擦
  • 这是最容易出问题的一条。MuJoCo 仿真中，MJCF 中写的 damping="1" 与 frictionloss 每一步都生效；但 Pinocchio 的 rnea 中不含这两项：Model 中虽然保存了 damping 与 friction 向量，rnea 不会自动将其加入
  • 其结果是：计算出的力矩在仿真中始终存在偏差，表现为稳态误差无法消除、跟踪误差停在 $10^{-5}$ 量级而不再下降
  • 补偿方法是显式加回：

for (int i = 0; i < model.nv; ++i) {
  tau[i] += model.damping[i] * v[i];                       // 粘性阻尼
  if (std::abs(v[i]) > 1e-3)                               // 库仑摩擦
    tau[i] += model.friction[i] * (v[i] > 0 ? 1.0 : -1.0);
}

• 速度判据 1e-3 用于避免在零速附近正负反复切换而产生抖振

• 第三，crba 只填上三角

  • crba 只计算上三角部分以节省时间，下三角保留的是内存中的旧值
  • 但 rnea 内部会重新计算，不受此影响。只有直接读取 crba 结果时才需要手动补全（如上面的代码段）
  • 未补全的后果是：用于计算 $M^{-1}$ 或求特征值时结果完全错误

2-5-6 控制

• 把前面这些能力组合起来，就是各种控制器的实现。它们在结构上有一个共同点：先用 Pinocchio 计算一个补偿项，再叠加一个反馈项

$$\tau =\underset{\text{前馈，Pinocchio 算}}{\underbrace{{\tau }_{ff}}}+\underset{\text{反馈，我们设计}}{\underbrace{{\tau }_{fb}}}$$

• 按"反馈作用所在的空间"分类，常见的有以下几种：

控制器	反馈空间	力矩形式	特点
关节 PD	关节空间	$\tau =K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})$	最简单，但有稳态误差
重力补偿 PD	关节空间	$\tau =g(q)+K_{p}e+K_d\dot{e}$	消除稳态误差，最常用
计算力矩	关节空间	$\tau =M(q)a_{cmd}+C\dot{q}+g$	把系统线性化成二阶系统
任务空间阻抗	笛卡尔空间	$\tau =J^T(K_p{e}_x+K_d{\dot{e}}_x)+C\dot{q}+g$	末端表现得像弹簧阻尼

• 四种中本期实现了三种：关节 PD 与重力补偿合并为 gravity 模式，计算力矩为 computed 模式，任务空间阻抗为 task 模式。其公式在第三章逐个展开

说人话：重力补偿保持稳定，计算力矩用于移动，空间阻抗用于柔顺。前两个管的是机械臂自己怎么动，最后一个管的是它跟环境接触时让不让步。

• 一个需要提前指出的细节：计算力矩与任务空间控制都需要动力学，但实现路径完全不同

  • 计算力矩需要 $M(q)\ddot{q}$ 这一项，只需将期望加速度 $a_{cmd}$ 传给 rnea，无需显式计算 $M$
  • 任务空间控制需要 $J^{T}F$ 这一项，只用到雅可比与 $C\dot{q}+g$，同样不需要计算 $M$
  • 因此控制回路中从头到尾没有调用过 crba。它只在 3-2-3 节的交叉验证里出现一次，用于与 MuJoCo 的 mj_fullM 比对
  • 这一点与常见做法不同：许多教程在计算力矩控制中先花半页篇幅计算 $M(q)$，实际上用 rnea 一步即可跳过

• 最后给出 Pinocchio 在整个控制系统中的位置：

层	谁负责	本期用什么
感知	相机、点云	前五期已完成
规划	轨迹生成	3-5 节的圆轨迹参数方程
建模	动力学模型	Pinocchio 的 Model
控制	力矩计算	Pinocchio 的 FK、Jacobian、RNEA
执行	施加力矩	MuJoCo 的 mjData.ctrl

2-6 官方 API

• 将本期用到、以及未用到但常用的 API 汇总为一张表，便于查阅：

API	头文件	作用
urdf::buildModel()	parsers/urdf.hpp	从 URDF 创建模型
mjcf::buildModel()	parsers/mjcf.hpp	从 MJCF 创建模型
Data(model)	multibody/data.hpp	构造数据容器，必须传 model
getJointId() / getFrameId()	multibody/model.hpp	名字查下标
neutral()	multibody/model.hpp	中性配置（各关节取中间值）
forwardKinematics()	algorithm/kinematics.hpp	正运动学，填 data.oMi
updateFramePlacements()	algorithm/frames.hpp	更新 frame 位姿，填 data.oMf
framePlacement()	algorithm/frames.hpp	取指定 frame 的位姿
computeJointJacobians()	algorithm/jacobian.hpp	算所有关节雅可比
getFrameJacobian()	algorithm/frames.hpp	取 frame 雅可比
computeFrameJacobian()	algorithm/frames.hpp	一步算完 frame 雅可比
crba()	algorithm/crba.hpp	质量矩阵，只填上三角
rnea()	algorithm/rnea.hpp	逆动力学
aba()	algorithm/aba.hpp	正动力学
computeCoriolisMatrix()	algorithm/rnea.hpp	科氏矩阵
computeGeneralizedGravity()	algorithm/rnea.hpp	只算重力项 $g(q)$

• 最后给出一个最小可运行例子，将 FK、雅可比与逆动力学串联一遍。该例子的结构与第三章完全一致，可直接作为模板：

#include <pinocchio/parsers/urdf.hpp>

#include <pinocchio/algorithm/kinematics.hpp>

#include <pinocchio/algorithm/frames.hpp>

#include <pinocchio/algorithm/jacobian.hpp>

#include <pinocchio/algorithm/rnea.hpp>

#include <Eigen/Core>

#include <cstdio>

int main() {
  // ---- 1. 建模 ----
  pinocchio::Model model;
  pinocchio::urdf::buildModel("panda.urdf", model);
  pinocchio::Data data(model);                  // 必须由 model 构造

  const pinocchio::FrameIndex ee = model.getFrameId("panda_hand");
  std::printf("nq=%d nv=%d njoints=%d nframes=%d\n",
              (int)model.nq, (int)model.nv,
              (int)model.njoints, (int)model.nframes);

  // ---- 2. 造一个配置 ----
  Eigen::VectorXd q = pinocchio::neutral(model);
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);

  // ---- 3. 正运动学 ----
  pinocchio::forwardKinematics(model, data, q);
  pinocchio::updateFramePlacements(model, data);   // 少了这行 oMf 就是旧值
  const pinocchio::SE3& oMf = data.oMf[ee];
  std::printf("末端位置: %.6f %.6f %.6f\n",
              oMf.translation().x(), oMf.translation().y(),
              oMf.translation().z());

  // ---- 4. 雅可比（6 x nv，世界系对齐）----
  Eigen::MatrixXd J(6, model.nv);
  J.setZero();                                     // 不能省
  pinocchio::computeFrameJacobian(model, data, q, ee,
                                  pinocchio::LOCAL_WORLD_ALIGNED, J);
  std::printf("雅可比尺寸: %ld x %ld\n", (long)J.rows(), (long)J.cols());

  // ---- 5. 逆动力学 ----
  const Eigen::VectorXd tau = pinocchio::rnea(model, data, q, v, a);
  std::printf("重力力矩范数: %.6f Nm\n", tau.norm());

  return 0;
}

• 编译使用 CMake 最为简便，find_package 会自动引入 Eigen、Boost 等依赖：

cmake_minimum_required(VERSION 3.16)
project(pin_demo CXX)
find_package(pinocchio REQUIRED)

add_executable(demo demo.cpp)
target_link_libraries(demo PRIVATE pinocchio::pinocchio)

source env.sh          # 先 source，否则 find_package 找不到 pinocchio
cmake -S . -B build && cmake --build build -j
./build/demo

• 上述代码已标出前面几节提到的三处问题：Data(model)、J.setZero()、updateFramePlacements()。这三点一个都不能漏，遗漏不会报错，只会静默给出错误结果

---

3 Pinocchio 在 MuJoCo 中对 Panda Franka 机械臂的底层操控画圆

3-1 整体思路介绍

• 前面两章分别说明 MuJoCo 与 Pinocchio 各自是什么，本章把两者接起来完成一件具体的事：
  • 在 MuJoCo 中仿真一台 Franka Emika Panda，由 Pinocchio 的 C++ 库计算关节力矩并写回 MuJoCo，使末端在水平面内画出一个圆
• 核心思路非常简单：
  1. 首先确认 MuJoCo 与 Pinocchio 如何共享同一份 MJCF、按名字对齐关节与执行器下标，并在每步把状态同步过去
  2. 接上之后，第一个里程碑是让它站稳：重力与科氏补偿加独立关节 PD，机械臂悬停不掉
  3. 站稳之后、正式开始圆规划之前，用阻尼最小二乘逆运动学在启动时解一次圆周最远点，确认这个圆落在机械臂的可达范围内
  4. 紧接着规划笛卡尔圆轨迹：末端的期望位置、速度与加速度都是时间的解析函数
  5. 控制最终要落到力矩上，这需要三块积木：正运动学取当前位姿、雅可比联系关节量与笛卡尔量、逆动力学把运动换算成力矩
  6. 最后是画圆：任务空间阻抗让末端表现得像弹簧阻尼，笛卡尔误差经 $J^T$ 映射成关节力矩

观前提示：本文调用的 Pinocchio 库，核心是机械臂底层的数学原理与驱动，大部分概念都是应用数学和正逆运动学求解，公式难免枯燥难懂（特别是第三章）。对于 VLA，我们并不会涉及这么底层的数学计算与理解，所以如果看起来不舒服的朋友们，可以选择性观看第三章

• 话以自此，对于仍逆水行舟的朋友们致以最崇高的敬意!!

---

3-2 双引擎对接与状态同步

• 本节是整章的基础，内容分三部分：先说明两个引擎之间每个时间步传递的数据（整体数据流），再完成四项桥接工作与状态同步，最后设立一道验收门。三步顺序不可交换，因为后一步依赖前一步的结果

• 整个系统每个时间步依次执行四个步骤，顺序不可颠倒：

步	动作	执行者	数据
1	从仿真读状态	MuJoCo	mjData.qpos 与 mjData.qvel 得到 q、v
2	算控制律	Pinocchio	由 (q, v, t) 得到 tau
3	写回仿真	MuJoCo	tau 得到 mjData.ctrl
4	推进物理	MuJoCo	mj_step 得到下一步状态

• 以"提供什么、消费什么"的形式列出各角色的输入与输出，比流程图更不易产生歧义：

角色	提供	消费
MuJoCo	模型加载、接触与积分、qpos 与 qvel、被动元件（阻尼、摩擦）	ctrl（力矩）
Pinocchio	$M$、$C$、$g$、$J$、oMf	q、v（由 MuJoCo 同步）
我们的控制器	tau	上述二者的输出

• 这里有一处设计选择需要说明：Pinocchio 不自行维护状态，而是每步从 MuJoCo 读取

  • 两个引擎必须使用同一份状态。若各自积分，一次误差就会使两边逐渐分离，最终 Pinocchio 计算补偿量所依据的位形与 MuJoCo 中的真实位形不再一致
  • 因此分工为：MuJoCo 是唯一的状态源，Pinocchio 是无状态的纯函数计算器。每步传入 (q, v) 并算出 tau，不保留任何跨步状态
  • 该设计还有一项额外好处，3-10 节会用到：整个控制器的输出只依赖 (q, v, t)。因此是否开启窗口、运行速度如何，结果都必须逐位一致，这是一项很强的自检条件

• 另有一条原则：一切数值以 mjData 与 mjModel 为准，不以 MJCF 文本为准

  • MJCF 面向人工编写，编译后的 mjModel 才是实际使用的值。例如 MJCF 中写 fullinertia，MuJoCo 会做特征分解并存为"主轴惯量加 iquat"，重建时产生 $10^{-8}$ 量级的往返误差
  • 因此 3-2-3 节的交叉验证一律比较编译后的值，而非 XML 中的字面值

Pinocchio 只负责计算，实际状态从 MuJoCo 读取后再进行下一步计算：任何执行都不可能理想，因此状态必须取自 MuJoCo

3-2-1 四项桥接工作

• 1. 一份 MJCF 供两个引擎使用
  • MuJoCo 读取 scene.xml（其中 include 了 panda.xml，并加入地面与光照），Pinocchio 读取纯机器人描述 panda.xml
  • 两边读取的是同一份机器人描述，从结构上排除了"MJCF 与 URDF 两份描述不一致"这类失配：不依赖人工维护一致性，而是只存在一份描述
  • 本机 moveit_resources_panda_description 中的 panda.urdf 不含任何 <inertial>，使用它会得到全零的动力学。这是不采用 URDF 路线的直接原因

// MuJoCo 侧：正常加载场景
mj_model = mj_loadXML(mjcf_mujoco.c_str(), nullptr, err, sizeof(err));

// Pinocchio 侧：直接解析纯机器人 MJCF
// 签名是 buildModel(文件名, model, verbose)，第三个参数是打印解析信息
pinocchio::mjcf::buildModel(mjcf_pinocchio, pin_model, false);

• Pinocchio 的 MJCF 解析器一次即可解出运动学树、fullinertia 惯量以及 armature、damping、friction。这一点尤为重要：3-8 节要补的阻尼与摩擦，其数值即来源于此

---

• 2. 索引全部按名字建立，不硬编码任何下标
  • 两个引擎的关节顺序不保证一致，执行器下标也不保证等于自由度下标。因此运行时按名字建立三张表：

for (int j = 0; j < mj_model->njnt; ++j) {
  // 本桥接层按单自由度关节逐元素同步状态。
  // 浮动基座与球关节需要分块处理，与其静默算错，不如明确拒绝。
  if (mj_model->jnt_type[j] != mjJNT_HINGE &&
      mj_model->jnt_type[j] != mjJNT_SLIDE) {
    throw std::runtime_error("只支持 1 自由度关节: " + jname(mj_model, j));
  }

  const std::string name = jname(mj_model, j);
  const int pid = pin_model.getJointId(name);
  if (pid >= (int)pin_model.njoints || pin_model.names[pid] != name) {
    throw std::runtime_error("Pinocchio 模型里找不到关节: " + name);
  }

  joints.push_back(name);
  mj_qadr.push_back(mj_model->jnt_qposadr[j]);              // mjData.qpos 的下标
  mj_vadr.push_back(mj_model->jnt_dofadr[j]);               // mjData.qvel 的下标
  pin_qidx.push_back(pin_model.joints[pid].idx_q());        // q 向量里的下标
  pin_vidx.push_back(pin_model.joints[pid].idx_v());        // v 向量里的下标
}

• q 与 v 使用两套下标（idx_q 与 idx_v）。这是 1-5 节与 2-4-1 节强调的 $nq\ne nv$ 在代码中的体现：宁可多维护一个数组，也不假设两者相等
• 这里还有一处主动报错的设计：getJointId 在找不到名字时返回越界值 njoints，而不抛异常。因此必须显式检查 pin_model.names[pid] != name，否则会带着越界下标继续执行，直到某处内存越界才崩溃

---

• 3. 执行器归一化为纯力矩
  • 1-6 节已说明，menagerie 的 Panda 执行器为位置伺服，直接把力矩写入 ctrl 会被当作目标角度。因此需在 mj_step 之前将其改为力矩型：

for (int a = 0; a < mj_model->nu; ++a) {
  // 夹爪执行器驱动的是 tendon 而非 joint，跳过
  if (mj_model->actuator_trntype[a] != mjTRN_JOINT) continue;

  mj_model->actuator_gaintype[a] = mjGAIN_FIXED;
  mj_model->actuator_biastype[a] = mjBIAS_NONE;
  for (int k = 0; k < mjNGAIN; ++k) mj_model->actuator_gainprm[a * mjNGAIN + k] = 0.0;
  for (int k = 0; k < mjNBIAS; ++k) mj_model->actuator_biasprm[a * mjNBIAS + k] = 0.0;
  mj_model->actuator_gainprm[a * mjNGAIN] = 1.0;   // 力矩 = 1.0 乘 ctrl
  mj_model->actuator_ctrllimited[a] = 0;           // 解除 ctrl 限幅，保留 forcerange
}

• 同时建立"哪个自由度受力矩控制"这张表。该表同时解决了夹爪的问题：它的执行器为 tendon 型，act_of_v 中对应项自然为 -1：

act_of_v.assign(pin_model.nv, -1);
for (int a = 0; a < mj_model->nu; ++a) {
  if (mj_model->actuator_trntype[a] != mjTRN_JOINT) continue;
  // C API 中 actuator_trnid 是平数组，(nu x 2) 的布局需自行计算下标
  const int jid  = mj_model->actuator_trnid[2 * a];
  const std::string jname_str = jname(mj_model, jid);
  for (size_t k = 0; k < joints.size(); ++k)
    if (joints[k] == jname_str) act_of_v[pin_vidx[k]] = a;
}

// 受力矩控制的关节 = 带关节型执行器的关节。Panda 上为 7 个臂关节
for (size_t k = 0; k < joints.size(); ++k)
  if (act_of_v[pin_vidx[k]] >= 0) arm_joints.push_back(joints[k]);

• 结果为：nq = nv = 9（7 个臂关节加 2 个手指关节），但只有 7 个自由度受力矩控制，nu = 8（7 个臂加 1 个夹爪）

---

• 4. 惯性参数：只验证，不回填
  • 该步在直觉上必要，实际却会导致错误，需要单独说明
  • 直觉做法是：MJCF 以 geom 密度推算质量时，Pinocchio 的解析器（读取 XML 文本）与 MuJoCo（读取编译后的 mjModel）可能产生分歧，于是将 mjModel 的惯性重建后写回 Pinocchio
  • 但该做法不可行：Pinocchio 会将固定关节的 body 惯性合并进父关节（本模型中 hand 的 0.73 kg 被并入 joint7），而 mjModel 按 body 分别存储。按 jnt_bodyid 逐个回填会丢失这部分质量
  • 实测后果为：误差从 $10^{-8}$ 恶化到 $2.4\times 10^{-1}$。即该修正使误差增大约一亿倍
  • 因此惯性参数一概不回填：校验通过即结束，不写回任何东西
  • 需要与惯性区分开的是被动参数（armature、damping、friction）。它们不属于惯量，但同样参与力矩计算，且 3-8 节要用到。这三项的处理策略与惯性相反：先校验，不一致时才按 MuJoCo 覆盖

// 只校验三组被动参数：与 MuJoCo 不一致时才按 MuJoCo 覆盖，一致就一个字节都不动
for (size_t k = 0; k < joints.size(); ++k) {
  const int a = mj_vadr[k], p = pin_vidx[k];        // MuJoCo 的 dof 下标 / Pinocchio 的 v 下标
  if (std::abs(pin_model.armature[p] - mj_model->dof_armature[a])     > 1e-12 ||
      std::abs(pin_model.damping[p]  - mj_model->dof_damping[a])      > 1e-12 ||
      std::abs(pin_model.friction[p] - mj_model->dof_frictionloss[a]) > 1e-12) {
    pin_model.armature[p] = mj_model->dof_armature[a];
    pin_model.damping[p]  = mj_model->dof_damping[a];
    pin_model.friction[p] = mj_model->dof_frictionloss[a];
    notes.push_back(joints[k]);                     // 记下被覆盖的关节，便于排查
  }
}
// 本模型上三者逐项一致，覆盖分支从未进入，因此最终一个数也没改

3-2-2 状态同步

• 桥接工作解决的是两套下标如何对齐，要使其真正发挥作用还差一步：状态同步。每个控制周期都需在两个引擎之间同步一次状态，两个方向各一个函数，合计不到十行：

// MuJoCo 到 Pinocchio：按名字映射的下标逐关节搬运
void MjPinBridge::mjToPin() {
  for (size_t k = 0; k < joints.size(); ++k) {
    q[pin_qidx[k]] = mj_data->qpos[mj_qadr[k]];
    v[pin_vidx[k]] = mj_data->qvel[mj_vadr[k]];
  }
}

// Pinocchio 到 MuJoCo：力矩写入 ctrl，仅写受力矩控制的自由度
void MjPinBridge::pinToMj() {
  for (size_t k = 0; k < joints.size(); ++k) {
    const int a = act_of_v[pin_vidx[k]];
    if (a >= 0) mj_data->ctrl[a] = tau[pin_vidx[k]];
  }
}

• 这两个函数很短，但有三处易出问题之处：

• 第一，qpos 与 qvel 使用不同的下标数组

  • 代码中 q 使用 mj_qadr，v 使用 mj_vadr。在 MuJoCo 中这两个数组是不同的（jnt_qposadr 与 jnt_dofadr）
  • Panda 全部为铰链关节，两者恰好相等，因此写错也不会表现出来。但换成带自由关节的机器人（例如四足、人形）时，qpos 的下标会逐段偏移，整个状态随之错位
  • 这种在当前模型上无法暴露错误的写法尤其需要警惕

• 第二，ctrl 的下标也不等于自由度下标

  • 写回时使用 act_of_v[...] 而非直接用 k，因为 MuJoCo 中 actuator_trnid 表明执行器序号与关节序号是两套编号
  • 本模型中两者恰好一致（执行器 1 到 7 对应关节 1 到 7），但这是本模型给出的巧合，而非接口约定。更换模型或调整夹爪位置即会出错

• 第三，不受控的自由度必须清零

  • 两个夹爪自由度不在 arm_joints 中，但它们在 q、v、tau 向量中都占据位置
  • rnea 也会算出它们的力矩（两个手指有质量，同样受重力），若不清零就会残留在 tau 中
  • 本项目在每个控制器的末尾统一清零：

// 夹爪自由度不受控：清零，避免残留分量写回 ctrl
for (size_t k = 0; k < b.joints.size(); ++k)
  if (b.act_of_v[b.pin_vidx[k]] < 0) b.tau[b.pin_vidx[k]] = 0.0;

• 这一条即使出错也不会立即报错：夹爪的力矩写不进 ctrl（pinToMj 中 a < 0 即跳过）。但若不处理，tau 中会残留来源不明的分量，后续在 tau 上叠加其他项时即会出现问题

• 回到本节开头提到的设计原则：MuJoCo 是唯一状态源。因此 mjToPin() 在每个控制周期开头调用，pinToMj() 在每个周期结尾调用，中间的 Pinocchio 计算不保留任何跨步状态

3-2-3 交叉验证

• 四项工作完成后，还需设立一道验收门：在同一组随机 (q, v) 下比较两个引擎的动力学与运动学量。结果不一致则不能用于控制：控制器算出的力矩对仿真中的机械臂而言是错误的，后续所有调参都失去意义

即通过状态同步确认实际执行与计算预测之间的差别有多大，据此判断能否正确执行

• 具体比较以下几项：

比较对象	比较方式	判据
重力与科氏	MuJoCo 的 qfrc_bias 对 Pinocchio 的 rnea(q, v, 0)	相对误差
质量矩阵	MuJoCo 的 mj_fullM 对 Pinocchio 的 crba	相对误差
末端位姿	mjData.xpos / xmat 对 data.oMf	绝对误差
总质量	两边所有 body 求和	—

// 采一个随机构型（在限位内）
mj_forward(mj_model, mj_data);
for (int d = 0; d < mj_model->nv; ++d) bias_mj[d] = mj_data->qfrc_bias[d];
mj_fullM(mj_model, mj_data, M_mj.data());

// Pinocchio 侧用同一组状态
const Eigen::VectorXd tau0 = pinocchio::rnea(
    pin_model, pin_data, q, v, Eigen::VectorXd::Zero(pin_model.nv));
Eigen::MatrixXd M_pin = pinocchio::crba(pin_model, pin_data, q);
// crba 只填上三角，比对前先补全
M_pin.triangularView<Eigen::StrictlyLower>() =
    M_pin.transpose().triangularView<Eigen::StrictlyLower>();

• 质量矩阵的阈值放宽到 $10^{-7}$ 有明确原因，并非随意取值：MuJoCo 将 MJCF 的 fullinertia 特征分解为"主轴惯量加 iquat"后存储，mj_fullM 使用的是分解后的值。实测 MJCF 中写 Iyy = 0.70661，mjModel 中重建为 0.70660997，该往返本身即带来约 $3\times 10^{-8}$ 的相对误差

• 这属于 MuJoCo 侧的表示误差，而非桥接错误，因此不应以 $10^{-16}$ 作为判据。分清误差来源，比调整阈值使其恰好通过更为重要

• 运行 --mode verify，完整输出如下：

./build/panda_pin --mode verify

• 两个引擎的动力学在 $10^{-10}$ 量级上一致，通过该步之后才能进入控制环节

3-3 关节 PD 与重力补偿控制

3-3-1 整体思路

• 两个引擎对接之后，第一个里程碑是使机械臂保持静止。先用一个更简单的模式验证闭环，本节的控制律为：

$$\tau =\underset{\text{rnea}(q,v,0)}{\underbrace{C(q,\dot{q})\dot{q}+g(q)}}+\underset{\text{关节 PD}}{\underbrace{K_p(q_d-q)+K_d(0-\dot{q})}}$$

说人话：rnea 负责重力补偿，PD 负责反馈控制。

• 先做这一步的原因是它将问题分离：只验证"读状态、算力矩、写回"这条链路是否通畅，不涉及笛卡尔空间、雅可比与轨迹跟踪

  • 目标位形 $q_d$ 取初始位形，因此期望速度与加速度均为零，PD 只需将漂移压回
  • 若这一步都不稳定，后面的任务空间控制必然有问题，而问题容易被误判为笛卡尔部分写错

• 控制律只作用于 7 个臂关节，而 Pinocchio 的 $q$ 与 $v$ 长度为 $nv=9$（含两个手指关节）。因此先写一个辅助函数，按名字映射取出受力矩控制的那些自由度：

// 从 pinocchio 的 v 空间全向量里取出受力矩控制的那些关节
Eigen::VectorXd pickArm(const MjPinBridge& b, const Eigen::VectorXd& full) {
  Eigen::VectorXd out(b.arm_joints.size());
  for (size_t k = 0; k < b.arm_joints.size(); ++k) {
    const auto it = std::find(b.joints.begin(), b.joints.end(), b.arm_joints[k]);
    out[k] = full[b.pin_vidx[std::distance(b.joints.begin(), it)]];
  }
  return out;
}

• 参数取 $K_p=400$、$K_d=40$：

void JointSpacePd::init(const MjPinBridge& b) {
  const int n = (int)b.arm_joints.size();
  q_des = pickArm(b, b.q);                        // 目标就是当前位形
  Kp = Eigen::VectorXd::Constant(n, 400.0);
  Kd = Eigen::VectorXd::Constant(n, 40.0);
}

void JointSpacePd::compute(MjPinBridge& b) {
  // 重力与科氏补偿
  b.tau = pinocchio::rnea(b.pin_model, b.pin_data, b.q, b.v,
                          Eigen::VectorXd::Zero(b.pin_model.nv));
  // 关节 PD
  for (size_t k = 0; k < b.arm_joints.size(); ++k) {
    const auto it = std::find(b.joints.begin(), b.joints.end(), b.arm_joints[k]);
    const int p = b.pin_vidx[std::distance(b.joints.begin(), it)];
    b.tau[p] += Kp[k] * (q_des[k] - b.q[p]) - Kd[k] * b.v[p];
  }
  // 夹爪自由度清零
  for (size_t k = 0; k < b.joints.size(); ++k)
    if (b.act_of_v[b.pin_vidx[k]] < 0) b.tau[b.pin_vidx[k]] = 0.0;
}

• $K_p$、$K_d$ 的取值方法如下：先把每个关节视为独立的二阶系统

  • 取 ${\omega }_n=20\text{rad/s}$、阻尼比 $\zeta =1$，则 $K_p={\omega }_n^2=400$、$K_d=2\zeta {\omega }_n=40$
  • 该结果是假设各关节解耦得到的。实际系统存在耦合，但以此作为起点已足够，运行后可依据结果调整
  • 这里同样得益于 3-8 节所述的计算力矩线性化：在补偿项准确的前提下，该二阶近似是成立的

• 运行 --mode gravity --seconds 3：

---------------- 运行结果 ----------------
  步数         : 1501  (仿真 3.002 s, 步长 0.0020 s)
  位形漂移     : 最大 1.776e-15 rad  (重力补偿应把它压到很小)

• 1.776e-15 rad 属于双精度浮点的舍入量级，即完全未移动。作为对照，同样的模式若去掉力矩（--mode free），机械臂会在重力作用下坠落

3-3-2 重力补偿

• 式中的补偿项来自逆动力学，即 $a=0$ 时的 rnea（3-8 节展开该函数的两次用法）：

$${\tau }_{comp}=\text{rnea}(q,v,0)=C(q,\dot{q})\dot{q}+g(q)$$

• 它解决的问题很直接：机械臂本身存在自重，无支撑时将会下坠。PD 控制器能够抵消其中一部分，但代价是引入稳态误差：误差需增大到 $K_{p}e=g(q)$ 才能平衡

  • 本模型中 $K_p=400$，重力力矩在几十牛米量级，稳态误差为 $e\approx g/K_p$，即几度的量级
  • 显式补上 $g(q)$ 之后，PD 只需处理扰动，不再承担自重，稳态误差才能消除

• 代码中写有该补偿项，并不等于补偿实际生效。验证方式是消融实验，而不是阅读代码：

模式	补偿开	补偿关	倍数
gravity（位形漂移）	1.776e-15 rad	—	数量级上就是"完全不动"
task（末端跟踪误差，最大）	4.000e-03 m	1.368e-01 m	34 倍
task（末端跟踪误差，平均）	2.144e-03 m	1.013e-01 m	47 倍

• 关闭补偿后误差增大三十余倍，这说明补偿确实在起作用，而非该轨迹恰好不需要补偿

• 这里还有一处设计取舍：补偿项中是否包含科氏项 $C\dot{q}$

  • 包含。rnea(q, v, 0) 一次即可给出两项，拆开则需多调用一次函数
  • 代价是无法单独验证重力项。若需要更干净的对照，可以改用 computeGeneralizedGravity(model, data, q)，它只返回 $g(q)$
  • 本期未采用该做法，因为运动速度很低（末端 0.1 m/s），科氏项本身很小，拆开对结果没有可见影响

• 至此一条最小闭环已经跑通：机械臂能够保持悬停。接下来按顺序补上画圆所需的内容：先确认目标点可达（3-4 节），再规划末端轨迹（3-5 节），然后备齐正运动学、雅可比与逆动力学三块积木（3-6 节至 3-8 节），最后画圆（3-9 节）

3-4 DLS 逆运动学与可达性校验

3-4-1 背景

• 在接入轨迹之前，需要先解决一个前置问题：该圆是否可达。手写的 DLS 逆运动学在本项目中不是控制回路的一环（阻抗控制不需要逆解），但解决了一个实际问题：如何确认画圆的目标点确实落在机械臂的可达范围内

3-4-2 DLS

• 先说明什么是 DLS（Damped Least Squares，阻尼最小二乘，也称 Levenberg-Marquardt）。逆运动学是反问题：给定末端目标位置，反求关节角。它除少数特殊构型外没有解析解，因此工程上采用迭代法，其核心是对误差做一阶近似：

$$e=x_{des}-f(q)\approx J(q)\Delta q⟹\Delta q=J^{+}e=J^T(J{J}^T{)}^{-1}e$$

• 直接使用伪逆存在一个严重问题：在奇异位形附近 $J$ 接近降秩，$J^T(J{J}^T{)}^{-1}$ 趋于无穷大，单步增量随之发散，迭代会立刻跑飞。DLS 的做法是给伪逆添加阻尼项：

$$\Delta q=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}e$$

• 加入 ${\lambda }^{2}I$ 之后，$J{J}^T+{\lambda }^{2}I$ 一定可逆，与 $J$ 接近降秩的程度无关，代价是收敛速度变慢、精度略有损失。$\lambda$ 即"稳定"与"精度"之间的调节参数，本节取 $\lambda =10^{-3}$，迭代上限 300 次，收敛判据为残差小于 $10^{-6}\text{m}$。完整推导见 2-5-4 节

3-4-3 代码实现

• 说回上面那个问题的答案
• 做法是取圆周上离起点最远的点（圆心与半径由控制器初始化给出，圆轨迹的参数方程见 3-5 节），即 $\theta =\pi$ 处，从圆心沿 $-x$ 方向走 $2r$：

// 目标取圆周上离起点最远的点（theta = pi，即 center 往 -x 走 2r）
// 若以当前末端位置为目标，残差恒为零，无法构成有效检验
const Eigen::Vector3d far_pt = ts.center + Eigen::Vector3d(-2.0 * ts.radius, 0, 0);
bool ik_ok = false;
double ik_res = 0.0;
const Eigen::VectorXd q_ik = dampedLeastSquaresIk(
    *bridge, far_pt, bridge->q, 300, 1e-3, 1e-6, &ik_ok, &ik_res);

• 这里有一处自设的检验陷阱：最初的目标取"当前末端位置"，残差恒等于 0，表面上完全收敛，实际未验证任何内容

  • 一个不可能失败的测试，无法验证任何内容
  • 改为圆周远点之后，才构成真正的可达性检查

• 完整的 DLS 实现为 2-5-4 节的骨架加上关节限位：

Eigen::VectorXd dampedLeastSquaresIk(MjPinBridge& b,
                                     const Eigen::Vector3d& target,
                                     const Eigen::VectorXd& q_init,
                                     int iters, double lambda, double tol,
                                     bool* ok, double* residual) {
  Eigen::VectorXd q = q_init;
  const double lam2 = lambda * lambda;
  bool converged = false;
  double res = 0.0;

  for (int it = 0; it < iters; ++it) {
    pinocchio::forwardKinematics(b.pin_model, b.pin_data, q);
    pinocchio::updateFramePlacements(b.pin_model, b.pin_data);
    const Eigen::Vector3d e = target - b.pin_data.oMf[b.ee_frame].translation();
    res = e.norm();
    if (res < tol) { converged = true; break; }

    // 只用位置雅可比的平移部分（3 x nv）。
    // setZero 不能省：computeFrameJacobian 只写运动链上那些关节的列，
    // 其余列原样保留，不清零就是未初始化内存（详见 3-7 节）
    Eigen::MatrixXd J(6, b.pin_model.nv);
    J.setZero();
    pinocchio::computeFrameJacobian(b.pin_model, b.pin_data, q, b.ee_frame,
                                    pinocchio::LOCAL_WORLD_ALIGNED, J);
    const Eigen::MatrixXd Jp = J.topRows(3);

    // dq = J^T (J J^T + lambda^2 I)^-1 e
    Eigen::Matrix3d JJt = Jp * Jp.transpose();
    JJt.diagonal().array() += lam2;
    const Eigen::Vector3d y = JJt.ldlt().solve(e);
    Eigen::VectorXd dq = Jp.transpose() * y;

    // 步长限制，防止在奇异位形附近一次跳太远
    const double max_step = 0.2;
    if (dq.norm() > max_step) dq *= max_step / dq.norm();
    q += dq;

    // 关节限位钳制
    for (size_t k = 0; k < b.arm_joints.size(); ++k) {
      const auto it2 = std::find(b.joints.begin(), b.joints.end(), b.arm_joints[k]);
      const int pidx = b.pin_qidx[std::distance(b.joints.begin(), it2)];
      q[pidx] = std::max(b.arm_qmin[k], std::min(b.arm_qmax[k], q[pidx]));
    }
  }

  if (ok) *ok = converged;
  if (residual) *residual = res;
  return q;
}

• 该检查在 task 模式启动时执行一次，--mode task --seconds 3 的输出如下：

[ik ] DLS 逆解圆周远点 (距起点 0.200 m): 收敛，残差 4.684e-11 m
[ik ] 正运动学回代校验残差 4.684e-11 m

• 逆解残差为 4.684e-11 m，正运动学回代校验得到同一个数，说明解确实到达目标位置，中间不存在状态污染

3-5 笛卡尔圆轨迹生成

3-5-1 圆的参数方程

• 目标可达性确认之后，定义末端的目标轨迹。目标为水平面内的圆，参数方程采用标准形式：

$$x_d(t)=x_c+r\cos \theta (t),y_d(t)=y_c+r\sin \theta (t),z_d(t)=z_c$$

• 其中 $(x_c,y_c,z_c)$ 是圆心，$r$ 是半径，$\theta (t)=\omega t$ 是相位。该写法的优点在于速度可解析求出，无需数值差分：

$${\dot{x}}_d=-r\omega \sin \theta ,{\dot{y}}_d=r\omega \cos \theta$$

3-5-2 问题描述

• 该形式的限制在于：它把 $t=0$ 的末端固定在圆周上，与圆心取在哪里无关。也就是说，起点与圆心之间必须隔着一个半径，两者不能重合

$$x_d(0)=x_c+r,y_d(0)=y_c$$

• 我们期望的是从当前末端位置出发逐渐扩展出一个圆。若圆心取当前末端位置，$t=0$ 时末端需由圆心突变到圆周，$\Vert e\Vert =r$，误差项 $K_{p}e$ 会产生很大的力矩，使机械臂产生一次冲击

说人话：圆的标准参数方程默认起点就落在圆周上，而我们的起点在圆心。硬套这个式子的话，第一步末端就得从圆心窜到圆周，误差一下子等于整个半径，力矩会猛地冲一下。所以改成让半径从零慢慢涨开的写法，起步就没有这个突跳了。

3-5-3 修改后的参数方程

• 因此轨迹改写为从圆心出发、半径平滑渐入的形式：

$$x_d(t)=x_c+r(t)(\cos \theta (t)-1),y_d(t)=y_c+r(t)\sin \theta (t)$$

• 改写后，$t=0$ 时 $\cos 0-1=0$ 且 $\sin 0=0$，末端落在圆心；$r(t)$ 由零平滑增长到目标半径，整条轨迹连续

3-5-4 平滑函数

• 平滑函数采用 smoothstep，其在两端的一阶导数均为零，因此起步与收尾都不出现速度突跳：

$$s(\tau )=3{\tau }^2-2{\tau }^3,\tau =\min \left(1,\frac{t}{t_{ramp}}\right),r(t)=r_{max}s(\tau )$$

• 期望速度需随之修改。半径随时间变化，求导时 $r$ 不能视为常数，需使用乘积法则：

$${\dot{x}}_d=\dot{r}(\cos \theta -1)-r\omega \sin \theta ,{\dot{y}}_d=\dot{r}\sin \theta +r\omega \cos \theta$$

• 其中 $\dot{r}=r_{max}\cdot s^{\prime }(\tau )/t_{ramp}$，$s^{\prime }(\tau )=6\tau (1-\tau )$

3-5-5 代码实现

• 对应的 C++ 代码仅十几行：

// 前 t_ramp 秒把半径从 0 平滑升到 radius，同时算出径向速度
const double s      = (t_ramp > 0) ? std::min(1.0, t / t_ramp) : 1.0;
const double smooth = s * s * (3.0 - 2.0 * s);            // smoothstep
const double dsmooth =                                     // smoothstep 对时间的导数
    (t < t_ramp && t_ramp > 0) ? (6.0 * s * (1.0 - s)) / t_ramp : 0.0;

const double r    = radius * smooth;
const double rdot = radius * dsmooth;

const double th = omega * t;
const double c  = std::cos(th), sn = std::sin(th);

// 圆在水平面内。t=0 时 cos(0)-1 = 0、sin(0) = 0，末端正好落在圆心
const Eigen::Vector3d p_d = center + r * Eigen::Vector3d(c - 1.0, sn, 0.0);
const Eigen::Vector3d v_d = rdot * Eigen::Vector3d(c - 1.0, sn, 0.0)
                          + r * omega * Eigen::Vector3d(-sn, c, 0.0);

• 本期的参数为：半径 0.10 m，角频率 1.0 rad/s，渐入时间 2.0 s。末端线速度峰值为 $r\omega =0.1\text{m/s}$，对 7 自由度机械臂而言处于较低水平

• 另一处细节是：姿态不随圆变化，全程保持不变

  • 圆位于水平面内，若末端姿态随之旋转，夹爪会在画圆的同时自转
  • 因此期望角速度恒为零，仅在 $K_p$ 中取一个较软的旋转刚度加以约束（3-9-4 节给出具体数值）

• 轨迹只给出了对末端运动的要求，还需将其转换为力矩。中间需要三块积木：正运动学取当前末端位姿（3-6 节）、雅可比联系关节量与笛卡尔量（3-7 节）、逆动力学把运动换算成力矩（3-8 节）

---

3-6 正运动学

• 正运动学在本项目中的作用很直接：每个控制周期都需要知道末端当前位置，才能算出它与目标的偏差

说人话：我们已经确保轨迹能够正确执行了，那么逆推计算出的中间结果应该和正推的一致。如果不一致，就是你算错了！

• 使用的方法即 2-5-1 节的两步，顺序不可颠倒：

// 两步都不能省：只算前一步，data.oMf 里还是旧值
pinocchio::forwardKinematics(b.pin_model, b.pin_data, b.q);
pinocchio::updateFramePlacements(b.pin_model, b.pin_data);

• 取得位姿之后，位置与姿态需分别处理，因为两者的误差定义方式完全不同：

const Eigen::Vector3d p = b.pin_data.oMf[b.ee_frame].translation();
const Eigen::Matrix3d R = b.pin_data.oMf[b.ee_frame].rotation();

• 位置误差即向量差 $e_p=p_d-p$，三个分量各自独立

• 姿态误差不能用同样方式计算。两个旋转矩阵相减没有物理意义，3-9-2 节说明如何用 $SO(3)$ 的对数映射将其转换为三维向量

• 末端 frame 的选取需要说明：我们使用 hand，即法兰盘，而非指尖或夹爪中心

  • 选用法兰的优点是它的位姿完全由 7 个臂关节决定，与夹爪开合无关，画圆的控制问题因此不涉及夹爪状态，无需将其纳入状态向量
  • 代价是若后续要做抓取，需要再补偿一段"法兰到夹持点"的固定变换。该变换为常数，加一个偏移即可

• 另有一个验证手段：用正运动学做回代校验

  • 无论是逆解还是轨迹生成，计算结果都可代回正运动学独立确认一次
  • 3-4 节的 DLS 逆运动学即按此验证：解出关节角后，将 q 暂时设为该解，再调用一次 eePosition()，检查残差是否一致。若两个残差不一致，说明中间存在状态污染

// 用正运动学独立回代一次，确认逆解确实到达目标点
Eigen::VectorXd q_save = bridge->q;
bridge->q = q_ik;
const double back = (bridge->eePosition() - far_pt).norm();
bridge->q = q_save;                      // 校验完必须还原，否则污染后续控制

3-7 雅可比

• 雅可比在本项目中有三处用途，均直接对应 2-5-2 节的公式：

用途	公式	用在哪
末端速度	$\dot{x}=J\dot{q}$	阻抗律里的阻尼项 $K_d({\dot{x}}_d-\dot{x})$
笛卡尔力映射回关节	$\tau =J^{T}F$	阻抗律的输出 $\tau =J^T\cdot \text{wrench}$
位置逆解	$\Delta q=J_p^T(J_p{J}_p^T+{\lambda }^{2}I{)}^{-1}e$	手写 DLS 逆运动学，只取前 3 行

• 三处用途都指向同一个函数，因此封装为 eeJacobian()：

Eigen::MatrixXd MjPinBridge::eeJacobian() {
  Eigen::MatrixXd J(6, pin_model.nv);
  J.setZero();
  pinocchio::computeFrameJacobian(pin_model, pin_data, q, ee_frame,
                                  pinocchio::LOCAL_WORLD_ALIGNED, J);
  return J;
}

• 本节的重点是 J.setZero()：computeFrameJacobian 只写入运动链上关节对应的列，其余列原样保留，不预先清零就会读到未初始化的内存；
  • Panda 的 hand frame 只挂在 7 个臂关节上，两个夹爪自由度对应的列始终不会被写入，该问题在无窗口时因那块堆内存恰好是零页而不显形，开启渲染窗口后才暴露为 mj_step 的 CTRL 警告（跟踪误差由 4.0e-03 m 变为 5.8e-01 m）。
• 修复方法是在 eeJacobian() 与 dampedLeastSquaresIk() 两个调用点各加一行 J.setZero()，修复后两种运行方式的结果逐位一致，该检查后续一直在使用

3-8 逆动力学

• rnea 在本项目中被同一个函数调用了两次，通过不同传参承担不同角色。这是整个控制器设计中最为经济的一处：

说人话：逆动力学这个函数，加速度传零向量，它就只算重力补偿；把期望加速度一起传进去，它顺手把惯性也算上，直接给出完整力矩。所以一个函数干了两件事，不必另外去算质量矩阵。但它不管关节阻尼与摩擦，这两项在仿真里一直生效，得我们自己补回去，不补误差就压不下去。

期望加速度	得到什么	用在哪
a = 0	$C(q,\dot{q})\dot{q}+g(q)$，即补偿项	gravity、task 两个模式
a = a_cmd	完整逆动力学 $M\ddot{q}+C\dot{q}+g$	computed 模式

// 用法一：仅需补偿项。a 传零向量，惯性项 M·0 即为零
b.tau = pinocchio::rnea(b.pin_model, b.pin_data, b.q, b.v,
                        Eigen::VectorXd::Zero(b.pin_model.nv));

// 用法二：完整逆动力学。前馈加速度加 PD 反馈一起传进去
Eigen::VectorXd a_cmd = ad;                                   // 期望加速度前馈
for (size_t k = 0; k < b.arm_joints.size(); ++k) {
  const auto it = std::find(b.joints.begin(), b.joints.end(), b.arm_joints[k]);
  const int p = b.pin_vidx[std::distance(b.joints.begin(), it)];
  a_cmd[p] += Kp[k] * (qd[p] - b.q[p]) + Kd[k] * (vd[p] - b.v[p]);
}
b.tau = pinocchio::rnea(b.pin_model, b.pin_data, b.q, b.v, a_cmd);

• 用法二需要展开说明，它是"计算力矩控制"这一名称的由来
  • 将 $\tau =M{a}_{cmd}+C\dot{q}+g$ 代入运动方程 $M\ddot{q}+C\dot{q}+g=\tau$，两边相减：

$$M(q)\ddot{q}=M(q)a_{cmd}⟹\ddot{q}=a_{cmd}$$

* 即**在计算准确的条件下，该控制器将原本耦合、非线性的机械臂化为 7 个互相独立的二重积分器**
* 因此可对每个关节单独设计 $a_{cmd}$，只需一个简单的 PD：$a_{cmd} = \ddot q_d + K_p(q_d - q) + K_d(\dot q_d - \dot q)$
* 误差方程随之变为标准的二阶系统 $\ddot e + K_d \dot e + K_p e = 0$，$K_p$ 与 $K_d$ 分别对应自然频率与阻尼比

• 此处并未显式计算 $M(q)$

  • 惯性项 $M\ddot{q}$ 由 rnea 一并算出，我们只需传入 $a_{cmd}$
  • 2-5-6 节所说的"控制回路中没调用过 crba"即指此事。常见教材在讲解计算力矩时先计算 $M$ 再求逆，这一步并无必要

• 但 rnea 有一处必须补齐的缺失：它不含关节阻尼与库仑摩擦

  • MJCF 中写有 damping="1"，MuJoCo 每一步都施加 $-1\cdot \dot{q}$ 的阻力，而 rnea 中不含该项
  • 不补齐的后果是：计算出的力矩始终缺少该分量，跟踪误差停在 $10^{-5}$ 量级而不再下降
  • 补法即 2-5-5 节的代码，本项目封装为 addPassiveCompensation()：

// MuJoCo 的关节阻尼在仿真中始终生效，而 rnea() 里没有它，必须显式补回来
for (size_t k = 0; k < b.joints.size(); ++k) {
  const int p = b.pin_vidx[k];
  (*tau)[p] += b.pin_model.damping[p] * b.v[p];
  // 库仑摩擦按速度方向反向。速度很小时不施加，避免在零速附近抖振
  if (std::abs(b.v[p]) > 1e-3)
    (*tau)[p] += b.pin_model.friction[p] * (b.v[p] > 0 ? 1.0 : -1.0);
}

• 这里有一处细节：damping 与 friction 的值取自 Pinocchio 的 Model，而该 Model 由同一份 MJCF 解析而来。之所以不读 mjModel，是为了让整条控制链只依赖一个模型来源：两个引擎各自读取，但读的是同一份文件

3-9 任务空间阻抗画圆

3-9-1 阻抗控制律

• 三块积木已备齐，可以进入画圆环节。控制律采用任务空间阻抗控制：不直接规定关节的运动，而是使末端表现为一个弹簧阻尼系统，目标点移动时末端随之运动

$$\tau =J^T(q)[K_p{e}_x+K_d({\dot{x}}_d-\dot{x})]+C(q,\dot{q})\dot{q}+g(q)$$

• 先补充什么是弹簧阻尼系统。它由一个弹簧与一个阻尼器并联而成：弹簧产生与位移成正比的回复力，阻尼器产生与速度成正比的阻力；上式中括号内的两项正是这两者的合力，$K_p$ 是弹簧刚度，$K_d$ 是阻尼器的黏性系数

说人话：就是让末端更柔顺。位置控制是死盯着目标点不放，这里则让末端像一个挂在目标点上的弹簧：被推开一段就顶回来，推得越远顶得越狠，遇到意外接触会先让一步。代价是正常运行时它也会稍微偏离目标

• 为什么不能只用位置控制：位置控制会不计代价地守住目标点。而末端执行器恰恰要做抓取、按压、插装这类与环境接触的操作，一旦接触，守住位置就等于顶住对方，接触力随偏差迅速上升，轻则抖动、重则损坏工件
• 各项的分工如下：

项	作用
$K_p{e}_x$	位置与姿态误差产生的"弹簧力"，把末端拉向目标
$K_d({\dot{x}}_d-\dot{x})$	速度误差产生的"阻尼力"，抑制振荡
$J^T(\cdot )$	用虚功原理把笛卡尔力搬回关节空间
$C\dot{q}+g$	补偿自重与耦合，让 PD 只需对付扰动

• 与 3-3 节的关节 PD 相比，唯一的本质变化是反馈量在笛卡尔空间计算，再经 $J^T$ 映射回关节空间。PD 与补偿项的结构完全相同

3-9-2 姿态误差的表示

• 误差向量 $e_x\in {\mathbb{R}}^6$ 中，姿态的三个分量需要专门处理。这是本节的核心技术点：

• 位置误差是直接的向量差：

$$e_p=p_d-p\in {\mathbb{R}}^3$$

• 姿态误差不能用该方式。两个旋转矩阵相减没有物理意义，欧拉角差在接近奇异点时又会剧烈跳变。正确做法是在 $SO(3)$ 上计算误差，再用对数映射将其转换为三维向量：

$$e_R=\log \left(R_d{R}^T\right)\in {\mathbb{R}}^3$$

• 其中 $\log$ 是 $SO(3)$ 的矩阵对数，其逆运算为 $\exp$。直观理解如下：

  • $R_d{R}^T$ 是从当前姿态到目标姿态所需的旋转
  • 对该旋转取对数即得到旋转向量：方向为旋转轴，模长为旋转角
  • 这是三自由度姿态误差的一种合理表示，且在 $R_d=R$ 附近光滑，不存在万向节死锁

• 实现上可直接用 Eigen 的 AngleAxis 取得：

// 姿态误差用 SO(3) 对数映射，比欧拉角差更稳
const Eigen::Matrix3d dR = R_des * R.transpose();
const Eigen::AngleAxisd aa(dR);
Eigen::Vector3d e_rot = Eigen::Vector3d::Zero();
if (aa.angle() > 1e-12) e_rot = aa.angle() * aa.axis();   // 角度乘轴即旋转向量

• aa.angle() > 1e-12 这一判据不能省略：旋转角接近零时，AngleAxis 的轴向量未定义（零除零），相乘会得到 NaN

3-9-3 控制律实现

• 将六维误差与六维期望速度拼接后，控制律仅十几行：

Eigen::Matrix<double, 6, 1> err;
err << p_d - p, e_rot;

const Eigen::MatrixXd J = b.eeJacobian();                 // 6 x nv，世界系对齐
Eigen::Matrix<double, 6, 1> xdot = J * b.v;               // 末端当前速度
Eigen::Matrix<double, 6, 1> xdot_d;
xdot_d << v_d, 0, 0, 0;                                   // 姿态不转，期望角速度为零

// 阻抗律：wrench = Kp·e + Kd·(ẋ_d − ẋ)
const Eigen::Matrix<double, 6, 1> wrench =
    Kp.cwiseProduct(err) + Kd.cwiseProduct(xdot_d - xdot);

// 补偿项：重力与科氏，再加关节阻尼与摩擦
b.tau = pinocchio::rnea(b.pin_model, b.pin_data, b.q, b.v,
                        Eigen::VectorXd::Zero(b.pin_model.nv));
b.tau.noalias() += J.transpose() * wrench;
addPassiveCompensation(b, &b.tau);

• 这里的 J 即 3-7 节中带 setZero() 的 eeJacobian()

3-9-4 参数整定与运行结果

• 增益的取法亦有讲究。平移与旋转分别取值，且平移刚度明显大于旋转刚度。圆心与目标姿态也在此处一并确定：

void TaskSpaceImpedance::init(MjPinBridge& b) {
  center = b.eePosition();         // 圆心取初始末端位置，t = 0 时误差项为零
  R_des = b.eeRotation();          // 目标姿态取初始朝向，全程不变
  // 平移刚度 600，旋转刚度只有 60（十分之一）
  Kp << 600, 600, 600, 60, 60, 60;
  Kd << 45, 45, 45, 8, 8, 8;
}

• 前两行正是 3-5 节那套轨迹参数的来源：圆心取当前末端位置，$t=0$ 时 $\cos 0-1=0$ 且 $\sin 0=0$，误差项为零，因此不存在起步冲击；目标姿态取当前朝向且全程不变，对应 3-5-5 节所说的"姿态不随圆变化"

• 这样配置的原因如下：

  • 平移需要跟踪准确，因为画圆考察的是位置轨迹，所以 $K_p$ 取较大值
  • 旋转需要放松，因为在水平面画圆时姿态本就不需要变化，刚度过大会放大雅可比旋转行的噪声，导致工具摆动
  • $K_d$ 大致按 $2\sqrt{K_p}$ 估计，即取临界阻尼附近的值。平移 $2\sqrt{600}\approx 49$，实际取 45；旋转 $2\sqrt{60}\approx 15$，实际取 8，比临界阻尼更软

• 另一个参数是渐入时间。3-5 节的轨迹半径从零增长，该过程需要一个时间尺度：

const double s      = (t_ramp > 0) ? std::min(1.0, t / t_ramp) : 1.0;   // t_ramp 是渐入时间
const double smooth = s * s * (3.0 - 2.0 * s);

• 本期取 t_ramp = 2.0 s。取值过短会在起步时产生较大冲击，过长则整个仿真结束时仍未画完一个圆

• 该参数本质上允许控制器缓慢逼近。阻抗控制的特点即在于此：它允许末端暂时滞后于目标，而不像计算力矩那样强制要求跟踪

• 完整运行 --mode task，末端跟踪结果如下：
  

---------------- 运行结果 ----------------
  步数         : 1501  (仿真 3.002 s, 步长 0.0020 s)
  末端跟踪误差 : 最大 4.000e-03 m, 平均 2.144e-03 m

• 4.000e-03 m 的跟踪误差不是缺陷，而是阻抗控制的固有特性：
  • 阻抗控制中末端位置误差与出力成正比：$F=K_{p}e$。若要产生出力，就必须存在误差
  • 圆周运动需要向心加速度，$a=r{\omega }^2=0.10\times 1.0^2=0.1{\text{m/s}}^2$。末端与负载质量对应的该力除以 $K_p=600$，即得到毫米级的稳态误差
  • 需要更小的误差时可增大 $K_p$，代价是系统更刚硬，遇到意外接触时冲击力也更大。这正是柔顺控制需要权衡的问题
  • 若改用计算力矩控制，误差可压到 $10^{-5}$ 量级，但这是以刚性换取的：一旦遇到未建模的接触，力会失控

3-10 最终效果

• 五个模式全部运行的结果如下（窗口与无窗口的结果逐位一致）：

模式	控制律	关键指标	说明
free	$\tau =0$	—	对照组：不施加力矩，机械臂在重力作用下坠落
gravity	rnea(q,v,0) 加关节 PD	位形漂移 1.776e-15 rad	双精度舍入量级，即完全未移动
computed	rnea(q,v,a_cmd)	关节跟踪误差 2.443e-05 rad（最大）	关节空间正弦轨迹跟踪
task	$J^T$ 阻抗 加 rnea(q,v,0)	末端误差 4.000e-03 m（最大）	末端画圆，毫米级
verify	无控制	全部 PASS	交叉验证，退出码 0

• 前三个模式已在 3-3 节与 3-8 节说明，这里重点考察 task 画圆的结果：

末端跟踪误差 : 最大 4.000e-03 m, 平均 2.144e-03 m

• 半径 0.10 m 的圆，跟踪误差为毫米级，相对误差在 4% 以内。考虑到这是阻抗控制（本身即为带误差工作的控制方式），该结果符合预期
• 该误差还包含两部分不属于控制器的问题：
  • 一部分是圆轨迹渐入阶段（前 2.0 s）的暂态。半径在该阶段持续变化，控制器始终在跟踪一个移动的目标
  • 一部分是 MuJoCo 的数值积分误差。本模型使用的积分器是 implicitfast（panda.xml 中的 <option integrator="implicitfast"/>），时间步为 $2\times 10^{-3}\text{s}$。期望轨迹由连续解析式给出，实际位置由离散步进给出，两者之间必然存在与步长同阶的偏差。其量级可用一个更粗略的标尺估计：若完全不做重力补偿，显式格式在重力作用下每步会累积 $\frac{1}{2}g\Delta t\cdot t$ 的位置偏差，代入 $g=9.81$、$\Delta t=0.002$、$t=1$ 得 0.00981 m。本模型中重力已由前馈补掉，实际离散化偏差小于该值，但仍与毫米级的跟踪误差处于同一数量级

3-11 工程部署

• 控制律至此全部讲完，最后补充工程的编译与运行方式。项目目录结构如下：

mujoco/
├── CMakeLists.txt          # 唯一的构建脚本
├── env.sh                  # 环境：ROS、MUJOCO_DIR、PANDA_DIR 都在这儿
├── empty-scene.xml         # 1-4 节手写的空场景
├── README.md               # 依赖、编译、运行说明
├── src/
│   ├── main.cpp            # 主循环，按 --mode 分发到不同控制律
│   ├── mj_pin_bridge.*     # 双引擎建模、索引映射、被动参数校验
│   ├── controllers.*       # 重力补偿、计算力矩、任务空间阻抗
│   └── viewer.*            # GLFW 窗口与 mjv/mjr 渲染
├── scripts/
│   └── render_check.py     # 离屏渲染自检，确认走的是 GPU
└── third_party/
    └── mujoco_menagerie/   # 1-5 节稀疏克隆下来的模型

• src/ 下的模块划分对应本章的三条线索：

  • mj_pin_bridge 是 3-2 节的主角，两个引擎读取同一份 MJCF、两套下标的对齐方式、惯性参数的处理全部集中在此文件
  • controllers 存放 3-3 节至 3-9 节推导出的控制律，每推出一个公式便对应到该文件中的一个函数
  • viewer 只负责绘制 mjData，与动力学无关，因此单独成层，移除后不影响控制

• CMakeLists.txt 中唯一需要说明的是两个引擎的接入方式，两者的接法完全不同：

# MuJoCo：pip wheel 里没有 CMake 配置文件，必须手动封装成一个 imported target

execute_process(
  COMMAND python3 -c "import mujoco, os; print(os.path.dirname(mujoco.__file__))"
  OUTPUT_VARIABLE MUJOCO_DIR OUTPUT_STRIP_TRAILING_WHITESPACE)

# 用通配符取 .so，把版本号让开

file(GLOB MUJOCO_LIBS "${MUJOCO_DIR}/libmujoco.so*")
list(GET MUJOCO_LIBS 0 MUJOCO_LIB)

add_library(mujoco::mujoco SHARED IMPORTED)
set_target_properties(mujoco::mujoco PROPERTIES
  IMPORTED_LOCATION             "${MUJOCO_LIB}"
  INTERFACE_INCLUDE_DIRECTORIES "${MUJOCO_DIR}/include")

# Pinocchio：ROS 那边带了 CMake 配置，find_package 会把依赖一并带出来

find_package(pinocchio REQUIRED)

target_link_libraries(panda_pin PRIVATE mujoco::mujoco pinocchio::pinocchio)

• 这一差别的原因如下：

  • Pinocchio 只需 find_package。它由 apt 安装，/opt/ros/humble 下带有完整的 CMake 配置，Eigen、urdfdom、Boost 等传递依赖都会自动带出；手工拼 -I 反而容易遗漏 urdfdom_headers 这类头文件
  • MuJoCo 必须自行封装。它由 pip 安装，wheel 中只有 include/ 与 libmujoco.so，不含任何 CMake 配置，因此路径需自行计算，再通过 IMPORTED 告知 CMake
  • 两边指向的是同一个 .so，C++ 与 Python 共用 pip 安装的那一份，版本在结构上不可能不一致
  • MUJOCO_DIR 由 python3 -c 在配置阶段计算得到，并非硬编码路径。升级 MuJoCo 时只需 pip 安装新版本，CMake 无需任何改动

• 编译与运行共三条命令：

source env.sh                # 必须先 source，否则 find_package 找不到 pinocchio
cmake -S . -B build
cmake --build build -j

./build/panda_pin --help     # 列出可用模式

• 程序共五个模式，本章前面各节分别对应其中之一：

模式	做什么
verify	两个引擎的动力学交叉验证，3-2-3 节的判据
free	对照组，不施加力矩，机械臂在重力作用下坠落
gravity	重力与科氏补偿加关节 PD，保持在指定位形
computed	计算力矩，跟踪关节正弦轨迹
task	任务空间阻抗控制，末端画圆

• 五个模式的实际结果在 3-10 节统一对比
• 窗口显示依赖 GLFW（sudo apt install libglfw3-dev）。未安装时仍可编译，只是无法开窗，程序自动退回无窗口模式

3-11-1 窗口模式的 vsync 阻塞

• 部署与运行中还有一处与控制无关、但会被误判为程序卡死的问题，一并记录在此：

• 窗口模式的 vsync 在某些 X 配置下会阻塞 1 秒

  • 现象：无窗口时运行正常，开启窗口后即停滞不动；用 time 计量，仿真 0.5 s 耗时 60 s 以上
  • 排查：在渲染函数中加入计时，发现 glfwSwapBuffers 每次恰好阻塞 1.0000 s。这种整数级数值通常指向超时，而不是计算量过大
  • 根因：glfwSwapInterval(1) 会等待垂直同步。在某些 X 配置下（屏幕未扫描输出、显示器休眠，或根本不存在真实输出的 X server），NVIDIA GLX 的 vblank 等待会走 1 秒超时
  • 后果：整个仿真被拖慢到每秒 4 步，表面上与卡死无异
  • 修法：默认关闭 vsync。主循环本身已用墙钟做 60 Hz 节流，再叠加一层 vsync 属于重复

// 默认关掉 vsync：主循环已经用墙钟做了 60 Hz 节流，再叠一层是重复的。
// 实测中还发现一处问题：某些 X 配置下 NVIDIA GLX 的 vblank 等待会走
// 1 秒超时，于是 glfwSwapBuffers 每次恰好阻塞 1.0000 s。
// 需要 vsync 的话设 PANDA_PIN_VSYNC=1。
const char* vs = std::getenv("PANDA_PIN_VSYNC");
glfwSwapInterval((vs && *vs == '1') ? 1 : 0);

• 至此，传统机械臂这条线收尾。下一期开始进入 VLA：用本期搭建的 MuJoCo 环境采集演示数据，接入 ACT 或 SmolVLA 这类模型，使机械臂由人工设定的控制律转向自主学习运动方式

---

总结

• 本期将传统机械臂的方案补全：仿真用 MuJoCo，控制用 Pinocchio

• MuJoCo 部分包含三方面内容：

  • 定位与选型依据（凸优化的接触求解、广义坐标、与学习生态的衔接）
  • 安装方式（pip wheel 复用给 C++，需锁定 numpy 版本）
  • 核心抽象（mjModel 与 mjData，以及 $nq$ 不一定等于 $nv$）

• Pinocchio 部分从正运动学贯穿到动力学：

  • FK、雅可比、逆运动学、速度与加速度、动力学方程，均配有公式与最小 C++ 片段
  • 三处易错点：Data(model) 的构造、调用 computeFrameJacobian 前必须 setZero()、rnea 不含阻尼与摩擦

• 第三章将两者衔接为完整的控制闭环：

  • 同一份 MJCF 供两个引擎读取，索引全部按名字建立
  • 交叉验证通过，作为可进入控制的验收判据：重力与科氏项在 $10^{-10}$ 量级一致；质量矩阵受 MuJoCo 惯量表示误差限制，判据放宽到 $10^{-7}$
  • 四个控制模式由易到难：无力矩对照、关节 PD 加重力补偿、计算力矩、任务空间阻抗（verify 用于动力学校验，不计入控制模式）
  • 最终在 MuJoCo 中使用 Pinocchio 计算的力矩，使 Panda 末端绘出半径 0.10 m 的圆，跟踪误差为毫米级

• 下一期进入 VLA 部分，基于这套环境采集数据并训练模型

• 如有错误，欢迎指出！感谢观看！