算法接口

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

1. get_origin_coord_to_target_coord - 坐标转换算法

方法：Result get_origin_coord_to_target_coord(SOCKETFD socketFd, Coord originCoord, std::vector<double> originPos, Coord targetCoord, std::vector<double>& targetPos, bool& convert_state, int form = 0, const std::vector<double>& reference_pos = {});

功能描述：在不同坐标系之间进行空间坐标转换。从关节坐标系转换为直角坐标系时为正运动学解算，从直角坐标系转换为关节坐标系时为逆运动学解算。接口仅做纯数学数值运算，不会驱动机械臂运动、不会触发伺服下电；逆解失败（奇异点位姿、目标位姿超出可达空间）时返回 EXCEPTION（-5）。推荐使用 Coord 枚举重载版本，int 旧版已废弃。接口依赖机械臂通信链路，建立连接后才可调用。工具坐标系（TOOL）转换由控制器端实现（语义与控制器端一致，SDK 透传）。

⚠ 废弃提示：带 convert_state 出参的重载已废弃——该出参冗余，逆解结果已反映在返回值。请使用无 convert_state 的枚举重载。

参数：

socketFd：已连接的句柄。
originCoord：原坐标系，Coord 枚举（JOINT / BASE / TOOL / USER）。
originPos：要进行转换的坐标值 [X, Y, Z, A, B, C]：
- 关节坐标：0-6 轴，范围 [-10000, 10000]（度）
- 基坐标：0-2 轴范围 [-10000, 10000]（mm），3-6 轴范围 [-3.1416, 3.1416]（rad）
- 工具坐标：同基坐标
- 用户坐标：同基坐标
targetCoord：目标坐标系，Coord 枚举（JOINT / BASE / TOOL / USER）。
targetPos：输出，转换后的坐标值（点位数组）。
convert_state：输出，true 逆解成功 / false 逆解失败。
form：形态（默认 0，与控制器协议对齐）。
reference_pos：参考点（默认空）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 正解：关节坐标 → 基坐标
    std::vector<double> joint_pos = {10, 20, 30, 0, 0, 0}; // 关节角度 [°]
    std::vector<double> cartesian_pos;
    bool convert_state = false;
    auto ret = tl::get_origin_coord_to_target_coord(
        socket_fd, tl::Coord::JOINT, joint_pos, tl::Coord::BASE, cartesian_pos, convert_state);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 坐标转换失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 正解成功: 位移 X=" << cartesian_pos[0]
              << " Y=" << cartesian_pos[1] << " Z=" << cartesian_pos[2] << " (mm)" << std::endl;

    // 逆解：基坐标 → 关节坐标
    std::vector<double> joint_back;
    ret = tl::get_origin_coord_to_target_coord(
        socket_fd, tl::Coord::BASE, cartesian_pos, tl::Coord::JOINT, joint_back, convert_state);
    if (ret != tl::Result::SUCCESS || !convert_state) {
        std::cerr << "[ERROR] 逆解失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 逆解成功，关节角度:";
    for (const auto& j : joint_back) {
        std::cout << " " << j;
    }
    std::cout << std::endl;

2. get_quat2rpy - 四元数转欧拉角

方法：Result get_quat2rpy(SOCKETFD socketFd, std::vector<double> quat_vector, std::vector<double>& rpy_res);

功能描述：将四元数转换为欧拉角，接口依赖机械臂通信链路，建立连接后才可调用。

参数：

socketFd：已连接的句柄。
quat_vector：输入，四元数向量，4 元素 [w, x, y, z]（w 在首位；须为单位四元数）。
rpy_res：输出，欧拉角向量，3 元素 [rx, ry, rz]（弧度）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 四元数转欧拉角
    std::vector<double> quat = {1.0, 0.0, 0.0, 0.0}; // [w, x, y, z]
    std::vector<double> rpy;
    auto ret = tl::get_quat2rpy(socket_fd, quat, rpy);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 四元数转欧拉角失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 欧拉角 [rad]: Rx=" << rpy[0]
              << " Ry=" << rpy[1] << " Rz=" << rpy[2] << std::endl;

3. get_rpy2quat - 欧拉角转四元数

方法：Result get_rpy2quat(SOCKETFD socketFd, std::vector<double> rpy_vector, std::vector<double>& quat_res);

功能描述：将欧拉角转换为四元数，接口依赖机械臂通信链路，建立连接后才可调用。

参数：

socketFd：已连接的句柄。
rpy_vector：输入，欧拉角向量，3 元素 [rx, ry, rz]（弧度）。
quat_res：输出，四元数向量，4 元素 [w, x, y, z]（w 在首位）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 欧拉角转四元数
    std::vector<double> rpy = {0.2, 0.3, 0.5}; // rad
    std::vector<double> quat;
    auto ret = tl::get_rpy2quat(socket_fd, rpy, quat);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 欧拉角转四元数失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 四元数 [w, x, y, z]: "
              << quat[0] << ", " << quat[1] << ", " << quat[2] << ", " << quat[3] << std::endl;

4. get_rpy2r - 欧拉角转旋转矩阵

方法：Result get_rpy2r(SOCKETFD socketFd, std::vector<double> rpy_vector, std::vector<double>& r_res);

功能描述：将欧拉角转换为旋转矩阵，接口依赖机械臂通信链路，建立连接后才可调用。

参数：

socketFd：已连接的句柄。
rpy_vector：输入，欧拉角向量，3 元素 [rx, ry, rz]（弧度）。
r_res：输出，旋转矩阵 9 元素，行主序。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 欧拉角转旋转矩阵
    std::vector<double> rpy = {0.0, 0.0, M_PI_2}; // rad
    std::vector<double> rot_mat;
    auto ret = tl::get_rpy2r(socket_fd, rpy, rot_mat);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 欧拉角转旋转矩阵失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 旋转矩阵（3x3，行主序）:" << std::endl;
    for (int i = 0; i < 3; ++i) {
        std::cout << "  " << rot_mat[i * 3] << " " << rot_mat[i * 3 + 1] << " " << rot_mat[i * 3 + 2] << std::endl;
    }

5. get_tr2r - 位姿转旋转矩阵

方法：Result get_tr2r(SOCKETFD socketFd, std::vector<double> tr_matrix, std::vector<double>& r_res);

功能描述：将位姿矩阵（4x4 齐次变换矩阵）转换为旋转矩阵（3x3），提取旋转部分，平移部分丢弃，接口依赖机械臂通信链路，建立连接后才可调用。

参数：

socketFd：已连接的句柄。
tr_matrix：输入，位姿矩阵，vector 长度 16（行主序，R 部分无量纲，T 部分单位 mm）。
r_res：输出，旋转矩阵，vector 长度 9（行主序）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 位姿矩阵转旋转矩阵（构造绕 Z 转 90°、平移 (100, 200, 300) mm 的位姿矩阵）
    double cos90 = cos(M_PI_2), sin90 = sin(M_PI_2);
    std::vector<double> tr_in = {
        cos90, -sin90, 0.0, 100.0,
        sin90,  cos90, 0.0, 200.0,
        0.0,    0.0,   1.0, 300.0,
        0.0,    0.0,   0.0, 1.0
    };
    std::vector<double> R;
    auto ret = tl::get_tr2r(socket_fd, tr_in, R);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 位姿转旋转矩阵失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 旋转矩阵（3x3，行主序）:" << std::endl;
    for (int i = 0; i < 3; ++i) {
        std::cout << "  " << R[i * 3] << " " << R[i * 3 + 1] << " " << R[i * 3 + 2] << std::endl;
    }

6. get_r2tr - 旋转矩阵转位姿矩阵

方法：Result get_r2tr(SOCKETFD socketFd, std::vector<double> r_matrix, std::vector<double>& tr_res);

功能描述：将旋转矩阵（3x3）转换为位姿矩阵（4x4 齐次变换矩阵），平移部分输出为 0。接口依赖机械臂通信链路，建立连接后才可调用。

参数：

socketFd：已连接的句柄。
r_matrix：输入，旋转矩阵，vector 长度 9（行主序）。
tr_res：输出，位姿矩阵，vector 长度 16（行主序，R 部分无量纲，T 部分为 0）。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 旋转矩阵转位姿矩阵（R 为前面 get_tr2r 的输出）
    std::vector<double> tr_out;
    auto ret = tl::get_r2tr(socket_fd, R, tr_out);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 旋转矩阵转位姿矩阵失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 位姿矩阵（4x4，行主序，平移部分为 0）:" << std::endl;
    for (int i = 0; i < 4; ++i) {
        std::cout << "  " << tr_out[i * 4] << " " << tr_out[i * 4 + 1] << " "
                  << tr_out[i * 4 + 2] << " " << tr_out[i * 4 + 3] << std::endl;
    }