7000端口接口说明

以下功能在连接7000端口后使用

头文件：include/tl_servo_ext.h
相关示例代码：examples\cpp\ex_servoj.cpp

1. open_servoJ - 打开关节跟踪模式

方法：Result open_servoJ(SOCKETFD socketFd, std::vector<double> vmax, std::vector<double> amax, std::vector<double> jmax);

功能描述：打开关节跟踪模式（底层 servoJ 透传）。连接 7000 端口后调用本接口即可进入透传模式，之后可高频调用 set_servoJ_pos 实时下发关节角。

参数：

socketFd：socket（7000 伺服端口）。
vmax：关节最大速度约束（固定 7 元素，°/s）。
amax：关节最大加速度约束（固定 7 元素，°/s²）。
jmax：关节最大加加速度约束（固定 7 元素，°/s³）。

返回值：

0：成功。
非0：失败（任一约束向量长度 ≠ 7 时返回参数错误）。

使用示例：

C++

    // 打开 servoJ 关节实时跟踪模式
    // 注意: SDK 底层协议要求 servoJ 相关向量固定为 7 元素，
    //       6 轴机器人在第 7 元素（索引 6）补 0，7 轴机器人为真实 J7 约束值
    std::vector<double> vmax = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0};  // °/s
    std::vector<double> amax = {60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0};  // °/s²
    std::vector<double> jmax = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // °/s³

    auto ret = tl::open_servoJ(socket_fd, vmax, amax, jmax);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] open_servoJ 失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 关节跟踪模式已打开" << std::endl;

2. close_servoJ - 关闭关节跟踪模式

方法：Result close_servoJ(SOCKETFD socketFd);

功能描述：关闭关节跟踪模式（底层 servoJ 透传），停止实时关节角下发。

参数：

socketFd：socket（7000 伺服端口）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 关闭 servoJ 关节跟踪模式
    auto ret = tl::close_servoJ(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[WARN] close_servoJ 返回: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 关节跟踪模式已关闭" << std::endl;

3. set_servoJ_pos - 设置实时跟踪关节位置

方法：Result set_servoJ_pos(SOCKETFD socketFd, std::vector<double> q);

功能描述：实时发送目标关节角到控制器（透传模式，10ms 周期高频调用）。控制器直接响应，实现平滑实时跟踪。

参数：

socketFd：socket（7000 伺服端口）。
q：目标关节角，7 元素向量（度）；6 轴机器人在第 7 元素（索引 6）补 0（外部轴），7 轴机器人第 7 元素为真实 J7 关节角。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 高频发送关节角（J1 从 0° 平滑转到 30°）
    std::vector<double> q = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    for (int i = 0; i < 200; ++i) {
        q[0] += (30.0 / 200.0);       // 每次增加 0.15°
        auto ret = tl::set_servoJ_pos(socket_fd, q);
        if (ret != tl::Result::SUCCESS) {
            std::cerr << "[ERROR] set_servoJ_pos 失败，错误码: " << ret << std::endl;
            return -1;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 100Hz
    }
    std::cout << "[信息] J1=30° 到位" << std::endl;