GMR

核心思路：提出两阶段的ik求解方法作重定向，比之前更准。也是基于mink的两阶段求解优化

此处两阶段有先后关系。第一阶段求解结果第二阶段使用继续，想到那关于warmup

整体链路：
SMPL-X / BVH / Xsens / FBX 人体数据
→ 统一为 {人体骨骼名: (全局位置, 全局四元数)}
→ 按人体身高、骨段比例、坐标系偏移校准
→ 生成机器人各 link 的 SE(3) 目标
→ mink + MuJoCo 求解带关节/速度约束的 IK
→ 输出机器人 qpos = 根位置 + 根四元数 + 关节角

对比方法：PHC, ProtoMotions

两阶段的意义
以 SMPL-X → G1 为例：
第一阶段高权重跟踪骨盆、左右脚的位置，并主要对腿、躯干、手臂做朝向匹配。第二阶段从第一阶段结果继续求解，加入髋、膝、肩、肘、腕的位置匹配，并提高脚朝向的权重。

落地主要靠人体目标的高度对齐和离线后处理，而非接触优化：
我们发现使用PHC，对于某些序列，这会产生严重（30厘米或更高）的漂浮。我们通过在重定向序列上运行运动学来解决这个问题，在每一帧存储最小身体高度，然后用平均最小身体高度抵消整个运动。其他方法生成的重定向不需要类似的后处理。

人体到目标的校准

每个「数据源 × 机器人」有一份配置，例如 general_motion_retargeting/ik_configs/
smplx_to_g1.json:1。它定义：
人体骨骼到机器人 link 的匹配；位置与旋转的权重；每个骨骼的局部位置/旋转补偿；不同四肢的缩放系数；两阶段 IK 的目标集。

原仓库代码提供一个全局offset手动调整小软件：

导入的是BVH动捕采集的数据，可以选择全身任意一个关节，调整他相对父级x,y,z轴的旋转（deg）
hip是人体基frame，他相对于mujoco世界坐标系的旋转

全局轴转换已由代码自动完成。这个面板额外用于修正每个关节可能存在的固定局部旋转偏差。

关于零位对齐

总结：不处理，第一帧求解ik自己对齐。

关于坐标标定旋转的思考

之前实验经常问题是人带着传感器手臂朝前，机器人手臂乱走。
本质就是坐标系标定，旋转的问题。

人带着tracker，各个关节位置指导，旋转也知道，只是local frame(除世界坐标系外各个关节)传感器的和机器人的对不齐。

但是：

• 通过记录机器人和人的位置，计算RᵀR 很不准
• 实际local frame有不是规则的单位矩阵，没法简单改顺序和机器人对齐

子关节的旋转不会改变“子关节自身的位置”，
但会影响它的子孙关节的位置和姿态。

我的问题
全局轴转换 →人体 FK → 每个关键段的 rot_offset/pos_offset，。BVH 子关节位置本来就是由父关节旋转和 OFFSET 正向计算出来的这两个如何理解？ 我理解：全局轴转换对齐后，对于子关节除了hip外的，local frame原始方向其实不重要关键是相对于父frame怎么移动的。比如原始下local相对父frame x移动，现在我旋转了local frame其实不影响，关键是旋转后孙frame要正确变换即可

回答：
因此：
如果只关心关节点位置，子关节 local frame 的朝向确实不重要。只要父关节姿态和 OFFSET 正确，子关节位置就能正确计算。
如果关心子关节自己的姿态，local frame 就重要，因为 global_rot_child 包含了它的局部旋转。
子关节的旋转不会改变“子关节自身的位置”，但会影响它的子孙关节的位置和姿态。
你说的“旋转 local frame 后，只要孙 frame 正确变换即可”，本质上就是正确的坐标系换基。但换基时必须同时修改：
该关节的局部旋转；
父关节坐标系中的 OFFSET；
所有子孙关节的局部旋转和局部偏移。
不能只把当前关节的 X/Y/Z 交换一下。
对于坐标系换基，如果父 frame 的固定变换是 A_parent，当前子 frame 的固定变换是 A_child，则局部旋转应变为：
R_local,new = A_parentᵀ R_local,old A_child
局部偏移应变为：
offset_new = A_parentᵀ offset_old
这样重新 FK 后，所有关节点的世界位置保持不变，只是每个 frame 的坐标表达方式改变。

