底层运动控制服务

约 4544 字大约 15 分钟

2025-05-29

提供机器人系统底层运动控制服务，通过LowLevelMotionController可以通过话题通信方式实现对机器人的关节进行指令控制和状态获取。

接口定义

LowLevelMotionController 是面向底层开发的运动控制器，支持对手臂、腿、头、腰、手等运动部件的直接控制与状态订阅，以及身体IMU数据的获取。

LowLevelMotionController

项目	内容
函数名	LowLevelMotionController
函数声明	`LowLevelMotionController();`
功能概述	构造函数，初始化低层控制器对象。
备注	构造内部资源。

~LowLevelMotionController

项目	内容
函数名	~LowLevelMotionController
函数声明	`virtual ~LowLevelMotionController();`
功能概述	析构函数，释放资源。
备注	清理底层资源。

Initialize

项目	内容
函数名	Initialize
函数声明	`virtual bool Initialize() override;`
功能概述	初始化控制器，建立底层连接。
返回值	`true` 表示成功，`false` 表示失败。
备注	首次调用必须初始化。

Shutdown

项目	内容
函数名	Shutdown
函数声明	`virtual void Shutdown() override;`
功能概述	关闭控制器，释放底层资源。
备注	配合 Initialize 使用。

SetPeriodMs

项目	内容
函数名	SetPeriodMs
函数声明	`void SetPeriodMs(uint64_t period_ms);`
功能概述	设置控制周期时间（毫秒）。
参数说明	`period_ms`：周期时间，单位为毫秒。
备注	非阻塞接口，控制器内部使用的周期参数，控制关节指令的发送频率。

SubscribeArmState

项目	内容
函数名	SubscribeArmState
函数声明	`void SubscribeArmState(ArmJointStateCallback callback);`
功能概述	订阅手臂关节状态数据
参数说明	callback：接收数据处理函数
备注	非阻塞接口。

PublishArmCommand

项目	内容
函数名	PublishArmCommand
函数声明	`Status PublishArmCommand(const JointCommand& command);`
功能概述	发布手臂控制指令
参数说明	command：目标位置/速度等
返回值	`true` 表示成功，`false` 表示失败。
备注	非阻塞接口。

SubscribeLegState

项目	内容
函数名	SubscribeLegState
函数声明	`void SubscribeLegState(LegJointStateCallback callback);`
功能概述	订阅腿部关节状态数据
参数说明	callback：接收数据处理函数
备注	非阻塞接口。

PublishLegCommand

项目	内容
函数名	PublishLegCommand
函数声明	`Status PublishLegCommand(const JointCommand& command);`
功能概述	发布腿部控制指令
参数说明	command：目标位置/速度等
返回值	`true` 表示成功，`false` 表示失败。
备注	非阻塞接口，发布或订阅时调用。

SubscribeHeadState

项目	内容
函数名	SubscribeHeadState
函数声明	`void SubscribeHeadState(HeadJointStateCallback callback);`
功能概述	订阅头部关节状态数据
参数说明	callback：接收数据处理函数
备注	非阻塞接口。

PublishHeadCommand

项目	内容
函数名	PublishHeadCommand
函数声明	`Status PublishHeadCommand(const JointCommand& command);`
功能概述	发布头部控制指令
参数说明	command：目标位置/速度等
返回值	`true` 表示成功，`false` 表示失败。
备注	非阻塞接口。

SubscribeWaistState

项目	内容
函数名	SubscribeWaistState
函数声明	`void SubscribeWaistState(WaistJointStateCallback callback);`
功能概述	订阅腰部关节状态数据
参数说明	callback：接收数据处理函数
备注	非阻塞接口。

PublishWaistCommand

项目	内容
函数名	PublishWaistCommand
函数声明	`Status PublishWaistCommand(const JointCommand& command);`
功能概述	发布腰部控制指令
参数说明	command：目标位置/速度等
返回值	`true` 表示成功，`false` 表示失败。
备注	非阻塞接口。

SubscribeHandState

项目	内容
函数名	SubscribeHandState
函数声明	`void SubscribeHandState(HandStateCallback callback);`
功能概述	订阅手部状态数据
参数说明	callback：接收数据处理函数
备注	非阻塞接口。

PublishHandCommand

项目	内容
函数名	PublishHandCommand
函数声明	`Status PublishHandCommand(const HandCommand& command);`
功能概述	发布手部控制指令
参数说明	command：手部关节目标位置等
返回值	`true` 表示成功，`false` 表示失败。
备注	非阻塞接口。

SubscribeBodyImu

