调用夹爪实现码垛任务

一、简介

本案例使用机器人，末端安装onRobot夹爪，实现类似码垛的功能
需要的配方文件作为附件置于文末，运行时需要放置在同目录下

二、操作流程

1、以编译的方式安装elite-cs-sdk的python sdk，确保文末的配方文件放置在同目录下
2、修改代码中机器人的IP地址，ROBOT_IP更改为本地机器人ip地址
3、修改代码中所有点位的位置，确保所有点位可达，且保证点位均为笛卡尔位姿
4、执行下方的python文件，此时机器人会重复从取料点PICK_POSE夹取物体，分别搬运物体到九个目标位置。

三、常见问题

1、Q:为什么夹爪夹不住物体
A:夹爪通过modbus RTU来控制，tool_modbus_write_registers用于写modbus数据，其中0x41为设备地址，地址0控制夹爪宽度，地址1控制夹爪力度，地址2控制夹爪速度，地址3控制夹爪命令。所以在使用的时候需要调整夹爪的宽度和力度来夹取物体

四、代码和附件

主要流程：
  1. DashboardClientInterface().connect(ROBOT_IP, 29999) 连 Dashboard
  2. dash.powerOn() 上电 → sleep 2s → dash.brakeRelease() 释放抱闸 → dash.disconnect()
  3. EliteDriverConfig() 配置 headless 模式 → EliteDriver(config) 构造 → 轮询 driver.isRobotConnected() 等就绪
  4. send_40011(1, "tool_serial_config(...)") 通过 40011 端口初始化末端 RS-485 Modbus 夹爪
  5. send_40011("tool_modbus_write_registers(0x41, 0, 800)") + send_40011("...0x41, 3, 1") → gripper_open() 张开夹爪
  6. cartesian_move(driver, HOME_POSE, 2.0) → 内部：
  • driver.setTrajectoryResultCallback(on_done) 注册完成回调
  • driver.writeTrajectoryControlAction(START, 1, 200) 启动轨迹
  • driver.writeTrajectoryPoint(pose, time, blend, cartesian=True) 下发笛卡尔目标点
  • 循环 driver.writeTrajectoryControlAction(NOOP, 0, 200) 保活 + Future 等完成
  • driver.writeIdle(0) 回 idle
  1. 码垛循环（按 P1→P9 顺序 3×3 网格）：
  • cartesian_move(PICK_Z_UP) → cartesian_move(PICK_POSE) → gripper_close() 夹取
  • cartesian_move(PICK_Z_UP) → cartesian_move(Z_UP) → cartesian_move(PLACE_POSE) → gripper_open() 放置
  • cartesian_move(Z_UP) 抬起
  1. driver.stopControl() 清理
涉及接口：
组件
接口
用途
Dashboard (29999)
connect / powerOn / brakeRelease / disconnect
上电初始化
EliteDriver
isRobotConnected / writeTrajectoryControlAction(START/NOOP) / writeTrajectoryPoint(cartesian=True) / setTrajectoryResultCallback / writeIdle / stopControl
笛卡尔轨迹控制
40011 (Socket)
send_40011 → tool_serial_config / tool_modbus_write_registers
末端 RS-485 Modbus 夹爪控制
"""
Elite CS Robot Palletizing (Cartesian Trajectory)
- EliteDriver trajectory control (Cartesian coordinates)
- Gripper: Tool-end RS-485 Modbus RTU (via port 40011 EliteScript)
- Fixed pick point, 3x3 grid place points

Points [x, y, z, Rx, Ry, Rz] (meters/radians):
  HOME: [0.488, -0.148, 0.493, 3.141, 0, -1.571]
  PICK: [0.657, -0.170, 0.087, 3.106, -0.013, -1.645]  # 2026-04-24 10:50

  3x3 Grid (y-axis up):
       x=0.499   x=0.567   x=0.634
  y=-0.029  P7       P8       P9     top row
  y=-0.100  P4       P5       P6     mid row
  y=-0.171  P1*      P2       P3     bottom row
       * = original calibration point

  Calibration corners:
    BL: [0.499, -0.171, 0.194]
    TL: [0.494, -0.029, 0.194]
    TR: [0.629, -0.027, 0.194]
  Grid span: dx=0.135, dy=0.142
  Cell spacing: dx=0.068, dy=0.071
"""

