底层运动控制服务

约 1790 字大约 6 分钟

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`	`std::vector<float>`	期望位置数组（7个自由度）

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

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

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

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

字段名	类型	描述
`status_word`	`int16_t`	状态
`pos`	`std::vector<float>`	实际位置
`toq`	`std::vector<float>`	实际力矩（单位：Nm）
`cur`	`std::vector<float>`	实际电流（单位：A）
`error_code`	`int16_t`	错误码（0 表示正常）

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

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

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

字段名	类型	描述
`operation_mode`	`int16_t`	工作模式（如混合环力控（3）、串联PID（4）、ADRC位控模式（5）等）
`pos`	`float`	目标位置（单位：rad 或 m）
`vel`	`float`	目标速度（单位：rad/s 或 m/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-3关节operation_mode需要从模式：200切切换到模式：5（ADRC位控模式）进行指令下发；
腿部1-6关节operation_mode需要从模式：200切切换到模式：3（混合环力控模式）和ADRC位控模式（5）进行指令下发；

`JointCommand` — 所有关节控制命令

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

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`joints`	`std::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 个，腰部 3 个：

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

URDF参考

机器人URDF

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

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