总结：Local frame 不标准可以转，因为位置不变当前关节定位就准，只是要相应给后续自关节都转。这样可以把全身local坐标系转成统一朝向处理。

整体求解流程（以smpl-x数据为例）

SMPL-X 全局关节数据
→ 转 numpy
→ 人体尺度调整
→ 姿态/位置补偿
→ 地面偏移
→ 设置 Mink FrameTask 目标
→ table1 IK
→ table2 IK
→ 返回 MuJoCo qpos

一、Mink 实际求解什么

每个 FrameTask 约束一个机器人 body 的位姿：

mink.FrameTask(
    frame_name=frame_name,
    frame_type="body",
    position_cost=pos_weight,
    orientation_cost=rot_weight,
    lm_damping=1,
)

对于第 i 个目标，Mink 内部会计算：

• 机器人 body 当前位姿
• 目标位姿
• 位置误差
• 旋转误差
• 当前 body 对机器人速度的 Jacobian

概念上，每次求解的是一个速度型 QP：

min_v Σᵢ ‖Wᵢ (Jᵢ v − eᵢ / Δt)‖² + λ‖v‖²

其中：

• v：机器人广义速度
• Jᵢ：第 i 个 body 的位姿 Jacobian
• eᵢ：当前 body 到目标 body 的 6D 位姿误差
• Wᵢ：位置和旋转权重
• λ：阻尼项
• Δt：MuJoCo timestep

求出的速度通过：

self.configuration.integrate_inplace(vel, dt)

积分成新的机器人状态。

所以这不是一次性解析 IK，而是：

1. 线性化当前状态；
2. 解一个带约束的速度优化问题；
3. 积分；
4. 重新计算误差；
5. 继续迭代。

---

三、两阶段 IK

对应代码在 motion_retarget.py。

对于 smplx_to_h1.json：

"use_ik_match_table1": true,
"use_ik_match_table2": true

因此每一帧会依次执行：

table1 → 更新 Configuration
       → table2 → 更新同一个 Configuration

每个 table 的执行方式都是：

1. 先求解一次；
2. 计算新误差；
3. 如果误差下降超过 0.001，则继续；
4. 最多额外迭代 10 次。

因此一组 table 最多执行 11 次 IK，两个 table 最多约 22 次求解。

注意它判断的是：

curr_error - next_error > 0.001

这是“误差下降量”阈值，不是“绝对误差小于某个值”。如果第一次求解后误差只下降 0.0005，就会直接停止，即使误差仍然很大。

---

五、人体缩放

配置中所有人体关节的基础缩放大多是 1.1：

"human_scale_table": {
    "pelvis": 1.1,
    "spine3": 1.1,
    "left_hip": 1.1,
    "right_hip": 1.1,
    ...
}

初始化时会计算：

scale_ratio = actual_human_height / 1.8

最终缩放值为：

effective_scale = config_scale × scale_ratio

例如人体高度正好是 1.8 m，则：

effective_scale = 1.1

也就是说，这个配置会把人体骨架整体放大约 10%。这通常是为了补偿：

• SMPL-X 人体模型和 H1 的比例差异；
• body 原点和真实关节中心的差异；
• 机器人机械结构与人体关节位置的差异。

缩放实现不是简单地把所有世界坐标乘一个系数，而是：

scaled_root = root_pos × root_scale

scaled_body =
    scaled_root + (body_pos − root_pos) × body_scale

因此，人体根节点先被缩放，然后其他关节围绕人体根节点进行局部缩放。

---

六、H1 的关节链

H1 XML 的腿部结构是：

pelvis
 ├─ left_hip_yaw_link
 │   └─ left_hip_roll_link
 │       └─ left_hip_pitch_link
 │           └─ left_knee_link
 │               └─ left_ankle_link
 └─ right_hip_yaw_link
     └─ right_hip_roll_link
         └─ right_hip_pitch_link
             └─ right_knee_link
                 └─ right_ankle_link

躯干和手臂：

pelvis
 └─ torso_link
     ├─ left_shoulder_pitch_link
     │   └─ left_shoulder_roll_link
     │       └─ left_shoulder_yaw_link
     │           └─ left_elbow_link
     └─ right_shoulder_pitch_link
         └─ right_shoulder_roll_link
             └─ right_shoulder_yaw_link
                 └─ right_elbow_link

H1 关节角的 XML 顺序是：

