ROS2 Interface 开发•No.4 上肢关节运动规划

ROS2 接口开发

更新于：2026年8月17日

1.1 接口概述

上肢关节运动规划接口用于批量执行关节运动规划任务，支持队列化管理和自动执行。开发者可以在 YAML 配置文件中定义多个运动任务（如伸手、握手、复位等），程序会按顺序加载并执行每个任务，同时监控执行状态。

该接口适用于需要编排多个上肢动作序列的场景，与关节覆盖控制（第5章）的区别在于：运动规划是"发送一次请求，等待执行完成"，而覆盖控制是"持续高频发布命令"。

1.2 消息字段

通信接口：上肢关节运动规划通过以下 ROS2 话题完成。

  • 规划请求话题：/joint_motion_plan_request，消息类型：interface_protocol/msg/JointMotionPlanRequest
  • 规划状态话题：/joint_motion_plan_state，消息类型：interface_protocol/msg/JointMotionPlanState

本节后续给出消息字段的详细定义。

JointMotionPlanRequest.msg 字段定义（来自 GitHub）：

PLAIN
## request_type constants
uint8 REQUEST_PLAN_EXECUTE=0    # Plan and execute
uint8 REQUEST_CANCEL=1          # Cancel specified request_id
uint8 REQUEST_RESET=2           # Reset to default pose

int32 request_id
uint8  request_type
bool      use_gravity_compensation
int32[]   joint_indices
float64[] target_positions
float64[] target_velocities
float64   execution_time
float64[] stiffness
float64[] damping

JointMotionPlanState.msg 字段定义：

PLAIN
uint8 STATUS_DISABLED = 0
uint8 IDLE = 1
uint8 EXECUTING = 2
uint8 EXITING = 3

int32 request_id
uint8  status
float64 progress          # 0.0~1.0

通信接口：

话题名称消息类型方向说明
/motion/joint_motion_plan/requestJointMotionPlanRequest.msg客户端 → 规划器发送运动规划请求
/motion/joint_motion_plan/stateJointMotionPlanState.msg规划器 → 客户端接收规划器状态和执行进度

1.3 请求类型与状态常量

请求类型常量：

常量值说明
REQUEST_PLAN_EXECUTE0正常运动规划，执行到目标位置
REQUEST_CANCEL1取消指定 request_id 的任务
REQUEST_RESET2重置到默认姿态，忽略大部分参数

状态常量：

常量值说明
STATUS_DISABLED0禁用状态
IDLE1空闲状态，可以接收新请求
EXECUTING2执行中
EXITING3退出中

1.4 前置条件

  • 必须进入 lower_body_balance（下肢平衡）模式，上肢处于低阻尼步态状态
  • 手柄切换：[LB, CROSS_X_DOWN]
  • 请求只需要发送一次，规划器会自动执行
  • request_id 必须递增（当前 ID + 1），可通过订阅状态话题获取当前 ID

1.5 YAML 配置文件格式

运动规划任务通过 YAML 配置文件定义。配置文件包含全局关节索引和任务队列两部分，不同机型的关节索引和参数有差异。YAML 中的 duration 字段对应 JointMotionPlanRequest.msg 中的 execution_time 字段，表示期望执行时间（单位：秒）。

PM01 配置示例（握手动作）：

YAML
motion_plan:
  # 全局关节索引，上肢从 12 开始
  - joint_indices: [12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23]

  # 任务1：伸出右手
  - motion: extend_right_hand   # 注意：该示例已使用正确拼写 extend
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [200.0]           # 腰部关节
      - [10.0, 10.0, 5.0, 10.0, 5.0]  # 左侧上肢
      - [10.0, 10.0, 5.0, 10.0, 5.0]  # 右侧上肢
      - [100.0]           # 头部关节
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0]

  # 任务2：收回右手
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [200.0]
      - [10.0, 10.0, 5.0, 10.0, 5.0]
      - [10.0, 10.0, 5.0, 10.0, 5.0]
      - [100.0]
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0]

以下为 T800 开发版和 T800 Pro 的配置示例，与 PM01 的差异主要体现在 joint_indices、stiffness 和 damping 值上：

T800 开发版配置示例（13 个上肢关节，含头部俯仰+偏航）：

YAML
motion_plan:
  # 全局关节索引，T800 开发版上肢含头部共 13 个关节
  - joint_indices: [12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24]

  # 任务1：伸出右手
  - motion: extend_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [400.0]           # 腰部关节
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # 左侧上肢
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # 右侧上肢
      - [100.0, 100.0]    # 头部关节（俯仰+偏航）
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

  # 任务2：收回右手
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [400.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [100.0, 100.0]
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

T800 Pro 配置示例（关节索引跳跃，灵巧手占据中间索引）：

YAML
motion_plan:
  # T800 Pro 关节索引跳跃（灵巧手占据中间索引），上肢共 13 个关节
  - joint_indices: [12, 13, 14, 15, 16, 17, 27, 28, 29, 30, 31, 41, 42]

  # 任务1：伸出右手
  - motion: extend_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [500.0]           # 腰部关节
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # 左侧上肢
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # 右侧上肢
      - [100.0, 100.0]    # 头部关节
    damping:
      - [10.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

  # 任务2：收回右手
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [500.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [100.0, 100.0]
    damping:
      - [10.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

机型差异对比：

机型关节索引腰部刚度腰部阻尼
PM01[12~23]，共 12 个200.03.0
T800 开发版[12~24]，共 13 个400.03.0
T800 Pro[12, 13, 14, 15, 16, 17, 27, 28, 29, 30, 31, 41, 42]，共 13 个500.010.0

配置文件第一项必须包含 joint_indices，target_positions 数量必须与 joint_indices 数量一致。RESET 类型请求会忽略大部分参数，将关节重置到默认位置。

1.6 示例：多任务队列执行

文件名：joint_multiple``_motion_plan_example.py

运行命令：

Bash
# PM01
python3 src/interface_example/scripts/joint_multiple_motion_plan_example.py -f src/interface_example/config/pm01/motion_plan_shake_hand.yaml

# T800 开发版
python3 src/interface_example/scripts/joint_multiple_motion_plan_example.py -f src/interface_example/config/t800/motion_plan_shake_hand.yaml

# T800 Pro
python3 src/interface_example/scripts/joint_multiple_motion_plan_example.py -f src/interface_example/config/t800pro/motion_plan_shake_hand.yaml

程序流程：从配置文件加载任务队列 → 按顺序发送运动规划请求 → 监控执行状态 → 处理任务完成和错误情况。任务会按顺序执行，不能并行。

1.7 注意事项

  • 必须进入 lower_body_balance 模式
  • request_id 必须递增（当前 ID + 1），如果状态不同步，等待下一个状态消息
  • 配置文件第一项必须包含 joint_indices
  • target_positions 数量必须与 joint_indices 数量一致
  • RESET 类型请求会忽略大部分参数，将关节重置到默认位置
  • 任务执行失败不会自动重试，需要手动处理
  • 使用 Ctrl+C 可以随时中断程序

📎 本章涉及的开源仓库源码文件（点击文件名跳转 GitHub）：JointMotionPlanRequest.msg · JointMotionPlanState.msg · joint_multiple_motion_plan_example.py