一、简介

二、运行流程

• elite-cs-series-sdk.dll
• ssh.dll
• libcrypto-3-x64.dll

三、可能出现的问题

四、代码和附件

1. RobotSdk(robotIp, scriptPath) 构造：

• EliteDriverConfig{} 配置 headless 模式 + 脚本路径 → EliteDriver(config) 构造
• DashboardClient().connect(robotIp) 连 Dashboard
• RtsiIOInterface(outputRecipe, inputRecipe, 100.0).connect(robotIp) 连 RTSI，100Hz

2. robot.powerOn() → 内部 dashboard_->powerOn()
3. robot.brakeRelease() → 内部 dashboard_->brakeRelease()
4. HfdApi() 构造 → LoadLibrary("HFD_API64.dll") 动态加载 DLL → GetProcAddress 绑定 12 个函数指针
5. hfd.keyCombination() → open() → init() → calibrateDevice() → enableForce() → enableExpertMode() → enableDevice(true) → disableExpertMode() → setGravityCompensation(true) 摇杆初始化
6. hfd.getButton() 轮询，检测 0→1 上升沿
7. 按下：记录 hfd.getPositionAndOrientation() 作为原点

• robot.getActualTcpPose() → 内部 io_->getActualTCPPose() 读 TCP 位姿
• calculateXyz / calculateRxRyRz 算基准 V5/V51
• robot.connectExternalControl() → 内部 driver_->sendExternalControlScript() + 轮询 driver_->isRobotConnected()

8. 运行中循环：

• hfd.getPositionAndOrientation() 读摇杆 → 减原点得偏移 pose
• poseMul / userFrameToTcpFrame 用户坐标系变换
• robot.writeServojCartesian(targetPose, 100) → 内部 driver_->writeServoj(pose, timeoutMs, cartesian=true)
• 4ms 周期

9. 再按按钮：robot.writeIdle(500) → hfd.keyCombinationOff() → sleep 1s → hfd.keyCombinationOn()
10. 析构：driver_->stopControl() / io_->disconnect() / dashboard_->disconnect()

#include <Elite/DashboardClient.hpp>
#include <Elite/DataType.hpp>
#include <Elite/EliteDriver.hpp>
#include <Elite/Log.hpp>
#include <Elite/RtsiIOInterface.hpp>

// Elite SDK 路径: C:\Users\SZ02IT0615\source\repos\EliteExample\EliteExample\elite-cs-sdk
// VS 项目需在 "VC++ 目录 → 包含目录" 中添加上述路径

#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <exception>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>

#define NOMINMAX
#include <windows.h>
using DllHandle = HMODULE;

using namespace ELITE;
using Pose = std::array<double, 6>;
using Matrix4 = std::array<std::array<double, 4>, 4>;

namespace {

constexpr const char* kRobotIp = "172.16.15.70";
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";
constexpr double kPi = 3.14159265358979323846;

constexpr const char* kHfdDllName = "HFD_API64.dll";

double degToRad(double deg) {
    return deg / 57.29577951308232;
}

Pose mmDegToMRad(const Pose& pose) {
    Pose out = pose;
    out[0] /= 1000.0;
    out[1] /= 1000.0;
    out[2] /= 1000.0;
    out[3] = degToRad(out[3]);
    out[4] = degToRad(out[4]);
    out[5] = degToRad(out[5]);
    return out;
}

Matrix4 identityMatrix() {
    Matrix4 mat{};
    for (int i = 0; i < 4; ++i) {
        mat[i][i] = 1.0;
    }
    return mat;
}

Matrix4 eulerToMatrix(const Pose& pose) {
    const double cx = std::cos(pose[3]);
    const double sx = std::sin(pose[3]);
    const double cy = std::cos(pose[4]);
    const double sy = std::sin(pose[4]);
    const double cz = std::cos(pose[5]);
    const double sz = std::sin(pose[5]);

    Matrix4 mat = identityMatrix();
    mat[0][0] = cy * cz;
    mat[0][1] = cz * sy * sx - sz * cx;
    mat[0][2] = cz * sy * cx + sz * sx;
    mat[1][0] = cy * sz;
    mat[1][1] = sz * sy * sx + cz * cx;
    mat[1][2] = sz * sy * cx - cz * sx;
    mat[2][0] = -sy;
    mat[2][1] = cy * sx;
    mat[2][2] = cy * cx;
    mat[0][3] = pose[0];
    mat[1][3] = pose[1];
    mat[2][3] = pose[2];
    return mat;
}

Pose matrixToEuler(const Matrix4& mat) {
    Pose pose{};
    pose[0] = mat[0][3];
    pose[1] = mat[1][3];
    pose[2] = mat[2][3];

    const double sy = -mat[2][0];
    const double cy = std::sqrt(std::max(0.0, 1.0 - sy * sy));
    constexpr double kEpsilon = 1e-9;

    if (cy > kEpsilon) {
        pose[3] = std::atan2(mat[2][1], mat[2][2]);
        pose[4] = std::atan2(sy, cy);
        pose[5] = std::atan2(mat[1][0], mat[0][0]);
    } else {
        pose[3] = std::atan2(-mat[1][2], mat[1][1]);
        pose[4] = std::atan2(sy, cy);
        pose[5] = 0.0;
    }

    return pose;
}

Matrix4 matMul(const Matrix4& lhs, const Matrix4& rhs) {
    Matrix4 out{};
    for (int row = 0; row < 4; ++row) {
        for (int col = 0; col < 4; ++col) {
            double value = 0.0;
            for (int k = 0; k < 4; ++k) {
                value += lhs[row][k] * rhs[k][col];
            }
            out[row][col] = value;
        }
    }
    return out;
}

Matrix4 matInv(const Pose& pose) {
    Matrix4 src = eulerToMatrix(pose);
    Matrix4 out = identityMatrix();

    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            out[row][col] = src[col][row];
        }
    }

    for (int row = 0; row < 3; ++row) {
        out[row][3] =
            -(out[row][0] * src[0][3] + out[row][1] * src[1][3] + out[row][2] * src[2][3]);
    }
    return out;
}

