Joint Command Composition

多来源关节命令合成

一个状态的最终关节命令不一定全部来自模型。常见来源包括策略推理、ROS 话题、IK、遥操作、 固定姿态、公式轨迹和安全控制器。框架把“命令如何产生”与“命令如何合并”分开：生产者只 维护自己的具名 JointTargetView，JointCommandComposer 只负责合成。

模型 / 话题 / IK / 轨迹 / 状态逻辑
              -> 多个 JointTargetView
              -> JointCommandComposer
              -> 状态自然 MotorFrame（N）
              -> JointCommandResolver
              -> 当前机器人 MotorFrame（M）

最小示例

下面的状态以 29 关节策略为基础，额外接入两个头部关节。头部缓冲可以由任意生产者更新。

from bxi_example_py_elf3.framework.mod_api import (
    JointCommandComposer,
    JointCommandLayer,
    JointLayout,
    JointTargetBuffer,
)

HEAD_JOINTS = JointLayout(("neck_y_joint", "neck_z_joint"))
STATE_OUTPUT_JOINTS = JointLayout(
    (*POLICY_JOINTS.names, *HEAD_JOINTS.names)
)

self.head_command = JointTargetBuffer(HEAD_JOINTS)
self.head_command.kp[:] = (16.747, 16.747)
self.head_command.kd[:] = (1.066, 1.066)

self.composer = JointCommandComposer(
    STATE_OUTPUT_JOINTS,
    (
        JointCommandLayer("policy", self.policy.output.joints),
        JointCommandLayer("head", self.head_command.view),
    ),
)

控制周期只更新来源缓冲并合成：

self.head_command.position[:] = latest_head_target
frame = self.composer.compose()
ctx.set_motor_target(frame)

compose() 每次返回同一个长期 MotorFrame。来源布局、输出索引和所有权仅在 Composer 构造时检查和编译。

覆盖现有控制源

挥手状态通常需要覆盖基础策略的部分手臂关节。覆盖必须显式声明：

self.composer = JointCommandComposer(
    STATE_OUTPUT_JOINTS,
    (
        JointCommandLayer("policy", self.policy.output.joints),
        JointCommandLayer("wave", self.wave_command.view, override=True),
        JointCommandLayer("head", self.head_command.view),
    ),
)

构造阶段遵守以下规则：

1. 输出布局中的每个关节必须至少被一个 Layer 覆盖。
2. Layer 不得包含输出布局以外的关节。
3. 两个普通 Layer 不得拥有同一个关节。
4. 后置 Layer 只有设置 override=True 才能替换已有所有者。
5. 每个来源必须提供完整的 position、kp 和 kd，Composer 不猜控制增益。

这些检查使拼写错误、遗漏关节和意外抢占在状态进入控制循环前失败。

话题和其他异步来源

Composer 不直接依赖 ROS。状态可以订阅话题，也可以从 Mod Resource、共享内存或 SDK 获取 目标，但不能让 callback 在 Composer 读取时同时改写同一数组。推荐的数据流是：

异步 callback -> 带锁或双缓冲的最新快照
控制周期      -> 把完整快照写入状态 JointTargetBuffer
              -> composer.compose()

状态必须显式决定来源超时后的行为，例如保持最后有效目标、切换固定目标或请求安全状态。 Composer 不会隐式沿用其他 Layer、补零或修改所有权。

与机器人关节数量的关系

Composer 的输出是状态语义，不应根据当前硬件静默改变。例如一个动作语义上控制 31 个关节， 它在 29 关节机器人上仍可输出 31 关节 MotorFrame；最终 Resolver 按名称保留机器人存在的 29 个关节，并对裁掉的两个输出发出一次 warning。

反方向，如果状态只输出 29 个关节而机器人有 31 个，Resolver 会要求平台为另外两个关节 提供 JointCommandDefaults。动态话题或轨迹属于状态命令来源，不能放进固定 Defaults。

性能约束

• Layer 和 JointTargetBuffer 在准备阶段创建，不能每周期创建。
• 来源数组使用 float32 并原地更新。
• Composer 缓存名称映射和输出 MotorFrame。
• compose() 不执行名称查找、日志输出或模型调用。
• advance=False 的 Transition 采样不得推进来源的时间、history 或相位。

本地性能可用以下命令验证：

python3 tools/benchmark/joint_mapping_benchmark.py