No.5 上肢关节覆盖控制 | 众擎开源平台

1.1 接口概述

上肢关节覆盖控制接口用于实时控制机器人上肢关节的位置、速度和力矩。与运动规划接口（第4章）的"发送一次请求，等待完成"模式不同，覆盖控制需要开发者以固定频率持续发布控制命令，支持五次多项式插值的平滑轨迹生成。

该接口适用于需要在行走过程中实时控制上肢动作的场景，例如挥手、指向、往复运动等。覆盖控制会在 lower_body_balance 模式下与下肢步态并行工作。

1.2 消息字段

通信接口：上肢关节覆盖控制通过以下 ROS2 话题完成。

覆盖命令话题：/joint_override_command，消息类型：interface_protocol/msg/JointOverrideCommand
关节状态反馈：/joint_states，消息类型：sensor_msgs/msg/JointState（ROS2 标准消息）

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

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

PLAIN

std_msgs/Header header
float64 weight
int32[] joint_indices
float64[] position
float64[] velocity
float64[] feed_forward_torque
float64[] torque
float64[] stiffness
float64[] damping

通信接口：

话题名称	消息类型	方向	说明
`/motion/joint_motion_plan/state`	`JointMotionPlanState.msg`	规划器 → 客户端	读取规划器状态
`/motion/joint_override_command`	`JointOverrideCommand.msg`	客户端 → 规划器	发送覆盖控制命令

1.3 前置条件

必须进入 lower_body_balance（下肢平衡）模式
手柄切换：[LB, CROSS_X_DOWN]
发布频率建议为 100 Hz，以保证控制平滑性
joint_indices 中的索引需要与机器人配置一致
stiffness 和 damping 参数需要根据具体关节调整
位置单位为弧度，注意角度转换

1.4 示例：往复运动控制

文件名：upper_joint_override_example.py

本示例展示了如何使用 JointOverrideCommand 消息控制特定关节进行往复运动，内置了一个简单的五次多项式插值算法。

运行命令：

Bash

python3 src/interface_example/scripts/upper_joint_override_example.py

完整 import 块（在下方核心代码之前需引入）：

Python

import sys
from typing import List, Tuple

import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header

from interface_protocol.msg import (
    JointOverrideCommand,  # type: ignore
    MotionState,
)

说明：下方核心代码引用的 QuinticSplineInterpolator、JointTrajectoryPlanner、JointOverridePublisher 三个类定义于 GitHub 脚本 upper_joint_override_example.py，可直接复用。

核心代码：

Python

# 设置控制频率
timer_freq = 100.0  # Hz

# 定义起始和目标位置
start_pos = np.array([0.0, 0.0])    # 弧度
end_pos = np.array([1.57, -1.57])   # 弧度

# 指定要控制的关节索引
joint_indices = [14, 19]

# 设置控制参数
stiffness = np.array([20.0, 20.0])
damping = np.array([1.0, 1.0])

# 生成轨迹（持续5秒）
planner = JointTrajectoryPlanner(frequency=timer_freq)
trajectory = planner.plan(start_pos, end_pos, duration=5.0)

# 创建发布节点并开始往复运动
node = JointOverridePublisher(
    freq=timer_freq,
    trajectory=trajectory,
    joint_indices=joint_indices,
    velocity=np.array([0.0, 0.0]),
    feed_forward_torque=np.array([0.0, 0.0]),
    torque=np.array([0.0, 0.0]),
    stiffness=stiffness,
    damping=damping,
)

rclpy.spin(node)

往复运动会自动在轨迹终点反向执行，形成持续来回运动。

1.5 关键参数说明

消息字段关键参数（JointOverrideCommand.msg）：

参数	说明	调参建议
`joint_indices`	关节索引，根据机器人配置确定	上肢关节通常从 12 开始
`stiffness`	刚度值，影响位置跟踪精度	值越大跟踪越紧，但过高可能导致震荡
`damping`	阻尼值，影响运动平滑性	值越大运动越平滑，但响应变慢
`weight`	覆盖命令权重（示例脚本中默认设为 1.0）	单一覆盖源时设为 1.0

示例脚本参数（JointTrajectoryPlanner / 发布节点）：

参数	说明	调参建议
`duration`	运动持续时间	影响运动速度，时间越短运动越快
`frequency`	控制频率	建议 100 Hz 或更高

1.6 注意事项

确保目标位置在关节的运动范围内
刚度和阻尼参数需要根据实际机器人调试
往复运动会自动在轨迹终点反向执行
使用 Ctrl+C 可以安全退出程序

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