机器人 / 系统课时

夹紧物体并更新附着碰撞体

夹紧物体并更新附着碰撞体

机械臂夹住物体后,需要让 MoveIt 2 将它从世界碰撞对象转换为随末端移动的附着碰撞体。否则,后续抬升和搬运规划可能错误地把物体留在原位,或漏检被夹物体与环境的碰撞。本课以 ROS 2 Jazzy 和 Panda 双指夹爪为例,完成夹爪闭合、碰撞对象附着及状态验证。

学习目标

  • 区分世界碰撞对象和附着碰撞对象,理解夹取前后的状态迁移。
  • 使用 Panda 夹爪控制接口执行闭合,并通过 AttachedCollisionObject 配置附着关系和允许接触的连杆。
  • 验证物体已从世界对象集合移入机器人附着对象集合,并掌握放置时的逆向操作。

先确认状态切换

在 MoveIt 2 中,CollisionObject 描述规划场景中的障碍物,AttachedCollisionObject 则描述连接到机器人某个连杆上的物体。附着后的物体随对应连杆运动,其几何体仍参与与环境及其他机器人连杆的碰撞检测。

以编号为 grasp_box 的工件为例,完整夹取过程包含以下状态:

  1. 夹取前:物体属于世界碰撞对象,具有固定的世界位姿;机械臂接近时需要避开它,除非专门设置接触许可。
  2. 闭合夹爪:夹爪控制器完成闭合动作,系统根据关节反馈以及可用的接触、力或视觉信息判断是否成功夹持。
  3. 建立附着:使用物体原有 ID,将其附着到 panda_hand,并允许左右手指与它接触。
  4. 搬运阶段:同一物体从世界对象集合中消失,进入机器人附着对象集合,后续轨迹规划将物体视为随手机械臂运动的几何体。
  5. 放置阶段:物体落到目标支撑面并松开夹爪后,解除附着,恢复为世界碰撞对象。

Panda 夹爪夹取前后世界碰撞对象与附着碰撞对象的状态迁移

这里有一个容易混淆的区别:关闭夹爪不等于附着物体。夹爪控制器负责真实或模拟关节运动,而附着操作负责修改 MoveIt 规划场景。两者必须按顺序执行,且附着不能替代物理抓取成功检测。

让夹爪完成闭合

先启动已经配置 Panda 机器人模型、move_group 和夹爪控制器的 MoveIt 2 环境。已安装 Panda MoveIt 示例资源时,可以在终端 A 执行:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py

预期结果:RViz 显示 Panda 机器人,move_group 和相关控制节点启动,终端持续运行。若使用自己的配置包,应改为实际的启动文件。

在终端 B 查询当前动作接口:

bash
source /opt/ros/jazzy/setup.bash
ros2 action list -t

采用 Jazzy 的 parallel_gripper_action_controller 时,预期输出包含以下一行:

text
/panda_hand_controller/gripper_cmd [control_msgs/action/ParallelGripperCommand]

不同版本或自定义控制器可能使用 control_msgs/action/GripperCommand。必须以实际查询结果为准,不能向不同类型的动作接口发送同一种消息。

对于上述 ParallelGripperCommand 接口,发送示例闭合目标:

bash
ros2 action send_goal /panda_hand_controller/gripper_cmd control_msgs/action/ParallelGripperCommand "{command: {name: [panda_finger_joint1], position: [0.0175]}}"

当动作服务器接收目标时,控制台输出包含:

text
Goal accepted with ID: <ID>

其中 <ID> 为每次请求生成的实际标识。命令随后等待动作结果,返回字段和最终状态取决于控制器及执行情况。

示例目标 0.0175 的单位为米,表示 panda_finger_joint1 的目标关节位置。在对称双指夹爪模型中,两侧手指各移动到约 17.5 mm 的位置时,名义总开口约为 35 mm。它不是夹持力,也不意味着一定已经夹住物体。

创建附着碰撞体

