双机器人协同透传

一、简介

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

二、操作流程

1、以编译的方式安装elite-cs-sdk
2、确保俩台机器人有充足的运动空间，程序启动时，从属机器人会先运动到主机器人的相同位姿
3、修改代码中两台机器人的IP地址，MASTER_IP是主动机器人的地址，FOLLOW_IP是从动机器人的地址
5、创建工程文件，修改附加包含目录、附加库目录、附加依赖项
6、把 Release 目录下的三个 DLL 复制到编译输出目录（和 .exe 同级）：
  • elite-cs-series-sdk.dll
  • ssh.dll
  • libcrypto-3-x64.dll
7、下载附件中的 external_control.script ，修改代码中的绝对路径 kScriptPath、kOutputRecipe和kInputRecipe，生成解决方案

三、常见问题

1、Q:为什么从属机器人运动更慢，无法跟上主机器人的运动速度。
A:优先确保俩台机器人的速度滑块设定的全局速度是一致的。如果俩台机器人全局速度一致仍然有滞后，可以再适度加大RTSI_FREQ的值。
2、Q:为什么示教器出现弹窗，提示External Control speed limit。
A:在SDK内，设置了移动指令速度忽略值，当某个移动质量会导致移动速度超过该最大值时，会忽略该运动指令，并在示教器上进行弹窗提醒。如果想要解决这个问题，需要找到SDK内的source/resources/external_control.script 文件，并将文件中JOINT_IGNORE_SPEED变量的值修改更大，单位是rad/s。需要注意，如果传递的值有突变可能性，则该值不适宜改到太大，避免出现异常值导致的风险。

四、代码和附件

主要流程：
  1. SetConsoleCtrlHandler(ConsoleCtrlHandler) 注册 Ctrl+C 退出信号
  2. ensureRecipes() 检查/自动创建配方文件
  3. 起子线程 masterReaderThread()：
  • RtsiIOInterface(outputRecipe, inputRecipe, 250.0).connect(kMasterIp) 连 master RTSI，250Hz
  • 循环 io.getActualJointPositions() 读六关节 → 加写锁写 g_latestJoints → g_masterReady = true
  • io.isConnected() 检测断连 → io.disconnect() 退出
  1. 起子线程 followServoThread()：
  • 轮询 g_masterReady 等 master 第一帧数据（超时 10s）
  • EliteDriverConfig{} 配置：servoj_time=0.004 / servoj_lookahead_time=0.03 / servoj_gain=2000 / headless_mode=true + 自定义端口 50005~50008
  • EliteDriver(config) 构造 → 轮询 driver.isRobotConnected() 等反向连接（超时 15s）
  • readRobotJoints(kFollowIp, recipe, recipe, 50.0, followCurrent) → 内部 RtsiIOInterface::connect / getActualJointPositions / disconnect 读 follow 当前关节
  • 软启动：interpolateJoints(followCurrent, masterTarget, t) 50 步线性插值 → 每步 driver.writeServoj(joints, 0)
  • 正常透传：250Hz 循环加读锁取 g_latestJoints → driver.writeServoj(joints, 0) → 断连检测自动停止
  1. masterThread.join() → g_running = false → followThread.join() 清理
涉及的接口：
组件
接口
RTSI (master)
RtsiIOInterface(recipe, recipe, 250) / connect / getActualJointPositions / isConnected / disconnect
RTSI (follow)
同上，50Hz 临时连一次读当前关节
EliteDriver (follow)
EliteDriver(config) / isRobotConnected / writeServoj / stopControl
#include <Elite/DataType.hpp>
#include <Elite/EliteDriver.hpp>
#include <Elite/RtsiIOInterface.hpp>

#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <exception>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <thread>

#define NOMINMAX
#include <windows.h>

using namespace ELITE;

