高层全身协调与控制接口
约 5618 字大约 19 分钟
开发者可通过标准的ROS2话题(Topic)、服务(Service)和动作(Action)机制,便捷地获取机器人状态、发送控制指令,实现复杂任务的编程与集成。
Topic:订阅式话题,接收方订阅某个消息,发送方根据订阅列表向接收方发送消息,主要用于中高频或持续的数据交互。
Service:问答式服务,通过请求实现数据获取或操作。用于低频或功能切换时的数据交互。
Action:持续式动作,用于长时间运行的任务,支持任务执行过程中的状态反馈和取消操作,适用于复杂动作控制和任务执行。
高层全身协调与控制接口用于配置机器人运行参数(TCP坐标系/负载/速度)并执行笛卡尔空间运动(关节运动/直线运动/轨迹跟踪),调用内置运动学求解器(正/逆运动学)和切换高级模式(零力拖动/阻抗控制/遥操作)。
真机
| Service | /control/set_property |
|---|---|
| 生效版本 | ≥V0.3.0 |
| 接入方式 | Service Call
|
| 接口说明 | 动态配置机器人运行参数,支持速度比例、末端执行器位姿、错误清除、零力拖动使能、运行模式切换、阻抗参数设置等功能 |
| 接口参数 | string property_type{ //操作类型,枚举 "SpeedRatio"; //设置机器人的运行速度比例 "TcpPrimary"; //设置左臂末端执行器的位置和姿态,用四元数表示 "TcpSecondary"; //设置右臂末端执行器的位置和姿态,用四元数表示 "ClearError"; //清除机器人错误状态 "SafeLevel"; //设置安全等级,统一调控机器人的碰撞检测、运动限制、力控阈值等一系列安全行为,数值越大,安全限制越严格 "EnableDrag"; //启用和关闭零力拖动功能 "EnableSleep"; //是否进入休眠(待机)状态 "SwitchRunMode"; //切换运行模式 "JointStiffness"; //在关节阻抗模式下设置关节刚度,数值越大,关节刚性越强。 "JointDamping"; //在关节阻抗模式下设置关节阻尼,数值越大,关节阻尼越大。 "CartStiffness"; //在笛卡尔阻抗模式下设置机器人在笛卡尔空间的刚度系数。 "CartDampingRatio"; //在笛卡尔阻抗模式下设置机器人在笛卡尔空间的阻尼比。 }; |
| string value{ //value的值与 property_type 关联 -若 property_type 为 SpeedRatio ,value 取值范围为 [0.01, 1.0] ,数据类型是 float64,示例:"value": "0.5"; -若 property_type 为 TcpPrimary ,value 的格式为 "x, y, z, qw, qx, qy, qz",数据类型是 float64,位置的单位是米,姿态的单位是弧度,用四元数表示,示例:"value": "x, y, z, qw, qx, qy, qz "; -若 property_type 为 TcpSecondary ,value 的格式为 "x, y, z, qw, qx, qy, qz",数据类型是 float64,位置的单位是米,姿态的单位是弧度,用四元数表示,示例:"value": "x, y, z, qw, qx, qy, qz "; -若 property_type 为 ClearError ,value 是空字符串,示例: "value": " "; -若property_type为 SafeLevel,value 取值范围为[0.1, 1.0],数据类型是 float64,示例: "value": "0.3"; -若 property_type 为 EnableDrag ,value 的数据类型是 bool,"1"表示启用零力拖动功能,"0"表示关闭零力拖动功能,示例: "value": "1"; -若 property_type 为 EnableSleep,value 取值范围为的数据类型是 bool,"1"表示启用休眠,"0"表示禁用休眠(唤醒),示例: "value":"1"; -若 property_type 为 SwitchRunMode,value 的数据类型是 double,"0"表示位置模式,"1"表示关节阻抗模式,"2"表示拖动模式,"3"表示笛卡尔阻抗模式; -若 property_type 为 JointStiffness,value 的数据类型是 float64,单位是 牛米/弧度(Nm/rad),示例:value: '50';也可以分别设置手臂关节每个关节的刚度大小, 数据数量必须为双臂自由度 14, 长度错误时无效,示例:value: '50, 50, 10, 10, 10, 10, 10, 50, 50, 10, 10, 10, 10, 10'; -若 property_type 为 JointDamping,value 的数据类型是 float64,单位是 牛米秒/弧度(Nm·s/rad),示例:value: '50',必须与JointStiffness配合使用,一起构成完整的阻抗控制; -若 property_type 为 CartStiffness,value 的数据类型是 float64,单位是牛米/弧度(Nm/rad),示例:value: '200'; -若 property_type 为 CartDampingRatio,value 的数据类型是 float64,示例:value: '0.8'; } | |
| uint64 request_id:返回此次指令的指令号,用于追踪指令执行状态 |
| Action | /control/move_robot |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | Action Call
|
| 接口说明 | 控制机器人多模式运动,覆盖关节运动、直线运动、实时流控(关节 / 末端位姿 ),支持单臂 / 双臂 / 全身关节控制 |
| 接口参数 | string type{ //设置运动运动控制模式,枚举 "MoveJoint"; //控制机器人各关节移动到指定角度,实现关节空间运动 "MoveLine"; //控制末端执行器沿直线运动到目标位姿 "StartJointStreaming"; //持续接收关节位置指令,动态调整关节状态 "StartEEPoseStreaming"; //持续接收末端位姿指令,动态调整末端位置 "StopMove"; //停止运动 }; |
| string motion_group{ //指定参与运动控制的机器人部分,枚举 "Primary"; //左臂,关节数量为 7,适用于单臂独立任务 "Secondary"; //右臂,关节数量为 7,适用于单臂独立任务 "BothArms"; //双臂,关节数量为 14,适用于双臂协同任务 "Torso" ; //躯干,关节数量为3 "WholeBody"; //全身关节, 关节数量为20,适用于全身协同运动 }; | |
| float32[] target{ //target 的值与 type 和 motion_group 关联 -若 type 为 MoveJoint,target 为关节角度数组,格式为 [关节1位置,… ,关节n位置] ,n 由 motion_group 决定,例如选择 Primary (左臂)时 n 为7, 选择 BothArms(双臂)时 n 为14。角度的单位为弧度。 -若 type 为 MoveLine,target 为末端目标位姿或躯干位姿,末端目标位姿格式为 [x, y, z, qw, qx, qy, qz] ,躯干位姿格式为[x, z , ry],位置的单位是米,姿态的单位是弧度。 -若 type 为 StartJointStreaming / StartEEPoseStreaming,按对应控制逻辑,实时传入动态目标数据。 }; | |
| bool success:返回布尔值,"1"表示此次指令成功执行,"0"表示此次指令执行失败 | |
| uint64 request_id:返回此次指令的指令号,用于追踪指令执行过程、关联日志或状态查询 |
| Topic | /feedback/robot_server_state |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | 订阅(subscribe) |
| 接口说明 | 获取机器人服务器当前状态,包括运行模式、任务队列、关节与末端执行器位姿 |
| 接口参数 | uint64 timestamp:时间戳 |
| string status{ //枚举,机器人服务器当前状态 Running; //运行中,可配合 latest_queued_id/latest_finished_id 追踪任务流程 Init; //初始状态,如果掉OP也会切换到此状态 Error; //异常,需结合 latest_errordrop_id 定位报错请求 Idle; //空闲中,无任务队列 }; | |
| string run_mode{ //枚举,机器人运行模式 Position; //位置模式 Drag; //拖动模式 Impedance; //阻抗模式 Sleep; //休眠模式(会下使能) }; | |
| uint64 latest_queued_id:最新入列的请求编号,表示当前已进入队列等待执行的最新请求ID,反映待执行任务的优先级与队列长度,若队列持续增长(latest_queued_id 频繁更新 ),需排查任务执行效率。 | |
| uint64 latest_finished_id:最新完成的请求编号,表示已经执行完成的最新请求ID,用于验证任务执行结果 | |
| uint64 latest_errordrop_id:最新报错指令编号,是故障排查的关键线索 | |
| string[] motor_status:20 个关节电机的运行状态, "OP"表示正常运行,格式为["OP", "OP", ...] | |
| float64[] motor_temperature: 20 个关节电机的温度,格式为[33, 33, 33, ...] ,单位°C | |
| uint8[] motor_error_code:电机错误码,格式为[0, 0, 0, ...] ,0 : 无错误,10 : 速度误差超出限制值,20 : 位置误差超出限制值,30 : 主站掉线 | |
| float64[] joint_position:机器人各关节的当前位置,格式为 [-0.0015, -0.0015, -0.0011, ...],单位为弧度(rad) | |
| float64[] joint_velocity:机器人20个关节的关节速度,格式为[0.0000, 0.0000, 0.0000, ...],单位为弧度/秒 | |
| float64[] joint_torque:机器人20个关节的实际关节力矩,格式为[-2.0010, 1.3920, -2.6100, ...] ,单位为Nm | |
| float64[] force_sensor: 六维力传感器数据,格式为[0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000] ,前六位为左手, 后六位为右手 | |
| float64[] ee_pose:末端执行器(End-Effector)的位姿,用四元数表示,格式为 [x, y, z, qw, qx, qy, qz, x, y, z, qw, qx, qy, qz],前7位 [x, y, z, qw, qx, qy, qz]为左臂位姿,后七位[x, y, z, qw, qx, qy, qz]为右臂位姿 ,位置的单位为米,姿态的单位为弧度。 | |
| float64[] torso_pose_X_Z_RY:躯干位姿[X坐标, Z坐标, RY旋转],格式为[-0.0022, 1.1298, -0.0041] ,X/Z 坐标单位为米,RY 旋转单位为弧度 |
| Topic | /control/streaming_command |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | 发布(publish)
|
| 接口说明 | 发送连续控制指令,驱动机器人左右臂运动 |
| 接口参数 | uint64 request_id:与 start streaming request 绑定的唯一 ID,用于指令标识,机器人服务器通过该 ID 区分不同流控任务,避免多任务指令混淆 |
| bool streaming_finished:标记当前指令是否为流控序列的最后一帧,True 表示发送后立即终止本次流控,机器人停止接收后续指令,可用于结束轨迹、复位手臂;False 表示持续激活流控模式,机器人等待下一帧指令,维持连续运动(如轨迹跟踪需高频发送多帧指令 )。 | |
float32[] position:支持两种方式,需严格匹配 motion_group 配置。
|
仿真环境
| Service | /control/set_property_sim |
|---|---|
| 生效版本 | ≥V0.3 |
| 接入方式 | Service Call
|
| 接口说明 | 动态配置机器人运行参数,支持速度比例、末端执行器位姿、错误清除、零力拖动使能等功能 |
| 接口参数 | string property_type{ //操作类型,枚举 "SpeedRatio"; //设置机器人的运行速度比例 "TcpPrimary"; //设置左臂末端执行器的位置和姿态,用四元数表示 "TcpSecondary"; //设置右臂末端执行器的位置和姿态,用四元数表示 "ClearError"; //清除机器人错误状态 "SafeLevel"; //设置安全等级,统一调控机器人的碰撞检测、运动限制、力控阈值等一系列安全行为,数值越大,安全限制越严格 "EnableDrag"; //启用和关闭零力拖动功能 "EnableSleep"; //是否进入休眠(待机)状态 "SwitchRunMode"; //切换运行模式 "JointStiffness"; //在关节阻抗模式下设置关节刚度,数值越大,关节刚性越强。 "JointDamping"; //在关节阻抗模式下设置关节阻尼,数值越大,关节阻尼越大。 "CartStiffness"; //在笛卡尔阻抗模式下设置机器人在笛卡尔空间的刚度系数。 "CartDampingRatio"; //在笛卡尔阻抗模式下设置机器人在笛卡尔空间的阻尼比。 }; |
| string value{ //value的值与 property_type 关联 -若 property_type 为 SpeedRatio ,value 取值范围为 [0.01, 1.0] ,数据类型是 float64,示例:"value": "0.5"; -若 property_type 为 TcpPrimary ,value 的格式为 "x, y, z, qw, qx, qy, qz",数据类型是 float64,位置的单位是米,姿态的单位是弧度,用四元数表示,示例:"value": "x, y, z, qw, qx, qy, qz "; -若 property_type 为 TcpSecondary ,value 的格式为 "x, y, z, qw, qx, qy, qz",数据类型是 float64,位置的单位是米,姿态的单位是弧度,用四元数表示,示例:"value": "x, y, z, qw, qx, qy, qz "; -若 property_type 为 ClearError ,value 是空字符串,示例: "value": " "; -若property_type为 SafeLevel,value 取值范围为[0.1, 1.0],数据类型是 float64,示例: "value": "0.3"; -若 property_type 为 EnableDrag ,value 的数据类型是 bool,"1"表示启用零力拖动功能,"0"表示关闭零力拖动功能,示例: "value": "1"; -若 property_type 为 EnableSleep,value 取值范围为的数据类型是 bool,"1"表示启用休眠,"0"表示禁用休眠(唤醒),示例: "value":"1"; -若 property_type 为 SwitchRunMode,value 的数据类型是 double,"0"表示位置模式,"1"表示关节阻抗模式,"2"表示拖动模式,"3"表示笛卡尔阻抗模式; -若 property_type 为 JointStiffness,value 的数据类型是 float64,单位是 牛米/弧度(Nm/rad),示例:value: '50'; -若 property_type 为 JointDamping,value 的数据类型是 float64,单位是 牛米秒/弧度(Nm·s/rad),示例:value: '50',必须与JointStiffness配合使用,一起构成完整的阻抗控制; -若 property_type 为 CartStiffness,value 的数据类型是 float64,单位是牛米/弧度(Nm/rad),示例:value: '200'; -若 property_type 为 CartDampingRatio,value 的数据类型是 float64,示例:value: '0.8'; } | |
| uint64 request_id:返回此次指令的指令号,用于追踪指令执行状态 |
| Action | /control/move_robot_sim |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | Action Call
|
| 接口说明 | 控制机器人多模式运动,覆盖关节运动、直线运动、实时流控(关节 / 末端位姿 ),支持单臂 / 双臂 / 全身关节控制 |
| 接口参数 | string type{ //设置运动运动控制模式,枚举 "MoveJoint"; //控制机器人各关节移动到指定角度,实现关节空间运动 "MoveLine"; //控制末端执行器沿直线运动到目标位姿 "StartJointStreaming"; //持续接收关节位置指令,动态调整关节状态 "StartEEPoseStreaming"; //持续接收末端位姿指令,动态调整末端位置 "StopMove"; //停止运动 }; |
| string motion_group{ //指定参与运动控制的机器人部分,枚举 "Primary"; //左臂,关节数量为 7,适用于单臂独立任务 "Secondary"; //右臂,关节数量为 7,适用于单臂独立任务 "BothArms"; //双臂,关节数量为 14,适用于双臂协同任务 "Torso" ; //躯干,关节数量为3 "WholeBody"; //全身关节, 关节数量为20,适用于全身协同运动 }; | |
| float32[] target{ //target 的值与 type 和 motion_group 关联 -若 type 为 MoveJoint,target 为关节角度数组,格式为 [关节1位置,… ,关节n位置] ,n 由 motion_group 决定,例如选择 Primary (左臂)时 n 为7, 选择 BothArms(双臂)时 n 为14。角度的单位为弧度。 -若 type 为 MoveLine,target 为末端目标位姿或躯干位姿,末端目标位姿格式为 [x, y, z, qw, qx, qy, qz] ,躯干位姿格式为[x, z , ry],位置的单位是米,姿态的单位是弧度。 -若 type 为 StartJointStreaming / StartEEPoseStreaming,按对应控制逻辑,实时传入动态目标数据。 }; | |
| bool success:返回布尔值,"1"表示此次指令成功执行,"0"表示此次指令执行失败 | |
| uint64 request_id:返回此次指令的指令号,用于追踪指令执行过程、关联日志或状态查询 |
| Topic | /feedback/robot_server_state_sim |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | 订阅(subscribe) |
| 接口说明 | 获取机器人服务器当前状态,包括运行模式、任务队列、关节与末端执行器位姿 |
| 接口参数 | uint64 timestamp:时间戳 |
| string status{ //枚举,机器人服务器当前状态 Running; //运行中,可配合 latest_queued_id/latest_finished_id 追踪任务流程 Init; //初始状态,如果掉OP也会切换到此状态 Error; //异常,需结合 latest_errordrop_id 定位报错请求 Idle; //空闲中,无任务队列 }; | |
| string run_mode{ //枚举,机器人运行模式 Position; //位置模式 Drag; //拖动模式 Impedance; //阻抗模式 Sleep; //休眠模式(会下使能) }; | |
| uint64 latest_queued_id:最新入列的请求编号,表示当前已进入队列等待执行的最新请求ID,反映待执行任务的优先级与队列长度,若队列持续增长(latest_queued_id 频繁更新 ),需排查任务执行效率。 | |
| uint64 latest_finished_id:最新完成的请求编号,表示已经执行完成的最新请求ID,用于验证任务执行结果 | |
| uint64 latest_errordrop_id:最新报错指令编号,是故障排查的关键线索 | |
| string[] motor_status:20 个关节电机的运行状态, "OP"表示正常运行,格式为["OP", "OP", ...] | |
| float64[] motor_temperature: 20 个关节电机的温度,格式为[33, 33, 33, ...] ,单位°C | |
| uint8[] motor_error_code:电机错误码,格式为[0, 0, 0, ...] ,0 : 无错误,10 : 速度误差超出限制值,20 : 位置误差超出限制值,30 : 主站掉线 | |
| float64[] joint_position:机器人各关节的当前位置,格式为 [-0.0015, -0.0015, -0.0011, ...],单位为弧度(rad) | |
| float64[] joint_velocity:机器人20个关节的关节速度,格式为[0.0000, 0.0000, 0.0000, ...],单位为弧度/秒 | |
| float64[] joint_torque:机器人20个关节的实际关节力矩,格式为[-2.0010, 1.3920, -2.6100, ...] ,单位为Nm | |
| float64[] force_sensor: 六维力传感器数据,格式为[0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000] ,前六位为左手, 后六位为右手 | |
| float64[] ee_pose:末端执行器(End-Effector)的位姿,用四元数表示,格式为 [x, y, z, qw, qx, qy, qz, x, y, z, qw, qx, qy, qz],前7位 [x, y, z, qw, qx, qy, qz]为左臂位姿,后七位[x, y, z, qw, qx, qy, qz]为右臂位姿 ,位置的单位为米,姿态的单位为弧度。 | |
| float64[] torso_pose_X_Z_RY:躯干位姿[X坐标, Z坐标, RY旋转],格式为[-0.0022, 1.1298, -0.0041] ,X/Z 坐标单位为米,RY 旋转单位为弧度 |
| Topic | /control/streaming_command_sim |
|---|---|
| 生效版本 | ≥V0.2 |
| 接入方式 | 发布(publish)
|
| 接口说明 | 发送连续控制指令,驱动机器人左右臂运动 |
| 接口参数 | uint64 request_id:与 start streaming request 绑定的唯一 ID,用于指令标识,机器人服务器通过该 ID 区分不同流控任务,避免多任务指令混淆 |
| bool streaming_finished:标记当前指令是否为流控序列的最后一帧,True 表示发送后立即终止本次流控,机器人停止接收后续指令,可用于结束轨迹、复位手臂;False 表示持续激活流控模式,机器人等待下一帧指令,维持连续运动(如轨迹跟踪需高频发送多帧指令 )。 | |
float32[] position:支持两种方式,需严格匹配 motion_group 配置。
|