📹 openarmx_teach | 轨迹录制与回放工具¶

本包提供录制与回放两类脚本，面向 OpenArmX 双臂 + 双夹爪场景。录制阶段从 /joint_states 采样生成统一 YAML；回放阶段自动按关节命名拆分，分别下发到左右臂的 FollowJointTrajectory 与夹爪的 GripperCommand Action，支持多机器人同步广播。

✨ 功能概览¶

长时间录制：record_joint_states_always 按频率采样 /joint_states，手动开始、暂停、保存，支持命名空间。
多控制器并行回放：play_joint_trajectory 自动分组左右臂与左右夹爪，同时执行。
多机器人同步广播：--arm-prefix robot1 robot2 ... 一条命令同时驱动多台机器人，通过同一 asyncio.gather 保证同步触发。
关节过滤：支持指定关节列表、左臂、右臂、双臂或全部关节。
速率缩放：--rate-scale 让轨迹整体加速或减速。
夹爪智能调度：位置均值映射为标量，去噪并压缩微小变化；可用臂的反馈时间触发夹爪动作（--sync-feedback / --sync-margin）。

🔧 依赖与构建¶

cd openarmx_ws
colcon build --packages-select openarmx_teach
source install/setup.bash

依赖 rclpy、control_msgs、trajectory_msgs、PyYAML，通常随 ROS 2 / apt 环境安装。

⚠️ 请先完成编译并 source 工作空间，否则录制与回放命令无法找到。

🚀 典型流程¶

启动硬件或仿真，并确保对应控制器已启动。
示教前把 KP / KD 调到 0，降低阻抗，方便手动拖动。
新开终端开始录制：拖动机器人，按 SPACE 开始，再按 SPACE 暂停，按 w 保存。
回放前恢复默认或更高的 KP / KD，确保电机有足够跟随能力。
回放时先用 --all-joints 验证，再按需要缩小到单臂或夹爪；若夹爪时序重要，可加 --sync-feedback。
如果出现 Warning: Joint 'xxx' not found，检查 YAML 中的 joint_names 是否与当前控制器实际关节名一致。

⚠️ 录制前必须降 KP/KD，回放前必须恢复 KP/KD，这是整套流程里最关键的两个步骤。

1️⃣ 第一步：开启 CAN 通道¶

每次开机后需要初始化 CAN 接口：

📌 多机器人场景下，请确认每台机器人使用独立 CAN 口，不要混用。

sudo ip link set can0 down
sudo ip link set can0 type can bitrate 1000000
sudo ip link set can0 up

sudo ip link set can1 down
sudo ip link set can1 type can bitrate 1000000
sudo ip link set can1 up

# 多机器人时继续初始化 can2、can3...
sudo ip link set can2 down
sudo ip link set can2 type can bitrate 1000000
sudo ip link set can2 up

sudo ip link set can3 down
sudo ip link set can3 type can bitrate 1000000
sudo ip link set can3 up

2️⃣ 第二步：启动机器人¶

单机器人（不带重力补偿）¶

ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    control_mode:=mit

单机器人（带重力补偿）¶

💡 重力补偿可消除 MIT 模式下因重力引起的约 6° 稳态误差，精度要求较高时建议开启。

ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    control_mode:=mit \
    enable_forward_effort:=true

多机器人（不带重力补偿）¶

每台机器人在独立终端启动，通过 arm_prefix 区分命名空间：

📌 arm_prefix 会影响后续录制、参数设置和回放命令中的命名空间，请保持前后一致。

# 机器人 1（终端 1）
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    arm_prefix:=robot1 control_mode:=mit

# 机器人 2（终端 2）
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can2 left_can_interface:=can3 \
    arm_prefix:=robot2 control_mode:=mit

多机器人（带重力补偿）¶

# 机器人 1
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    arm_prefix:=robot1 control_mode:=mit enable_forward_effort:=true

# 机器人 2
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can2 left_can_interface:=can3 \
    arm_prefix:=robot2 control_mode:=mit enable_forward_effort:=true

3️⃣ 第三步：调节 KP / KD（示教前必做）¶

⚠️ 示教录制前需将 KP / KD 调为 0，使机器人进入零阻抗状态，方便手动拖动。

在 RViz 中添加面板：Panels -> Add New Panel -> KPKDPanel -> OK

单机器人¶

命名空间输入框留空，选择“双臂”，将 KP / KD 滑轨拖到最左（0），点击应用KP/KD到所有关节。

必做：确认 KP=0、KD=0 后再开始拖动机器人。

多机器人¶

在命名空间输入框填入对应机器人的前缀，例如 robot1，分别对每台机器人操作。

关于前馈补偿与示教¶

💡 启用了重力补偿（enable_forward_effort:=true）时，KP / KD 置零后机器人会更“跟手”。如需降低这种辅助，可在不重启节点的情况下临时关闭前馈：

# 单机器人
ros2 param set /gravity_comp_node enable_compensation false

# 多机器人（robot1）
ros2 param set /robot1/gravity_comp_node enable_compensation false

示教完成后恢复：

ros2 param set /gravity_comp_node enable_compensation true

