系统数据流说明

本文用于说明：操作员穿戴外骨骼后，整套系统怎样把人的动作和按键，转换成从臂、腿部、底盘、夹爪等执行动作。

内容按当前 ti5_controller 实现整理，只描述主流程。

1. 系统角色

整套系统可以分成 4 层：

  1. 外骨骼 / 手柄输入层

    • 采集双臂姿态、IMU、扳机、摇杆、面键、拓展键

  2. 主臂节点 ti5_master_controller_node

    • 把外骨骼输入转换成 ROS 话题

    • 负责模式判断、按键逻辑、腿控会话管理

  3. 从臂节点 ti5_follow_controller_node

    • 订阅主臂话题

    • 转换为从臂电机目标，并通过 CAN 下发

  4. 其它执行节点

    • 底盘节点：订阅 rocker_cmd 和部分 control_cmd

    • 夹爪 / 灵巧手节点：订阅 trigger_cmd 和部分 control_cmd

2. 总体数据流

最核心的数据流可以概括为：

备注

外骨骼传感器/按键
→ 主臂节点读取串口数据
→ 主臂节点判断模式、解析姿态和按键
→ 主臂发布关节/控制/摇杆/扳机等话题
→ 从臂或其它执行节点订阅相应话题
→ 从臂节点生成电机目标
→ CAN 下发到各电机
→ 电机反馈经 CAN 返回
→ 从臂发布状态话题
→ 主臂在需要时再利用部分状态做闭环辅助

如果只看“外骨骼控制双臂”的主链路，就是：

备注

外骨骼双臂姿态
→ 主臂 right_arm_joint_cmd / left_arm_joint_cmd
→ 从臂订阅并转换
→ 控制查询指令下发双臂电机
→ 电机反馈刷新从臂状态缓存
→ 从臂发布 right_arm_states / left_arm_states

3. 主臂节点做了什么

主臂节点是整套系统的“输入解释器”和“总协调器”。

它主要完成 5 件事：

3.1 读取外骨骼原始数据

主臂通过串口持续读取外骨骼数据，包括：

  • 左右臂姿态

  • 头部 / 腰部 IMU

  • 扳机量

  • 摇杆量

  • 普通按键

  • 拓展按键

3.2 判断当前模式

主臂根据两只手当前档位，判断系统处于哪种状态：

  • BOTH_1F：遥操主模式

  • BOTH_3F：退出遥操 / 不控制

  • NONE：两手档位不一致的过渡状态

进入或退出遥操时，主臂会发布对应 control_cmd，通知从臂和底盘等节点切换行为。

3.3 把人体动作转成 ROS 话题

主臂会把外骨骼输入拆成几类话题：

  • 双臂动作

    • right_arm_joint_cmd

    • left_arm_joint_cmd

  • 头腰动作

    • head_joint_cmd

    • waist_joint_cmd

  • 腿部动作

    • leg_joint_cmd

  • 扳机

    • trigger_cmd

  • 摇杆

    • rocker_cmd

  • 功能命令

    • control_cmd

  • 按键记录

    • bottom_cmd

其中：

  • *_joint_cmd 是“希望机器人去到哪里”

  • control_cmd 是“系统状态切换或特殊控制命令”

  • bottom_cmd 主要用于记录按键事件，不直接驱动从臂动作

3.4 管理特殊流程

除了正常发关节目标，主臂还负责一些流程控制：

  • 遥操进入 / 退出

  • 单臂暂停 / 恢复

  • 速度档切换

  • 腿控开始 / 结束

  • 腰控暂停 / 恢复

  • 零重力相关命令

  • 录包开始 / 停止

这些都通过 control_cmd 协调。

3.5 在需要时使用从臂状态

主臂通常是“发命令为主”，但在腿控等流程里也会订阅从臂状态，例如：

  • leg_states

它用这些反馈来锁存腿部当前状态，保证腿部逆解是从当前实际姿态继续运动，而不是从假定值开始。

4. 从臂节点做了什么

从臂节点是“ROS 控制层到电机执行层”的桥梁。

它主要完成 4 件事：

4.1 订阅主臂的控制话题

从臂根据配置订阅主臂发布的话题，例如：

  • right_arm_joint_cmd

  • left_arm_joint_cmd

  • head_joint_cmd

  • waist_joint_cmd

  • leg_joint_cmd

  • control_cmd

不同机器人组是否启用，取决于 ti5_follow_config.yaml。

4.2 把目标角转换成当前执行目标

从臂收到主臂的关节话题后，会按各组配置分别处理：

  • 双臂：作为双臂电机目标

  • 头部：按 R/P/Y 语义映射到头部电机

  • 腰部：按配置映射到腰部电机

  • 腿部：当前 leg_joint_cmd 直接按腿电机角处理

也就是说：

  • 主臂发的是“控制语义”

  • 从臂负责把这些语义映射到具体电机顺序和目标值

4.3 通过 CAN 下发给电机

从臂节点内部使用 CanManager：

  • 初始化时扫描电机在哪个 canX

  • 建立 电机 ID -> CAN 口 映射

  • 运行时按映射把命令发到对应 CAN 口

遥操控制时，主下发命令是：

  • 控制查询指令：设置目标位置并读回反馈

所以从臂不是“只发不读”，而是：

  • 一边下发位置目标

  • 一边利用控制查询附带反馈刷新位置缓存

待机或未跟主臂控制时，从臂还会按周期对相应电机组发 主动查询（只读位置），保证 *_states 反馈较新；遥操在控组则主要靠控制查询反馈更新，不再叠加周期主动查询。

4.4 发布从臂状态

从臂会把当前缓存的电机状态重新发布成 ROS 话题，例如：

  • right_arm_states

  • left_arm_states

  • head_states

  • waist_states

  • leg_states

