MoveIt2 抓取與夾爪控制
- ros2
- moveit2
- grasping
- gripper
- manipulation
問題定義
前一節已經能讓手臂規劃並移動到任意目標姿態，但「抓取」不只是移動到一個點這麼單純——手臂要先靠近物體、控制夾爪閉合、確認抓穩了再撤離，過程中還要考慮抓到的物體本身也會變成潛在的碰撞來源（撤離時不能讓抓著的物體撞到旁邊其他東西）。這一節把這些步驟串成一套完整的抓取流程。
核心概念說明
夾爪是獨立的 Planning Group
延續上一節提到的 planning group 概念，夾爪（gripper）通常會被設定成跟手臂本體（arm）分開的獨立群組，因為兩者的控制邏輯本質不同：手臂需要複雜的碰撞規劃找出關節軌跡，夾爪多半只需要簡單地在「開」「閉」（或帶力道回饋的漸進閉合）之間切換，不需要用同一套取樣式規劃邏輯處理。這也是為什麼前面章節的程式碼範例裡，group_name 分別指定 left_arm，實務上完整系統還會有一個對應的 left_gripper 群組。
Pick 動作的三階段序列：接近、抓取、撤離
一次完整的抓取，概念上拆成三個階段，每個階段有不同的規劃策略：
- 接近（approach）：從目前姿態移動到「抓取姿態前方一小段距離」的預備位置，這一段通常用自由度較高的取樣式規劃（如上一節介紹的 OMPL）即可，因為這段路徑不需要特別精準的直線軌跡。
- 抓取（grasp）：從預備位置沿著特定方向（通常是夾爪的正前方軸線）直線移動到最終抓取姿態，同時控制夾爪閉合。這一段通常改用笛卡爾路徑規劃（要求末端執行器沿著明確指定的直線軌跡移動），而不是自由的取樣式規劃，確保靠近物體的動作符合預期方向，不會產生奇怪角度的碰撞風險。
- 撤離（retreat）：抓穩物體後，同樣沿直線方向撤離到安全距離，再交給一般的取樣式規劃移動到下一個目標位置。
Attached Object：抓取後改變碰撞檢查的對象
物體被抓起來之後，它的位置不再是固定的，而是跟著夾爪一起移動——這代表 Planning Scene 需要知道「這個物體現在是手臂的一部分」，之後規劃撤離或移動路徑時，才會正確地把這個物體的幾何形狀也納入碰撞檢查（避免抓著的物體撞到旁邊的東西），而不是仍然把它當成場景裡一個固定不動的障礙物。這個狀態切換稱為 attached object：抓取成功時把物體附著到夾爪 link 上，任務結束放開時要記得分離，讓它重新變回場景裡一個獨立的物體。
實作範例：完整的 Pick 序列
import rclpy
from rclpy.node import Node
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
class PickSequence(Node):
def __init__(self):
super().__init__("pick_sequence")
self.moveit = MoveItPy(node_name="pick_sequence_moveit")
self.arm = self.moveit.get_planning_component("left_arm")
self.gripper = self.moveit.get_planning_component("left_gripper")
def add_target_object(self, object_id, x, y, z):
obj = CollisionObject()
obj.id = object_id
obj.header.frame_id = "base_link"
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.BOX
primitive.dimensions = [0.05, 0.05, 0.05]
obj.primitives.append(primitive)
pose = PoseStamped().pose
pose.position.x, pose.position.y, pose.position.z = x, y, z
obj.primitive_poses.append(pose)
obj.operation = CollisionObject.ADD
self.moveit.get_planning_scene_monitor().apply_collision_object(obj)
def approach(self, grasp_pose: PoseStamped, offset=0.1):
pre_grasp = PoseStamped()
pre_grasp.header.frame_id = grasp_pose.header.frame_id
pre_grasp.pose = grasp_pose.pose
pre_grasp.pose.position.z += offset
self.arm.set_start_state_to_current_state()
self.arm.set_goal_state(pose_stamped_msg=pre_grasp, pose_link="left_gripper_link")
plan = self.arm.plan()
if plan:
self.moveit.execute(plan.trajectory, controllers=[])
return plan
def grasp(self, grasp_pose: PoseStamped, object_id: str):
self.arm.set_start_state_to_current_state()
self.arm.set_goal_state(pose_stamped_msg=grasp_pose, pose_link="left_gripper_link")
plan = self.arm.plan()
if not plan:
self.get_logger().error("無法規劃到最終抓取姿態")
return False
self.moveit.execute(plan.trajectory, controllers=[])
self.gripper.set_goal_state(configuration_name="close")
gripper_plan = self.gripper.plan()
if gripper_plan:
self.moveit.execute(gripper_plan.trajectory, controllers=[])
self.moveit.get_planning_scene_monitor().apply_attached_collision_object(
object_id, link_name="left_gripper_link"
)
return True
def retreat(self, grasp_pose: PoseStamped, offset=0.15):
post_grasp = PoseStamped()
post_grasp.header.frame_id = grasp_pose.header.frame_id
post_grasp.pose = grasp_pose.pose
post_grasp.pose.position.z += offset
self.arm.set_start_state_to_current_state()
self.arm.set_goal_state(pose_stamped_msg=post_grasp, pose_link="left_gripper_link")
plan = self.arm.plan()
if plan:
self.moveit.execute(plan.trajectory, controllers=[])
return plan
呼叫端把三個階段依序串起來：
def main():
rclpy.init()
node = PickSequence()
node.add_target_object("target_cube", x=0.35, y=0.1, z=0.15)
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_link"
grasp_pose.pose.position.x = 0.35
grasp_pose.pose.position.y = 0.1
grasp_pose.pose.position.z = 0.15
grasp_pose.pose.orientation.w = 1.0
node.approach(grasp_pose)
success = node.grasp(grasp_pose, "target_cube")
if success:
node.retreat(grasp_pose)
node.destroy_node()
rclpy.shutdown()
預期輸出
[INFO] [move_group]: Solution found in 0.312 seconds
[INFO] [move_group]: Solution found in 0.089 seconds
[INFO] [move_group]: Attaching object 'target_cube' to link 'left_gripper_link'
[INFO] [move_group]: Solution found in 0.401 seconds
第二個「Solution found」耗時明顯短於前後兩次（0.089 秒），這是符合預期的——抓取階段用的是限制更嚴格的直線笛卡爾規劃，搜尋空間比自由的取樣式規劃小很多，通常收斂得更快。
常見錯誤與除錯技巧
錯誤一：撤離階段規劃失敗，回報跟剛抓到的物體本身碰撞
[ERROR] [move_group]: Goal state is in collision with 'target_cube'
原因：這正是上面 FAQ 提到的 attached object 機制沒有正確設定——如果抓取成功後忘記呼叫 apply_attached_collision_object，Planning Scene 仍然把 target_cube 當成場景裡一個「固定在原地」的獨立障礙物，但實際上物體已經被夾爪抓起來、位置理應跟著手臂移動，規劃器因而誤判撤離路徑會撞上一個「其實已經不在原地」的障礙物。
排除方式：確認抓取動作完成、夾爪確實閉合之後，緊接著呼叫 attach 操作，順序錯誤（例如在夾爪還沒真正閉合前就先 attach）也可能導致問題，實務上會在 attach 前加入短暫等待或讀取夾爪回饋確認閉合完成。
錯誤二：抓取階段的直線路徑規劃回報無法達成完整路徑
[WARN] [move_group]: Cartesian path only achieved 0.62 fraction
原因：0.62 代表笛卡爾路徑規劃器只找到能完成 62% 直線距離的可行軌跡，通常是因為途中某處手臂的關節角度會超出限制、或發生碰撞，導致無法沿著完整的直線走到底。
排除方式：檢查抓取姿態的朝向是否合理（例如是否要求手臂用一個接近奇異點、需要極端關節角度的姿態才能達成），必要時調整 offset 距離（本文範例的接近距離 0.1 公尺），讓接近階段的起點跟最終抓取姿態之間的直線距離更短、途中角度變化更平緩，降低沿途碰到關節限制的機率。
小結
抓取任務拆成接近、抓取、撤離三階段，接近與撤離用自由度較高的規劃，中間關鍵的靠近動作改用限制更嚴格的直線笛卡爾規劃確保軌跡符合任務語意，attached object 機制確保物體被抓起後正確參與後續的碰撞檢查。到這裡，第 7 章機械手臂支線（MoveIt2 架構、逆向運動學規劃、抓取序列）已經涵蓋了操作型任務的核心流程——這一節的抓取姿態輸入，也正是本站介紹過的 GG-CNN 論文這類抓取偵測演算法要解決的問題，兩者可以視為完整抓取系統的上下游銜接。如果你的專案方向是操作型應用，這三篇已經足以支撐後續深入的實機調校；如果想繼續探索其他方向，可以接著看第 8 章感知與 AI 整合。
延伸閱讀
常見問題
- 抓取的目標姿態（要伸到哪裡去抓）是怎麼決定的？
- 本節聚焦在『已知抓取姿態之後，MoveIt2 怎麼執行完整的抓取序列』，但『抓取姿態該選在哪裡』本身是另一個獨立的問題，通常交給專門的抓取偵測演算法處理——例如本站介紹過的 GG-CNN，就是從深度影像直接生成像素級的抓取品質、角度、夾爪寬度，可以把它的輸出結果，轉換成本節示範的 Pick 序列所需要的抓取姿態輸入。
- attached object 會一直附著在夾爪上嗎？
- 不會，attached object 是一個明確的狀態切換：抓取成功後呼叫服務把物體『附著』到夾爪 link 上，物體才會跟著手臂一起移動、並在後續規劃時被視為手臂的一部分參與碰撞檢查；放開物體時要記得呼叫對應的『分離』操作，把物體重新變回 Planning Scene 裡一個獨立、固定位置的障礙物，否則規劃器會誤以為物體還黏在夾爪上，導致後續規劃出現不合理的碰撞判斷。
- 為什麼抓取要拆成接近、抓取、撤離三個階段，不能直接規劃到最終抓取姿態就好？
- 直接規劃到最終抓取姿態，取樣式規劃器可能會選出一條在快接近物體時路徑角度很奇怪、甚至從側面撞向物體的軌跡——因為規劃器只知道終點姿態，不知道『這是一個抓取任務，最後一段應該沿著特定方向直線靠近』這個任務語意。拆成三階段，其中『接近』與『撤離』通常用限制較嚴格的笛卡爾直線規劃（而不是自由的取樣式規劃），確保最後靠近與離開的動作符合物理直覺、不會撞到物體周圍的東西。