人体动作重定向原理与实现：GMR、PyRoKi 与 OmniRetarget

把人体动作搬到机器人上，困难不在于读取一段关节角，而在于人体与机器人不是同一套运动学系统。肩宽、腿长、关节轴、自由度和物体尺寸变化后，“姿态看起来像”与“脚不滑、手仍贴着箱子”可能互相冲突。

理解 GMR、PyRoKi 和 OmniRetarget，先要问四件事：优化变量是什么，误差怎样定义，哪些条件必须满足，时间上联合优化多长。 求解器名字只回答其中一部分。

下面先从 FK 和误差定义推导局部求解，再分别看单帧速度级 IK、多帧轨迹优化和交互网格约束。QP、Gauss–Newton、LM 与 SQP 对应不同的局部模型；Rest Pose 是姿态先验，CasADi 是建模与自动微分工具，不能把这些名称当成同一层级的算法排列。

1. 人体关节角不是机器人的监督真值

设机器人浮动基座位姿为 $T\in SE(3)$，关节角为 $q\in\mathbb R^n$，第 $i$ 个连杆的世界位姿为：

重定向先从人体动作构造目标 $\hat T_i$，再求机器人自己的 $(T,q)$。直接设置 $q^{\mathrm{robot}}=q^{\mathrm{human}}$ 一般没有意义：两个模型的同名关节未必有相同转轴、零位和旋转分解顺序。

尺度适配也不是随便乘一个身高比。将人体手、脚位置相对根节点缩放后，要重新检查脚底与地面的关系、手与物体表面的关系。人体和物体如果分别缩放、再分别“落地”，接触点可能已经错位，优化器只是忠实拟合了错误目标。

对于交互任务，建议先在物体坐标系定义手部目标：

这样箱子整体平移时，“手在箱子哪一侧”仍保持稳定。改变箱子尺寸则需要显式重新映射接触位置，不能假定这个相对坐标自动适用。

2. IK 如何变成一个残差问题

位置残差可定义为：

旋转误差应在局部切空间定义，例如：

这里的符号、左右乘约定和 Jacobian 必须一致。四元数 $q$ 与 $-q$ 表示同一旋转，不能直接把四元数四维差值作为全局姿态距离。

将位置、旋转、姿态先验、平滑等残差堆叠：

常见归一化方式是 $r_p=(p-\hat p)/\sigma_p$、$r_R=\phi/\sigma_R$。例如 $\sigma_p=0.02$ m，2 cm 位置误差对应无量纲残差 1；$\sigma_R=0.1$ rad，约 $5.73^\circ$ 误差也对应残差 1。这样讨论权重才不会混淆米与弧度。

PyRoKi 也不需要“同时求 FK 和 IK”两个未知问题。FK 是给定关节状态后的可微计算图；IK 优化器通过 FK 计算 residual，再改变状态。

3. Jacobian 在哪里计算，线性化到底近似了什么

在当前迭代点 $x_k$，有：

其中，$\oplus$ 表示在位姿流形上的增量更新。浮动基座旋转增量通常是三维角速度或旋转向量，不能把四元数分量直接加上任意四维更新。

Jacobian 为“当前状态下，状态小变化会让 residual 改变多少”。它不是整段动作只算一次的常量。一个 2 m 长度尺度的简化机构，若一阶导数约为 2 m/rad，目标相差 2 cm，则局部角度修正约 0.01 rad；远离当前点后，同一线性比例通常不再成立。

图 1：三类方法都要定义目标、求局部模型、更新状态；区别在目标结构、变量范围和约束。

有限差分可以验证解析或自动微分 Jacobian：

应扫描多个 $h$，例如 $10^{-3},10^{-4},10^{-5}$，看误差是否先下降再受舍入影响。碰撞距离在最近点切换处可能不可微，不能在这些位置机械要求光滑函数的误差规律。

4. GMR／Mink：在当前姿态附近求速度级 IK

Mink 的任务一般要求误差按指定速率衰减。使用 $e(q)=f(q)-\hat f$ 的简化记号，可写成：

