ROS2 Action实战：用MoveIt! Commander轻松控制机械臂完成抓取任务

ROS2 Action实战：用MoveIt! Commander轻松控制机械臂完成抓取任务

机械臂的抓取-放置（pick-and-place）是工业自动化和服务机器人中最常见的任务之一。想象一下，当你需要让机械臂从传送带上抓取一个零件，精确地移动到另一个位置进行装配或包装时，传统的手动编程方式不仅耗时，而且难以应对环境变化。ROS2的MoveIt!框架结合Action机制，为这类任务提供了优雅的解决方案。

1. 环境准备与基础配置

在开始编写抓取任务脚本前，我们需要确保开发环境正确配置。以下是一个典型的ROS2 Humble开发环境检查清单：

• 系统要求：Ubuntu 22.04 LTS
• ROS2版本：Humble Hawksbill（建议完整桌面版安装）
• 关键软件包：

  sudo apt install ros-humble-moveit ros-humble-moveit-commander ros-humble-moveit-visual-tools

提示：如果使用真实机械臂，还需要安装对应的驱动程序包。例如Franka Panda机械臂需要ros-humble-franka-ros。

验证MoveIt!安装是否成功：

ros2 pkg list | grep moveit

应该能看到moveit_commander、moveit_core等关键包名。

2. MoveIt! Commander核心API解析

MoveIt! Commander是MoveIt!提供的Python接口，它封装了底层Action通信细节，让开发者可以更直观地控制机械臂。让我们深入几个关键类和方法：

2.1 MoveGroupCommander类

这是控制机械臂运动的核心类，主要方法包括：

关节空间运动示例：

# 获取当前关节角度 joint_goal = move_group.get_current_joint_values() # 修改目标角度（以Panda机械臂为例） joint_goal[0] = 0.0 # panda_joint1 joint_goal[1] = -0.785 # panda_joint2 # 执行运动 move_group.go(joint_goal, wait=True)

2.2 PlanningSceneInterface类

这个类用于管理机器人周围的环境碰撞物体，在抓取任务中至关重要：

from moveit_commander.planning_scene_interface import PlanningSceneInterface scene = PlanningSceneInterface() # 添加一个桌面障碍物 box_pose = PoseStamped() box_pose.header.frame_id = "panda_link0" box_pose.pose.position.z = -0.1 scene.add_box("table", box_pose, size=(1.0, 1.0, 0.2))

3. 完整抓取任务实现

现在我们将实现一个完整的pick-and-place流程，分为四个阶段：

3.1 预抓取位姿规划

def move_to_pregrasp(move_group, object_pose): # 创建目标位姿（比实际抓取位置高5cm） grasp_pose = object_pose grasp_pose.position.z += 0.05 # 设置末端执行器方向（根据夹爪类型调整） quat = quaternion_from_euler(0, 0, 0) grasp_pose.orientation.x = quat[0] grasp_pose.orientation.y = quat[1] grasp_pose.orientation.z = quat[2] grasp_pose.orientation.w = quat[3] move_group.set_pose_target(grasp_pose) return move_group.go(wait=True)

3.2 夹爪控制实现

虽然MoveIt!主要处理机械臂运动，但夹爪控制通常需要自定义Action：

class GripperCommander: def __init__(self): self._gripper_action = ActionClient( self, GripperCommand, '/panda_gripper/grip' ) def grip(self, width, force=10.0): goal_msg = GripperCommand.Goal() goal_msg.command.position = width goal_msg.command.max_effort = force self._gripper_action.send_goal_async(goal_msg)

3.3 携带物体移动

def move_with_object(move_group, target_pose): # 1. 提升物体避免碰撞 lift_pose = move_group.get_current_pose().pose lift_pose.position.z += 0.15 if not move_group.go(lift_pose, wait=True): return False # 2. 水平移动到目标位置上方 target_pose.position.z = lift_pose.position.z if not move_group.go(target_pose, wait=True): return False # 3. 下降放置 target_pose.position.z -= 0.15 return move_group.go(target_pose, wait=True)

3.4 错误处理与状态监控

def execute_pick_and_place(): try: # 初始化MoveIt!接口 move_group = MoveGroupCommander("panda_arm") gripper = GripperCommander() # 执行任务链 if not move_to_pregrasp(move_group, object_pose): raise RuntimeError("预抓取移动失败") if not gripper.grip(0.05): # 闭合夹爪 raise RuntimeError("抓取失败") if not move_with_object(move_group, target_pose): raise RuntimeError("物体移动失败") gripper.grip(0.08) # 松开夹爪 except Exception as e: move_group.stop() gripper.grip(0.08) # 确保夹爪松开 raise e

4. 高级技巧与性能优化

4.1 运动规划参数调优

通过调整MoveIt!的规划器参数可以提高成功率：

# 设置规划器参数 move_group.set_planning_time(5.0) # 增加规划时间 move_group.set_num_planning_attempts(10) # 尝试更多次 move_group.set_max_velocity_scaling_factor(0.5) # 降低速度

4.2 轨迹重定时与碰撞检测

# 获取规划结果 plan = move_group.plan() # 重定时轨迹以适应实际速度 from moveit_msgs.msg import RobotTrajectory trajectory = RobotTrajectory() trajectory.joint_trajectory = plan.joint_trajectory # 使用iterative_time_parameterization算法 from trajectory_processing import iterative_time_parameterization success = iterative_time_parameterization.compute_time_stamps( trajectory, velocity_scaling_factor=0.5 )

4.3 可视化调试工具

MoveIt!提供了强大的可视化工具：

from moveit_visual_tools import MoveItVisualTools visual_tools = MoveItVisualTools("panda_link0") # 显示末端执行器路径 visual_tools.publish_trajectory_line( plan.trajectory, move_group.get_end_effector_link() ) visual_tools.trigger()

在实际项目中，我发现将机械臂的加速度限制在最大值的70%可以显著提高运动稳定性，特别是在携带负载时。另外，为关键运动步骤添加视觉验证（如用OpenCV检查抓取位置）可以大幅提高任务可靠性。