# 或
ros2 param set /robot1/gravity_comp_node enable_compensation true

4️⃣ 第四步：录制动作¶

单机器人¶

ros2 run openarmx_teach record_joint_states_always --rate 10 --outfile demo.yaml

多机器人（指定前缀）¶

# 录制 robot1 的动作
ros2 run openarmx_teach record_joint_states_always --arm-prefix robot1 --rate 10 --outfile demo_robot1.yaml

默认输出文件名为 joint_states_stream_YYYYMMDD_HHMMSS.yaml。采样频率 --rate（Hz）会决定 time_from_start 的步长，计算方式为 (i+1)*dt。

✅ 推荐录制频率：10-20 Hz。常规示教用 10 Hz 或 20 Hz 即可。

⌨️ 键盘控制¶

按键	功能
`SPACE` / `p`	开始 / 暂停录制
`c`	清空缓存（需确认）
`w`	保存并退出（需确认）
`q`	不保存退出

🧾 录制参数¶

参数	说明
`--rate`	采样频率（Hz），默认 10
`--outfile`	输出文件路径，默认自动生成带时间戳的文件名
`--arm-prefix`	机器人命名空间前缀，多机器人时使用，如 `robot1`
`--topic`	直接指定话题名，优先级高于 `--arm-prefix`

joint_names 采用首次收到消息时的顺序，后续每一帧都会重排到同一顺序后再写入文件。

📌 如果你只想录某一台机器人，优先使用 --arm-prefix，这样不容易选错话题。

5️⃣ 第五步：恢复 KP / KD（回放前必做）¶

⚠️ 录制完成后，回放前必须恢复 KP / KD，否则电机无力，轨迹无法正常执行。

在 KPKDPanel 中点击所有恢复默认按钮，再点击应用KP/KD到所有关节。

默认 KP / KD 为安全起见设得较低。若需要更精准地跟随录制路径，可适当调大 KP / KD，或切换到 CSP 位置模式（control_mode:=csp）。

⚠️ CSP 精度更高，但负载能力约 3 kg，带载不当可能损坏电机。

6️⃣ 第六步：播放动作¶

单机器人¶

ros2 run openarmx_teach play_joint_trajectory demo.yaml --all-joints

多机器人同步播放¶

ros2 run openarmx_teach play_joint_trajectory demo.yaml --all-joints --arm-prefix robot1 robot2

所有机器人的轨迹通过同一个 asyncio.gather 同时发出，保证多机器人动作整齐同步。

✅ 首次回放建议先用 --all-joints 全量验证，确认无误后再切换到单臂或夹爪测试。

默认 Action 名称：

左臂：/left_joint_trajectory_controller/follow_joint_trajectory
右臂：/right_joint_trajectory_controller/follow_joint_trajectory
左夹爪：/left_gripper_controller/gripper_cmd
右夹爪：/right_gripper_controller/gripper_cmd

🎯 常用参数¶

参数	说明
`--all-joints`	自动分组所有关节，同时发给左右臂和夹爪
`--left-arm`	仅播放左臂
`--right-arm`	仅播放右臂
`--both-arms`	仅播放双臂，不含夹爪
`--joints <list>`	自定义关节子集
`--arm-prefix`	命名空间前缀，支持多个，如 `robot1 robot2`
`--rate-scale`	速率缩放，`>1` 加速，`<1` 减速，默认 `1.0`
`--sync-feedback`	用臂轨迹反馈时间驱动夹爪调度，提升同步精度
`--sync-margin`	反馈时间加上 margin 大于等于目标时间时触发夹爪，单位秒
`--action <name>`	单控制器模式；名称中包含 `gripper` 时使用 `GripperCommand`

▶️ 播放示例¶

# 50% 速度播放，并启用夹爪同步
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --all-joints --rate-scale 0.5 --sync-feedback

# 仅播放左臂
ros2 run openarmx_teach play_joint_trajectory demo.yaml --left-arm

# 单控制器模式，仅测试左夹爪
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --action /left_gripper_controller/gripper_cmd \
    --joints openarmx_left_finger_joint1 openarmx_left_finger_joint2

# 三台机器人同步播放
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --all-joints --arm-prefix robot1 robot2 robot3

🗂️ YAML 格式¶

joint_names: [openarmx_left_joint1, openarmx_left_joint2, ..., openarmx_right_joint1, ...]
points:
  - positions: [0.1, 0.2, ...]
    time_from_start: 0.1
  - positions: [0.15, 0.25, ...]
    time_from_start: 0.2

⚠️ 使用注意¶

time_from_start 由采样频率推导，不是真实执行时间戳；若控制器启动较慢或存在延迟，可适当调整 --rate-scale 或 --sync-margin。
多机器人播放时，--arm-prefix 的顺序不影响同步性，所有机器人仍通过同一 asyncio.gather 同时触发。
录制频率过低会让轨迹稀疏；过高会增大文件体积，且对夹爪意义有限，推荐 10-20 Hz。
夹爪点位在发送前会自动压缩微小或过密变化，录制时尽量保持清晰的开合动作。
回放前确保 Action Server 已启动且名称匹配默认配置，否则会快速报错或超时。