关节位置和速度限制可以线性化进约束：

实现也可以以 $\Delta q=\Delta t\,v$ 为变量；两种形式的系数要同步变换。不能一边用速度 Jacobian，一边把位置增量直接当速度约束。

展开平方得到二次目标：

这就是 QP 出现的原因，不是因为整个机器人运动学本来就是线性的。Mink 文档 与 GMR 源码 可对应检查任务构造与积分过程。

约束怎样进入求解，而不是事后裁剪

对上面的二次目标，最优点满足 KKT 条件：

其中，$\nu$ 是不等式约束的乘子。未碰到限位时，相应乘子为零；限位激活后，求解器联合调整各自由度，让目标误差与有效约束同时满足这些条件。先求无约束解、再逐关节裁剪，一般不满足带约束问题的驻点条件，其他末端的误差也会随之改变。

碰撞距离也可在当前姿态线性化。设有符号间距为 d，正数表示分离，要求保留安全间距：

例如当前间距 4 mm、安全间距 10 mm，则线性模型要求这次更新至少增加 6 mm。它是局部几何条件，不包含动量、摩擦和接触力；更新幅度过大或最近碰撞点切换时，必须重新计算真实距离。这个区别也是后文 OmniRetarget 同时设置非穿透约束和步长界的原因。

GMR 支持两组匹配任务，分别调用 mink.solve_ik 并继续迭代。两阶段执行不自动等于严格的词典序层级优化：如果第二阶段没有显式锁定第一阶段的最优任务，第一阶段误差仍可能被重新改变。

硬约束也有前提

如果关节已在 1.49 rad、上限 1.50 rad，$\Delta t=0.02$ s，那么局部位置约束要求 $v\le0.5$ rad/s；目标再远也不能越过这个界。

但局部不穿透约束是在当前姿态的近似，单步太大时真实非线性几何仍可能穿透。因此需要步长控制、更新后距离检查，而不是仅看到优化器返回 optimal 就认为整段轨迹物理可执行。

GMR 的匹配参数：当前任务只启用一级

重定向没有策略回报，下面列的是优化代价。本文的 SMPL-X 重定向案例将身体／下肢比例设为 0.9、上肢为 0.75，并固定头 yaw／pitch 为 0；这些是目标生成参数，不是奖励系数。以下 cost 来自该任务的实际匹配表，绝不是 GMR 对所有机器人的统一常数。

点击名称可跳到对应的公式与实现说明，并自动展开。

逐项实现

1
2

task = mink.FrameTask(frame_name="base_link", frame_type="body",
                      position_cost=100.0, orientation_cost=500.0)

1
2
3

position_cost = 80.0
orientation_cost = 200.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 8.0
orientation_cost = 30.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 5.0
orientation_cost = 20.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 4.0
orientation_cost = 10.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 6.0
orientation_cost = 10.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 4.0
orientation_cost = 10.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

1
2
3

position_cost = 3.0
orientation_cost = 10.0
# 配合该末端的 frame transform 和目标 offset 构造 FrameTask

5. 非线性最小二乘：从梯度到 Gauss–Newton

对 $F(x)=\frac12\sum_i r_i(x)^2$ 求导：

Gauss–Newton 忽略第二项，求：

当 residual 很小或自身近似线性时，这个近似通常更合适。它不是“对 objective 只用一阶信息”：线性化 residual 后平方，已经得到一个二次子问题。

实现时应求解线性系统，避免显式计算 inverse(J.T @ J)。正规方程会平方条件数；根据规模和稀疏性可以选择 QR、Cholesky、迭代求解等。公式中的逆矩阵描述的是映射关系，不要求代码显式构造逆矩阵。

LM 为什么会更稳

Levenberg–Marquardt 解：

其中 $D$ 可以是单位阵或与尺度有关的对角阵。$\lambda$ 小时接近 GN，大时接近经过缩放的梯度步。阻尼随局部模型预测与真实下降的吻合程度调整，不能只理解成“永远加一个很大的稳定常数”。

