一、简介

本案例使用俩台机器人，其中从机器人跟随主机器人的运动，进行同步运动
需要的配方文件作为附件置于文末，运行时需要放置在同目录下

二、操作流程

1、以编译的方式安装elite-cs-sdk的python sdk，确保文末的配方文件放置在同目录下

2、确保俩台机器人有充足的运动空间，程序启动时，从属机器人会先运动到主机器人的相同位姿

3、修改代码中两台机器人的IP地址，MASTER_IP是主动机器人的地址，FOLLOW_IP是从动机器人的地址

4、执行下方的python文件，此时从属机器人会监听主机器人的运动，并保持跟随。

三、常见问题

1、Q:为什么从属机器人运动更慢，无法跟上主机器人的运动速度。

A:优先确保俩台机器人的速度滑块设定的全局速度是一致的。如果俩台机器人全局速度一致仍然有滞后，可以再适度加大RTSI_FREQ的值。

2、Q:为什么示教器出现弹窗，提示External Control speed limit。

A:在SDK内，设置了移动指令速度忽略值，当某个移动质量会导致移动速度超过该最大值时，会忽略该运动指令，并在示教器上进行弹窗提醒。如果想要解决这个问题，需要找到SDK内的source/resources/external_control.script 文件，并将文件中JOINT_IGNORE_SPEED变量的值修改更大，单位是rad/s。需要注意，如果传递的值有突变可能性，则该值不适宜改到太大，避免出现异常值导致的风险。

四、代码和附件

代码流程图如下图所示：

主要流程：

_ensure_recipes() 检查/创建配方文件
起子线程 master_reader_thread()：

cs.RtsiIOInterface(output_recipe, input_recipe, 250).connect(MASTER_IP) 连 master RTSI，250Hz
循环 getActualJointPositions() 读六关节 → 加锁写 _latest_joints

主线程等 master 第一帧数据就绪
起子线程 follow_servo_thread()：

cs.EliteDriverConfig() 配置伺服参数：servoj_time=0.004 / servoj_lookahead_time=0.03 / servoj_gain=2000 / headless_mode=True
cs.EliteDriver(config) 构造 → 轮询 driver.isRobotConnected() 等反向连接
cs.RtsiIOInterface(recipe, recipe, 50).connect(FOLLOW_IP) 连 follow RTSI 读实际关节位置
follow_io.getActualJointPositions() 读 follow 当前关节作为软启动起点
软启动阶段：interpolate_joints(follow_current, master_target, t) 50 步线性插值，每步 driver.writeServoj(joints, timeout_ms=0)
follow_io.disconnect() 释放 RTSI
正常运行：250Hz 循环 writeServoj(_latest_joints) 透传

signal_handler 捕获 SIGINT/SIGTERM → _running = False
driver.stopControl(wait_ms=3000) 清理

用到的接口：

组件	接口	用途
RTSI (master)	`RtsiIOInterface(recipe, recipe, 250) / connect / getActualJointPositions / disconnect`	250Hz 读 master 关节
RTSI (follow)	`RtsiIOInterface(recipe, recipe, 50) / connect / getActualJointPositions / disconnect`	读 follow 当前关节（仅软启动用）
EliteDriver (follow)	`EliteDriver(config) / isRobotConnected / writeServoj / stopControl`	关节伺服透传

"""
透传控制：master -> follow
以 250Hz 读取 master 的实际关节位置，通过 EliteDriver.writeServoj 实时透传给 follow。

依赖库：
    elite-cs-sdk

依赖文件（与本脚本同目录）：
    output_recipe.txt  —— 至少包含 actual_joint_positions
    input_recipe.txt   —— 标准输入配方（可为默认或自定义）
"""

import elite_cs_sdk as cs
import os
import sys
import time
import threading
import signal
import math

# ──────────────────────────────────────────────
# 配置
# ──────────────────────────────────────────────
MASTER_IP = "172.16.15.45"          # 主动机器人IP
FOLLOW_IP = "192.168.254.138"       # 从动机器人IP

RTSI_FREQ = 250.0          # Hz，RTSI 数据同步频率
SERVOJ_TIME = 1.0 / RTSI_FREQ   # servoj 指令时间间隔（秒）

# EliteDriver 伺服参数（可根据实际调整）
SERVOJ_LOOKAHEAD_TIME = 0.03   # 轨迹平滑时间，范围 [0.03, 0.2]，越小响应越快
SERVOJ_GAIN = 2000             # 伺服增益

# 软启动参数（解决初始位置差太大问题）
SOFT_START_DURATION = 1.0     # 软启动持续时间（秒）
SOFT_START_STEPS = 50          # 软启动步数

