【MoveIt 2】利用MoveIt任务构造器实现多阶段物体抓取与放置任务

1. 为什么需要MoveIt任务构造器从“硬编码”到“乐高式”编程如果你曾经尝试用MoveIt 2的MoveGroupInterface来写一个完整的“抓取-移动-放置”任务我猜你大概率会经历一段“痛苦”的时光。我刚开始做机械臂应用的时候也是这么过来的写一个巨大的函数里面塞满了move_group.setPoseTarget()、move_group.plan()、move_group.execute()中间还要穿插着夹爪控制、碰撞检测的开关。代码很快就变成了一团乱麻调试起来更是噩梦——你很难说清楚到底是接近物体的路径出了问题还是抓取姿态的逆运动学解算失败了。MoveIt任务构造器MoveIt Task Constructor 简称MTC就是为了解决这个痛点而生的。你可以把它想象成一个专门为机器人复杂任务设计的“乐高积木”系统。它的核心思想特别直观把一个大任务拆成一个个小步骤它称之为“阶段”然后像搭积木一样把它们组合起来。举个例子一个完整的物体抓取与放置任务可以拆解成接近物体让机械臂末端移动到物体上方一个预备位置。生成抓取位姿计算手爪应该以什么角度去抓物体。执行抓取控制手爪闭合。提升物体垂直向上提起物体避免碰撞。移动到放置点带着物体运动到目标区域上方。放置物体打开手爪放下物体。撤退机械臂移开回到安全位置。在MTC里上面每一步都是一个独立的“阶段”Stage。MTC的强大之处在于它会自动帮你处理阶段之间的衔接和状态传递。比如“生成抓取位姿”阶段会产生多个可能的抓取姿势MTC会自动为每一个姿势去尝试规划“接近物体”和“执行抓取”的路径并最终筛选出整体最优、最可行的完整解决方案。这种“分而治之”的思路让代码结构变得无比清晰调试也变成了按阶段排查效率提升不是一点半点。2. 核心概念拆解生成器、传播器、连接器与容器要玩转MTC得先理解它的几个核心“积木类型”。官方文档可能讲得比较学术我用我自己的理解给你翻译一下。2.1 阶段的三种“流向”MTC里的阶段根据它们如何产生和传递数据分成了三类。你可以把它们想象成水管数据机器人的状态、位姿等就是水流。生成器Generator像是一个水源。它自己凭空产生数据解决方案然后同时向“上游”和“下游”传递。最典型的例子就是CurrentState阶段它获取机器人当前的真实关节状态作为任务的起点。另一个例子是GenerateGraspPose它不关心机器人现在在哪只根据物体模型生成一堆可能的抓取位姿。传播器Propagator像是一个水管工。它接收来自一个邻居上游或下游的数据进行加工处理然后把结果传递给对面的邻居。比如MoveRelative阶段你给我一个起始状态我帮你计算一个沿着指定方向移动一段距离后的新状态然后把这个新状态传下去。它的工作依赖于输入。连接器Connector像是一个管道接头。它本身不产生新数据也不做复杂的变换它的唯一目的就是尝试把上游和下游两个状态给连接起来。最常用的就是Connect阶段它调用底层的运动规划器比如OMPL尝试规划一条无碰撞的运动轨迹把前后两个阶段的状态连起来。如果连不上这个方案就失败了。理解这三者的区别对于后面设计任务流程至关重要。生成器是任务的起点或分支点传播器定义了确定的动作如直线移动、关节插值连接器则负责处理那些需要复杂规划的自由运动。2.2 容器管理阶段的“工具箱”单个阶段能力有限MTC用“容器”来把多个阶段打包成一个更复杂的单元。主要有三种序列容器Serial Container这是最常用的容器。它把里面的子阶段按顺序串起来只有当前一个阶段成功了才会尝试下一个。整个序列容器最终只输出一条从头到尾都成功的完整路径。我们的“抓取”和“放置”子任务通常就是用序列容器来组织的。包装器Wrapper它只包装一个子阶段用来对这个子阶段的结果进行修改或过滤。比如ComputeIK就是一个典型的包装器它包裹一个GenerateGraspPose阶段对后者生成的每一个抓取位姿进行逆运动学解算只保留那些有解且合理的位姿。并行容器Parallel Container它里面可以放多个子阶段并且会尝试所有子阶段。这常用于实现备选方案。比如你可以设置一个并行容器里面一个子阶段尝试用右手抓取另一个子阶段尝试用左手抓取MTC会并行计算最后选用成功且成本最低的那个方案。在实际项目中我经常用序列容器来构建任务的主干流程用包装器来处理位姿生成和IK解算这种“生成-筛选”逻辑只有在需要设计复杂策略如双手抓取备选时才会用到并行容器。3. 手把手搭建一个抓取与放置任务光说不练假把式我们直接来看代码。下面我会基于一个经典的Panda机械臂抓取圆柱体场景带你一步步用MTC构建任务。假设你已经有一个配置好的MoveIt 2工作空间和Panda机器人模型。3.1 项目初始化与任务骨架首先我们创建一个任务对象并设置一些全局属性。这些属性就像任务的“全局变量”后面的阶段可以继承使用。mtc::Task task; task.stages()-setName(Pick and Place Demo); task.loadRobotModel(node_); // 加载机器人模型 // 定义一些后面会频繁用到的组名和框架名 const std::string arm_group_name panda_arm; const std::string hand_group_name hand; const std::string hand_frame panda_hand; // 设置任务级属性子阶段可以通过继承来获取这些值 task.setProperty(group, arm_group_name); // 默认运动规划组 task.setProperty(eef, hand_group_name); // 末端执行器组 task.setProperty(ik_frame, hand_frame); // 逆运动学解算的参考框架接下来添加任务的起点——当前状态。这是一个生成器阶段它捕获机器人启动时的关节角度。auto stage_state_current std::make_uniquemtc::stages::CurrentState(current); task.add(std::move(stage_state_current));然后我们定义几种常用的规划器。不同的阶段适合用不同的规划器。PipelinePlanner功能最全通常调用OMPL等采样规划器适合解决复杂的、有障碍物的路径规划。JointInterpolationPlanner简单快速直接在关节空间线性插值适合像打开/关闭夹爪这种简单、已知明确关节目标值的动作。CartesianPath笛卡尔空间规划器控制末端执行器沿直线运动非常适合“接近”、“提升”、“撤退”这种需要保持末端姿态的精确移动。auto sampling_planner std::make_sharedmtc::solvers::PipelinePlanner(node_); auto interpolation_planner std::make_sharedmtc::solvers::JointInterpolationPlanner(); auto cartesian_planner std::make_sharedmtc::solvers::CartesianPath(); cartesian_planner-setMaxVelocityScalingFactor(1.0); cartesian_planner-setMaxAccelerationScalingFactor(1.0); cartesian_planner-setStepSize(.01); // 笛卡尔路径的步长影响精度和平滑度3.2 构建“抓取”序列容器抓取是一个典型的顺序过程我们用序列容器来组织。auto grasp std::make_uniquemtc::SerialContainer(pick object); // 将任务的属性暴露给这个容器内的所有子阶段 task.properties().exposeTo(grasp-properties(), {eef, group, ik_frame}); grasp-properties().configureInitFrom(mtc::Stage::PARENT, {eef, group, ik_frame});第一步接近物体。我们使用MoveRelative阶段让手爪沿其自身Z轴方向即指尖方向向下移动一段距离接近物体。{ auto stage std::make_uniquemtc::stages::MoveRelative(approach object, cartesian_planner); stage-properties().set(marker_ns, approach); // 在RViz中显示标记时用的命名空间 stage-properties().set(link, hand_frame); // 相对移动所参考的连杆 stage-setMinMaxDistance(0.05, 0.1); // 移动距离在5cm到10cm之间 geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id hand_frame; vec.vector.z -1.0; // 沿手爪坐标系Z轴负方向移动靠近物体 stage-setDirection(vec); grasp-insert(std::move(stage)); }第二步生成并筛选抓取位姿。这是抓取任务的核心。我们先用一个GenerateGraspPose阶段围绕物体生成多个可能的抓取方向位姿。这里我设置angleDelta为M_PI/630度意味着它会在绕物体轴线的多个角度上采样。{ auto stage std::make_uniquemtc::stages::GenerateGraspPose(generate grasp pose); stage-properties().set(marker_ns, grasp_pose); stage-setPreGraspPose(open); // 生成的是“预抓取”位姿即手爪张开时的位姿 stage-setObject(object); // 目标物体的ID需要在规划场景中添加 stage-setAngleDelta(M_PI / 6); // 采样角度间隔 stage-setMonitoredStage(current_state_ptr); // 关联到当前状态以获取物体信息但是生成的这些位姿只是几何位姿不一定有对应的、无碰撞的机器人关节解。所以我们需要一个ComputeIK包装器来解算逆运动学。// 定义从物体坐标系到末端执行器坐标系的变换。 // 这个变换决定了手爪相对于物体的抓取位置和姿态。 // 例如这里设置了一个旋转并让手爪在物体Z轴正方向偏移0.1米。 Eigen::Isometry3d grasp_frame_transform; Eigen::Quaterniond q Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()); grasp_frame_transform.linear() q.matrix(); grasp_frame_transform.translation().z() 0.1; auto wrapper std::make_uniquemtc::stages::ComputeIK(grasp pose IK, std::move(stage)); wrapper-setMaxIKSolutions(8); // 最多尝试8个逆运动学解 wrapper-setMinSolutionDistance(1.0); // 解之间的最小关节空间距离用于去重 wrapper-setIKFrame(grasp_frame_transform, hand_frame); // 设置IK解算的框架 wrapper-properties().configureInitFrom(mtc::Stage::PARENT, {eef, group}); wrapper-properties().configureInitFrom(mtc::Stage::INTERFACE, {target_pose}); grasp-insert(std::move(wrapper)); }第三步允许碰撞。在手爪闭合抓住物体之前我们需要暂时允许手爪的连杆与物体之间发生碰撞否则规划器会认为抓取动作本身就是在碰撞从而规划失败。{ auto stage std::make_uniquemtc::stages::ModifyPlanningScene(allow collision (hand,object)); auto hand_links task.getRobotModel()-getJointModelGroup(hand_group_name)-getLinkModelNamesWithCollisionGeometry(); stage-allowCollisions(object, hand_links, true); // 第三个参数为true表示允许碰撞 grasp-insert(std::move(stage)); }第四步闭合手爪。使用简单的关节插值规划器移动到夹爪闭合的预设关节位置在SRDF中通常定义为close。{ auto stage std::make_uniquemtc::stages::MoveTo(close hand, interpolation_planner); stage-setGroup(hand_group_name); stage-setGoal(close); grasp-insert(std::move(stage)); }第五步附着物体。告诉规划场景现在物体已经被“拿”在手上了会随着机械臂一起运动。我们需要保存这个阶段的指针因为后面的放置阶段需要知道物体被附着在哪。{ auto stage std::make_uniquemtc::stages::ModifyPlanningScene(attach object); stage-attachObject(object, hand_frame); attach_object_stage stage.get(); // 保存指针供后续使用 grasp-insert(std::move(stage)); }第六步提升物体。抓取完成后垂直向上提起物体离开桌面。{ auto stage std::make_uniquemtc::stages::MoveRelative(lift object, cartesian_planner); stage-properties().configureInitFrom(mtc::Stage::PARENT, {group}); stage-setMinMaxDistance(0.1, 0.2); stage-setIKFrame(hand_frame); stage-properties().set(marker_ns, lift); geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id world; // 在世界坐标系下垂直向上移动 vec.vector.z 1.0; stage-setDirection(vec); grasp-insert(std::move(stage)); }至此抓取序列容器构建完毕将其加入主任务。task.add(std::move(grasp));3.3 连接抓取与放置抓取和放置是两个相对独立的子任务中间通常需要一段自由空间运动。我们用一个Connect阶段来衔接。auto stage_move_to_place std::make_uniquemtc::stages::Connect( move to place, mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } } ); stage_move_to_place-setTimeout(5.0); // 规划超时时间 stage_move_to_place-properties().configureInitFrom(mtc::Stage::PARENT); task.add(std::move(stage_move_to_place));3.4 构建“放置”序列容器放置过程是抓取的逆过程。auto place std::make_uniquemtc::SerialContainer(place object); task.properties().exposeTo(place-properties(), {eef, group, ik_frame}); place-properties().configureInitFrom(mtc::Stage::PARENT, {eef, group, ik_frame});第一步生成放置位姿。我们使用GeneratePlacePose阶段。这里我们简单地将放置目标点设置在物体当前坐标系Y轴正方向0.5米处只是一个示例。setMonitoredStage是关键它告诉这个阶段物体当前附着在attach_object_stage所描述的状态下。{ auto stage std::make_uniquemtc::stages::GeneratePlacePose(generate place pose); stage-properties().set(marker_ns, place_pose); stage-setObject(object); geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id object; target_pose_msg.pose.position.y 0.5; target_pose_msg.pose.orientation.w 1.0; stage-setPose(target_pose_msg); stage-setMonitoredStage(attach_object_stage); // 关联到附着阶段 auto wrapper std::make_uniquemtc::stages::ComputeIK(place pose IK, std::move(stage)); wrapper-setMaxIKSolutions(2); wrapper-setMinSolutionDistance(1.0); wrapper-setIKFrame(object); wrapper-properties().configureInitFrom(mtc::Stage::PARENT, {eef, group}); wrapper-properties().configureInitFrom(mtc::Stage::INTERFACE, {target_pose}); place-insert(std::move(wrapper)); }第二步打开手爪。放置物体。{ auto stage std::make_uniquemtc::stages::MoveTo(open hand, interpolation_planner); stage-setGroup(hand_group_name); stage-setGoal(open); place-insert(std::move(stage)); }第三步禁止碰撞与分离物体。物体放下后需要重新禁止手爪与物体的碰撞并将物体从手爪上分离使其恢复为场景中独立的、静止的物体。{ auto stage std::make_uniquemtc::stages::ModifyPlanningScene(forbid collision (hand,object)); auto hand_links task.getRobotModel()-getJointModelGroup(hand_group_name)-getLinkModelNamesWithCollisionGeometry(); stage-allowCollisions(object, hand_links, false); // false表示禁止碰撞 place-insert(std::move(stage)); } { auto stage std::make_uniquemtc::stages::ModifyPlanningScene(detach object); stage-detachObject(object, hand_frame); place-insert(std::move(stage)); }第四步放置后撤退。手爪打开后稍微向后移动避免与放置的物体发生干涉。{ auto stage std::make_uniquemtc::stages::MoveRelative(retreat after place, cartesian_planner); stage-properties().configureInitFrom(mtc::Stage::PARENT, {group}); stage-setMinMaxDistance(0.05, 0.1); stage-setIKFrame(hand_frame); stage-properties().set(marker_ns, retreat); geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id world; vec.vector.z -1.0; // 与世界坐标系Z轴负方向一致即向下或远离 stage-setDirection(vec); place-insert(std::move(stage)); }将放置容器加入主任务。task.add(std::move(place));最后一步返回Home位置。任务完成后让机械臂回到一个预设的安全姿态。auto stage_move_home std::make_uniquemtc::stages::MoveTo(return home, interpolation_planner); stage_move_home-setGroup(arm_group_name); stage_move_home-setGoal(ready); // “ready”是SRDF中定义的一个命名姿态 task.add(std::move(stage_move_home));4. RViz可视化与调试让规划过程一目了然代码写好了但你怎么知道它规划得对不对MTC与RViz的深度集成提供了无与伦比的可视化调试能力。首先你需要确保启动了包含moveit_task_constructor_visualization插件的RViz配置。在教程的启动文件中通常已经配好了。启动后在RViz的显示面板里找到MotionPlanning Task并添加它。当你运行MTC节点后这个插件面板就会亮起来。它的左侧以树形结构展示了你任务的所有阶段从最外层的任务到内部的序列容器再到每一个具体的阶段如“approach object”、“generate grasp pose”等。这个视图让你对整个任务的层次结构一目了然。分阶段调试的精髓在这里你可以用鼠标点击树形结构中的任何一个阶段。选中后右侧面板会列出这个阶段所有尝试过的解决方案并按照成本通常是路径长度排序。每个解决方案前面可能有一个对勾成功或叉号失败。这时最神奇的事情发生了点击右侧列表中的任何一个解决方案RViz的3D视图里就会动画演示从任务开始一直到当前选中阶段为止的完整运动轨迹。比如你点击“generate grasp pose”阶段下的某个解决方案机器人就会运动到对应的那个抓取预位姿。这让你可以逐个检查每个抓取位姿是否合理接近路径是否平滑。如果你发现“close hand”阶段失败了你可以点开它前面的“generate grasp pose”阶段看看是哪个抓取位姿导致了后续的失败然后回溯到“ComputeIK”阶段看看是不是那个位姿的IK解本身就有问题或者碰撞检查没通过。这种按阶段、按解决方案的逐层调试方法对于理解复杂任务为何失败比传统的单步跟踪代码高效太多了。在终端里MTC也会打印出每个阶段的传播结果用箭头和数字表示前后向传播的成功与失败数量这同样是定位问题阶段的有力工具。5. 实战避坑指南与性能优化根据我自己的项目经验直接照搬教程代码可能会遇到几个典型的“坑”。第一个坑物体位姿与抓取变换矩阵。GenerateGraspPose和ComputeIK中涉及的坐标系变换是新手最容易出错的地方。GenerateGraspPose生成的位姿是物体坐标系下的。而ComputeIK的setIKFrame函数第一个参数是“工具框架”Tool Frame相对于“末端执行器框架”EEF Frame的变换。很多同学会把这里搞反或者单位弄错米和毫米。我的建议是先在RViz里用交互式标记Interactive Marker手动调整一个看起来正确的抓取位姿然后打印出这个位姿的变换矩阵再填到代码里这样最稳妥。第二个坑规划超时与解的数量。Connect阶段或者PipelinePlanner可能会因为环境复杂而规划超时。适当增加setTimeout的值比如从5秒加到30秒是常规操作。另外GenerateGraspPose的setAngleDelta和ComputeIK的setMaxIKSolutions需要权衡。角度增量越小、IK解数量越多找到可行方案的概率越大但计算时间也呈指数增长。在实际应用中我通常会先设置一个较大的角度增量如90度和较少的IK解数量如3个进行快速测试确保流程通顺然后再调小增量、增加数量来寻找更优解。第三个坑碰撞检测的开关时机。allowCollisions的调用时机必须精确。必须在手爪闭合之前允许碰撞在放置后手爪打开之后再禁止碰撞。顺序错了规划一定会失败。另外注意allowCollisions操作的对象是链接名Link Name而不是关节组名。用getLinkModelNamesWithCollisionGeometry()来获取是正确的做法。关于性能优化对于简单的拾放任务MTC的开销是可以接受的。但如果你的任务非常复杂阶段很多或者GenerateGraspPose采样非常密集规划时间可能会很长。这时可以考虑使用ParallelContainer来并行计算多个备选抓取策略谁先成功就用谁的。在Connect阶段尝试不同的底层规划器配置比如换用RRT-Connect这种更快的规划算法。精简不必要的阶段或者将一些固定的、无碰撞的路径用MoveTo关节插值代替Connect来规划。最后MTC的学习曲线前期确实有点陡但一旦你理解了“阶段-容器”这个范式并且习惯了RViz的可视化调试方式你会发现用它来构建和维护复杂的机器人任务比传统方法要轻松和可靠得多。它强迫你写出结构清晰的代码而清晰的代码本身就是最好的文档。