标量例子：$r(x)=x^2-1$，从 $x=0.1$ 出发，GN 给出 $\Delta x=4.95$，直接走到 5.05，远离目标；取 $\lambda=1,D=1$，LM 步长约 0.1904，更新到 0.2904，残差有所下降。它展示的是阻尼的作用，不证明任意问题上某个固定 $\lambda$ 都最好。

6. PyRoKi：当变量跨越多个时刻

一类轨迹重定向目标为：

单帧残差只连接该帧状态，速度项连接两帧，加速度项连接三帧，因此整体 Jacobian 很大，却有可利用的稀疏结构。它不是简单把 GMR 的 QP 换成 LM，收益也不能全部归功于求解器。

PyRoKi 是机器人优化工具；整段优化来自使用它的具体建模方式。某个 PyRoKi 应用可以逐帧工作，GMR 外部也可以增加离线平滑。讨论时必须限定实现。

本文的整段轨迹优化将关节速度上限写成以下残差：

1
2

velocity = (q_now - q_prev) / dt
residual = weight * maximum(abs(velocity) - max_velocity, 0.0)

这是软惩罚，不能说它“保证不超速”。并且 residual 乘 weight 后再平方，目标中的实际权重是 weight**2。如果想让目标系数为 $w$，应该乘 $\sqrt w$。

一个 0.1 rad 的相邻帧差，在 50 Hz 下是 5 rad/s，在 100 Hz 下是 10 rad/s。丢失 FPS 元数据后，再漂亮的速度限制参数也不对应原始运动。

重采样还会改变平滑项的尺度。相邻角度差与关节速度不是同一个量，二阶差分也不直接等于加速度：

对同一段连续动作加密采样，相邻角度差通常减小，而物理速度并没有因此减小。若希望比较相同物理时长内的速度／加速度代价，可按时间积分构造目标：

这是对量纲一致目标的说明，不表示下表每个实现都采用了这个归一化。实际代码可能直接惩罚帧间差；此时修改 FPS 后，必须检查分母、求和方式与权重，不能只保留原来的配置数字。

脚接触项则约束接触期间的脚部运动和几何关系。是否启用自碰撞还要看运行配置：本文的轨迹优化案例将 self-collision 设为 0，不能仅因为框架提供碰撞模块，就宣称该次输出经过自碰撞优化。

基于 PyRoKi 的整段轨迹优化：代价与权重

下面列出整段关键点重定向使用的最小二乘代价。除特别说明外，“残差 ×w”意味着目标内相应系数为 w²（忽略所有项共享的 1/2 约定）；权重没有可跨任务比较的统一单位。

逐项实现

1

r_local = local_geometry_error * 1.0

1

r_global = 3.0 * (robot_points_scaled - target_points_scaled).reshape(-1)

1

r_root = (T_now.inverse() @ T_prev).log().flatten() * 1.0

1

r_smooth = 4.0 * (q_now - q_prev)

1
2

dt = subsample_factor / input_fps
r_speed = 50.0 * np.maximum(np.abs((q_now - q_prev) / dt) - 20.0, 0.0)

1

costs.append(pk.costs.limit_cost(robot_batch, joint_variables, 100.0))

1
2
3

r_foot = 30.0 * contact * np.concatenate([
    ankle_now - ankle_prev, toe_now - toe_prev,
    [ankle_now[2] - toe_now[2]]])

1

r_tilt = contact * (R_foot[2, 2] - 1.0)

1
2
3

r_scale = np.concatenate([(S - 1).ravel(),
                          100 * (S - S.T).ravel(),
                          100 * np.maximum(-S, 0).ravel()])

1

rest_cost_is_appended = False

1

self_collision_weight = 0.0

7. OmniRetarget：保留的是交互关系

只匹配手的世界位置，可能把手放到箱子内部；只匹配人体姿态，可能让脚悬空。OmniRetarget 把人体／机器人与场景采样点的关系写进 interaction mesh，用局部相对结构描述交互，再配合关节和几何约束。OmniRetarget 论文

论文原图：OmniRetarget 作者，Figure 1；未修改。来源：论文／原图。图中“5 Rewards”描述论文方案，不等于本文适配后的搬箱奖励数量。

