机器人 / 系统课时

组合搬运释放动作与状态切换

组合搬运释放动作与状态切换

把一次 Panda 搬运拆成可检查的接近、抓取、抬升、搬运、放置和撤离阶段。每步都要核对夹爪状态、物体附着关系与执行结果;如果抓取或释放失败,立即阻断后续阶段,并修复规划场景。

学习目标

  • 定义机械臂阶段、夹爪状态和物体附着状态的合法转换。
  • 用 MTC 阶段组织抓取与释放,控制接触碰撞许可。
  • 在失败时阻断后续动作,核对释放后的世界碰撞体。

阶段与状态必须同步

以 Panda 的 panda_arm(机械臂)、hand(夹爪)、panda_hand(附着连杆)和场景物体 object 为例。夹爪的 open、close 是示例 SRDF 的命名姿态,不是所有机器人通用的名称。

顺序阶段 规划或执行动作 完成后夹爪 完成后物体
接近 打开夹爪,接近抓取位并直线前进约 0.10 m open world
抓取 允许手指接触,执行 close;确认夹持后附着 closed attached
抬升 沿世界 Z 正向上移约 0.10 m closed attached
搬运 在附着物体参与碰撞检测的条件下规划至放置区 closed attached
放置 降至经验证的释放高度 closed attached
释放 open 成功后恢复接触限制,再解除附着 open world
撤离 沿无碰撞方向后退约 0.10 m open world

world 表示物体属于规划场景的环境碰撞体;attached 表示物体随 panda_hand 移动并参与机械臂碰撞检测。同一个物体 ID 不应同时作为世界物体和附着物体存在。附着只是规划场景状态,不等于传感器证明实际抓牢了物体;真机必须结合夹爪位置、力或抓取检测结果放行抬升。

Panda 抓取搬运释放的动作阶段、夹爪与物体附着状态

在 MTC 中连接动作和场景变更

将抓取阶段放入 mtc::SerialContainer("pick object"),放置阶段放入 mtc::SerialContainer("place object")。接近、抬升、撤离使用 MoveRelative 配 CartesianPath;抓取位和放置位用 GenerateGraspPose、GeneratePlacePose 加 ComputeIK 求解;两处远距离移动由 Connect 配 PipelinePlanner 连接。可先将接近距离设为 0.10~0.15 m、抬升和撤离设为 0.10~0.30 m,再按工作台净空与可达性调整。

以下是片段示例:放在已有 task、grasp、place、interpolation_planner、cartesian_planner 的 MTC createTask() 对应容器构建位置。task 已加载 Panda 模型,场景已加入 ID 为 object 的碰撞体;完整的 IK、轨迹阶段和容器连接由 MTC 抓放示例提供,此片段不能单独编译。

cpp
namespace mtc = moveit::task_constructor;

// 夹持时仅允许物体与夹爪碰撞几何接触
const auto hand_links = task.getRobotModel()
    ->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry();
auto allow = std::make_unique<mtc::stages::ModifyPlanningScene>("allow contact");
allow->allowCollisions("object", hand_links, true);
grasp->insert(std::move(allow));

auto close = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
close->setGroup("hand");
close->setGoal("close");
grasp->insert(std::move(close));

auto attach = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
attach->attachObject("object", "panda_hand");
grasp->insert(std::move(attach));

// 物体已附着,抬升方向使用 world 坐标系
auto lift = std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);
lift->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
lift->setMinMaxDistance(0.1, 0.3);
lift->setIKFrame("panda_hand");
geometry_msgs::msg::Vector3Stamped upward;
upward.header.frame_id = "world";
upward.vector.z = 1.0;
lift->setDirection(upward);
grasp->insert(std::move(lift));

// place 中的放置位姿求解要安排在 open 之前
auto open = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
open->setGroup("hand");
open->setGoal("open");
place->insert(std::move(open));

