机器人 / 系统课时
在仿真工位验收抓取放置任务
在仿真工位验收抓取放置任务
使用 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 为实际位置。
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 的差量更新,避免覆盖现有机器人状态。
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 等指定抓取接触连杆碰撞,不能全局关闭工件碰撞检测。

先启动下一节的场景状态监测脚本,等它等待服务并进入轮询后,再在终端 C 启动完整任务:
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 的返回码判断。
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()预期输出形式(位置误差是运行时计算值,不保证每次相同):
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,与对应轮次的监测输出一起归档。至少连续执行三轮:每轮均符合上述四类证据才可标记“通过”;只通过运动学场景测试时,应明确标注“未验证真实接触、摩擦和物体稳定性”,不能据此宣布真实机械臂抓取已安全可靠。