🤖 第13课：抓取与递送

📌 抓取与递送概述

抓取与递送是服务机器人最核心的物理交互能力——从取咖啡到送快递，都需要可靠的操作：

识别目标 → 定位 → 运动规划 → 接近 → 抓取 → 验证 →
运输 → 稳定性监测 → 到达 → 释放 → 确认

📌 机械臂运动学

机械臂的正运动学和逆运动学是抓取的数学基础：

import math

class RoboticArm:
    """2自由度机械臂运动学模拟"""
    def __init__(self, l1=0.3, l2=0.25):
        self.l1 = l1; self.l2 = l2
        self.theta1 = 0; self.theta2 = 0
        self.gripper_open = True
        self.gripper_width = 0.08

    def forward_kinematics(self, t1, t2):
        """正运动学: 关节角→末端位置"""
        x = self.l1 * math.cos(t1) + self.l2 * math.cos(t1 + t2)
        y = self.l1 * math.sin(t1) + self.l2 * math.sin(t1 + t2)
        return (x, y)

    def inverse_kinematics(self, x, y):
        """逆运动学: 末端位置→关节角"""
        d = math.sqrt(x*x + y*y)
        if d > self.l1 + self.l2 or d < abs(self.l1 - self.l2):
            return None  # 不可达
        
        cos_t2 = (d*d - self.l1*self.l1 - self.l2*self.l2) / (2*self.l1*self.l2)
        cos_t2 = max(-1, min(1, cos_t2))
        t2 = math.acos(cos_t2)
        
        alpha = math.atan2(y, x)
        beta = math.atan2(self.l2 * math.sin(t2), self.l1 + self.l2 * math.cos(t2))
        t1 = alpha - beta
        
        return (t1, t2)

    def grasp(self, obj_pos, obj_size=0.06):
        """执行抓取"""
        ik = self.inverse_kinematics(*obj_pos)
        if ik is None:
            return {"success": False, "reason": "目标不可达"}
        if obj_size > self.gripper_width:
            return {"success": False, "reason": f"物体过大({obj_size}m > {self.gripper_width}m)"}
        
        self.theta1, self.theta2 = ik
        self.gripper_open = False
        ee = self.forward_kinematics(self.theta1, self.theta2)
        return {"success": True, "joint_angles": (round(math.degrees(self.theta1),1), round(math.degrees(self.theta2),1)),
                "ee_position": (round(ee[0],3), round(ee[1],3))}

    def release(self):
        self.gripper_open = True
        return {"success": True, "action": "release"}

arm = RoboticArm()
print("机械臂运动学与抓取模拟")
print("=" * 55)

# 测试逆运动学
targets = [(0.4, 0.3), (0.3, 0.4), (0.2, 0.2), (0.5, 0.0)]
for x, y in targets:
    ik = arm.inverse_kinematics(x, y)
    if ik:
        ee = arm.forward_kinematics(*ik)
        print(f"  目标({x},{y}) → 关节角({math.degrees(ik[0]):.1f}°,{math.degrees(ik[1]):.1f}°) → 末端({ee[0]:.3f},{ee[1]:.3f})")
    else:
        print(f"  目标({x},{y}) → ❌不可达")

# 测试抓取
print("\n抓取测试:")
grasp_tests = [((0.35, 0.25), 0.05, "咖啡杯"), ((0.4, 0.3), 0.10, "大包裹"), ((0.6, 0.1), 0.03, "远处钥匙")]
for pos, size, name in grasp_tests:
    result = arm.grasp(pos, size)
    if result["success"]:
        print(f"  ✅ {name}: 关节角{result['joint_angles']} 末端{result['ee_position']}")
    else:
        print(f"  ❌ {name}: {result['reason']}")

print("\n✅ 机械臂验证通过")

📌 递送任务管理

class DeliveryTask:
    """递送任务管理"""
    def __init__(self):
        self.items = {}; self.next_id = 0
        self.stages = ["pickup", "transport", "deliver", "confirm"]

    def create_task(self, item_name, from_loc, to_loc, recipient, priority=0):
        tid = f"D{self.next_id:03d}"; self.next_id += 1
        self.items[tid] = {
            "id": tid, "item": item_name, "from": from_loc, "to": to_loc,
            "recipient": recipient, "priority": priority,
            "stage": "pickup", "status": "pending",
            "log": [f"[创建] {item_name} 从{from_loc}送到{to_loc}给{recipient}"]
        }
        return tid

    def advance(self, tid, event="success"):
        task = self.items.get(tid)
        if not task: return None
        
        current_idx = self.stages.index(task["stage"])
        if event == "success" and current_idx < len(self.stages) - 1:
            task["stage"] = self.stages[current_idx + 1]
            task["log"].append(f"[{task['stage']}] 完成")
            if task["stage"] == "confirm":
                task["status"] = "completed"
        elif event == "fail":
            task["status"] = "failed"
            task["log"].append(f"[失败] {task['stage']}阶段失败")
        return task

