MoveIt Gateway Node 文档
概述
moveit_gateway 是 IB-Robot 项目中的核心运动控制节点，充当高层控制接口与底层 MoveIt2 运动规划框架之间的网关。该节点专门针对 SO101 5自由度机械臂设计，解决了 5DOF 机械臂在逆运动学（IK）求解中的特殊约束问题。
推荐启动场景
Ubuntu 启动 MoveIt Planning 控制（仿真）
ros2 launch robot_config robot.launch.py \
robot_config:=so101_single_arm \
control_mode:=moveit_planning \
use_sim:=true
如需在板端或无图形界面的环境中运行，可关闭 RViz：
ros2 launch robot_config robot.launch.py \
robot_config:=so101_single_arm \
control_mode:=moveit_planning \
use_sim:=true \
moveit_display:=false
端侧开发板启动 MoveIt Planning 控制（真机）
端侧开发板可运行 openEuler Embedded 或 OpenHarmony，真机场景下与 Ubuntu 的控制接口保持一致，只需关闭仿真模式：
ros2 launch robot_config robot.launch.py \
robot_config:=so101_single_arm \
control_mode:=moveit_planning
两种场景都通过 /cmd_pose 发送位姿命令，通过 /robot_status/ee_pose 观察末端位姿反馈。
主要功能
1. 位姿控制 (Pose Control)
- 订阅话题:
/cmd_pose(geometry_msgs/Pose) - 功能: 接收目标位姿，通过 IK 求解关节角度，控制机械臂运动
- 特点: 支持多种约束策略和分层容差机制
2. 末端执行器状态发布
- 发布话题:
/robot_status/ee_pose(geometry_msgs/PoseStamped) - 功能: 实时发布末端执行器在 base 坐标系中的位姿
3. 关节状态同步
- 订阅话题:
/joint_states(sensor_msgs/JointState) - 功能: 同步当前关节状态，作为 IK 求解的起始状态
5DOF 机械臂的特殊处理
问题背景
5自由度机械臂只有 5 个关节，无法满足完整的 6DOF 位姿约束（3个位置 + 3个姿态）。直接使用标准 IK 求解器会导致：
NO_IK_SOLUTION错误- IK 求解成功率低
解决方案
1. 仅位置 IK (Position-Only IK)
在 kinematics.yaml 中配置：
position_only_ik: True
为什么需要设置为 True：
5DOF 机械臂只有 5 个关节自由度，无法同时满足完整的 6DOF 位姿约束（3个位置 + 3个姿态）。当 IK 求解器尝试同时满足位置和姿态约束时，往往会出现：
NO_IK_SOLUTION错误：目标姿态在数学上不可达- 求解成功率低：大部分目标位姿无解
配置后的行为：
- KDL 使用
ChainIkSolverPos求解器（而非ChainIkSolverPos_NR） - 只优化位置：求解器只尝试使末端执行器到达目标位置
- 忽略姿态约束：优化过程中不考虑姿态目标
- 姿态仍可验证：传入的
orientation不会影响求解，但可通过constraints验证最终姿态是否可接受
位姿如何影响求解：
虽然 position_only_ik: True 忽略姿态优化，但传入的目标姿态仍有影响：
- 作为初始猜测：不同的姿态目标可能引导求解器收敛到不同的关节空间解
- 约束验证：如果同时传入
constraints，最终解仍需满足姿态容差要求 - 数值稳定性：合理的目标姿态有助于 Newton-Raphson 迭代收敛
2. 姿态约束优化
虽然启用了 position_only_ik，我们仍然提供姿态参考，通过以下方法：
a) Z轴约束 (constrain_to_z_axis_only)
- 保持末端执行器 Z 轴方向不变
- 放松绕 Z 轴的旋转（roll）
- 这符合 5DOF 机械臂的能力：2DOF 用于 Z 轴方向（pitch + yaw），剩余 3DOF 用于位置
b) 姿态投影 (project_orientation_to_shoulder_xz_plane)
- 将姿态投影到 shoulder 坐标系的 XZ 平面
- 适应 5DOF 机械臂的运动学约束
3. 分层容差策略
从严格到宽松依次尝试：
| 策略 | 容差 (x, y, z) | 说明 |
|---|---|---|
| Strict | (0.1, 0.1, 0.05) | X/Y: ±5.7°, Z: ±2.8° |
| Medium | (0.3, 0.3, 0.1) | X/Y: ±17°, Z: ±5.7° |
| Relaxed | (0.5, 0.5, 0.15) | X/Y: ±28°, Z: ±8.6° |
| Z-axis only | (1.0, 1.0, 0.2) | X/Y: ±57°, 几乎不约束 |
| No constraints | None | 完全无姿态约束 |
4. 多策略 Fallback
1. Gripper Z-axis constraint → 尝试所有容差
- 只约束 gripper 的 Z 轴方向
2. Shoulder XZ plane projection → 尝试所有容差
- 将姿态投影到 shoulder 坐标系 XZ 平面
- 几何上更精确适配 5DOF 约束
3. Current orientation (保持当前姿态) → 尝试所有容差
4. Default orientation (无旋转) → 尝试所有容差
坐标系变换
Shoulder 坐标系计算
为了准确判断工作空间可达性，节点将目标位置从 base 坐标系转换到 shoulder 坐标系：
P_shoulder = R × (P_base - T)
其中：
P_base: 目标点在 base 坐标系中的位置T: shoulder 原点在 base 坐标系中的平移偏移R: base 到 shoulder 的旋转矩阵P_shoulder: 目标点在 shoulder 坐标系中的位置
日志输出
[INFO] Target Pose: x=-0.046, y=-0.000, z=0.423
[INFO] Target in shoulder frame: x=-0.034, y=0.012, z=-0.323
[INFO] Distance from base origin: 0.426 m
[INFO] Distance from shoulder origin: 0.325 m
控制流程图
┌───────────────────────────────────────────────────────────┐
│ moveit_gateway 节点 │
└───────────────────────────────────────────────────────────┘
│
▼
┌───────────────┐
│ /cmd_pose │
│ (Pose) │
└───────┬───────┘
│
▼
┌───────────────┐
│ cmd_pose_ │
│ callback │
└───────┬───────┘
│
▼
┌─────────────────────────────────────────┐
│ 坐标系变换与距离计算 │
│ 1. 获取 TF 变换 │
│ 2. 计算 p_relative = P_base - T │
│ 3. 应用旋转变换 P_shoulder = R × p_relative │
│ 4. 计算距离 │
└───────────────────┬─────────────────────┘
│
▼
┌─────────────────────────────────────────┐
│ 多策略 IK 求解 │
│ │
│ 策略循环: │
│ 1. Z轴约束 │
│ 2. Shoulder XZ平面投影 │
│ 3. 当前姿态 │
│ 4. 默认姿态 │
│ │
│ 容差循环: │
│ 1. Strict (0.1) │
│ 2. Medium (0.3) │
│ 3. Relaxed (0.5) │
│ 4. Z-only (1.0) │
│ 5. None │
└───────────────────┬─────────────────────┘
│
▼
┌─────────────────────────────────────────┐
│ solve_and_move() │
│ 1. 创建 Constraints (如果有容差) │
│ 2. 调用 compute_ik_async() │
│ 3. 等待结果 │
│ 4. move_to_joint() │
└───────────────────┬─────────────────────┘
│
▼
┌───────────────────┐
│ IK 成功？ │
└───┬───────────┬───┘
│ YES │ NO
▼ ▼
┌───────────────┐ ┌───────────────┐
│ 移动成功 │ │ 尝试下一个 │
│ │ │ 策略/容差 │
└───────────────┘ └───────────────┘
│
▼
┌───────────────────────────────────────────────────────────┐
│ 发布 /robot_status/ee_pose │
│ (10Hz 定时器) │
└───────────────────────────────────────────────────────────┘
约束创建流程
┌─────────────────────────────────────────────────────────────┐
│ create_orientation_constraint │
├─────────────────────────────────────────────────────────────┤
│ 输入: │
│ - target_quat: 目标四元数 (x, y, z, w) │
│ - link_name: "gripper" │
│ - frame_id: "base" │
│ - tolerances: (x_tol, y_tol, z_tol) │
│ │
│ 输出: │
│ - OrientationConstraint 消息 │
│ │
│ 字段设置: │
│ - orientation.x/y/z/w: 目标姿态 │
│ - absolute_x_axis_tolerance: X轴容差 (放松 roll) │
│ - absolute_y_axis_tolerance: Y轴容差 (放松 roll) │
│ - absolute_z_axis_tolerance: Z轴容差 (保持方向) │
│ - weight: 1.0 (约束权重) │
└─────────────────────────────────────────────────────────────┘
四元数数学工具
数学函数使用 scipy.spatial.transform.Rotation 和 NumPy 实现。
quaternion_multiply(q1, q2)
四元数乘法：q = q1 × q2
- 使用 scipy:
R.from_quat() * R.from_quat()
quaternion_conjugate(q)
四元数共轭：q* = [-x, -y, -z, w]
- 使用 scipy:
R.from_quat().inv()
quaternion_to_rotation_matrix(q)
四元数转 3×3 旋转矩阵，返回 np.ndarray
- 使用 scipy:
R.from_quat().as_matrix()
rotation_matrix_to_quaternion(R)
3×3 旋转矩阵转四元数
- 使用 scipy:
R.from_matrix().as_quat()
constrain_to_z_axis_only(quat)
只约束末端执行器 Z 轴方向，放松绕 Z 轴旋转
- 使用 scipy 进行四元数/矩阵转换
- 使用 NumPy 向量化：
np.linalg.norm(),np.dot(),np.cross(),np.column_stack()
project_orientation_to_shoulder_xz_plane(quat)
将姿态投影到 shoulder 坐标系 XZ 平面
- 使用 scipy 进行四元数/矩阵转换
- 使用 NumPy 向量化，同上
配置文件
kinematics.yaml
arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.01
kinematics_solver_timeout: 2.0
kinematics_solver_attempts: 50
position_only_ik: True # 5DOF 机械臂关键配置
moveit_controllers.yaml
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- arm_trajectory_controller
- gripper_trajectory_controller
arm_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints: ["1", "2", "3", "4", "5"]
gripper_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints: ["6"]
ROS2 话题接口
订阅话题
| 话题名 | 消息类型 | 频率 | 说明 |
|---|---|---|---|
| /cmd_pose | geometry_msgs/Pose | 按需 | 目标位姿命令 |
| /joint_states | sensor_msgs/JointState | 100Hz | 关节状态反馈 |
发布话题
| 话题名 | 消息类型 | 频率 | 说明 |
|---|---|---|---|
| /robot_status/ee_pose | geometry_msgs/PoseStamped | 10Hz | 末端执行器位姿 |
使用示例
发送位姿命令
ros2 topic pub /cmd_pose geometry_msgs/Pose "{
position: {x: 0.15, y: 0.0, z: 0.25},
orientation: {x: 0.0, y: 0.0, z: 0.707, w: 0.707}
}" --once
查看末端位姿
ros2 topic echo /robot_status/ee_pose
多线程模型
节点使用 MultiThreadedExecutor 和 ReentrantCallbackGroup 实现线程安全：
- 回调组: 所有订阅者使用同一个可重入回调组
- 执行器: 多线程执行器处理并发回调
- 线程安全: IK 调用使用 async 模式，避免内部 spin_once 调用
日志级别
- INFO: 目标位姿、距离信息、IK 成功
- WARNING: IK 失败、TF 查询失败、无效关节状态
- DEBUG: 关节状态更新、策略尝试详情
故障排查
IK 持续失败
- 检查位置是否超出工作空间（shoulder 距离应 < 0.35m）
- 确认
position_only_ik: True已设置 - 查看日志中的距离信息
TF 查询失败
- 确认 robot_state_publisher 正在运行
- 检查 URDF 中是否定义了 shoulder link
- 使用
ros2 run tf2_tools view_frames查看 TF 树
关节状态无效
- 确认 joint_states 包含所需的 5 个关节
- 检查关节名称是否为 ['1', '2', '3', '4', '5']
IK 求解器技术细节
Goal vs Constraints
compute_ik_async 调用时同时传入：
quat_xyzw：目标姿态（主要优化目标）constraints：姿态容差约束（验证条件）
KDL 求解器行为
Newton-Raphson 迭代：
- 目标：最小化误差函数
E = E_position + E_orientation - 优先收敛到
pose_stamped.pose的精确值 constraints主要用于验证最终解，而非扩大搜索空间
5DOF 限制：
- 当目标姿态数学上不可达（6DOF 约束 vs 5DOF 自由度）时
- KDL 可能无法收敛，返回
NO_IK_SOLUTION
Position-Only IK 配置
配置 position_only_ik: True：
# kinematics.yaml
position_only_ik: True
启用后的行为：
- KDL 使用
ChainIkSolverPos而非ChainIkSolverPos_NR - 只优化位置，忽略姿态
- constraints 中的姿态约束仍然会被验证
- 解算器在位置精确匹配的前提下，自由调整姿态