Pose poseInv(const Pose& pose) {
    return matrixToEuler(matInv(pose));
}

Pose poseMul(const Pose& lhs, const Pose& rhs) {
    return matrixToEuler(matMul(eulerToMatrix(lhs), eulerToMatrix(rhs)));
}

Pose basePoseToUserPose(const Pose& user, const Pose& basePose) {
    return poseMul(poseInv(user), basePose);
}

Pose userFrameToTcpFrame(const Pose& userPose, const Pose& target) {
    return poseMul(userPose, target);
}

Pose fromVector6d(const vector6d_t& value) {
    Pose pose{};
    for (std::size_t i = 0; i < 6; ++i) {
        pose[i] = value[i];
    }
    return pose;
}

vector6d_t toVector6d(const Pose& pose) {
    vector6d_t out{};
    for (std::size_t i = 0; i < 6; ++i) {
        out[i] = pose[i];
    }
    return out;
}

double round5(double value) {
    return std::round(value * 100000.0) / 100000.0;
}

class HfdApi {
public:
    HfdApi() {
        module_ = LoadLibraryA(kHfdDllName);
        if (!module_) {
            throw std::runtime_error("Failed to load " + std::string(kHfdDllName));
        }

        hfdOpen_ = load<int(*)()>("hfdOpen");
        hfdInit_ = load<void(*)(int)>("hfdInit");
        hfdCalibrateDevice_ = load<void(*)(int)>("hfdCalibrateDevice");
        hfdGetPosition_ = load<void(*)(double*, double*, double*, int)>("hfdGetPosition");
        hfdGetOrientationRad_ =
            load<void(*)(double*, double*, double*, int)>("hfdGetOrientationRad");
        hfdEnableForce_ = load<void(*)(bool, int)>("hfdEnableForce");
        hfdSetForce_ = load<void(*)(double, double, double, int)>("hfdSetForce");
        hfdEnableExpertMode_ = load<void(*)()>("hfdEnableExpertMode");
        hfdEnableDevice_ = load<void(*)(bool, int)>("hfdEnableDevice");
        hfdDisableExpertMode_ = load<void(*)()>("hfdDisableExpertMode");
        hfdSetGravityCompensation_ =
            load<void(*)(bool, int)>("hfdSetGravityCompensation");
        hfdGetButton_ = load<int(*)(int, int)>("hfdGetButton");
    }

    ~HfdApi() {
        if (module_) {
            FreeLibrary(module_);
        }
    }

    int open() const {
        return hfdOpen_();
    }