import os
import sys
import time
import socket
import ast
import concurrent.futures

import elite_cs_sdk as cs
from elite_cs_sdk import EliteDriver, EliteDriverConfig, TrajectoryControlAction

# ─────────────────────────────────────────────
# Config
# ─────────────────────────────────────────────
ROBOT_IP = "192.168.1.77"

HOME_POSE = [0.488, -0.148, 0.493, 3.141, 0.0, -1.571]
PICK_POSE = [0.65666, -0.169701, 0.087426, 3.106247, -0.013438, -1.644996]

# 3x3 grid place points
# Row 0 (bottom, y=-0.171): P1(左下) P2(下中) P3(右下)
# Row 1 (mid,    y=-0.100): P4(中左) P5(正中) P6(中右)
# Row 2 (top,    y=-0.029): P7(左上) P8(上中) P9(右上)
PLACE_LIST = [
    {"name": "PLACE_1", "pose": [0.425235, -0.170958, 0.088221, 3.106262, -0.013438, -1.644997]},  # P1(左下)
    {"name": "PLACE_2", "pose": [0.496959, -0.170958, 0.088258, 3.106247, -0.013444, -1.645001]},  # P2(下中)
    {"name": "PLACE_3", "pose": [0.568684, -0.188144, 0.08811, 3.106257, -0.013438, -1.644996]},  # P3(右下)
    {"name": "PLACE_4", "pose": [0.425235, -0.102441, 0.088258, 3.106247, -0.013444, -1.645001]},  # P4(中左)
    {"name": "PLACE_5", "pose": [0.496959, -0.102441, 0.088258, 3.106247, -0.013444, -1.645001]},  # P5(正中)
    {"name": "PLACE_6", "pose": [0.568684, -0.102441, 0.088258, 3.106247, -0.013444, -1.645001]},  # P6(中右)
    {"name": "PLACE_7", "pose": [0.444792, -0.033923, 0.088444, 3.106221, -0.013457, -1.645011]},  # P7(左上)
    {"name": "PLACE_8", "pose": [0.496959, -0.033923, 0.088258, 3.106247, -0.013444, -1.645001]},  # P8(上中)
    {"name": "PLACE_9", "pose": [0.568684, -0.033923, 0.088258, 3.106247, -0.013444, -1.645001]},  # P9(右上)
]

Z_UP_OFFSET = 0.20
LOOP_COUNT = 0
TRAJECTORY_TIME = 2.0
BLEND_RADIUS = 0.0
WAIT_GRIPPER = 1.5


# ─────────────────────────────────────────────
# Utils
# ─────────────────────────────────────────────

def safe_print(msg: str):
    try:
        print(msg)
    except UnicodeEncodeError:
        print(msg.encode("ascii", "ignore").decode("ascii"))


def send_40011(req_id: int, cmd: str, timeout: float = 5.0) -> str:
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.settimeout(timeout)
    try:
        sock.connect((ROBOT_IP, 40011))
        sock.sendall(f"req {req_id}{cmd}\n".encode())
        return sock.recv(4096).decode("utf-8", errors="replace").strip()
    finally:
        sock.close()


def check_40011(cmd: str, resp: str):
    if "failure" in resp.lower():
        safe_print(f"[40011] WARN: '{cmd}' -> {resp}")
    else:
        safe_print(f"[40011] OK: {cmd}")


# ─────────────────────────────────────────────
# Gripper
# ─────────────────────────────────────────────

def gripper_init():
    cmd = "tool_serial_config(True, 1000000, 2, 1, 8, True, 0)"
    resp = send_40011(1, cmd)
    check_40011(cmd, resp)
    time.sleep(0.5)


