机器人 / 系统课时

在仿真工位验收抓取放置任务

在仿真工位验收抓取放置任务

使用 ROS 2 Jazzy、MoveIt 2 和 Panda 虚拟控制器,搭建有工作台与圆柱工件的抓取放置工位,并以规划结果、控制器反馈、附着状态和最终位置四类证据完成一次可复现验收。这里验证的是运动规划与场景状态链路,不代表已通过 Gazebo 接触动力学测试。

学习目标

  • 搭建具有明确坐标、尺寸和控制器状态的 Panda 验收场景。
  • 连续执行预抓取、夹紧、附着、搬运、释放与撤离,并定位中断阶段。
  • 使用规划场景服务记录状态迁移,按数值阈值判定任务是否通过。

固定工位和验收条件

使用 Ubuntu 24.04、ROS 2 Jazzy、MoveIt 2,以及已按MoveIt Task Constructor 官方完整教程编译的 moveit2_tutorials 和 mtc_tutorial。要求 mtc_tutorial 已实现教程的完整抓取与放置阶段,而非只有张开夹爪的初始代码。其规划组是 panda_arm、夹爪组是 hand,附着连杆为 panda_hand。

为了直接复用教程中的圆柱抓取目标,所有坐标取 world 系,单位为米:

对象 尺寸 中心位置或目标
table 工作台 0.80 × 0.80 × 0.05 (0.50, 0, -0.08),台面高度 -0.055
object 圆柱 高 0.10,半径 0.02 初始 (0.50, -0.25, 0)
放置位置 圆柱中心目标 (0.50, 0.25, 0)

工件底部在 -0.05,距台面 5 毫米,专门留出几何碰撞裕量;这是规划场景测试用的虚拟设置,并非真实支撑接触。合格判据为:能生成完整任务解、执行返回成功、工件经历世界对象→附着对象→世界对象,且释放后的中心位置误差不超过 0.02 米。不得把“RViz 看见物体移动”单独当作验收通过。

启动 Panda 与工作台

终端 A 启动官方 MTC 演示环境,保持运行;终端 B 执行后续检查。若工作空间名称不同,替换 ~/ws_moveit 为实际位置。

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
ros2 launch moveit2_tutorials mtc_demo.launch.py

预期结果:RViz 显示 Panda 机械臂和 Motion Planning Tasks 面板,move_group、机器人状态发布器及仿真控制器正常启动。该终端持续运行,不应在启动后退出。

在终端 B 检查控制器,再调用 MoveIt 的场景服务加入工作台。这里使用 ApplyPlanningScene 的差量更新,避免覆盖现有机器人状态。

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
ros2 control list_controllers
ros2 service call /apply_planning_scene moveit_msgs/srv/ApplyPlanningScene "{scene: {is_diff: true, robot_state: {is_diff: true}, world: {collision_objects: [{id: table, header: {frame_id: world}, pose: {position: {x: 0.5, y: 0.0, z: -0.08}, orientation: {w: 1.0}}, primitives: [{type: 1, dimensions: [0.8, 0.8, 0.05]}], primitive_poses: [{orientation: {w: 1.0}}], operation: 0}]}}}"

预期结果:panda_arm_controller、夹爪控制器及 joint_state_broadcaster 在输出中为 active;服务响应的 success 为 true,RViz 规划场景出现工作台。type: 1 是 SolidPrimitive.BOX,operation: 0 是 CollisionObject.ADD。若控制器名称因配置不同而变化,以本机实际控制器映射为准,必须确认机械臂与夹爪都能执行。

连续执行并观察阶段

验收路径依次是:夹爪张开、移动到预抓取、求取抓取姿态及接近路径、闭合夹爪、将 object 附着到 panda_hand、抬升、搬运、下降、解除附着、张开夹爪、撤离并返回 ready。附着阶段只允许与 panda_leftfinger、panda_rightfinger 等指定抓取接触连杆碰撞,不能全局关闭工件碰撞检测。

Panda 工位抓取搬运释放的执行与场景状态验收图

先启动下一节的场景状态监测脚本,等它等待服务并进入轮询后,再在终端 C 启动完整任务:

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
ros2 launch mtc_tutorial pick_place_demo.launch.py 2>&1 | tee panda_pick_place.log

