工业机器人仿真 v3.0 (Python 基础语法综合实战

🐍 字符串处理与文件I/O脚本（码垛工单）环境加载中...

robot.log("--- 启动智能码垛系统 ---")

# ==========================================
# 工位坐标说明（单位：米，机器人坐标系，Y 向上为正）
# --------------------------------------------------
# 初始位置: AGV 停在供箱托盘正前方 PARK.pick = (x=2.5, z=-2.0)，朝向 +Z（面向供箱托盘）
# 供箱托盘中心: (x=2.5,  z=0.4)  —— 2×2×2 预置 8 只料箱
# 托盘A（任务1 单层码垛）: 中心 (-1.9, z=4.3)  停泊位 PARK[1] = (-1.9, z=1.9)
# 托盘B（任务2 底层 / 任务3 顶层）: 中心 (0.2, z=4.3)  停泊位 PARK[2,3] = (0.2, z=1.9)
# 托盘顶面高度 PALLET_TOP = 0.13
# 料箱尺寸 BOX = (w=0.42, h=0.34, d=0.42)
# ==========================================
#
# 机器人 API 说明:
#   robot.log(msg)            : 在控制台输出一行日志
#   robot.drive_to(x, z)      : AGV 自动导航到 (x, z) 停泊位（走相互垂直的直线段）
#   robot.next_box()          : 读取供箱托盘下一只待取料箱信息（JSON 字符串）
#   robot.arm_to(x, y, z)     : 机械臂 TCP（末端中心）移动到世界坐标 (x, y, z)
#   robot.align_grip()        : 腕部回转对正夹爪指面（用于夹取与放置）
#   robot.gripper(state)      : 夹爪动作
#                               - "wide"   : 完全张开（取箱前预备）
#                               - "close"  : 闭合夹紧料箱（同时吸附最近料箱）
#                               - "narrow" : 微张松爪（箱子原地留下，码放到位）
#   robot.carry_pose()        : 收拢为搬运预备姿态
#   robot.task(task_no)       : 读取任务配置（JSON 字符串：name / pallet / park / slots / next）
#   robot.setup()             : 读取场景全局配置（boxH / palletTop / parkPick / supplyTotal）
# ==========================================

# 从仿真场景读取工位坐标与箱子参数配置
# cfg 是字典，包含以下键:
#   boxH        : 料箱高度（米） 0.34
#   palletTop   : 托盘顶面高度（米） 0.13
#   parkPick    : 供箱托盘取箱停泊位 [x, z] = [2.5, -2.0]
#   supplyTotal : 供箱托盘预置料箱总数 8
import json
cfg = json.loads(robot.setup())

# BOX_H : 料箱高度（米），用于计算码放时箱中心 Y 坐标
BOX_H = cfg["boxH"]
# TOP_Y : 托盘顶面高度（米），码放时所有箱子的底面都贴在此高度上
TOP_Y = cfg["palletTop"]
# APPROACH : 接近/撤离安全高度偏移（米）
#   取箱时：先下降到 (by + APPROACH) 安全高度，再垂直下探到 by 抓取
#   码放时：先下降到 (y + APPROACH) 安全高度，再垂直下探到 y 释放
#   该值越小臂体越贴近箱面/托盘，值越大越安全但臂体伸展更远
APPROACH = 0.35

robot.log(f"📥 供箱托盘已预置 {cfg['supplyTotal']} 只料箱")
robot.log("⚙️ 机器人开始按顺序执行码垛作业...")

# 任务清单（按工单顺序定义三个码垛任务）
#   t_no   : 任务编号（与 robot.task() 一致）
#   t_name : 任务名（用于日志）
#   t_count: 本任务要码放的箱子数量
tasks = [
    {"no": 1, "name": "单层码垛", "count": 4},
    {"no": 2, "name": "底层码垛", "count": 2},
    {"no": 3, "name": "顶层码垛", "count": 2},
]

# 取箱流程：从供箱托盘取一只箱
# 步骤:
#   1) AGV 行驶到供箱托盘停泊位 cfg["parkPick"] = (2.5, -2.0)
#   2) 查询供箱托盘上按顺序下一只待取料箱（JSON），取出后返回 info
#   3) 机械臂移动到料箱中心正上方（抬高 APPROACH）
#   4) 腕部回转对正夹爪指面
#   5) 垂直下探到料箱中心高度
#   6) 夹爪闭合（同时吸附该料箱到指垫上）
#   7) 垂直提起料箱到安全高度
#   8) 收拢为搬运预备姿态（臂体抬起，避免与供箱托盘碰撞）
# 返回: True=取到箱；False=供箱托盘已空
async def fetch_box():
    await robot.drive_to(cfg["parkPick"][0], cfg["parkPick"][1])   # AGV 驶向供箱托盘停泊位
    raw = robot.next_box()                                          # 读取下一只待取料箱信息
    if raw is None:                                                 # 供箱托盘已空
        robot.log("⚠️ 供箱托盘上已经没有料箱了")
        return False
    info = json.loads(raw)                                          # 解析料箱位置 JSON
    # bx, by, bz : 料箱中心的世界坐标 (x, y, z)
    bx, by, bz = info["pos"]
    robot.log(f"🥡 取第 {info['index']} 只料箱（取后剩余 {info['remaining']} 箱）")
    await robot.arm_to(bx, by + APPROACH, bz)                       # 机械臂对准箱正上方（安全高度）
    await robot.align_grip()                                        # 腕部回转对正夹爪指面
    await robot.arm_to(bx, by, bz)                                  # 垂直下探到箱中心
    await robot.gripper("close")                                    # 闭合夹爪（吸附料箱）
    await robot.arm_to(bx, by + APPROACH, bz)                       # 垂直提起料箱
    await robot.carry_pose()                                        # 收拢为搬运姿态
    return True

# 码放流程：把当前夹持的箱子放到指定任务的下一个槽位上
# 步骤:
#   1) 读取任务配置（pallet 中心、停泊位、槽位列表、当前进度）
#   2) 取出下一个待码放的槽位 [dx, dy, dz]（相对托盘中心的偏移）
#   3) 计算槽位的世界坐标 (x, y, z)，其中 y = 托盘顶面 + 层高 + 箱高/2
#   4) AGV 载箱驶向该任务对应的停泊位 PARK[no]
#   5) 机械臂对准槽位正上方（抬高 APPROACH）
#   6) 腕部回正对齐托盘方向
#   7) 垂直下探到槽位
#   8) 微张夹爪（料箱原地留下，自动核对是否落在槽位上）
#   9) 抬臂撤离到安全高度
#  10) 收拢为搬运预备姿态
async def place_box(task_no):
    t = json.loads(robot.task(task_no))                              # 读取任务配置
    # slot = [dx, dy, dz]: 相对托盘中心的偏移
    #   dx, dz : 水平方向偏移（米）
    #   dy     : 层高偏移（米），用于堆叠第二层
    slot = t["slots"][t["next"]]
    # 计算槽位的世界坐标
    x = t["pallet"][0] + slot[0]                                     # 槽位世界 X
    y = TOP_Y + slot[1] + BOX_H / 2                                  # 槽位世界 Y（箱中心高度）
    z = t["pallet"][1] + slot[2]                                     # 槽位世界 Z
    await robot.drive_to(t["park"][0], t["park"][1])                 # AGV 载箱驶向停泊位
    await robot.arm_to(x, y + APPROACH, z)                           # 机械臂对准槽位正上方
    await robot.align_grip()                                         # 腕部回正对齐托盘
    await robot.arm_to(x, y, z)                                      # 垂直下探到槽位
    await robot.gripper("narrow")                                    # 微张松爪，箱子留在槽位
    await robot.arm_to(x, y + APPROACH, z)                           # 抬臂撤离
    await robot.carry_pose()                                         # 收拢为搬运预备姿态

# 主循环：按任务清单顺序逐任务执行码垛
# supply_empty : 供箱托盘是否已取空（True 时停止所有作业）
supply_empty = False
for t in tasks:
    if supply_empty:
        break
    t_no = t["no"]
    t_name = t["name"]
    t_count = t["count"]
    # info: 任务静态配置（用于查询该任务最多能码放多少箱）
    info = json.loads(robot.task(t_no))
    robot.log(f"--- 开始任务{t_no}：{t_name}（码放 {t_count} 箱）---")
    # 实际码放箱数 = min(本任务计划箱数, 该任务可用槽位数)
    for i in range(min(t_count, len(info["slots"]))):
        if not await fetch_box():                                   # 取箱失败（供箱已空）
            supply_empty = True
            robot.log("🛑 料箱不足，码垛作业中止")
            break
        await place_box(t_no)                                       # 码放刚取到的箱

robot.log("✅ 全部码垛任务已完成")

🔧 状态: 待机

📦 码垛: T1:0/4 T2:0/2 T3:0/2

📥 供箱: 8 箱

🤏 夹爪: 🟢 张开