图中蓝色区域对应 InteractionMeshRetargeter 的几何求解，绿色区域表示改变物体／地形后重新求解参考，紫色区域才是物理控制器训练。数据增强不能只把物体缩放后沿用原手部轨迹，否则交互关系已经被破坏。

论文交互网格原图，来源：OmniRetarget／原图，未修改。

图中的连边说明优化器同时看到身体与环境的局部关系。在代码中，正运动学产生顶点 V，拉普拉斯矩阵 L 将其变成局部坐标，J_L = (L ⊗ I₃) J_V 把关节增量映射到这些坐标的变化。只对每只手独立做位置 IK，不具有这套共享几何残差。

设顶点矩阵 $V$ 包含身体关键点与物体／场景点，拉普拉斯坐标为：

将所有点一起平移 $c$，有 $L(V+\mathbf1c^T)=LV$。因此这种表征天然削弱了整体平移的影响，更突出手、脚与邻近环境的相对关系。它不是自动拥有旋转、尺度和所有接触物理不变性；使用哪个坐标系、如何缩放，仍影响目标。

局部优化可概括为：

同时加入关节限位、足部锁定、非穿透等约束。

代码里的子问题不应一概叫普通 QP

Holosoma 的交互重定向将局部子问题写为：

1
2
3
4
5
6

# 教学化表达；省略尺寸和辅助变量
constraints += [normal_jacobian @ dq >= separation_rhs]
constraints += [q_min <= q + dq, q + dq <= q_max]
constraints += [cp.SOC(step_size, dq)]
problem = cp.Problem(cp.Minimize(quadratic_objective), constraints)
problem.solve(solver=cp.CLARABEL)

cp.SOC(step_size, dq) 表示 $\|\Delta q\|_2\le\Delta$，属于二阶锥约束。因此该子问题是带二次目标的凸锥优化，可以转写为 SOCP；不能严格等同于只有线性约束的标准 QP。

另一处值得注意的是：某些初始化失败分支会移除 SOC 步长限制再尝试求解。这个回退提高找到初始姿态的机会，却弱化了该次局部线性化的步长保护。应检查真实距离和约束残差，而不是把回退后成功当成与原约束等价。

基于 Omni 的普通交互与柔顺交互

普通交互与柔顺交互使用同一类交互重定向基础。柔顺交互额外改变的是执行／训练时的参考及交互处理。重定向让手在几何上贴合箱子，柔顺让真实接触出现偏差时可以退让；这两层不能互相替代。

因此比较 GMR、PyRoKi、OmniRetarget 时，讨论的是数据生成与优化建模；比较普通物体交互与柔顺交互时，讨论的是接触中的闭环行为。不能把后者的接触力下降直接归功于某种离线重定向求解器。

OmniRetarget：基础代价与物体交互约束

以下以箱体交互适配为例，列出优化目标的系数与生效条件。关节限位、非穿透、足部锁定和 SOC 步长界属于约束，不应虚构成一个有限的“奖励权重”。

逐项实现

1
2

r = np.sqrt(23.0) * (laplacian - target_laplacian)
objective = cp.sum_squares(r)

1
2

dq_smooth = q_previous_frame - q_linearization
objective = cp.sum_squares(dq - dq_smooth)

1
2

if floor is not None:
    objective += weight * cp.square(cp.pos(floor - root_z_new))

1
2
3
4

com_new = com_xy + J_com @ dq
for axis in range(2):
    objective += 300 * (cp.square(cp.pos(lo[axis] - com_new[axis]))
                      + cp.square(cp.pos(com_new[axis] - hi[axis])))

1
2

z_new = corner_z + J_z @ dq
objective += 100 * cp.square(z_new)

1
2

n_new_xy = n[:2] + J_normal_xy @ dq
objective += 30 * cp.sum_squares(n_new_xy)

1

base_facing_weight = 0.0

1
2

nominal_cost = w_nominal * cp.sum_squares(q_new[idx] - q_nominal[idx])
zero_center_cost = cp.sum_squares(cp.multiply(np.sqrt(Q_diag), q_new))