这些状态有两个作用：

  1. 给上层观察和录包使用

  2. 给主臂某些需要反馈的流程使用，例如腿控起始锁存

5. 外骨骼遥操双臂的完整流程

这是最常用的一条主链。

5.1 操作员动作

操作员穿戴外骨骼，在遥操模式下活动双臂。

5.2 主臂生成双臂目标

主臂节点读取外骨骼姿态后：

  • 结合 retargeting 配置

  • 生成右臂 7 关节目标

  • 生成左臂 7 关节目标

并分别发布：

  • /<MASTER_ID>/right_arm_joint_cmd

  • /<MASTER_ID>/left_arm_joint_cmd

5.3 从臂执行双臂命令

从臂节点订阅到这两个话题后：

  • 检查当前是否处于 REMOTE_OPERATION

  • 检查该侧是否被暂停

  • 若允许执行，则转换成对应电机目标

  • 调用 CAN 管理器发送控制查询指令

5.4 电机反馈回流

电机执行后返回反馈帧，从臂 RX 线程接收后：

  • 刷新双臂电机位置缓存

  • 刷新速度缓存

  • 定时发布 right_arm_states / left_arm_states

这样就形成了“主臂发目标，从臂执行并更新状态”的主闭环。

6. 外骨骼控制腿部的完整流程

腿部不是持续跟随人体腿姿，而是“遥操模式下按键触发的离散运动”。

流程如下：

6.1 操作员按下腿控键

当前腿控键来自左右手拓展键，按键映射由主臂配置决定。

例如：

  • 前进

  • 后退

  • 上升

  • 下降

6.2 主臂先请求腿部当前状态

主臂不会立刻发腿目标，而是先发：

  • control_cmd = refresh_leg_states

从臂收到后，会主动读一次腿部当前位置并发布 leg_states。

6.3 主臂锁存当前腿部起点

主臂收到新的 leg_states 后：

  • 锁存当前腿部实际姿态

  • 锁存当前末端姿态

  • 只修改末端位置，不改末端姿态

同时主臂发布：

  • control_cmd = leg_hold_disable

  • control_cmd = set_waist_teleop_paused

表示：

  • 解除腿部 hold，允许腿动

  • 暂停腰部遥操，避免和腿部共享轴冲突

6.4 主臂持续发布腿电机目标

只要按键还按着，主臂就会：

  • 按固定速度积分末端位置

  • 做腿部逆解

  • 发布 leg_joint_cmd

从臂收到后直接按腿电机角执行，经控制查询指令下发。

6.5 结束腿控会话

松开按键后：

  • 主臂停止发布新的腿部运动目标

  • 主臂发 control_cmd = leg_hold_enable

  • 从臂把非共享腿轴置入去使能保持

随后进入约 1 秒冷却期。

冷却结束时：

  • 主臂刷新腰部 IMU baseline

  • 恢复腰部 IMU 缓存更新

  • 恢复腰部话题发布

  • 发 control_cmd = set_waist_teleop_paused(0)

这样腰部控制会从腿控结束后的姿态继续，不会突然跳变。

7. 外骨骼控制底盘的完整流程

底盘走的是另一条支路，不经过从臂 CAN。

流程如下：

7.1 主臂发布摇杆和模式命令

主臂持续发布：

  • rocker_cmd

进入 BOTH_1F 时，还会发布允许底盘控制的命令：

  • control_cmd = enter_chassis_control

7.2 底盘节点直接订阅

底盘节点不经过从臂，而是直接订阅主臂：

  • rocker_cmd

  • control_cmd

7.3 底盘节点生成速度命令

底盘节点把摇杆量映射为：

  • 前后速度

  • 横向速度

  • 角速度

再通过底盘 SDK 直接发送给底盘。

所以底盘控制链路是：

外骨骼摇杆 -> 主臂 rocker_cmd -> 底盘节点 -> 底盘 SDK

它和从臂 CAN 是分开的。

8. 外骨骼控制夹爪 / 灵巧手的流程

夹爪和灵巧手也不经过从臂 CAN 主链。

8.1 扳机控制

主臂持续发布：

  • trigger_cmd

其中：

  • 右扳机对应右手执行器

  • 左扳机对应左手执行器

8.2 执行节点直接订阅

Dex1 或傲意灵巧手节点直接订阅 trigger_cmd：

  • Dex1：把扳机量映射成开合角

  • 傲意灵巧手：把扳机量映射成手指闭合量

8.3 模式切换

若按下对应模式键，主臂会发：

  • aoyihand_toggle_left_mode

  • aoyihand_toggle_right_mode

灵巧手节点据此切换控制模式。

9. control_cmd 在系统里的作用

control_cmd 是整套系统里最重要的“流程控制总线”。

它不表示连续关节目标，而表示：

  • 进入 / 退出某种模式

  • 某个流程开始 / 结束

  • 某个功能暂停 / 恢复

  • 某个参数切换

当前典型用途包括：

  • 进入遥操

  • 进入底盘控制

  • 单臂暂停 / 恢复

  • 速度档切换

  • 腿部状态刷新

  • 腿部 hold 开 / 关

  • 腰部遥操暂停 / 恢复

  • 零重力相关流程

  • 录包开始 / 停止

可以把它理解为：

  • *_joint_cmd 负责“机器人动到哪”

  • control_cmd 负责“系统当前该怎么工作”

10. 用一句话概括整套遥操链路

整个系统的本质是：

操作员通过外骨骼提供姿态、按键、摇杆和扳机输入，主臂节点把这些输入拆成关节目标和流程命令；从臂节点再把关节目标转换成 CAN 电机命令执行，同时把反馈重新发布成状态话题，从而实现双臂、腿部、底盘和末端执行器的协同遥操。