项目	内容
函数名	SubscribeBodyImu
函数声明	`void SubscribeBodyImu(const BodyImuCallback& callback);`
功能概述	订阅机体 IMU 数据
参数说明	callback：IMU 数据处理函数
备注	非阻塞接口。

类型定义

`SingleHandJointCommand` — 单个手部关节的控制命令

字段名	类型	描述
`operation_mode`	`int16_t`	控制字
`pos`	`vector<float>`	期望位置数组（7个自由度，单位：rad）

灵巧手operation_mode需要从模式：200切换到模式：4启动，并进行指令下发；
灵巧手关节指令范围值：[0-1000]

`HandCommand` — 整个手部控制命令

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`cmd`	`vector<SingleHandJointCommand>`	控制命令数组，依次为左手和右手

`SingleHandJointState` — 单个手部关节的状态

字段名	类型	描述
`status_word`	`int16_t`	状态字
`pos`	`vector<float>`	当前位置（单位：rad）
`toq`	`vector<float>`	当前力矩（单位：Nm）
`cur`	`vector<float>`	当前电流（单位：A）
`error_code`	`int16_t`	错误码

`HandState` — 整个手部状态信息

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`state`	`vector<SingleHandJointState>`	所有手部关节状态（左手、右手顺序）

`SingleJointCommand` — 单个关节的控制命令

字段名	类型	描述
`operation_mode`	`int16_t`	控制模式标识： • 200=准备状态 • 3=混合环力控（位置+力矩） • 4=串联PID（位置环+速度环） • 5=ADRC位控（自抗扰控制）
`pos`	`float`	目标位置（单位：rad）
`vel`	`float`	速度限制（单位：rad/s）
`toq`	`float`	力矩限制（单位：Nm）
`kp`	`float`	位置环比例增益
`kd`	`float`	速度环微分增益

左臂或者右臂1-5关节operation_mode需要从模式：200切换到模式：4（串联PID模式）进行指令下发；
左臂或者右臂6-7关节operation_mode需要从模式：200切换到模式：5（ADRC位控模式）进行指令下发；
头部1-2关节operation_mode需要从模式：200切切换到模式：5（ADRC位控模式）进行指令下发；
腰部1-2关节operation_mode需要从模式：200切切换到模式：5（ADRC位控模式）进行指令下发；
腿部1-6关节operation_mode需要从模式：200切切换到模式：3（混合环力控模式）和ADRC位控模式（5）进行指令下发；

`JointCommand` — 关节控制命令

下肢包含 12 个关节项，上肢 14 个，头部 2 个，腰部 2 个：

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`joints`	`vector<SingleJointCommand>`	所有关节的控制命令

`SingleJointState` — 单个关节的状态信息

字段名	类型	描述
`status_word`	`int16_t`	当前关节状态（自定义状态机编码）
`posH`	`float`	实际位置（高精度编码器读数）
`posL`	`float`	实际位置（低精度编码器读数，用于冗余校验）
`vel`	`float`	当前速度（单位：rad/s 或 m/s，根据关节类型自动切换）
`toq`	`float`	当前力矩（单位：Nm）
`current`	`float`	当前电流（单位：A）
`err_code`	`int16_t`	错误码（如编码器异常、电机过流等，详见错误码对照表）

`JointState` — 关节状态数据

下肢包含 12 个关节项，上肢 14 个，头部 2 个，腰部 2 个：

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`joints`	`vector<SingleJointState>`	所有关节的状态数据

关节电机顺序

头部关节

索引	关节名
0	`joint_hy`
1	`joint_hp`

上臂关节

索引	关节名
0	`joint_la1`
1	`joint_la2`
2	`joint_la3`
3	`joint_la4`
4	`joint_la5`
5	`joint_la6`
6	`joint_ra7`
7	`joint_ra1`
8	`joint_ra2`
9	`joint_ra3`
10	`joint_ra4`
11	`joint_ra5`
12	`joint_ra6`
13	`joint_ra7`

腰部关节

索引	关节名
0	`joint_wr`
1	`joint_wy`

腿部关节

索引	关节名
0	`left hip roll`
1	`left hip yaw`
2	`left hip pitch`
3	`left knee pitch`
4	`left ankle pitch`
5	`left ankle roll`
6	`right hip roll`
7	`right hip yaw`
8	`right hip pitch`
9	`right knee pitch`
10	`right ankle pitch`
11	`right ankle roll`

URDF参考

机器人URDF

底层运动控制机器人状态介绍

机器人底层运动主要是开发关节的三环控制给开发人员进行机器人运动能力的二次开发，基本的控制状态切换机制：