Graspness推理服务接入ROS2：从点云到机械臂抓取实战

简介这份资源面向机器人抓取方向的研究者与工程开发者聚焦无序3D场景下的6自由度抓取难题。其核心是集成Graspness推理服务完成抓取姿态预测并借助ROS2与MoveIt2打通从姿态输出到机械臂运动规划、避障与执行的全链路可用于家庭、仓库、零售等杂乱环境的物品抓取验证。压缩包共215个文件约2.2MB以37个py脚本、17个cpp与18个hpp源码、26个stl模型、14个xacro与13个yaml配置、13个dae网格及rviz、srdf等为主覆盖推理桥接、UR机械臂与夹爪建模、手眼标定及运动规划配置。资源另附说明文档与示例资料帮助读者理解系统设计原理与各组件协作方式。目前已有36人学习适合具备ROS2基础、希望快速复现Graspness抓取流程的中高级开发者参考。1. 从一堆散乱零件到稳定抓取Graspness 推理服务接进 ROS2 到底解决了什么机械臂在结构化产线上抓一个固定姿态的工件和在一个堆满杂物的桌面上抓一个随机摆放的零件难度完全不在一个量级。前者用示教点位就能跑后者需要系统在几秒内回答三个问题抓哪里、怎么抓、抓完怎么过去。Graspness 这类基于点云的无序 3D 场景抓取方法解决的正是第一个和第二个问题——它不依赖物体 CAD 模型直接从深度相机拍到的杂乱点云里预测 6-Dof 抓取姿态。但预测出姿态只是半成品真正让机械臂动起来还得把推理结果喂给 ROS2再由 MoveIt2 做运动规划与执行。这套链路的价值在于它把「感知算法」和「机器人执行」之间的黑匣子打通了让一个在论文里跑分很高的抓取网络变成一台能实际作业的设备。适合谁看正在做 ROS2 机器人项目、手里有深度相机和机械臂、想把抓取算法落地的工程师以及想理解 6-Dof 抓取姿态预测如何与 MoveIt2 协同的从业者。2. Graspness 推理服务与 ROS2 节点设计先想清楚数据怎么流2.1 为什么把推理做成独立服务而不是塞进规划节点很多人的第一版实现是把点云预处理、Graspness 推理、抓取姿态筛选、MoveIt2 规划全写在一个 ROS2 节点里。跑起来能出结果但调试时非常痛苦推理慢一点整个节点卡住MoveIt2 的规划线程也跟着阻塞想换模型得把规划代码一起重新编译。常见做法是把 Graspness 推理封装成一个独立的 ROS2 服务端节点规划节点作为客户端按需请求。这样做的直接好处是推理和规划解耦推理节点可以单独重启、单独压测规划节点也能在等待推理结果时保持响应。从 ROS2 的通信模型看这里适合用 Service 而不是 Topic。抓取姿态预测是一次请求一次响应有明确的输入输出边界不需要持续流式传输。Action 更适合长耗时且需要中途反馈的任务而 Graspness 推理通常在几百毫秒到两秒之间用 Service 足够。如果后续要做连续抓取可以在上层再包一层 Action 来管理「请求推理→规划→执行」的完整流程。2.2 定义服务接口请求点云返回抓取姿态数组服务接口的设计直接决定后续代码好不好写。请求里至少要包含一帧有序或无序点云、相机内参、以及可选的抓取数量上限。响应里返回一组 6-Dof 抓取姿态每个姿态包含平移、旋转四元数、抓取宽度和置信度分数。# 在 ROS2 功能包里定义服务接口 mkdir -p ~/grasp_ws/src/grasp_msgs/srv cd ~/grasp_ws/src/grasp_msgs/srv cat PredictGrasp.srv EOF sensor_msgs/PointCloud2 cloud sensor_msgs/CameraInfo camera_info int32 max_grasps --- geometry_msgs/PoseArray grasp_poses float32[] widths float32[] scores bool success string message EOF这段接口定义里cloud是输入点云camera_info用于把像素坐标反投影到相机坐标系max_grasps控制返回的候选数量。响应中的grasp_poses是PoseArray每个Pose的position是抓取中心点orientation是夹爪接近方向。widths和scores与grasp_poses一一对应规划节点可以根据分数排序后依次尝试。success和message用于区分「推理失败」和「推理成功但没找到可行抓取」这两种情况排错时非常有用。提示PoseArray的header.frame_id必须和点云坐标系一致否则 MoveIt2 规划时会因为坐标系不匹配直接报错。2.3 推理节点的主循环从订阅点云到发布服务响应推理节点启动后不需要持续订阅点云而是在服务被调用时才处理请求。这样避免无谓的计算浪费也避免点云缓存带来的时序错乱。import rclpy from rclpy.node import Node from grasp_msgs.srv import PredictGrasp import numpy as np class GraspnessServer(Node): def __init__(self): super().__init__(graspness_server) self.srv self.create_service( PredictGrasp, predict_grasp, self.handle_predict) self.model self.load_model() def load_model(self): # 实际项目中在这里加载 Graspness 权重 # 常见做法是导出 ONNX 或 TorchScript 后用 onnxruntime 推理 self.get_logger().info(Graspness model loaded) return None def handle_predict(self, request, response): try: points self.pointcloud2_to_numpy(request.cloud) if points.shape[0] 100: response.success False response.message too few points return response grasps, widths, scores self.infer(points) response.grasp_poses self.to_pose_array(grasps) response.widths widths.tolist() response.scores scores.tolist() response.success True response.message ok except Exception as e: response.success False response.message str(e) return response def infer(self, points): # 调用 Graspness 推理返回 N x 7 的位姿和对应宽度分数 # 这里用随机数据占位实际替换为模型前向 n 10 grasps np.random.rand(n, 7).astype(np.float32) widths np.random.rand(n).astype(np.float32) * 0.08 scores np.random.rand(n).astype(np.float32) return grasps, widths, scoreshandle_predict是服务回调收到请求后先做点云数量检查少于 100 个点直接返回失败避免模型在空输入上崩溃。infer里是模型前向的占位实际项目中需要把 Graspness 的网络导出成 ONNX 或 TorchScript再用 onnxruntime 或 libtorch 加载。返回的grasps是 N 行 7 列前三维是位置后四维是四元数。to_pose_array负责把 numpy 数组转成 ROS2 消息注意四元数顺序是 x, y, z, w。2.4 点云预处理把深度相机的原始数据变成 Graspness 能吃的输入Graspness 对输入点云的质量敏感。深度相机直出的点云通常包含桌面、地面、支架等背景直接送进网络会稀释有效抓取区域。常见做法是先做直通滤波限制工作空间再用 RANSAC 去掉最大平面最后用体素下采样控制点数。def preprocess(self, points): # points: N x 3 numpy array # 1. 直通滤波保留机械臂工作空间内的点 mask (points[:, 2] 0.2) (points[:, 2] 1.2) points points[mask] # 2. 体素下采样体素边长 5mm voxel_size 0.005 indices np.floor(points / voxel_size).astype(np.int32) _, unique_idx np.unique(indices, axis0, return_indexTrue) points points[unique_idx] # 3. 去掉点数过少的区域避免噪声 if points.shape[0] 20000: points points[np.random.choice(points.shape[0], 20000, replaceFalse)] return points直通滤波的阈值要根据实际相机安装高度和机械臂臂展调整z轴范围太小会切掉目标物体太大则引入背景。体素下采样的边长 5mm 是一个经验值物体尺寸在几厘米到十几厘米时比较合适太小则点数爆炸太大则抓取精度下降。最后限制最大点数是为了控制推理时间Graspness 的推理耗时和点数近似线性20000 点是一个在精度和速度之间比较平衡的值。3. 从抓取姿态到机械臂运动MoveIt2 规划与执行的落地细节3.1 把 PoseArray 转成 MoveIt2 能理解的抓取目标推理服务返回的PoseArray是在相机坐标系下的而 MoveIt2 规划需要的是机械臂基坐标系下的目标。中间要经过手眼标定矩阵变换。假设相机安装在机械臂末端标定矩阵为T_ee_cam当前末端位姿为T_base_ee则抓取姿态在基坐标系下为T_base_grasp T_base_ee * T_ee_cam * T_cam_grasp。import tf2_ros from geometry_msgs.msg import PoseStamped import tf_transformations class GraspPlanner(Node): def __init__(self): super().__init__(grasp_planner) self.tf_buffer tf2_ros.Buffer() self.tf_listener tf2_ros.TransformListener(self.tf_buffer, self) self.move_group MoveGroupInterface(self, arm_group) def pose_to_base(self, pose_cam, camera_frame): # 查询相机到基坐标系的变换 try: trans self.tf_buffer.lookup_transform( base_link, camera_frame, rclpy.time.Time()) except Exception as e: self.get_logger().error(fTF lookup failed: {e}) return None # 构造 4x4 变换矩阵 T self.transform_to_matrix(trans) p_cam np.array([pose_cam.position.x, pose_cam.position.y, pose_cam.position.z, 1.0]) p_base T p_cam q_cam [pose_cam.orientation.x, pose_cam.orientation.y, pose_cam.orientation.z, pose_cam.orientation.w] R tf_transformations.quaternion_matrix(q_cam)[:3, :3] R_base T[:3, :3] R q_base tf_transformations.quaternion_from_matrix( np.vstack([np.hstack([R_base, np.zeros((3,1))]), [0,0,0,1]])) return p_base[:3], q_baselookup_transform查询的是base_link到相机坐标系的变换注意方向不要搞反。transform_to_matrix把 ROS2 的 Transform 消息转成 4x4 齐次矩阵。位置变换直接乘矩阵姿态变换只取旋转部分相乘再转回四元数。这里容易翻车的地方是四元数顺序ROS2 消息里是 x, y, z, w而tf_transformations也是同样顺序但有些库用 w, x, y, z混用会导致姿态完全错误。3.2 用 MoveIt2 的笛卡尔路径规划接近抓取位姿直接把抓取位姿设为规划目标MoveIt2 可能会规划出一条从侧面撞过去的路径。更稳妥的做法是先规划到抓取位姿沿接近方向后退 10cm 的预抓取位姿再走直线接近。def plan_and_execute(self, target_pose, approach_offset0.1): # target_pose: PoseStamped in base frame # 计算预抓取位姿沿抓取方向后退 pre_pose copy.deepcopy(target_pose) q target_pose.pose.orientation # 抓取方向通常是夹爪 z 轴后退即沿 z 轴负方向平移 R tf_transformations.quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] approach_dir R[:, 2] # z 轴 pre_pose.pose.position.x - approach_dir[0] * approach_offset pre_pose.pose.position.y - approach_dir[1] * approach_offset pre_pose.pose.position.z - approach_dir[2] * approach_offset # 先规划到预抓取位姿 self.move_group.set_pose_target(pre_pose) plan self.move_group.plan() if not plan: self.get_logger().warn(pre-grasp plan failed) return False self.move_group.execute(plan, waitTrue) # 再走笛卡尔直线到抓取位姿 waypoints [pre_pose.pose, target_pose.pose] plan, fraction self.move_group.compute_cartesian_path( waypoints, 0.01, 0.0) if fraction 0.9: self.get_logger().warn(fcartesian path only {fraction*100:.1f}%) return False self.move_group.execute(plan, waitTrue) return Trueapproach_offset取 10cm 是一个保守值实际要根据夹爪长度和物体周围空间调整。compute_cartesian_path的第二个参数是步长 1cm第三个参数是跳跃阈值设为 0 表示不允许跳跃。fraction低于 0.9 说明直线路径被障碍物挡住或超出关节限位这时候不要强行执行应该换下一个候选抓取姿态重试。这里的一个血泪经验是笛卡尔规划对起始位姿很敏感如果预抓取位姿本身离障碍物太近直线段很容易失败所以预抓取位姿的选取要留足余量。3.3 抓取姿态的筛选与排序别让机械臂去试不可能的姿态Graspness 输出的候选抓取可能有几十个但其中很多在运动学上不可达或者会和场景碰撞。在规划之前先做一轮筛选能省下大量规划时间。筛选条件阈值说明置信度分数 0.5低于阈值的抓取通常不稳定抓取宽度0.01m ~ 0.08m超出夹爪行程的直接丢弃接近方向与桌面夹角 15 度避免夹爪和桌面干涉IK 可解至少一组解用 MoveIt2 的 IK 服务快速验证碰撞检测无碰撞用 PlanningScene 的碰撞检查筛选顺序建议按计算代价从低到高先按分数和宽度过滤再做 IK 验证最后做碰撞检测。IK 验证可以用 MoveIt2 的/compute_ik服务传入目标位姿和末端连杆名看是否返回有效解。碰撞检测用/check_state_validity服务把 IK 解对应的关节状态传进去。这两步都是服务调用比完整规划快得多。注意IK 验证时要用和实际执行相同的末端连杆名否则验证通过的姿态在实际规划时可能因为连杆碰撞而失败。3.4 执行阶段的夹爪控制与力反馈规划成功后执行阶段需要控制夹爪闭合。ROS2 里夹爪通常通过 GripperActionController 或自定义的 Action 接口控制。发送闭合命令后不要立刻认为抓取成功要读取夹爪的力反馈或位置反馈判断是否夹住物体。def close_gripper(self, width, force20.0): goal GripperCommand.Goal() goal.command.position width goal.command.max_effort force self.gripper_client.send_goal(goal) self.gripper_client.wait_for_result() result self.gripper_client.get_result() # 如果实际位置明显小于目标宽度说明夹住了物体 if result.position width * 0.8: return True return Falsemax_effort的单位取决于夹爪驱动器的配置常见是牛顿或百分比。result.position是夹爪实际停止的位置如果它比目标宽度小很多说明夹爪在闭合过程中碰到了物体并停止这是抓取成功的信号。如果实际位置等于目标宽度说明夹爪空闭抓取失败。这个判断逻辑比单纯看 Action 是否成功要可靠得多。4. 避坑与排查Graspness 接 ROS2 时最容易翻车的五个地方4.1 点云坐标系和 TF 树对不上规划直接报错现象推理服务返回的抓取姿态在 RViz2 里看起来位置正确但 MoveIt2 规划时报「No transform from [camera_link] to [base_link]」。原因通常是 TF 树里缺少相机到基座的静态变换或者手眼标定结果没有发布成 static_transform_publisher。解决方法是检查ros2 run tf2_tools view_frames生成的 TF 树确认相机坐标系和基坐标系之间存在连通路径。如果是眼在手外配置标定矩阵要发布成从 base_link 到 camera_link 的静态变换如果是眼在手上则发布从末端连杆到相机的变换。4.2 推理服务超时导致规划节点卡死现象规划节点调用推理服务后长时间无响应整个节点像死了一样。原因是 ROS2 Service 的默认超时时间较长而 Graspness 推理在点数多或模型大时可能超过两秒。解决方法是在客户端设置合理的超时并在服务端做点数上限保护。另外推理节点如果是单线程执行器服务回调会阻塞其他回调建议用 MultiThreadedExecutor 并给服务回调分配合适的 callback group。from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor self.srv_cb_group ReentrantCallbackGroup() self.srv self.create_service( PredictGrasp, predict_grasp, self.handle_predict, callback_groupself.srv_cb_group)4.3 抓取姿态的四元数符号翻转导致机械臂绕远路现象规划出的路径看起来机械臂要转一大圈才到目标或者直接报关节限位。原因是四元数 q 和 -q 表示同一旋转但 MoveIt2 在插值时可能选择符号相反的那个导致走长路径。解决方法是在发送目标前统一四元数符号确保与当前末端姿态的四元数点积为正。def align_quaternion(q_target, q_current): dot sum(a*b for a, b in zip(q_target, q_current)) if dot 0: q_target [-x for x in q_target] return q_target4.4 体素下采样参数不当导致抓取精度下降现象推理出的抓取姿态位置偏差大夹爪经常夹偏。原因是体素下采样边长设得太大点云分辨率不够抓取中心点估计不准。解决方法是在推理时间允许的前提下减小体素边长或者对候选抓取区域做局部精细采样。另一个常见错误是下采样前没有做直通滤波背景点占多数下采样后目标物体的点更稀疏。4.5 MoveIt2 规划组配置错误导致规划失败现象所有抓取姿态都规划失败日志显示「Unable to find a valid state」。原因通常是 SRDF 里规划组的关节列表不完整或者末端连杆没有正确设置。解决方法是检查 MoveIt2 配置包里的 SRDF 文件确认规划组包含所有需要运动的关节末端连杆名和 IK 求解器配置一致。另外如果用了 OMPL 的默认配置规划时间可能不够可以在ompl_planning.yaml里把longest_valid_segment_fraction调小或者增加规划尝试次数。5. 进阶用 Graspness 的抓取分数做在线重规划与失败恢复实际抓取中第一次尝试失败是常态。与其让整个流程停下来等人处理不如利用 Graspness 输出的分数做在线重规划。我的习惯是维护一个候选抓取队列按分数从高到低排序每次取队首执行失败后取下一个直到成功或队列耗尽。队列耗尽后重新拍一帧点云调用推理服务生成新的候选队列。这个循环最多重试三轮三轮都失败再报错。def grasp_loop(self, max_rounds3): for round_idx in range(max_rounds): cloud self.capture_cloud() resp self.predict_client.call(cloud) if not resp.success: continue candidates self.rank_candidates(resp) for pose, width, score in candidates: if not self.check_reachable(pose): continue if self.plan_and_execute(pose): if self.close_gripper(width): self.get_logger().info( fgrasp success at round {round_idx}) return True else: self.open_gripper() self.get_logger().warn(fround {round_idx} all failed) return Falserank_candidates除了按分数排序还可以加入一个「与上次失败姿态的距离惩罚」避免反复尝试同一个区域。check_reachable做 IK 和碰撞的快速验证。close_gripper返回 True 后最好再抬升机械臂到安全高度并检查物体是否还在夹爪里这一步可以用力反馈或视觉确认。验证这套系统是否可靠我一般会做两个测试一是固定场景下连续抓取 50 次统计成功率和平均耗时二是随机摆放 20 个不同物体看系统能否在三次重试内完成抓取。成功率低于 80% 时优先检查点云质量和手眼标定精度这两个因素对 Graspness 的影响远大于模型本身。另一个容易被忽略的点是推理服务的冷启动时间如果每次重启节点都要重新加载模型第一次调用的延迟会明显偏高可以在节点启动时先做一次空推理预热。这套方案值不值得投入取决于你的场景是否真的「无序」。如果物体总是以固定姿态出现传统示教加视觉定位成本更低。但如果物体随机堆叠、姿态不可预测Graspness 加 ROS2 加 MoveIt2 的组合是目前比较务实的落地路径。我踩过最大的坑是过早优化推理速度把点云降采样到 5000 点以下结果抓取精度惨不忍睹后来把点数恢复到 20000 并改用 ONNX Runtime 的 GPU 推理整体耗时反而更可控。希望帮到你。本文还有配套的精品资源点击获取