底层运动控制服务

约 4141 字大约 14 分钟

2025-01-27

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

接口定义

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

LowLevelMotionController

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

initialize

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

shutdown

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

set_period_ms

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

项目	内容
函数名	subscribe_arm_state
函数声明	`void subscribe_arm_state(callback)`
功能概述	订阅手臂关节状态数据
参数说明	callback：接收数据处理函数, 签名为 callback(data: JointState) -> None
备注	非阻塞接口。

publish_arm_command

项目	内容
函数名	publish_arm_command
函数声明	`Status publish_arm_command(JointCommand command)`
功能概述	发布手臂控制指令
参数说明	command：目标位置/速度等
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	非阻塞接口。

项目	内容
函数名	subscribe_leg_state
函数声明	`void subscribe_leg_state(callback)`
功能概述	订阅腿部关节状态数据
参数说明	callback：接收数据处理函数, 签名为 callback(data: JointState) -> None
备注	非阻塞接口。

publish_leg_command

项目	内容
函数名	publish_leg_command
函数声明	`Status publish_leg_command(JointCommand command)`
功能概述	发布腿部控制指令
参数说明	command：目标位置/速度等
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	非阻塞接口，发布或订阅时调用。

项目	内容
函数名	subscribe_head_state
函数声明	`void subscribe_head_state(callback)`
功能概述	订阅头部关节状态数据
参数说明	callback：接收数据处理函数, 签名为 callback(data: JointState) -> None
备注	非阻塞接口。

publish_head_command

项目	内容
函数名	publish_head_command
函数声明	`Status publish_head_command(JointCommand command)`
功能概述	发布头部控制指令
参数说明	command：目标位置/速度等
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	非阻塞接口。

项目	内容
函数名	subscribe_waist_state
函数声明	`void subscribe_waist_state(callback)`
功能概述	订阅腰部关节状态数据
参数说明	callback：接收数据处理函数, 签名为 callback(data: JointState) -> None
备注	非阻塞接口。

publish_waist_command

项目	内容
函数名	publish_waist_command
函数声明	`Status publish_waist_command(JointCommand command)`
功能概述	发布腰部控制指令
参数说明	command：目标位置/速度等
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	非阻塞接口。

项目	内容
函数名	subscribe_hand_state
函数声明	`void subscribe_hand_state(callback)`
功能概述	订阅手部状态数据
参数说明	callback：接收数据处理函数, 签名为 callback(data: HandState) -> None
备注	非阻塞接口。

publish_hand_command

项目	内容
函数名	publish_hand_command
函数声明	`Status publish_hand_command(HandCommand command)`
功能概述	发布手部控制指令
参数说明	command：手部关节目标位置等
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	非阻塞接口。

项目	内容
函数名	subscribe_body_imu
函数声明	`void subscribe_body_imu(callback)`
功能概述	订阅机体 IMU 数据
参数说明	callback：IMU 数据处理函数, 签名为 callback(data: Imu) -> None
备注	非阻塞接口。

类型定义

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

字段名	类型	描述
`operation_mode`	`int16_t`	控制模式（如位置、力矩、阻抗等），默认值为0
`pos`	`list[float]`	期望位置数组（7个自由度）

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

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

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

字段名	类型	描述
`status_word`	`int16_t`	状态
`pos`	`list[float]`	实际位置（单位视控制器定义）
`toq`	`list[float]`	实际力矩（单位：Nm）
`cur`	`list[float]`	实际电流（单位：A）
`error_code`	`int16_t`	错误码（0 表示正常）

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

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`state`	`list[SingleHandJointState]`	所有手部关节状态（共两个），依次为左手和右手

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

字段名	类型	描述
`operation_mode`	`int16_t`	工作模式（如位置控制、速度控制、力矩控制等），默认值为200
`pos`	`float`	目标位置（单位：rad 或 m，取决于关节类型）
`vel`	`float`	目标速度（单位：rad/s 或 m/s）
`toq`	`float`	目标力矩（单位：Nm）
`kp`	`float`	位置环控制增益（比例项）
`kd`	`float`	速度环控制增益（微分项）

`JointCommand` — 关节控制命令

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`joints`	`list[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` — 关节状态

字段名	类型	描述
`timestamp`	`int64_t`	时间戳（单位：纳秒）
`joints`	`list[SingleJointState]`	关节状态数组

关节电机顺序

头部关节

索引	关节名
0	`joint_hy`

上臂关节

索引	关节名
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_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

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

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