# 逆运动学解算 IK Solver（逆运动学解算算子）用于将机器人双臂末端执行器的目标位姿转换为对应的关节角度指令。用户只需提供末端位姿轨迹数据（ROS2 rosbag格式），算子即可自动完成逆运动学求解，输出各关节的目标角度。 **当前仅支持双臂逆运动学解算。** **核心能力**： - 支持四种机器人型号：Galaxea R1、Agibot A2D、K100、青龙。 - 支持三种优先级策略：手臂优先、躯干优先、固定躯干。 - 支持逐帧独立求解或连续积分求解。 - 支持追加写入模式，保留原始rosbag中的其他传感器数据。 - 输入输出时间戳一一对应，保证时序一致性。 #### 处理流程 图1处理流程 ![](https://support.huaweicloud.com/usermanual-cloudrobo/zh-cn_image_0000002714332538.png) #### 参数配置 当预置算法选择"逆运动学解算算子"后，需填写以下参数： 表1参数说明 | 参数名 | 是否必填 | 默认值 | 可选值 | 说明 | |:---|:---|:---|:---|:---| | 机器人型号 (ROBOT_MODEL) | 否 | galaxea_r1 | galaxea_r1、agibot_a2d、k100、qinglong | 指定机器人型号，填写无效值时自动回退为galaxea_r1。 | | 优先级策略 (IK_SOLVER_PRIORITY_MODE) | 否 | PRIORITY_OPTION_DUAL_ARM | PRIORITY_OPTION_DUAL_ARM、PRIORITY_OPTION_TORSO、PRIORITY_OPTION_FIX_TORSO | 求解时关节运动的优先级策略，填写无效值时自动回退为手臂优先。 | | MOP模式 (MOP_MODE) | 否 | True | True、False | 是否每帧重置参考构型。 True：每帧独立求解，帧间无依赖； False：连续积分，前一帧结果影响后一帧。 | | 输入Topic (SUB_TOPIC) | 否 | /whole_body_ik_solver | 任意字符串 | 从输入rosbag中读取位姿数据的topic名称，需与输入数据中的topic一致。 | | 输出Topic (PUB_TOPIC) | 否 | /joint_plan_servo_controller | 任意字符串 | 输出rosbag中写入关节指令的topic名称。 | | 追加写入模式 (IK_BAG_APPEND_MODE) | 否 | True | True、False | - False：输出rosbag仅包含IK结果topic。 - True：保留输入rosbag中所有topic并追加IK结果。 | 表2优先级策略说明 | 策略值 | 名称 | 含义 | |:---|:---|:---| | PRIORITY_OPTION_DUAL_ARM | 手臂优先 | 手臂关节灵活运动，躯干关节辅助调整。适合需要手臂精确到达目标的场景。 | | PRIORITY_OPTION_TORSO | 躯干优先 | 躯干关节灵活运动，手臂关节辅助调整。适合需要躯干大幅运动的场景。 | | PRIORITY_OPTION_FIX_TORSO | 固定躯干 | 躯干关节几乎不动，仅手臂运动。适合躯干需保持稳定的场景。 | 表3MOP模式说明 | 模式值 | 含义 | 适用场景 | |:---|:---|:---| | True | 每帧从参考构型出发独立求解，帧与帧之间无依赖。 | 位姿轨迹跨度大、帧间变化剧烈的场景。 | | False | 以前一帧求解结果为起点连续积分，帧间有依赖。 | 位姿轨迹平滑、需要关节运动连续性的场景。 | 表4追加写入模式说明 | 模式值 | 输出内容 | 适用场景 | |:---|:---|:---| | False | 仅输出IK求解结果topic。 | 只需要关节角度指令的场景。 | | True | 保留输入rosbag中所有topic+IK结果topic。 | 需要同时保留原始传感器数据（如相机、IMU等）的场景。若输入rosbag中已存在与输出topic同名的数据，原始数据将被IK结果替换。 | #### 机器人型号规格 机器人型号规格如[表5]所示。 表5机器人型号总览 | 型号 | ROBOT_MODEL | 描述 | 总关节数 | 输出关节数 | 优先级策略支持 | |:---|:---|:---|:---|:---|:---| | Galaxea R1 | galaxea_r1 | 轮式双臂人形机器人 | 29 | 16 | 手臂优先/躯干优先/固定躯干 | | Agibot A2D | agibot_a2d | 双臂固定基座机器人 | 16 | 16 | 手臂优先/固定躯干 | | K100 | k100 | 双臂固定基座机器人 | 14 | 14 | 手臂优先/固定躯干 | | 青龙 | qinglong | 轮式双臂人形机器人 | 31 | 14 | 手臂优先/躯干优先/固定躯干 | #### 各型号输出关节布局 输出rosbag中每条消息的data数组包含对应机型的关节角度值，布局如下： 表6Galaxea R1（16个关节） | 索引 | 关节名称 | 说明 | |:---|:---|:---| | data\[0\] | torso_joint1 | 躯干关节1 | | data\[1\] | torso_joint2 | 躯干关节2 | | data\[2\] | torso_joint3 | 躯干关节3 | | data\[3\] | torso_joint4 | 躯干关节4 | | data\[4\] | left_arm_joint1 | 左臂关节1 | | data\[5\] | left_arm_joint2 | 左臂关节2 | | data\[6\] | left_arm_joint3 | 左臂关节3 | | data\[7\] | left_arm_joint4 | 左臂关节4 | | data\[8\] | left_arm_joint5 | 左臂关节5 | | data\[9\] | left_arm_joint6 | 左臂关节6 | | data\[10\] | right_arm_joint1 | 右臂关节1 | | data\[11\] | right_arm_joint2 | 右臂关节2 | | data\[12\] | right_arm_joint3 | 右臂关节3 | | data\[13\] | right_arm_joint4 | 右臂关节4 | | data\[14\] | right_arm_joint5 | 右臂关节5 | | data\[15\] | right_arm_joint6 | 右臂关节6 | 表7Agibot A2D（16个关节） | 索引 | 关节名称 | 说明 | |:---|:---|:---| | data\[0\] | joint_lift_body | 躯干升降关节 | | data\[1\] | joint_body_pitch | 躯干俯仰关节 | | data\[2\] | left_arm_joint1 | 左臂关节1 | | data\[3\] | left_arm_joint2 | 左臂关节2 | | data\[4\] | left_arm_joint3 | 左臂关节3 | | data\[5\] | left_arm_joint4 | 左臂关节4 | | data\[6\] | left_arm_joint5 | 左臂关节5 | | data\[7\] | left_arm_joint6 | 左臂关节6 | | data\[8\] | left_arm_joint7 | 左臂关节7 | | data\[9\] | right_arm_joint1 | 右臂关节1 | | data\[10\] | right_arm_joint2 | 右臂关节2 | | data\[11\] | right_arm_joint3 | 右臂关节3 | | data\[12\] | right_arm_joint4 | 右臂关节4 | | data\[13\] | right_arm_joint5 | 右臂关节5 | | data\[14\] | right_arm_joint6 | 右臂关节6 | | data\[15\] | right_arm_joint7 | 右臂关节7 | 表8K100（14个关节） | 索引 | 关节名称 | 说明 | |:---|:---|:---| | data\[0\] | left_arm_joint1 | 左臂关节1 | | data\[1\] | left_arm_joint2 | 左臂关节2 | | data\[2\] | left_arm_joint3 | 左臂关节3 | | data\[3\] | left_arm_joint4 | 左臂关节4 | | data\[4\] | left_arm_joint5 | 左臂关节5 | | data\[5\] | left_arm_joint6 | 左臂关节6 | | data\[6\] | left_arm_joint7 | 左臂关节7 | | data\[7\] | right_arm_joint1 | 右臂关节1 | | data\[8\] | right_arm_joint2 | 右臂关节2 | | data\[9\] | right_arm_joint3 | 右臂关节3 | | data\[10\] | right_arm_joint4 | 右臂关节4 | | data\[11\] | right_arm_joint5 | 右臂关节5 | | data\[12\] | right_arm_joint6 | 右臂关节6 | | data\[13\] | right_arm_joint7 | 右臂关节7 | K100为固定基座机器人，无躯干关节，因此仅输出 14 个手臂关节角度。 表9青龙（14个关节） | 索引 | 关节名称 | 说明 | |:---|:---|:---| | data\[0\] | left_arm_joint1 | 左臂关节1 | | data\[1\] | left_arm_joint2 | 左臂关节2 | | data\[2\] | left_arm_joint3 | 左臂关节3 | | data\[3\] | left_arm_joint4 | 左臂关节4 | | data\[4\] | left_arm_joint5 | 左臂关节5 | | data\[5\] | left_arm_joint6 | 左臂关节6 | | data\[6\] | left_arm_joint7 | 左臂关节7 | | data\[7\] | right_arm_joint1 | 右臂关节1 | | data\[8\] | right_arm_joint2 | 右臂关节2 | | data\[9\] | right_arm_joint3 | 右臂关节3 | | data\[10\] | right_arm_joint4 | 右臂关节4 | | data\[11\] | right_arm_joint5 | 右臂关节5 | | data\[12\] | right_arm_joint6 | 右臂关节6 | | data\[13\] | right_arm_joint7 | 右臂关节7 | 青龙虽为轮式人形机器人（含躯干关节），但当前版本仅输出手臂关节角度，躯干关节不包含在输出中。 #### 输入数据集说明 输入数据为ROS2 rosbag2格式，每个目录包含.db3文件和metadata.yaml。 输入rosbag中需包含一个std_msgs/msg/Float64MultiArray类型的topic（默认为/whole_body_ik_solver），每条消息携带14个float64值，表示双臂末端执行器的目标位姿。 #### 输入消息布局 所有机型输入格式相同，每帧14个float64。 表10青龙（14个关节） | 索引 | 含义 | 单位/说明 | |:---|:---|:---| | data\[0\] | 左臂末端位置x | 米，相对base_link坐标系，正值向前 | | data\[1\] | 左臂末端位置y | 米，正值向左 | | data\[2\] | 左臂末端位置z | 米，正值向上 | | data\[3\] | 左臂末端四元数qw | 四元数标量部分，无旋转时为1.0 | | data\[4\] | 左臂末端四元数qx | 四元数标量部分，无旋转时为0.0 | | data\[5\] | 左臂末端四元数qy | 四元数标量部分，无旋转时为0.0 | | data\[6\] | 左臂末端四元数qz | 四元数标量部分，无旋转时为0.0 | | data\[7\] | 右臂末端位置x | 米，相对base_link坐标系，正值向前 | | data\[8\] | 右臂末端位置y | 米，正值向左 | | data\[9\] | 右臂末端位置z | 米，正值向上 | | data\[10\] | 右臂末端四元数qw | 四元数标量部分，无旋转时为1.0 | | data\[11\] | 右臂末端四元数qx | 四元数标量部分，无旋转时为0.0 | | data\[12\] | 右臂末端四元数qy | 四元数标量部分，无旋转时为0.0 | | data\[13\] | 右臂末端四元数qz | 四元数标量部分，无旋转时为0.0 | #### 各机型位姿参考范围 位姿值需在机器人可达工作空间范围内，否则求解可能不收敛。 表11各机型位姿参考范围说明 | 参数 | R1 | A2D | K100 | 青龙 | 说明 | |:---|:---|:---|:---|:---|:---| | x（前后） | 0.2 \~ 0.8 | 0.2 \~ 0.6 | 0.2 \~ 0.6 | 0.2 \~ 0.8 | 0为底盘中心，正值向前 | | y（左右） | -0.5 \~ 0.5 | -0.4 \~ 0.4 | -0.4 \~ 0.4 | -0.5 \~ 0.5 | 正值向左 | | z（上下） | 0.8 \~ 1.8 | 0.8 \~ 1.6 | 0.8 \~ 1.6 | 0.8 \~ 1.8 | R1/A2D标准高度1.3m，K100/青龙1.2m | | qw, qx, qy, qz | -1.0 \~ 1.0 | -1.0 \~ 1.0 | -1.0 \~ 1.0 | -1.0 \~ 1.0 | 四元数标量部分 | ![](https://support.huaweicloud.com/usermanual-cloudrobo/public_sys-resources/note_3.0-zh-cn.png) 四元数约束：必须为单位四元数，即qw² + qx² + qy² + qz² = 1。 表12常用四元数参考 | 姿态 | qw | qx | qy | qz | |:---|:---|:---|:---|:---| | 无旋转 | 1.0 | 0.0 | 0.0 | 0.0 | | 绕Z轴旋转 90° | 0.7071 | 0.0 | 0.0 | 0.7071 | | 绕Y轴旋转 90° | 0.7071 | 0.0 | 0.7071 | 0.0 | | 绕X轴旋转 90° | 0.7071 | 0.7071 | 0.0 | 0.0 | #### 输入目录结构 输入目录下可包含一个或多个.db3文件（标准录制的db3数据包会对应1个同名yaml配置文件，在当前算子中yaml为可选文件），算子会自动递归扫描所有子目录并逐个处理： ``` ├── bag_0.db3 ├── bag_0_metadata.yaml ├── task_pick/ │ ├── session_01/ │ │ └── pick_rosbag_0.db3 │ └── session_02/ │ ├── pick_rosbag_0.db3 │ └── pick_rosbag_1.db3 └── task_place/ ├── place_rosbag_0.db3 └── place_rosbag_1.db3 ``` ![](https://support.huaweicloud.com/usermanual-cloudrobo/public_sys-resources/note_3.0-zh-cn.png) 算子会递归扫描输入目录及其所有子目录下的.db3文件，支持任意深度的目录嵌套。输出目录保持与输入相同的相对路径结构。 #### 输入数据注意事项 表13输入数据注意事项说明 | 事项 | 说明 | |:---|:---| | Topic匹配 | 输入rosbag中的topic名称必须与SUB_TOPIC参数值一致，否则会报错 | | 消息类型 | 输入末端位姿消息类型必须为std_msgs/msg/Float64MultiArray | | 数据长度 | 每条消息的data数组必须包含14个float64值（双臂机器人） | | 时间戳 | 多帧消息的时间戳必须递增 | | 空数据 | 输入目录为空或db3文件中无消息时，不会崩溃，但无输出 | | 位姿可达性 | 位姿超出工作空间范围时，求解可能不收敛，输出关节角可能不正确 | #### 输出数据集说明 输出数据为ROS2 rosbag2格式，输出目录结构与输入目录对应，每个输入db3文件生成一个输出rosbag子目录： ``` 输出目录/ ├── bag_0/ │ ├── bag_0.db3 │ └── metadata.yaml ├── bag_1/ │ ├── bag_1.db3 │ └── metadata.yaml └── ... ``` #### 输出消息布局 输出rosbag中包含一个std_msgs/msg/Float64MultiArray类型的topic（默认为/joint_plan_servo_controller），每条消息携带对应机型数量的float64值，表示各关节的目标角度（单位：弧度）。