下面使用 PlanningSceneInterface 完成完整的场景迁移。示例约定工件尺寸为 35 mm × 35 mm × 70 mm,已经位于夹爪之间,物体中心在 panda_hand 坐标系的正 Z 方向约 0.11 m 处。

这组位姿只是与 Panda 演示几何相配合的教学参数,实际抓取时必须由感知结果或已标定的抓取位姿确定,不能直接套用到其他机器人。

先创建一个 ROS 2 C++ 包:

bash
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create grasp_scene_demo --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface moveit_msgs shape_msgs geometry_msgs

预期输出包含:

text
package name: grasp_scene_demo

创建后,包内应有 package.xml 和 CMakeLists.txt。在 grasp_scene_demo/src/attach_box.cpp 中写入以下完整程序:

cpp
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <string>

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  const auto logger = rclcpp::get_logger("grasp_scene_demo");
  moveit::planning_interface::PlanningSceneInterface scene;
  const std::string id = "grasp_box";

  // 演示环境若尚无工件,先补建世界碰撞对象。
  // 实际抓取系统应复用感知模块已发布的对象及其ID。
  if (scene.getObjects({id}).count(id) == 0) {
    moveit_msgs::msg::CollisionObject world_object;
    world_object.header.frame_id = "panda_hand";
    world_object.id = id;

    shape_msgs::msg::SolidPrimitive box;
    box.type = shape_msgs::msg::SolidPrimitive::BOX;
    box.dimensions = {0.035, 0.035, 0.07};

    geometry_msgs::msg::Pose pose;
    pose.orientation.w = 1.0;
    pose.position.z = 0.11;

    world_object.primitives.push_back(box);
    world_object.primitive_poses.push_back(pose);
    world_object.operation = moveit_msgs::msg::CollisionObject::ADD;

    if (!scene.applyCollisionObject(world_object)) {
      RCLCPP_ERROR(logger, "Failed to add world object");
      rclcpp::shutdown();
      return 1;
    }
  }

  // 只提供已有世界对象的ID,MoveIt会继承其几何与当前位姿。
  moveit_msgs::msg::AttachedCollisionObject attached;
  attached.link_name = "panda_hand";
  attached.object.id = id;
  attached.object.operation = moveit_msgs::msg::CollisionObject::ADD;
  attached.touch_links = {"panda_leftfinger", "panda_rightfinger"};

  if (!scene.applyAttachedCollisionObject(attached)) {
    RCLCPP_ERROR(logger, "Failed to attach object");
    rclcpp::shutdown();
    return 1;
  }

  // 同时验证世界集合与附着集合,避免只检查服务调用返回值。
  const auto attached_objects = scene.getAttachedObjects({id});
  const auto world_objects = scene.getObjects({id});
  const bool ok = attached_objects.count(id) == 1 &&
                  world_objects.count(id) == 0 &&
                  attached_objects.at(id).touch_links.size() == 2;

  if (ok) {
    RCLCPP_INFO(logger, "attached=%zu world=%zu touch_links=%zu",
                attached_objects.size(), world_objects.size(),
                attached_objects.at(id).touch_links.size());
  } else {
    RCLCPP_ERROR(logger, "Planning scene transition verification failed");
  }

  rclcpp::shutdown();
  return ok ? 0 : 1;
}

这段程序先使用 applyCollisionObject() 同步添加尚不存在的演示物体,再使用 applyAttachedCollisionObject() 执行附着。对于已经存在的世界对象,AttachedCollisionObject 只需携带相同 ID、目标连杆和接触关系,MoveIt 会使用世界对象已有的几何和位姿,并将它从世界集合迁移到机器人状态。

touch_links 仅列出允许与被夹物体接触的 panda_leftfinger 和 panda_rightfinger。这不会放行物体与机械臂其他连杆或工作台之间的碰撞,也不会取消物体自身的环境碰撞检测。