    void init() const {
        hfdInit_(-1);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void calibrateDevice() const {
        hfdCalibrateDevice_(-1);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    Pose getPositionAndOrientation() const {
        double px = 0.0;
        double py = 0.0;
        double pz = 0.0;
        double rx = 0.0;
        double ry = 0.0;
        double rz = 0.0;
        hfdGetPosition_(&px, &py, &pz, -1);
        hfdGetOrientationRad_(&rx, &ry, &rz, -1);
        return {px, py, pz, rx, ry, rz};
    }

    int getButton() const {
        return hfdGetButton_(0, -1);
    }

    void enableForce() const {
        hfdEnableForce_(true, -1);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void setForce(double x, double y, double z) const {
        hfdSetForce_(x, y, z, -1);
    }

    void enableExpertMode() const {
        hfdEnableExpertMode_();
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void enableDevice(bool enable) const {
        hfdEnableDevice_(enable, -1);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void disableExpertMode() const {
        hfdDisableExpertMode_();
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void setGravityCompensation(bool enable) const {
        hfdSetGravityCompensation_(enable, -1);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    int keyCombination() const {
        const int handle = open();
        init();
        calibrateDevice();
        enableForce();
        enableExpertMode();
        enableDevice(true);
        disableExpertMode();
        setGravityCompensation(true);
        return handle;
    }

    void keyCombinationOn() const {
        enableExpertMode();
        enableDevice(true);
        disableExpertMode();
        setGravityCompensation(true);
    }

    void keyCombinationOff() const {
        setGravityCompensation(false);
        enableExpertMode();
        enableDevice(false);
        disableExpertMode();
    }

private:
    template <typename T>
    T load(const char* name) {
#ifdef _WIN32
        auto symbol = reinterpret_cast<T>(GetProcAddress(module_, name));
#else
        auto symbol = reinterpret_cast<T>(dlsym(module_, name));
        (void)dlerror;  // silence unused warning
#endif
        if (!symbol) {
            throw std::runtime_error(std::string("Missing HFD symbol: ") + name);
        }
        return symbol;
    }

    DllHandle module_{nullptr};
    int(*hfdOpen_)() = nullptr;
    void(*hfdInit_)(int) = nullptr;
    void(*hfdCalibrateDevice_)(int) = nullptr;
    void(*hfdGetPosition_)(double*, double*, double*, int) = nullptr;
    void(*hfdGetOrientationRad_)(double*, double*, double*, int) = nullptr;
    void(*hfdEnableForce_)(bool, int) = nullptr;
    void(*hfdSetForce_)(double, double, double, int) = nullptr;
    void(*hfdEnableExpertMode_)() = nullptr;
    void(*hfdEnableDevice_)(bool, int) = nullptr;
    void(*hfdDisableExpertMode_)() = nullptr;
    void(*hfdSetGravityCompensation_)(bool, int) = nullptr;
    int(*hfdGetButton_)(int, int) = nullptr;
};

class RobotSdk {
public:
    RobotSdk(const std::string& robotIp, const std::string& scriptPath)
        : robotIp_(robotIp),
          driver_(makeDriver(robotIp, scriptPath)),
          dashboard_(std::make_unique<DashboardClient>()),
          io_(std::make_unique<RtsiIOInterface>(kOutputRecipe, kInputRecipe, 100.0)) {
        if (!dashboard_->connect(robotIp_)) {
            throw std::runtime_error("Failed to connect dashboard");
        }
        if (!io_->connect(robotIp_)) {
            throw std::runtime_error("Failed to connect RTSI");
        }
    }

    ~RobotSdk() {
        disconnect();
    }

    void powerOn() {
        ELITE_LOG_INFO("Powering on robot");
        if (!dashboard_->powerOn()) {
            throw std::runtime_error("Power on failed");
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    void brakeRelease() {
        ELITE_LOG_INFO("Releasing brake");
        if (!dashboard_->brakeRelease()) {
            throw std::runtime_error("Brake release failed");
        }
    }

    void connectExternalControl() {
        if (!driver_->isRobotConnected()) {
            if (!driver_->sendExternalControlScript()) {
                throw std::runtime_error("Failed to send external control script");
            }
        }

        const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
        while (!driver_->isRobotConnected()) {
            if (std::chrono::steady_clock::now() >= deadline) {
                throw std::runtime_error("EliteDriver connection timeout");
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    Pose getActualTcpPose() const {
        return fromVector6d(io_->getActualTCPPose());
    }

    bool writeServojCartesian(const Pose& pose, int timeoutMs) {
        return driver_->writeServoj(toVector6d(pose), timeoutMs, true);
    }

    void writeIdle(int timeoutMs) {
        driver_->writeIdle(timeoutMs);
    }

    void disconnect() {
        if (driver_) {
            driver_->stopControl();
        }
        if (io_) {
            io_->disconnect();
        }
        if (dashboard_) {
            dashboard_->disconnect();
        }
    }

private:
    static std::unique_ptr<EliteDriver> makeDriver(const std::string& robotIp,
                                                   const std::string& scriptPath) {
        EliteDriverConfig config;
        config.robot_ip = robotIp;
        config.headless_mode = true;
        config.script_file_path = scriptPath;
        return std::make_unique<EliteDriver>(config);
    }

    std::string robotIp_;
    std::unique_ptr<EliteDriver> driver_;
    std::unique_ptr<DashboardClient> dashboard_;
    std::unique_ptr<RtsiIOInterface> io_;
};

Pose calculateXyz(const Pose& tcp, const Pose& pose) {
    const Pose v2 = poseInv(mmDegToMRad({-pose[0], pose[2], pose[1], 0.0, 0.0, 0.0}));
    const Pose v1 = basePoseToUserPose(mmDegToMRad({-340.1, -171.1, 311.4, 0.0, 0.0, 0.0}), tcp);
    return poseMul(v2, {v1[0], v1[1], v1[2], 0.0, 0.0, 0.0});
}

Pose calculateRxRyRz(const Pose& tcp, const Pose& pose) {
    const Pose v21 = poseInv({0.0, 0.0, 0.0, pose[3], pose[4], pose[5]});
    const Pose v1 = basePoseToUserPose(mmDegToMRad({-340.1, -171.1, 311.4, 0.0, 0.0, 0.0}), tcp);
    return poseMul(v21, {0.0, 0.0, 0.0, v1[3], v1[4], v1[5]});
}

}  // namespace

int main() {
    try {
        RobotSdk robot(kRobotIp, kScriptPath);
        HfdApi hfd;

        robot.powerOn();
        robot.brakeRelease();

        const int hfdHandle = hfd.keyCombination();

        bool running = false;
        int lastButton = hfd.getButton();
        Pose initHfd{};
        Pose v5{};
        Pose v51{};

        while (true) {
            const int currentButton = hfd.getButton();
            const bool buttonPressed = (currentButton == 1 && lastButton == 0);
            lastButton = currentButton;

            if (buttonPressed) {
                if (!running) {
                    initHfd = hfd.getPositionAndOrientation();
                    const Pose tcp = robot.getActualTcpPose();
                    const Pose zeroPose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
                    v5 = calculateXyz(tcp, zeroPose);
                    v51 = calculateRxRyRz(tcp, zeroPose);

                    robot.connectExternalControl();
                    std::cout << "External control connected, start motion" << std::endl;
                    running = true;
                } else {
                    robot.writeIdle(500);
                    hfd.keyCombinationOff();
                    std::cout << "Stop motion" << std::endl;
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                    hfd.keyCombinationOn();
                    running = false;
                }
            }

            if (running) {
                const Pose currentHfd = hfd.getPositionAndOrientation();
                Pose pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
                pose[2] = currentHfd[2] - initHfd[2];
                pose[1] = currentHfd[1] - initHfd[1];
                pose[0] = currentHfd[0] - initHfd[0];
                pose[3] = currentHfd[3] - initHfd[3];
                pose[4] = currentHfd[4] - initHfd[4];
                pose[5] = currentHfd[5] - initHfd[5];

                const Pose v6 = poseMul(mmDegToMRad({-pose[0], pose[2], pose[1], 0.0, 0.0, 0.0}), v5);
                const Pose result2 =
                    userFrameToTcpFrame(mmDegToMRad({-340.1, -171.1, 311.4, 0.0, 0.0, 0.0}), v6);

                const Pose v61 = poseMul({0.0, 0.0, 0.0, pose[3], pose[4], pose[5]}, v51);
                const Pose result21 =
                    userFrameToTcpFrame(mmDegToMRad({-340.1, -171.1, 311.4, 0.0, 0.0, 0.0}), v61);

                Pose targetPose{
                    round5(result2[0]),
                    round5(result2[1]),
                    round5(result2[2]),
                    kPi,
                    0.0,
                    round5(result21[5]),
                };

                std::cout << "target pose: ["
                          << targetPose[0] << ", "
                          << targetPose[1] << ", "
                          << targetPose[2] << ", "
                          << targetPose[3] << ", "
                          << targetPose[4] << ", "
                          << targetPose[5] << "]"
                          << std::endl;

                if (!robot.writeServojCartesian(targetPose, 100)) {
                    throw std::runtime_error("writeServoj(cartesian) failed");
                }
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(4));
        }
    } catch (const std::exception& ex) {
        std::cerr << ex.what() << std::endl;
        return 1;
    }
}