-
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 某个策略从 Robot Layout 读取的关节与顺序
Model Observation Layout 实际写入模型输入张量的关节与顺序
Model Action Layout 模型动作输出张量的关节与顺序
Policy Action Layout 策略解码或合成后产生的语义动作关节与顺序
State Output Layout 状态合成后的自然 MotorFrame 布局
Hardware Layout 无名称硬件协议要求的固定数组顺序，可选
它们可以同名同序，也可以完全不同。不要用一个全局“控制关节列表”同时表达这些概念。
当前数据流为：
具名机器人状态（任意消息顺序）
-> NamedJointStateSource
-> 稳定 Robot Layout（N）
-> JointInputBinding 按 Policy Observation Layout 选择（P）
-> 按 Model Observation Layout 组织输入（M）
-> 模型推理，返回 Model Action Layout（K）
-> Policy 按 Policy Action Layout 解码/合成（A）
-> 可选 JointCommandComposer 合并多个命令源
-> State Output MotorFrame（S）
-> JointCommandResolver 映射到 Robot Layout（N）
-> Transition 在完整 N 维帧上插值
-> 具名消息直接发布，或映射到 Hardware Layout
P、M、K、A、S 和 N 不要求相等。对直接解码的普通策略，P=M、K=A；局部模型或
带额外程序命令的策略不必相等。
具名消息由 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 关节机器人无法仅靠“裁剪模型输出”运行它。必须换模型，或者由 部署者显式实现有物理意义的虚拟观测源。
contract 必须写模型真实使用的顺序。例如 back-flip、forward-flip 和 history motion 的
ONNX metadata 是 Isaac 顺序，类就声明 ELF3_ISAAC_JOINTS；recover 和 normal 模型是另一
套顺序，类声明相应布局。不要先谎称所有策略都是 ELF3_POLICY_JOINTS，再在 _build_input()
里用 ISAAC_TO_MUJOCO 或 MUJOCO_TO_ISAAC 修正。
default_position、kp、kd、action_scale 也不允许成为四组脱离关节名的裸数组。固定
参数使用一张逐关节表，模型 metadata 参数则必须和经过校验的 joint_names 一起绑定：
from bxi_example_py_elf3.framework.joints import JointParameterSet
PARAMETERS = JointParameterSet.from_rows(
MODEL_JOINTS,
(
# name, default_position, kp, kd, action_scale
("joint_a", 0.0, 50.0, 2.0, 0.25),
("joint_b", 0.1, 40.0, 1.5, 0.20),
),
)
class WalkPolicy(JointPolicy):
joint_contract = PolicyJointContract(
observation=PARAMETERS.layout,
action=PARAMETERS.layout,
)from_rows() 要求每行名称与 MODEL_JOINTS.names 同序且完整，导入时就能发现漏项、重复项
或顺序错误。加载 metadata 时使用 JointParameterSet.from_arrays(MODEL_JOINTS, ...)，但
必须先检查 metadata 的 joint_names 与 MODEL_JOINTS.names 完全一致。若参数来自另一具名
布局，可用 parameters.select(model_layout) 在初始化阶段按名称产生目标参数集。控制周期只
读取已经绑定布局的参数，不做名称查找和换序。
ONNX/RKNN/OpenVINO 返回的是模型张量。Policy 必须知道每个关节字段对应的 Model Action
Layout，再把它解码成具名 JointTargetView；它还可以加入策略自身的确定性逻辑。例如
hello 使用的 no-arm 模型按 ELF3_LOWER_BODY_JOINTS 输入和输出 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、任意顺序、缺省项失败、所有权冲突以及输出缓冲复用。