auto forbid = std::make_unique<mtc::stages::ModifyPlanningScene>("restore collision");
forbid->allowCollisions("object", hand_links, false);
place->insert(std::move(forbid));

auto detach = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
detach->detachObject("object", "panda_hand");
place->insert(std::move(detach));

// 物体回到 world 后,机械臂才能撤离
auto retreat = std::make_unique<mtc::stages::MoveRelative>("retreat", cartesian_planner);
retreat->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
retreat->setMinMaxDistance(0.1, 0.3);
retreat->setIKFrame("panda_hand");
geometry_msgs::msg::Vector3Stamped backward;
backward.header.frame_id = "world";
backward.vector.x = -1.0;
retreat->setDirection(backward);
place->insert(std::move(retreat));

完整的抓取位姿生成、搬运连接与放置位姿求解可参照 MoveIt 官方 MTC 抓放示例。

allowCollisions(true) 只为夹爪与目标物体放开必要接触,不能对工作台或其他障碍物一并放行。真实夹爪执行 close 后仍须核对抓取反馈,再允许更新附着状态;规划成功本身不代表执行已经成功。

运行完整抓放演示

在已安装或从相应 ROS 2 分支编译 moveit_task_constructor_demo 的环境中,先检查安装的是包含该演示包的 MoveIt 2 环境。下面以 ROS 2 Jazzy 为例;其他发行版或源码工作空间要加载对应的 setup.bash。

bash
source /opt/ros/jazzy/setup.bash
ros2 pkg prefix moveit_task_constructor_demo

预期输出:

text
<PATH>

<PATH> 是实际安装前缀,各机器不同。若提示包不存在,先安装或编译 moveit_task_constructor_demo。随后在终端 A 启动 Panda 仿真与 RViz:

bash
ros2 launch moveit_task_constructor_demo demo.launch.py

预期结果:RViz 显示 Panda 模型和 Motion Planning Tasks 阶段面板,终端 A 保持运行;不同版本的日志时间和进程 ID 不固定。

另开终端 B,启动完整抓放任务:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_task_constructor_demo run.launch.py exe:=pick_place_demo

预期结果:Motion Planning Tasks 面板出现抓取与放置阶段及候选解。选择阶段可以查看对应轨迹;是否由演示程序自动执行取决于所用版本和运行配置。只在仿真中验证,确认碰撞场景和控制器配置后才考虑真机。

失败门控与释放后恢复

每个阶段必须等执行结果,而不是等固定的 2 秒就切到下一段。MTC 的 task.plan() 表示找到候选解;task.execute() 的成功返回才表示执行链报告成功。真机还要读取夹爪动作结果和实际夹持反馈。

失败位置 禁止的后续动作 处理方式
接近或 IK 无解 禁止闭合夹爪 检查参考帧、障碍物和目标朝向,重新规划
夹爪闭合失败 禁止 attach 和抬升 保持物体为 world,确认夹爪状态再恢复
抬升或搬运失败 禁止提前 open 或 detach 停止运动,保留 attached,检查实际位置和负载
释放或场景更新失败 禁止直接撤离或重新抓取 核实手指位置、物体支撑及规划场景,再决定恢复路径

正常释放后,用 PlanningSceneInterface::getAttachedObjects({"object"}) 检查附着列表为空,再用 getObjects({"object"}) 检查世界场景中存在该物体。如果放置后世界物体缺失或位姿错误,应使用观测到的放置位姿更新 CollisionObject,再通过 applyCollisionObject() 同步写入规划场景。比如 0.10 m 高的箱体放在表面高度 0.40 m 的台面上,箱体中心高度应为 0.45 m,而不是 0.40 m。

验收时至少满足三项:抬升与搬运期间物体仅处于 attached;夹爪张开且解除附着后物体仅处于 world;撤离轨迹与已放置物体、工作台都没有碰撞。三项全部成立,才算完成一次抓放循环。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

组合搬运释放动作与状态切换

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通