dm = DeliveryTask()
print("递送任务管理")
print("=" * 55)

t1 = dm.create_task("咖啡", "1楼茶水间", "3楼会议室A", "王经理", 3)
t2 = dm.create_task("快递", "1楼前台", "5楼总裁办", "张总", 5)

# 推进任务1
for stage in dm.stages:
    result = dm.advance(t1)
    if result:
        print(f"  任务{t1}: {stage} → {result['stage']} ({result['status']})")

# 推进任务2 - 部分失败
dm.advance(t2)  # pickup
dm.advance(t2)  # transport
dm.advance(t2, "fail")  # deliver fail

print(f"\n任务状态:")
for tid, task in dm.items.items():
    print(f"  {tid}: {task['item']} → {task['recipient']} [{task['status']}] @ {task['stage']}")
    for log in task['log']:
        print(f"    {log}")

print("\n✅ 递送任务验证通过")

📌 托盘稳定性检测

配送过程中稳定性是安全红线——咖啡不能洒，汤不能溢：

import math

class TrayStability:
    """托盘稳定性检测"""
    def __init__(self):
        self.max_tilt = 15  # 最大倾斜角度(度)
        self.max_accel = 2.0  # 最大加速度(m/s²)
        self.tray_size = (0.35, 0.25)  # 托盘尺寸

    def check_stability(self, items, tilt_angle, accel_x, accel_y):
        """检查托盘稳定性"""
        warnings = []
        tilt_rad = math.radians(tilt_angle)
        
        # 倾斜检查
        if abs(tilt_angle) > self.max_tilt:
            warnings.append(f"倾斜过大: {tilt_angle:.1f}° > {self.max_tilt}°")
        
        # 加速度检查
        accel = math.sqrt(accel_x**2 + accel_y**2)
        if accel > self.max_accel:
            warnings.append(f"加速度过大: {accel:.2f}m/s² > {self.max_accel}m/s²")
        
        # 物品滑动检查
        for item in items:
            friction = item.get("friction", 0.5)
            slide_force = accel / max(friction, 0.01)
            if slide_force > 1.0:
                warnings.append(f"{item['name']} 可能滑动 (滑动力{slide_force:.2f})")
        
        # 堆叠稳定性
        stack_height = sum(it.get("height", 0.05) for it in items)
        if stack_height > 0.3:
            warnings.append(f"堆叠过高: {stack_height*100:.0f}cm")
        
        return {"stable": len(warnings) == 0, "warnings": warnings}

    def suggest_speed(self, stability):
        """根据稳定性建议速度"""
        if stability["stable"]:
            return 1.0  # 正常速度
        if len(stability["warnings"]) <= 1:
            return 0.5  # 半速
        return 0.2  # 慢速

checker = TrayStability()
print("托盘稳定性检测")
print("=" * 55)

scenarios = [
    {"name": "正常配送", "items": [{"name":"咖啡","height":0.10,"friction":0.4}],
     "tilt": 3, "ax": 0.5, "ay": 0.3},
    {"name": "急转弯", "items": [{"name":"咖啡","height":0.10,"friction":0.4},{"name":"文件","height":0.02,"friction":0.6}],
     "tilt": 5, "ax": 2.5, "ay": 1.0},
    {"name": "过高堆叠", "items": [{"name":"餐食","height":0.08,"friction":0.3},
     {"name":"咖啡","height":0.10,"friction":0.4},{"name":"汤","height":0.12,"friction":0.2}],
     "tilt": 2, "ax": 0.3, "ay": 0.2},
]

for s in scenarios:
    result = checker.check_stability(s["items"], s["tilt"], s["ax"], s["ay"])
    speed = checker.suggest_speed(result)
    status = "✅ 稳定" if result["stable"] else "⚠️ 不稳定"
    print(f"\n📋 {s['name']}: {status}")
    if result["warnings"]:
        for w in result["warnings"]:
            print(f"  ⚠️ {w}")
    print(f"  建议速度: {speed*100:.0f}%")

print("\n✅ 稳定性检测验证通过")

📌 抓取策略

📌 练习

📌 成就