namespace {

constexpr const char* kMasterIp = "172.16.15.13";
constexpr const char* kFollowIp = "192.168.254.138";

constexpr double kRtsiFreq = 250.0;
constexpr float kServojTime = static_cast<float>(1.0 / kRtsiFreq);
constexpr float kServojLookaheadTime = 0.03f;
constexpr int kServojGain = 2000;

constexpr double kSoftStartDuration = 1.0;
constexpr int kSoftStartSteps = 50;

constexpr const char* kScriptPath =
    "C:\\Users\\SZ02IT0615\\source\\repos\\EliteExample\\EliteExample\\elite-cs-sdk\\resources\\external_control.script";
constexpr const char* kOutputRecipe =
    "C:\\Users\\SZ02IT0615\\source\\repos\\EliteExample\\EliteExample\\elite-cs-sdk\\resources\\output_recipe.txt";
constexpr const char* kInputRecipe =
    "C:\\Users\\SZ02IT0615\\source\\repos\\EliteExample\\EliteExample\\elite-cs-sdk\\resources\\input_recipe.txt";

using JointArray = std::array<double, 6>;

std::atomic<bool> g_running{true};
std::atomic<bool> g_masterReady{false};
JointArray g_latestJoints{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::shared_mutex g_jointsMutex;

double lerp(double a, double b, double t) {
    return a + (b - a) * t;
}

JointArray interpolateJoints(const JointArray& start, const JointArray& end, double t) {
    JointArray out{};
    for (std::size_t i = 0; i < out.size(); ++i) {
        out[i] = lerp(start[i], end[i], t);
    }
    return out;
}

double jointsMaxDiff(const JointArray& a, const JointArray& b) {
    double maxDiff = 0.0;
    for (std::size_t i = 0; i < a.size(); ++i) {
        maxDiff = std::max(maxDiff, std::abs(a[i] - b[i]));
    }
    return maxDiff;
}

void printJoints(const JointArray& joints, const char* prefix = nullptr) {
    if (prefix != nullptr) {
        std::cout << prefix << ": ";
    }

    std::cout << "[";
    for (std::size_t i = 0; i < joints.size(); ++i) {
        std::cout << std::fixed << std::setprecision(4) << joints[i];
        if (i + 1 != joints.size()) {
            std::cout << ", ";
        }
    }
    std::cout << "]" << std::endl;
}

BOOL WINAPI ConsoleCtrlHandler(DWORD ctrlType) {
    if (ctrlType == CTRL_C_EVENT || ctrlType == CTRL_BREAK_EVENT || ctrlType == CTRL_CLOSE_EVENT) {
        std::cout << "\n[INFO] Exit signal received, stopping..." << std::endl;
        g_running = false;
        return TRUE;
    }
    return FALSE;
}

bool fileExists(const char* path) {
    FILE* file = nullptr;
    if (fopen_s(&file, path, "r") != 0 || file == nullptr) {
        return false;
    }
    std::fclose(file);
    return true;
}

bool writeTextFile(const char* path, const char* content) {
    FILE* file = nullptr;
    if (fopen_s(&file, path, "w") != 0 || file == nullptr) {
        return false;
    }

    std::fputs(content, file);
    std::fclose(file);
    return true;
}

bool ensureRecipes() {
    bool ok = true;

    if (!fileExists(kOutputRecipe)) {
        std::cout << "[WARN] Missing output recipe, creating: " << kOutputRecipe << std::endl;
        if (!writeTextFile(kOutputRecipe, "actual_joint_positions\n")) {
            std::cerr << "[ERROR] Failed to create output recipe: " << kOutputRecipe << std::endl;
            ok = false;
        }
    }

    if (!fileExists(kInputRecipe)) {
        std::cout << "[WARN] Missing input recipe, creating: " << kInputRecipe << std::endl;
        if (!writeTextFile(kInputRecipe, "standard_digital_output_mask\nstandard_digital_output\n")) {
            std::cerr << "[ERROR] Failed to create input recipe: " << kInputRecipe << std::endl;
            ok = false;
        }
    }

    return ok;
}

bool readRobotJoints(const char* ip, const char* outputRecipe, const char* inputRecipe, double freq,
                     JointArray& joints) {
    try {
        RtsiIOInterface io(std::string(outputRecipe), std::string(inputRecipe), freq);
        if (!io.connect(std::string(ip))) {
            return false;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        const vector6d_t values = io.getActualJointPositions();
        io.disconnect();

        for (std::size_t i = 0; i < joints.size(); ++i) {
            joints[i] = values[i];
        }
        return true;
    } catch (const std::exception& e) {
        std::cerr << "[ERROR] Failed to read joints from " << ip << ": " << e.what() << std::endl;
        return false;
    }
}

void masterReaderThread() {
    try {
        std::cout << "[INFO] Connecting master RTSI: " << kMasterIp << std::endl;

        RtsiIOInterface masterIo(std::string(kOutputRecipe), std::string(kInputRecipe), kRtsiFreq);
        if (!masterIo.connect(std::string(kMasterIp))) {
            std::cerr << "[ERROR] Failed to connect master RTSI: " << kMasterIp << std::endl;
            g_running = false;
            return;
        }

        std::cout << "[INFO] Master RTSI connected at " << kRtsiFreq << " Hz" << std::endl;
        const auto interval = std::chrono::duration<double>(1.0 / kRtsiFreq);

        while (g_running.load()) {
            const auto start = std::chrono::high_resolution_clock::now();

            if (!masterIo.isConnected()) {
                std::cerr << "[WARN] Master RTSI disconnected" << std::endl;
                g_running = false;
                break;
            }

            const vector6d_t joints = masterIo.getActualJointPositions();
            JointArray copy{};
            for (std::size_t i = 0; i < copy.size(); ++i) {
                copy[i] = joints[i];
            }

            {
                std::unique_lock<std::shared_mutex> lock(g_jointsMutex);
                g_latestJoints = copy;
            }

            if (!g_masterReady.exchange(true)) {
                std::cout << "[INFO] First master joint frame received" << std::endl;
                printJoints(copy, "master");
            }

            const auto elapsed = std::chrono::high_resolution_clock::now() - start;
            const auto sleepTime = interval - elapsed;
            if (sleepTime > std::chrono::duration<double>(0.0)) {
                std::this_thread::sleep_for(sleepTime);
            }
        }

        masterIo.disconnect();
        std::cout << "[INFO] Master RTSI stopped" << std::endl;
    } catch (const std::exception& e) {
        std::cerr << "[ERROR] masterReaderThread exception: " << e.what() << std::endl;
        g_running = false;
    } catch (...) {
        std::cerr << "[ERROR] masterReaderThread unknown exception" << std::endl;
        g_running = false;
    }
}

void followServoThread() {
    try {
        const auto waitStart = std::chrono::steady_clock::now();
        const auto waitTimeout = std::chrono::seconds(10);

        while (!g_masterReady.load() && g_running.load()) {
            if (std::chrono::steady_clock::now() - waitStart > waitTimeout) {
                std::cerr << "[ERROR] Timed out waiting for master joints" << std::endl;
                g_running = false;
                return;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        if (!g_running.load()) {
            return;
        }

        JointArray masterTarget{};
        {
            std::shared_lock<std::shared_mutex> lock(g_jointsMutex);
            masterTarget = g_latestJoints;
        }

        EliteDriverConfig config;
        config.robot_ip = kFollowIp;
        config.script_file_path = kScriptPath;
        config.headless_mode = true;
        config.script_sender_port = 50006;
        config.reverse_port = 50005;
        config.trajectory_port = 50007;
        config.script_command_port = 50008;
        config.servoj_time = kServojTime;
        config.servoj_lookahead_time = kServojLookaheadTime;
        config.servoj_gain = kServojGain;

        std::cout << "[INFO] Connecting follow driver: " << kFollowIp << std::endl;
        EliteDriver driver(config);

        const auto connectDeadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
        while (!driver.isRobotConnected() && g_running.load()) {
            if (std::chrono::steady_clock::now() > connectDeadline) {
                std::cerr << "[ERROR] Timed out waiting for follow robot connection" << std::endl;
                g_running = false;
                return;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        if (!g_running.load()) {
            return;
        }

        std::cout << "[INFO] Follow robot connected" << std::endl;

        JointArray followCurrent{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
        if (readRobotJoints(kFollowIp, kOutputRecipe, kInputRecipe, 50.0, followCurrent)) {
            printJoints(followCurrent, "follow");
        } else {
            std::cout << "[WARN] Failed to read follow joints, using zeros as start pose" << std::endl;
        }

        std::cout << "[INFO] Initial max joint delta: " << std::fixed << std::setprecision(4)
                  << jointsMaxDiff(followCurrent, masterTarget) << " rad" << std::endl;

        std::cout << "[INFO] Starting soft alignment for " << kSoftStartDuration << " s" << std::endl;
        vector6d_t servoJoints{};
        const double stepInterval = kSoftStartDuration / static_cast<double>(kSoftStartSteps);

        for (int step = 0; step <= kSoftStartSteps && g_running.load(); ++step) {
            const double t = static_cast<double>(step) / static_cast<double>(kSoftStartSteps);
            const JointArray interp = interpolateJoints(followCurrent, masterTarget, t);

            for (std::size_t i = 0; i < servoJoints.size(); ++i) {
                servoJoints[i] = interp[i];
            }

            if (!driver.writeServoj(servoJoints, 0)) {
                std::cerr << "[WARN] writeServoj returned false during soft alignment, step " << step << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::duration<double>(stepInterval));
        }

        if (!g_running.load()) {
            driver.stopControl(1000);
            return;
        }

        std::cout << "[INFO] Entering streaming servo mode" << std::endl;
        const auto interval = std::chrono::duration<double>(1.0 / kRtsiFreq);

        while (g_running.load()) {
            const auto start = std::chrono::high_resolution_clock::now();

            JointArray latest{};
            {
                std::shared_lock<std::shared_mutex> lock(g_jointsMutex);
                latest = g_latestJoints;
            }

            for (std::size_t i = 0; i < servoJoints.size(); ++i) {
                servoJoints[i] = latest[i];
            }

            if (!driver.writeServoj(servoJoints, 0) && !driver.isRobotConnected()) {
                std::cerr << "[ERROR] Follow robot connection lost" << std::endl;
                g_running = false;
                break;
            }

            const auto elapsed = std::chrono::high_resolution_clock::now() - start;
            const auto sleepTime = interval - elapsed;
            if (sleepTime > std::chrono::duration<double>(0.0)) {
                std::this_thread::sleep_for(sleepTime);
            }
        }

        driver.stopControl(3000);
        std::cout << "[INFO] Follow driver stopped" << std::endl;
    } catch (const std::exception& e) {
        std::cerr << "[ERROR] followServoThread exception: " << e.what() << std::endl;
        g_running = false;
    } catch (...) {
        std::cerr << "[ERROR] followServoThread unknown exception" << std::endl;
        g_running = false;
    }
}

}  // namespace

int main() {
    if (!SetConsoleCtrlHandler(ConsoleCtrlHandler, TRUE)) {
        std::cerr << "[ERROR] Failed to register console handler" << std::endl;
        return 1;
    }

    std::cout << "========================================" << std::endl;
    std::cout << "  Teleoperation: master -> follow" << std::endl;
    std::cout << "  master: " << kMasterIp << std::endl;
    std::cout << "  follow: " << kFollowIp << std::endl;
    std::cout << "  frequency: " << kRtsiFreq << " Hz" << std::endl;
    std::cout << "========================================" << std::endl;

    try {
        if (!ensureRecipes()) {
            return 1;
        }

        std::thread masterThread(masterReaderThread);
        std::thread followThread(followServoThread);

        masterThread.join();
        g_running = false;
        followThread.join();

        std::cout << "[INFO] Program exited cleanly" << std::endl;
        return 0;
    } catch (const std::exception& e) {
        std::cerr << "[ERROR] main exception: " << e.what() << std::endl;
        g_running = false;
        return 1;
    } catch (...) {
        std::cerr << "[ERROR] main unknown exception" << std::endl;
        g_running = false;
        return 1;
    }
}