left_hip_yaw
left_hip_roll
left_hip_pitch
left_knee
left_ankle

right_hip_yaw
right_hip_roll
right_hip_pitch
right_knee
right_ankle

torso

left_shoulder_pitch
left_shoulder_roll
left_shoulder_yaw
left_elbow

right_shoulder_pitch
right_shoulder_roll
right_shoulder_yaw
right_elbow

配置文件中的字典顺序不决定 qpos 顺序，真正顺序来自 H1 XML。

---

七、人体到 H1 body 的映射

可以将两个 table 总结为：

H1 body	SMPL-X body	table1 位置/旋转	table2 位置/旋转
pelvis	pelvis	100 / 10	10 / 5
left/right_hip_roll_link	left/right_hip	0 / 10	10 / 5
left/right_knee_link	left/right_knee	0 / 10	10 / 5
left/right_ankle_link	left/right_foot	100 / 10	50 / 10
torso_link	spine3	0 / 10	10 / 0
left/right_shoulder_roll_link	left/right_shoulder	0 / 10	50 / 5
left/right_elbow_link	left/right_elbow	0 / 10	10 / 5

权重不是百分比，而是 QP 的代价权重。

例如：

• left_ankle_link 的位置权重为 100，说明脚的位置非常重要；
• left_hip_roll_link 在 table1 中位置权重为 0，只使用方向约束；
• left_shoulder_roll_link 在 table2 中位置权重为 50，说明第二阶段会重点修正肩部空间位置；
• torso_link 在 table2 中旋转权重为 0，因此腰部旋转主要由 table1 完成。

位置误差通常以米为单位，旋转误差以弧度为单位，因此不能直接把 100 和 10 理解为简单的十倍关系。

---

八、为什么目标 body 没有覆盖所有 H1 关节

例如配置中没有直接约束：

• left_hip_yaw_link
• left_hip_pitch_link
• left_shoulder_pitch_link
• left_shoulder_yaw_link

这是因为 FrameTask 约束的是一个 body 的完整世界位姿，而不是单个关节。

例如：

left_shoulder_roll_link

的位姿受到上游的：

shoulder_pitch + shoulder_roll

影响；

left_elbow_link

的位姿又受到：

shoulder_pitch + shoulder_roll + shoulder_yaw + elbow

影响。

因此通过肩部 body 和肘部 body 的位置/方向约束，可以间接求出整条手臂的 4 个关节。

同理，脚踝、膝盖、髋部 body 的组合约束可以间接求出腿部 5 个关节。

---

十、ground_height=-0.05 的实际效果

这是该配置最容易误解的地方。

初始化时：

self.ground = ik_config["ground_height"] * np.array([0, 0, 1])

因此：

ground = [0, 0, −0.05]

之后建立 offset 时：

pos_offset - self.ground

所有原本为 [0, 0, 0] 的位置 offset 会变成：

[0, 0, 0.05]

所以当前代码实际会对每个目标 body 添加一个“沿其局部 z 轴的 5 cm 偏移”，然后再旋转到世界坐标。

这不是把整个人体统一沿世界 z 轴移动 5 cm。

---

十一、当前实现中一个重要的 table2 问题

setup_retarget_configuration() 确实建立了：

self.pos_offsets2
self.rot_offsets2

但在 update_targets() 中始终只调用：

human_data = self.offset_human_data(
    human_data,
    self.pos_offsets1,
    self.rot_offsets1
)

随后 table2 使用的仍然是已经经过 table1 offset 处理过的人体数据。

因此：

• table2 的位置/旋转权重有效；
• table2 的 robot body 映射有效；
• table2 自己的 pos_offset / rot_offset 实际没有被应用。

对于当前 H1 配置影响不算特别明显，因为两组四元数大部分相同，脚踝只是符号相反；但如果以后单独调 table2 的坐标补偿，这些修改不会生效。

---

总结：

smplx_to_h1.json 的设计思路是：

1. 用 table1 先稳定 pelvis、脚踝和全身方向；
2. 用 table2 再补偿髋、膝、肩和脚的位置；
3. 通过人体缩放和固定四元数，把 SMPL-X 坐标系转换到 H1 body 坐标系；
4. 通过 H1 XML 的关节限制保证输出落在可行范围内。

先优化 table1
再单独优化 table2
而不是：
table1 永远严格满足
table2 只在 table1 的可行空间中优化