# 配方文件路径（与本脚本同目录）
_DIR = os.path.dirname(os.path.abspath(__file__))
OUTPUT_RECIPE = os.path.join(_DIR, "output_recipe.txt")
INPUT_RECIPE  = os.path.join(_DIR, "input_recipe.txt")

# ──────────────────────────────────────────────
# 全局状态
# ──────────────────────────────────────────────
_running = True
_latest_joints = None          # 最新一帧 master 关节位置
_follow_start_joints = None    # follow 启动时的实际位置
_lock = threading.Lock()


def _signal_handler(sig, frame):
    global _running
    print("\n[INFO] 收到退出信号，正在停止...")
    _running = False


# ──────────────────────────────────────────────
# 确保配方文件存在
# ──────────────────────────────────────────────
def _ensure_recipes():
    """若配方文件不存在则自动创建最小化版本。"""
    if not os.path.exists(OUTPUT_RECIPE):
        print(f"[WARN] 未找到 {OUTPUT_RECIPE}，自动创建最小化配方")
        with open(OUTPUT_RECIPE, "w") as f:
            f.write("actual_joint_positions\n")

    if not os.path.exists(INPUT_RECIPE):
        print(f"[WARN] 未找到 {INPUT_RECIPE}，自动创建默认配方")
        with open(INPUT_RECIPE, "w") as f:
            f.write("standard_digital_output_mask\nstandard_digital_output\n")


# ──────────────────────────────────────────────
# master 读取线程
# ──────────────────────────────────────────────
def master_reader_thread():
    """以 RTSI_FREQ Hz 持续读取 master 关节位置，写入 _latest_joints。"""
    global _latest_joints, _running

    print(f"[INFO] 连接 master RTSI ({MASTER_IP})...")
    master_io = cs.RtsiIOInterface(OUTPUT_RECIPE, INPUT_RECIPE, RTSI_FREQ)
    if not master_io.connect(MASTER_IP):
        print(f"[ERROR] 无法连接 master RTSI ({MASTER_IP})")
        _running = False
        return

    print(f"[INFO] master RTSI 连接成功，开始以 {RTSI_FREQ}Hz 读取关节位置")
    interval = 1.0 / RTSI_FREQ

    try:
        while _running:
            t0 = time.perf_counter()

            joints = master_io.getActualJointPositions()
            if joints is not None and len(joints) == 6:
                with _lock:
                    _latest_joints = list(joints)

            elapsed = time.perf_counter() - t0
            sleep_time = interval - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)
    finally:
        master_io.disconnect()
        print("[INFO] master RTSI 已断开")


# ──────────────────────────────────────────────
# 线性插值
# ──────────────────────────────────────────────
def lerp(a, b, t):
    """线性插值：t in [0,1]"""
    return a + (b - a) * t


def interpolate_joints(start, end, t):
    """对两个关节位置数组进行插值"""
    return [lerp(s, e, t) for s, e in zip(start, end)]


def joints_diff(a, b):
    """计算两个关节位置的最大差值（弧度）"""
    return max(abs(ai - bi) for ai, bi in zip(a, b))


