-
Notifications
You must be signed in to change notification settings - Fork 12
Joint Layout And Mapping
BXI 不把“数组第几个元素”当作跨模块协议。模型、状态、机器人消息和硬件接口都用
JointLayout 声明名称与顺序，名称是关节的语义主键；数字索引只在布局首次绑定时编译。
这套机制同时解决以下部署问题：
- 31 关节机器人继续运行旧 29 关节策略。
- 31 关节状态输出部署到只有 29 个执行器的机器人。
- 模型输入顺序、状态消息顺序和硬件固定顺序不同。
- 一个状态由模型、话题、IK、轨迹等多个命令源共同组成。
- Transition 在不同关节数量的状态之间安全插值。
Incoming Message Layout 状态消息本次携带的名称与顺序
Robot Layout 本次进程中机器人稳定的完整关节集合
Policy Observation Layout 某个策略实际读取的关节与模型顺序
Policy Action Layout 某个策略产生的语义动作关节与顺序
State Output Layout 状态合成后的自然 MotorFrame 布局
Hardware Layout 无名称硬件协议要求的固定数组顺序，可选
它们可以同名同序，也可以完全不同。不要用一个全局“控制关节列表”同时表达这些概念。
当前数据流为：
具名机器人状态（任意消息顺序）
-> NamedJointStateSource
-> 稳定 Robot Layout（N）
-> JointInputBinding 按 Policy Observation Layout 选择（M）
-> 模型推理
-> Policy 按 Policy Action Layout 解码（K）
-> 可选 JointCommandComposer 合并多个命令源
-> State Output MotorFrame（S）
-> JointCommandResolver 映射到 Robot Layout（N）
-> Transition 在完整 N 维帧上插值
-> 具名消息直接发布，或映射到 Hardware Layout
M、K、S 和 N 不要求相等。
具名消息由 NamedJointStateSource 归一化。第一条合法消息建立稳定 Robot Layout；后续消息
即使顺序改变，也会按名称重排到同一个长期 JointStateBuffer。本次进程运行期间关节集合
应保持不变；机器人从 29 改成 31 个物理关节后应重新启动部署进程，让首帧建立新布局。
每个策略用类声明自己的输入和输出契约：
class WalkPolicy(JointPolicy):
joint_contract = PolicyJointContract(
observation=WALK_OBSERVATION_JOINTS,
action=WALK_ACTION_JOINTS,
)JointInputBinding 在第一次看到 Robot Layout 时按名称编译 observation 索引：
- Robot 与 observation 名称同序：直接复用源 view，不复制。
- Robot 包含更多关节或顺序不同：写入策略长期复用的选择缓冲。
- Robot 缺少策略必需的观测关节：立即报错，不会伪造位置或速度。
因此 31 关节状态可以安全输入旧 29 关节策略；反过来，如果一个 31 关节模型的 observation 确实需要两个头部关节，29 关节机器人无法仅靠“裁剪模型输出”运行它。必须换模型，或者由 部署者显式实现有物理意义的虚拟观测源。
ONNX/RKNN/OpenVINO 返回的是模型张量。Policy 负责把张量解码成具名 JointTargetView，还
可以加入策略自身的确定性逻辑。例如 hello 使用的 no-arm 模型只直接控制 15 个关节，
Policy 用模型目标和自己的规则形成 29 关节策略输出。
State 还可以继续加入不来自模型的命令：
模型控制15
+ Policy内部控制14
= Policy Action 29
Policy 29
+ 挥手命令覆盖右臂3
+ 头部话题/IK/轨迹控制2
= Hello State Output 31
这种合成使用 JointCommandComposer。每个生产者维护自己的 JointTargetBuffer；Composer
只读取当前 position/kp/kd，不关心来源是模型、话题、IK还是公式。普通 Layer 不能与已有
所有者重叠，只有显式 override=True 的 Layer 可以覆盖已有命令。
完整示例见多来源关节命令合成。
状态把自然 MotorFrame 交给 ctx.set_motor_target()。JointCommandResolver 按名称处理，
不依赖“前29个”或“最后两个”这样的数组假设。
| State Output 与 Robot Layout 的关系 | 处理方式 |
|---|---|
| 名称和顺序完全相同 | 直接使用状态帧 fast path |
| 关节集合相同但顺序不同 | 按名称重排到完整 Robot Frame |
| 状态输出少于机器人 | 用显式 JointCommandDefaults 补齐缺少的硬件关节 |
| 状态输出多于机器人 | 只保留机器人存在的名称，首次绑定该布局时 warning 一次 |
| 两边各有独有名称 | 裁剪状态独有名称，并用 Defaults 补齐机器人独有名称 |
映射第一次遇到某个 State Output Layout 时编译并缓存。后续周期只执行 NumPy 数组写入， 不会重复查名称或输出 warning。
旧状态只输出 29 个身体关节，而新机器人还有两个头部关节。平台必须声明固定安全兜底：
from bxi_example_py_elf3.framework.joints import (
JointCommandDefaults,
JointDefault,
)
ROBOT_COMMAND_DEFAULTS = JointCommandDefaults(
{
"neck_y_joint": JointDefault(position=0.0, kp=16.747, kd=1.066),
"neck_z_joint": JointDefault(position=0.0, kp=16.747, kd=1.066),
}
)缺少任一默认项时框架停止该控制周期并说明缺失名称。它不会猜测 position/kp/kd、自动补零 或沿用上一帧。
JointCommandDefaults 只表示“该状态没有任何来源控制这个硬件关节时”的固定平台兜底。
话题、IK、正弦轨迹等动态命令应作为 State 的命令 Layer，不能写进 Defaults。
如果状态语义上输出 31 个关节，而机器人只有 29 个，Resolver 保留名称交集并忽略机器人 不存在的两个输出，同时通过 ROS logger 发出一次 warning：
state/policy MotorFrame contains joints that are absent from the robot layout
and will be ignored: ('neck_y_joint', 'neck_z_joint')
这只解决输出投影。模型若在输入侧也依赖缺失关节，仍会在 JointInputBinding 阶段失败。
状态完整输出全部关节时不读取 Defaults，也不发生裁剪。比如新的31关节模型或完成了头部
命令合成的 HelloState，都会直接进入完整布局 fast path。
状态切换时不能直接对两个自然帧的裸数组插值，因为两端可能关节数和顺序不同。 Transition 会先分别执行：
ctx.resolve_motor_frame(natural_from, full_from)
ctx.resolve_motor_frame(natural_to, full_to)两端都成为完整 Robot Layout 后，才混合 qpos、kp 和 kd。因此：
- 29 关节状态切到 31 关节状态时，新增关节从平台默认目标平滑进入新状态目标。
- 31 关节状态切到 29 关节状态时，新增硬件关节平滑回到默认目标。
- 在 29 关节机器人上，31 关节状态的额外输出在插值前已经被裁剪。
-
advance=False采样不能推进模型 history、话题消费游标或轨迹相位。
最终 Framework 输出已经是完整 Robot Layout。
下游消息携带关节名时直接发送：
publish(
names=frame.layout.names,
position=frame.qpos,
kp=frame.kp,
kd=frame.kd,
)如果 SDK、共享内存或 CAN 协议只有固定数组，必须另外声明 Hardware Layout：
encoder = FixedOrderJointCommandEncoder(HARDWARE_JOINTS)
hardware_target = encoder.encode(named_target)关节零偏、方向和比例属于 JointCalibration。推荐顺序是先从 Robot Layout 映射到 Hardware
Layout，再把语义角度转换成硬件单位。不要把硬件编号写进 Policy 或 State。
| 信息 | 原因 | 处理 |
|---|---|---|
source joint layout is missing joints |
Robot 状态缺少策略 observation | 检查消息名称、模型契约或更换模型 |
do not cover the complete composer output layout |
State 声明的输出关节没有命令来源 | 增加 Layer 或缩小 State Output Layout |
ownership conflicts |
两个 Layer 未授权地控制同一关节 | 明确所有者；确需覆盖时设置 override=True
|
no explicit JointCommandDefaults |
State 少于 Robot，且硬件关节没有兜底 | 在平台层增加安全 JointDefault
|
will be ignored |
State 输出了 Robot 不存在的关节 | 确认跨机器人部署符合预期；该布局只警告一次 |
robot joint layout changed after startup |
Runtime 接收到不同的稳定 Robot Layout | 停止控制并重新建立部署布局 |
-
JointLayout在类定义、准备阶段或首帧创建，不在控制周期创建。 - observation、Layer、Resolver 和 Hardware 映射只在布局首次出现时编译。
- 所有输入、选择、合成和最终输出缓冲长期复用。
- 控制周期不执行
list.index()、字典名称查找、深拷贝或数组创建。 - 相同布局走零复制或直接帧 fast path。
- Warning 只在新布局首次编译时输出，不进入稳定热路径。
一键测量观测选择、命令补齐、输出裁剪、多来源合成、重排和 fast path：
python3 tools/benchmark/joint_mapping_benchmark.py当前测试还覆盖 29→31、31→29、任意顺序、缺省项失败、所有权冲突以及输出缓冲复用。