📚 课程目录第4章实现简单的抓取策略

4. 实现简单的抓取策略

任务目标

本案例基于已知物体位姿（位置与姿态），设计一个启发式抓取策略，控制6自由度机械臂完成对固定位置物体的抓取。核心目标是：给定物体在基座坐标系下的位姿，通过逆运动学求解机械臂各关节角，使末端执行器运动到预抓取点，再沿接近方向平移至抓取点并闭合夹爪，完成抓取。

环境与依赖

Python 3.8+，NumPy
机械臂运动学库：使用 roboticstoolbox-python（或等效库，如 pinocchio）进行正逆运动学求解
若需可视化验证，可安装 matplotlib 或 trimesh（本案例不强制）

安装命令：pip install roboticstoolbox-python numpy

实现思路

启发式抓取策略分为三步：预抓取定位（末端移动到物体上方一定高度）、垂直下探（沿末端Z轴方向平移至抓取点）、闭合夹爪。物体位姿由齐次变换矩阵 T_obj 表示，抓取点取物体坐标系原点，接近方向取物体Z轴负方向（即从上方抓取）。

关键完整代码

import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
from spatialmath import SE3

# ---------- 1. 定义6自由度机械臂（标准DH参数示例） ----------
def create_arm():
    # 关节长度（单位：m）
    l1, l2, l3 = 0.3, 0.25, 0.15
    arm = DHRobot([
        RevoluteDH(a=0, alpha=np.pi/2, d=l1, offset=0),
        RevoluteDH(a=l2, alpha=0, d=0, offset=0),
        RevoluteDH(a=0, alpha=np.pi/2, d=0, offset=0),
        RevoluteDH(a=0, alpha=-np.pi/2, d=l3, offset=0),
        RevoluteDH(a=0, alpha=np.pi/2, d=0, offset=0),
        RevoluteDH(a=0, alpha=0, d=0.1, offset=0)
    ], name="simple_6dof")
    return arm

# ---------- 2. 启发式抓取策略 ----------
def heuristic_grasp(arm, T_obj, approach_dist=0.1, grasp_dist=0.05):
    """
    基于物体位姿的启发式抓取
    参数:
        arm: 机械臂模型
        T_obj: 物体在基座坐标系下的位姿 (SE3)
        approach_dist: 预抓取点距抓取点的距离（沿接近方向）
        grasp_dist: 夹爪闭合前末端平移距离
    返回:
        q_pre, q_grasp: 预抓取点和抓取点对应的关节角
    """
    # 抓取点 = 物体原点
    T_grasp = T_obj
    # 接近方向：物体Z轴负方向（从上往下抓）
    approach_vec = -T_obj.a  # SE3的a属性为Z轴单位向量

    # 预抓取点：沿接近方向后退 approach_dist
    T_pre = SE3(T_obj.A @ np.eye(4))
    T_pre.A[:3, 3] += approach_vec * approach_dist

    # 求解逆运动学（取第一个解）
    q_pre = arm.ikine_LM(T_pre, q0=np.zeros(6)).q
    q_grasp = arm.ikine_LM(T_grasp, q0=q_pre).q

    return q_pre, q_grasp

# ---------- 3. 主程序：执行抓取 ----------
if __name__ == "__main__":
    arm = create_arm()

    # 固定物体位姿：位于(0.4, 0.1, 0.2)，无旋转
    T_obj = SE3(0.4, 0.1, 0.2) * SE3.Rz(0)

    # 计算抓取轨迹点
    q_pre, q_grasp = heuristic_grasp(arm, T_obj)

    # 输出关节角（单位：rad）
    print("预抓取点关节角:", np.round(q_pre, 4))
    print("抓取点关节角:", np.round(q_grasp, 4))

    # 验证：正运动学检查末端位置
    T_check_pre = arm.fkine(q_pre)
    T_check_grasp = arm.fkine(q_grasp)
    print("预抓取点末端位置:", np.round(T_check_pre.t, 4))
    print("抓取点末端位置:", np.round(T_check_grasp.t, 4))
    print("物体位置:", np.round(T_obj.t, 4))

运行与验证

运行方式：将上述代码保存为 grasp_demo.py，在终端执行 python grasp_demo.py。
预期输出（基于代码逻辑可直接推出）：
预抓取点末端位置约为 [0.4, 0.1, 0.3]（物体上方0.1m），抓取点末端位置约为 [0.4, 0.1, 0.2]（与物体位置一致）。关节角为满足该位姿的某一组逆解。
通过条件：
程序无报错；输出的抓取点末端位置与物体位置误差小于1e-3m；预抓取点位于抓取点正上方（Z轴差值为approach_dist）。

常见问题

逆运动学无解：若物体位置超出机械臂工作空间，ikine_LM会返回NaN。此时应调整物体坐标或增加机械臂关节长度。
接近方向定义：本案例假设物体Z轴朝上，抓取方向为垂直向下。若物体姿态不同，需修改 approach_vec 的计算方式（例如取物体X轴负方向为接近方向）。
多解选择：ikine_LM返回单个解，实际应用中可结合关节限位选择最优解。