预期结果:Motion Planning Tasks 面板显示各阶段的候选解与失败分支;终端 C 的完整示例应经过 task.plan(5) 取得解,再由 task.execute(...) 执行,最终返回 MoveItErrorCodes::SUCCESS。若使用只发布解而不执行的定制版本,必须显式执行选中的解;规划成功不等于执行完成。

自动记录状态与位置

将下面的完整脚本保存为 panda_acceptance.py,在启动终端 C 之前,先于另一个已加载 ROS 环境的终端执行 python3 panda_acceptance.py。它通过 /get_planning_scene 轮询世界碰撞物体与附着物体,最多等待 120 秒;保持 table 在场景中,并校验工件最终中心位置。脚本只判断场景状态与位置,任务规划和执行成功仍须结合终端 C 的返回码判断。

python
import math
import time

import rclpy
from moveit_msgs.msg import PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene

def main():
    rclpy.init()
    node = rclpy.create_node("panda_acceptance")
    client = node.create_client(GetPlanningScene, "/get_planning_scene")
    if not client.wait_for_service(timeout_sec=15.0):
        raise RuntimeError("get_planning_scene unavailable")

    stages = []
    deadline = time.monotonic() + 120.0
    while time.monotonic() < deadline:
        request = GetPlanningScene.Request()
        request.components.components = (
            PlanningSceneComponents.WORLD_OBJECT_GEOMETRY
            | PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
        )
        future = client.call_async(request)
        rclpy.spin_until_future_complete(node, future, timeout_sec=2.0)
        if not future.done():
            continue
        if future.exception():
            raise future.exception()

        scene = future.result().scene
        world = {obj.id: obj for obj in scene.world.collision_objects}
        attached = {
            item.object.id
            for item in scene.robot_state.attached_collision_objects
        }
        if "table" not in world or ("object" in world and "object" in attached):
            print("SCENE_FAIL: table missing or object duplicated")
            break

        state = None
        if "object" in attached:
            state = "ATTACHED"
        elif "object" in world:
            state = "RELEASED" if "ATTACHED" in stages else "READY"

        if state and (not stages or state != stages[-1]):
            stages.append(state)
            print(state)

        if state == "RELEASED":
            p = world["object"].pose.position
            error = math.dist((p.x, p.y, p.z), (0.50, 0.25, 0.0))
            print(f"position_error_m={error:.3f}")
            passed = stages[:3] == ["READY", "ATTACHED", "RELEASED"] and error <= 0.02
            print("SCENE_PASS" if passed else "SCENE_FAIL")
            break
        time.sleep(0.1)
    else:
        print(f"SCENE_FAIL: timeout; stages={stages}")

    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

预期输出形式(位置误差是运行时计算值,不保证每次相同):

text
READY
ATTACHED
RELEASED
position_error_m=<实际值>
SCENE_PASS

若监测启动时工件已经附着,可能错过 READY,必须重置场景并在下一次任务启动前重新监测。只有 SCENE_PASS、规划成功、执行返回 SUCCESS、控制器保持活动且 RViz 轨迹无不允许的碰撞全部成立,才记为工位验收通过。

记录失败与复测

失败位置 应记录的证据 修复与复测
目标或接近规划失败 MTC 失败阶段、目标坐标、IK 或碰撞信息 检查 world 参考系、台面位置,缩小接近距离或调整抓取方向,再重新规划
轨迹执行中断 控制器状态、动作返回码、最后完成阶段 检查 FollowJointTrajectory 链路和夹爪控制器,不在执行失败后继续附着
已闭爪但未附着 robot_state.attached_collision_objects 是否含 object 检查附着连杆、对象 ID 与允许接触的指尖连杆
释放后位置错误 世界对象位姿、位置误差、放置目标 检查 GeneratePlacePose 参考系、解除附着时机和撤离方向

每次测试保存任务开始时间、工位尺寸、ROS 版本、失败阶段、规划结果、执行错误码、监测输出和最终结论。运行日志由 tee 保存为 panda_pick_place.log,与对应轮次的监测输出一起归档。至少连续执行三轮:每轮均符合上述四类证据才可标记“通过”;只通过运动学场景测试时,应明确标注“未验证真实接触、摩擦和物体稳定性”,不能据此宣布真实机械臂抓取已安全可靠。

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

目录与阅读设置

阅读设置

17

记录一个想法

在仿真工位验收抓取放置任务

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

登录哪都通