在生成的 CMakeLists.txt 中,保留已有的依赖查找和 ament_package(),并在 ament_package() 之前添加以下配置。

片段示例,依赖前面通过 ros2 pkg create 生成的完整 CMake 文件:

cmake
add_executable(attach_box src/attach_box.cpp)
ament_target_dependencies(attach_box
  rclcpp
  moveit_ros_planning_interface
  moveit_msgs
  shape_msgs
  geometry_msgs
)
install(TARGETS attach_box DESTINATION lib/${PROJECT_NAME})

这会创建可执行目标 attach_box,链接 MoveIt 规划场景接口及所需 ROS 消息库,并将程序安装到 ROS 2 可以通过 ros2 run 查找的位置。

编译并核对结果

保持 Panda 演示环境运行,在终端 B 编译并执行:

bash
cd ~/ros2_ws
colcon build --packages-select grasp_scene_demo
source install/setup.bash
ros2 run grasp_scene_demo attach_box

构建及规划场景更新成功时,预期输出包含以下关键内容;具体时间和其他日志会随环境变化:

text
Finished <<< grasp_scene_demo [<TIME>]
[INFO] [<TIME>] [grasp_scene_demo]: attached=1 world=0 touch_links=2

三个数字分别表示查询到了一个附着对象、零个同 ID 世界对象,以及两个允许接触的手指连杆。这里验证的是规划场景的数据状态,不是夹爪真实抓取力。

在 RViz 的 MotionPlanning 场景中,附着物体应出现在夹爪附近。随后以 panda_arm 规划一个低速抬升动作进行预览,物体的碰撞几何应跟随机械臂末端,而不是留在工作台原位置。需要确认更新已经同步到执行规划使用的 Planning Scene,才允许继续搬运。

出现异常时,可按以下顺序检查:

  1. 世界对象未消失:检查对象 ID 是否始终使用 grasp_box,以及附着查询是否真的返回该 ID。不要在附着后再单独发布一次世界对象 ADD。
  2. 附着接口失败:确认 move_group 正在运行,panda_hand 存在于机器人模型,且 /apply_planning_scene 服务可用。
  3. 工件显示位置不正确:检查创建世界对象时的 header.frame_id、物体尺寸和 primitive_poses,并确认机器人状态在建立物体时是最新的。
  4. 夹爪与物体出现碰撞报警:检查接触连杆的真实名称。不要为了消除报警把整条机械臂或所有世界障碍物加入允许碰撞集合。

放置后恢复世界障碍

放置物体需要执行附着操作的逆过程。机械臂首先将工件移动至目标支撑面,然后打开夹爪并确认物体已经释放,最后将工件恢复到世界碰撞对象集合。

在已有有效 scene 对象的 C++ 程序中,可以用以下代码解除附着。

片段示例,必须在物体完成放置并确认夹爪松开后调用:

cpp
moveit_msgs::msg::AttachedCollisionObject released;
released.object.id = "grasp_box";
released.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;

const bool success = scene.applyAttachedCollisionObject(released);

对于由已有世界对象附着而来的物体,MoveIt 在解除附着时会按照当前附着物体的全局位姿将它放回世界场景。随后应验证 getAttachedObjects({"grasp_box"}) 为空,而 getObjects({"grasp_box"}) 包含该对象。

如果物体实际落点与规划放置点存在偏差,应在感知重新定位后更新世界物体位姿,不能长期沿用估计位置。解除附着之前也不要直接删除物体的碰撞几何,否则机械臂回撤时可能遗漏与已放置工件的碰撞。

实际抓取流水线应以夹爪闭合反馈、附着状态确认、携物规划、释放确认、世界状态恢复作为连续的检查点。只有物理状态与规划场景状态保持一致,MoveIt 2 才能正确处理抓取与放置过程中不断变化的碰撞关系。

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

目录与阅读设置

阅读设置

17

记录一个想法

夹紧物体并更新附着碰撞体

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

登录哪都通