def gripper_open():
    send_40011(2, "tool_modbus_write_registers(0x41, 0, 800)")
    time.sleep(0.1)
    send_40011(3, "tool_modbus_write_registers(0x41, 3, 1)")
    time.sleep(WAIT_GRIPPER)


def gripper_close():
    send_40011(4, "tool_modbus_write_registers(0x41, 0, 300)")
    time.sleep(0.1)
    send_40011(5, "tool_modbus_write_registers(0x41, 3, 1)")
    time.sleep(WAIT_GRIPPER)


# ─────────────────────────────────────────────
# Cartesian Trajectory Move
# ─────────────────────────────────────────────

def cartesian_move(driver, pose, traj_time=TRAJECTORY_TIME):
    move_done = concurrent.futures.Future()

    def on_done(result):
        if not move_done.done():
            move_done.set_result(result)

    driver.setTrajectoryResultCallback(on_done)
    driver.writeTrajectoryControlAction(TrajectoryControlAction.START, 1, 200)
    driver.writeTrajectoryPoint(pose, time=traj_time, blend_radius=BLEND_RADIUS, cartesian=True)

    while not move_done.done():
        time.sleep(0.01)
        driver.writeTrajectoryControlAction(TrajectoryControlAction.NOOP, 0, 200)

    result = move_done.result()
    driver.writeIdle(0)
    safe_print(f"[Traj] Done: {result}")


# ─────────────────────────────────────────────
# Main
# ─────────────────────────────────────────────

def main():
    safe_print("=" * 50)
    safe_print("Elite CS Palletizing 3x3 (Cartesian)")
    safe_print(f"IP: {ROBOT_IP}  Points: {len(PLACE_LIST)}  Loop: {'INF' if LOOP_COUNT == 0 else LOOP_COUNT}")
    safe_print("=" * 50)

    # Init
    dash = cs.DashboardClientInterface()
    dash.connect(ROBOT_IP, 29999)
    dash.powerOn()
    time.sleep(2.0)
    dash.brakeRelease()
    time.sleep(1.0)
    dash.disconnect()

    script_path = os.path.join(os.path.dirname(cs.__file__), "external_control.script")
    config = EliteDriverConfig()
    config.robot_ip = ROBOT_IP
    config.script_file_path = script_path
    config.headless_mode = True
    driver = EliteDriver(config)
    time.sleep(1.0)
    while not driver.isRobotConnected():
        time.sleep(0.01)
    safe_print("[Driver] Connected")

    gripper_init()
    gripper_open()

    PICK_Z_UP = [PICK_POSE[0], PICK_POSE[1], PICK_POSE[2] + Z_UP_OFFSET,
                 PICK_POSE[3], PICK_POSE[4], PICK_POSE[5]]

    cartesian_move(driver, HOME_POSE, traj_time=2.0)

    # Palletizing loop
    cycle = 0
    while True:
        for item in PLACE_LIST:
            cycle += 1
            name = item["name"]
            pose = item["pose"]
            z_up = [pose[0], pose[1], pose[2] + Z_UP_OFFSET, pose[3], pose[4], pose[5]]

            safe_print(f"\n[{cycle}] {name} {[f'{v:.3f}' for v in pose]}")

            cartesian_move(driver, PICK_Z_UP)
            cartesian_move(driver, PICK_POSE)
            gripper_close()
            cartesian_move(driver, PICK_Z_UP)
            cartesian_move(driver, z_up)
            cartesian_move(driver, pose)
            gripper_open()
            cartesian_move(driver, z_up)

            if LOOP_COUNT > 0 and cycle >= LOOP_COUNT:
                safe_print(f"\n[DONE] {cycle} cycles")
                driver.stopControl()
                return

    driver.stopControl()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        sys.exit(0)
    except ConnectionRefusedError:
        safe_print(f"[ERR] Cannot connect {ROBOT_IP}")
        sys.exit(1)
    except Exception as e:
        safe_print(f"[ERR] {e}")
        raise