# ──────────────────────────────────────────────
# follow 跟随线程
# ──────────────────────────────────────────────
def follow_servo_thread():
    """以 RTSI_FREQ Hz 将 _latest_joints 透传给 follow（writeServoj）。"""
    global _running, _follow_start_joints

    # 构建 EliteDriver 配置
    import elite_cs_sdk
    script_path = os.path.join(os.path.dirname(elite_cs_sdk.__file__), "external_control.script")

    config = cs.EliteDriverConfig()
    config.robot_ip = FOLLOW_IP
    config.script_file_path = script_path
    config.headless_mode = True
    config.servoj_time = SERVOJ_TIME
    config.servoj_lookahead_time = SERVOJ_LOOKAHEAD_TIME
    config.servoj_gain = SERVOJ_GAIN

    print(f"[INFO] 连接 follow EliteDriver ({FOLLOW_IP})...")
    try:
        driver = cs.EliteDriver(config)
    except Exception as e:
        print(f"[ERROR] 无法连接 follow ({FOLLOW_IP}): {e}")
        _running = False
        return

    # 等待机器人反向连接
    print("[INFO] 等待 follow 机器人建立反向连接...")
    deadline = time.time() + 15
    while not driver.isRobotConnected():
        if time.time() > deadline:
            print("[ERROR] follow 机器人连接超时（15s），请检查网络和机器人状态")
            _running = False
            return
        time.sleep(0.1)

    # 使用 RTSI 接口读取 follow 当前实际关节位置（作为软启动起点）
    print("[INFO] 通过 RTSI 读取 follow 当前实际关节位置...")
    follow_io = cs.RtsiIOInterface(OUTPUT_RECIPE, INPUT_RECIPE, 50.0)
    if not follow_io.connect(FOLLOW_IP):
        print("[WARN] 无法连接 follow RTSI，使用 [0,0,0,0,0,0] 作为起点")
        follow_current = [0.0] * 6
    else:
        time.sleep(0.2)  # 等待第一帧数据
        follow_current = follow_io.getActualJointPositions()
        if follow_current is None or len(follow_current) != 6:
            print("[WARN] 无法读取 follow 关节位置，使用 [0,0,0,0,0,0] 作为起点")
            follow_current = [0.0] * 6
        else:
            print(f"[INFO] follow 当前关节: {[round(v, 4) for v in follow_current]}")

    _follow_start_joints = follow_current

    # 获取 master 目标位置
    with _lock:
        master_target = _latest_joints

    if master_target is None:
        print("[ERROR] 无法获取 master 目标位置")
        _running = False
        return

    print(f"[INFO] master 目标关节: {[round(v, 4) for v in master_target]}")
    diff = joints_diff(follow_current, master_target)
    print(f"[INFO] 初始位置差: {diff:.4f} rad (最大关节差)")

    # 软启动阶段：从 follow 当前实际位置平滑过渡到 master 位置
    print(f"[INFO] 开始软启动阶段 ({SOFT_START_DURATION}秒, {SOFT_START_STEPS}步)...")
    soft_start_interval = SOFT_START_DURATION / SOFT_START_STEPS
    for i in range(SOFT_START_STEPS + 1):
        if not _running:
            break

        t = i / SOFT_START_STEPS
        # 使用 ease-in-out 曲线使运动更平滑
        # t_smooth = t * t * (3 - 2 * t)  # smoothstep
        t_smooth = t  # 线性也可以，视情况调整

        joints = interpolate_joints(follow_current, master_target, t_smooth)

        ok = driver.writeServoj(joints, timeout_ms=0)
        if not ok:
            print(f"[WARN] 软启动第 {i+1} 步 writeServoj 返回 False")

        time.sleep(soft_start_interval)

    if not _running:
        driver.stopControl(wait_ms=1000)
        return

    # 软启动完成后关闭 RTSI 连接
    try:
        follow_io.disconnect()
    except Exception:
        pass

    print("[INFO] 软启动完成，进入正常透传模式")

    # ========== 正常透传模式 ==========
    interval = 1.0 / RTSI_FREQ

    try:
        while _running:
            t0 = time.perf_counter()

            with _lock:
                joints = _latest_joints

            if joints is not None:
                ok = driver.writeServoj(joints, timeout_ms=0)
                if not ok:
                    print("[WARN] writeServoj 返回 False，机器人可能已断开")
                    if not driver.isRobotConnected():
                        print("[ERROR] follow 连接丢失，停止透传")
                        break

            elapsed = time.perf_counter() - t0
            sleep_time = interval - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)
    finally:
        try:
            driver.stopControl(wait_ms=3000)
        except Exception:
            pass
        print("[INFO] follow EliteDriver 已停止")


# ──────────────────────────────────────────────
# 主函数
# ──────────────────────────────────────────────
def main():
    signal.signal(signal.SIGINT, _signal_handler)
    signal.signal(signal.SIGTERM, _signal_handler)

    _ensure_recipes()

    # 先等待 master 读到第一帧数据，再启动 follow 伺服
    t_master = threading.Thread(target=master_reader_thread, daemon=True, name="master-reader")
    t_master.start()

    print("[INFO] 等待 master 第一帧数据...")
    deadline = time.time() + 10
    while _latest_joints is None and _running:
        if time.time() > deadline:
            print("[ERROR] 等待 master 数据超时，退出")
            sys.exit(1)
        time.sleep(0.05)

    if not _running:
        sys.exit(1)

    print(f"[INFO] 收到 master 初始关节位置: {[round(v, 4) for v in _latest_joints]}")

    t_follow = threading.Thread(target=follow_servo_thread, daemon=True, name="follow-servo")
    t_follow.start()

    # 主线程等待两个子线程结束
    try:
        while _running and (t_master.is_alive() or t_follow.is_alive()):
            time.sleep(0.5)
    except KeyboardInterrupt:
        _signal_handler(None, None)

    t_master.join(timeout=3)
    t_follow.join(timeout=5)
    print("[INFO] 透传已停止，程序退出")


if __name__ == "__main__":
    main()

output_recipe.txt

22 字节

input_recipe.txt

27 字节

目录

首页

二次开发

Python

CS机器人 python SDK使用案例

复合案例

双机器人协同透传

双机器人协同透传

一、简介

二、操作流程

三、常见问题

四、代码和附件

目录

首页 二次开发 Python CS机器人 python SDK使用案例 复合案例 双机器人协同透传

一、简介

二、操作流程

三、常见问题

四、代码和附件

首页

二次开发

Python

CS机器人 python SDK使用案例

复合案例

双机器人协同透传