原方法与机器人适配的边界

GMR 的核心是 FrameTask 与速度级 IK；该机器人适配增加了身体比例、末端 offset、固定头关节及任务匹配表。PyRoKi 提供自动微分／最小二乘基础设施；整段时间变量、脚接触 residual、尺度先验和速度上限来自本文的轨迹优化建模。OmniRetarget 的 interaction mesh 是论文主线；这里的 CoM 支撑盒、根高度、足底角点与水平项是搬箱任务中的工程扩展，不应全部归为原论文方法。

这些改动共同解决的是“先让几何参考更适合机器人”。它们还没有实时外力状态；柔顺交互在执行阶段增加的导纳参考、夹持状态机和力奖励，见柔顺控制篇。对比实验应分别关闭某个离线代价或在线机制，避免一次更换整个系统后把收益归到单一算法。

8. SQP、Rest Pose 与 CasADi 应放在什么位置

对一般非线性约束问题：

SQP 在当前点构造近似：

其中，$B$ 通常近似拉格朗日函数 Hessian，而不只是 objective Hessian。SQP 的本质不是“支持 sparse 的 QP”；稀疏性是可利用的结构，序列线性化约束与二次建模才是方法核心。

Rest Pose 可以写为：

它解决冗余自由度缺乏偏好的问题，不应大到压过必要的手—物接触。若姿态先验很强，系统可能宁愿把手离开物体也要回到“漂亮”站姿。

CasADi 是表达、自动微分与构建优化问题的工具链；Mink 偏向速度级 IK；PyRoKi 面向可微机器人优化。把它们简单排列成“低级→高级求解器”会混淆软件层级。

GN 中的 $J^TJ$ 同样出现在 SLAM、bundle adjustment、ICP：都是 residual 线性化后的二次近似。机器人重定向的特殊处在于状态流形、接触、关节范围和后续动态可执行性，而不在于这个矩阵公式本身。

9. 怎样做有意义的三方比较

将上述三种建模方式并列比较：

这张表描述具体建模取向，不表示工具只能支持表中的一种方法。做输出质量比较时，固定同一机器人、同一动作与物体尺度、相同 FPS 和评价身体集合，再报告：

1. 几何精度：关键点误差、旋转误差，注明是否做根节点／朝向对齐。
2. 接触质量：接触阶段足滑距离、手—物体相对位置误差、最大穿透深度。
3. 时间质量：关节速度／加速度分位数、超限比例，而非只看播放是否顺滑。
4. 计算代价：每帧耗时、整段耗时、硬件与轨迹长度；离线方法不能和实时系统只比总时间。
5. 动态执行：将结果交给同一控制器，看整段完成率与饱和情况。几何优化不自动满足刚体动力学和摩擦锥。

例如 $q_t$ 很平滑，但支撑脚与质心关系不合理，跟踪策略仍可能摔倒。反过来，控制器足够强也可能掩盖部分参考缺陷，所以几何指标和执行指标应同时存在。

本文比较优化建模与实现机制，尚未在同一机器人、动作和场景上完成三种方法的输出质量基准。

10. 从重定向走向控制器

离线重定向得到的是期望轨迹，不是驱动器命令。ProtoMotions 可以把这些轨迹作为训练数据学习反馈控制，FALCON 一类系统也需要把动作参考与运动控制接口衔接。视觉差异可能来自动作源、坐标规范、控制参数和策略能力，不能一律解释成某种重定向算法更优。

最实用的排查顺序是：先证明 FK 与坐标映射正确，再检查 residual 与约束，再检查时间尺度，最后看闭环执行。否则一次 90 度坐标轴错误，会被误当成一个需要调很多权重的“优化问题”。

阅读依据

• GMR、Mink：任务与差分 IK。
• PyRoKi、ProtoMotions：轨迹重定向与动作数据管线。
• OmniRetarget、官方项目页、Holosoma：交互保持与实现。
• CasADi 文档：建模、自动微分和非线性优化接口。文中的任务适配系数与原方法的默认参数应分别理解。