算法接口

头文件：include/tl_interface.h

1. get_origin_coord_to_target_coord - 坐标转换算法

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

功能描述：在不同坐标系之间进行空间坐标转换，当从关节坐标系转换为直角坐标系时为正运动学解算，当从直角坐标系转换为关节坐标系时为逆运动学解算（接口仅做纯数学数值运算，不会驱动机械臂运动、不会触发伺服下电；遇到奇异点位姿、目标位姿超出可达空间时，直接返回非 0错误码）;接口依赖机械臂通信链路，建立机械臂连接后才可调用。工具坐标系（TOOL）转换由控制器端实现（语义与控制器端一致，SDK 透传）。

参数：

socketFd：已连接的句柄。
originCoord：原坐标系，0 1 2 3 关节直角工具用户。
originPos：原坐标数据。
targetCoord：目标坐标系，0 1 2 3 关节直角工具用户
targetPos：转换后的坐标。
form：形态。
reference_pos：参考点。

返回值：

0：成功
非 0：失败

修正：当前版本逆解不会下电，只会返回错误码

使用示例：

Python

# ============================================================

# 3. 坐标系转换（获取机器人当前位姿在不同坐标系下的表示）

# ============================================================

# 单位说明:

#   关节坐标 [J1..J6] --- 度 °

#   直角坐标 [X,Y,Z,Rx,Ry,Rz] --- 位移 mm，姿态 rad

#   工具/用户坐标 --- 同直角坐标

print_separator("坐标系转换")

print("--- 3. get_origin_coord_to_target_coord ---")

print("  坐标系: 0=关节(°)  1=直角(mm+rad)  2=工具(mm+rad) 3=用户(mm+rad)")

# 从关节坐标系(0) 转换到直角坐标系(1)

# 关节角度: J1=10°, J2=20°, J3=30°, J4=0°, J5=0°, J6=0°

joint_pos = VectorDouble()

joint_pos.push_back(10.0)

joint_pos.push_back(20.0)

joint_pos.push_back(30.0)

joint_pos.push_back(0.0)

joint_pos.push_back(0.0)

joint_pos.push_back(0.0)

joint_pos.push_back(0.0)

cartesian_pos = VectorDouble()

origin_reference_pos = VectorDouble([0]*7)

ret = get_origin_coord_to_target_coord(sock, 0, joint_pos, 1, cartesian_pos, 0, origin_reference_pos)

print_result("get_origin_coord_to_target_coord (关节→直角) 正解", ret)

print_vector("输入关节角度 [°]", joint_pos)

if ret == SUCCESS:

    print("  输出直角坐标 --- 前3位为位移[mm]，后3位为姿态[rad]:")

    print_vector("  直角坐标", cartesian_pos)

    if len(cartesian_pos) >= 6:

        print("    位移 X=" + str(cartesian_pos[0]) + "mm, Y=" + str(cartesian_pos[1])

              + "mm, Z=" + str(cartesian_pos[2]) + "mm")

        print("    姿态 Rx=" + str(cartesian_pos[3]) + "rad, Ry=" + str(cartesian_pos[4])

              + "rad, Rz=" + str(cartesian_pos[5]) + "rad")

        print("    姿态换算(°): Rx=" + str(cartesian_pos[3] *180.0 / math.pi)

              + ", Ry=" + str(cartesian_pos[4] * 180.0 / math.pi)

              + ", Rz=" + str(cartesian_pos[5] * 180.0 / math.pi))

# 逆解: 直角坐标系(1) → 关节坐标系(0)

# 使用正解输出的直角坐标作为输入，验证正反解一致性

if ret == SUCCESS and len(cartesian_pos) >= 6:

    joint_back = VectorDouble()

    ret = get_origin_coord_to_target_coord(sock, 1, cartesian_pos, 0, joint_back, 0, origin_reference_pos)

    print_result("get_origin_coord_to_target_coord (直角→关节) 逆解", ret)

    print("  输入直角坐标:")

    print("    位移 [mm]: X=" + str(cartesian_pos[0]) + ", Y=" + str(cartesian_pos[1])

          + ", Z=" + str(cartesian_pos[2]))

    print("    姿态 [rad]: Rx=" + str(cartesian_pos[3]) + ",Ry=" + str(cartesian_pos[4])

          + ", Rz=" + str(cartesian_pos[5]))

    if ret == SUCCESS:

        print_vector("  逆解关节角度 [°]", joint_back)

        # 对比原始关节角度，计算误差

        err = 0.0

        for i in range(min(len(joint_pos), len(joint_back))):

            err += abs(joint_pos[i] - joint_back[i])

        print("  正反解误差 (应接近0): " + str(err))

2. get_quat2rpy - 四元数转欧拉角

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

功能介绍：将标准四元数（x, y, z, w）转换为Rx-Ry-Rz（绕机体 X、Y、Z 轴依次旋转）顺序的 RPY 欧拉角，输出单位为弧度；接口依赖机械臂通信链路，建立机械臂连接后才可调用。

参数：

socketFd：已连接的句柄。
quat_vector: 被转换的四元数。
rpy_res: 接收欧拉角结果。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

# 建立连接

socket_fd = tl.connect_robot("192.168.1.13", "6001")

# RPY ↔ 四元数 正反验证

rpy_in = tl.VectorDouble([0.2, 0.3, 0.5])   # rad

quat = tl.VectorDouble()

rpy_out = tl.VectorDouble()

ret = tl.get_rpy2quat(socket_fd, rpy_in, quat)

if ret == 0:

    print("输入欧拉角 [rad]: " + str(list(rpy_in)))

    print("中间四元数 [x,y,z,w]: " + str(list(quat)))

ret = tl.get_quat2rpy(socket_fd, quat, rpy_out)

if ret == 0:

    print("还原欧拉角 [rad]: " + str(list(rpy_out)))

else:

    print(f"坐标转换失败，错误码：{ret}")

3. get_rpy2quat - 欧拉角转四元数

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

功能介绍：按Rx-Ry-Rz 机体轴旋转顺序，将 RPY 欧拉角转换为标准四元数（x, y, z, w）；调用前必须完成机械臂 Socket 通信连接，否则接口执行失败。

参数：

socketFd：已连接的句柄。
quat_vector：被转换的欧拉角。
rpy_res ：接收四元素的结果。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 1a. 欧拉角 → 四元数 → 欧拉角（正反验证）

print("--- 1a. RPY ↔ 四元数 正反验证 ---")

rpy_in = [0.2, 0.3, 0.5]  # rad

quat = VectorDouble()

rpy_out = VectorDouble()

ret = get_rpy2quat(sock, rpy_in, quat)

print("get_rpy2quat (RPY→四元数)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_vector("  输入欧拉角 [rad]", rpy_in)

print_vector("  中间四元数 [x, y, z, w]", quat)

ret = get_quat2rpy(sock, quat, rpy_out)

print("get_quat2rpy (四元数→RPY)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_vector("  还原欧拉角 [rad]", rpy_out)

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

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

功能介绍：按照Rx-Ry-Rz 机体轴旋转顺序，将 RPY 欧拉角换算为 3×3 旋转矩阵；需先建立机械臂 Socket通信连接才可调用接口，矩阵存储采用行主序排布。

参数：

socketFd：已连接的句柄。
rpy_vector：被转换的欧拉角。
r_res：接收旋转矩阵的结果。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 1b. 欧拉角 → 旋转矩阵

print("\n--- 1b. get_rpy2r: 欧拉角转旋转矩阵 ---")

rpy = [0.0, 0.0, math.pi / 2]  # rad

rot_mat = VectorDouble()

ret = get_rpy2r(sock, rpy, rot_mat)

print("get_rpy2r", ret)

print_vector("输入欧拉角 [rad]", rpy)

if ret == SUCCESS:

    print_matrix("输出旋转矩阵 (3x3, 行主序, 无量纲)", rot_mat, 3, 3)

5. get_tr2r - 位姿转旋转矩阵

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

功能介绍：从标准 4×4 齐次位姿（TR）矩阵中剥离提取左上角 3×3 旋转子矩阵；调用前必须建立机械臂 Socket 通信连接，位姿平移分量单位为 mm，矩阵整体采用行主序存储。

参数：

socketFd：已连接的句柄。
tr_matrix:被转换的位姿矩阵。
r_res:接收旋转矩阵的结果。

返回值：

0：成功
非 0：失败

使用示例：

Python

# ============================================================

# 2. 位姿矩阵 ↔ 旋转矩阵 --- 正反验证

# ============================================================

# 单位说明:

# 旋转矩阵 R (3×3) --- 无量纲

# 平移向量 T [Tx,Ty,Tz] --- 毫米 mm

# 位姿矩阵 TR (4×4) --- R 部分无量纲，T 部分单位 mm

# 位姿矩阵 (行主序 16 元素):

# [Rxx Rxy Rxz Tx] 第 0 行

# [Ryx Ryy Ryz Ty] 第 1 行

# [Rzx Rzy Rzz Tz] 第 2 行

# [ 0 0 0 1] 第 3 行

print_separator("位姿矩阵 ↔ 旋转矩阵")

# 2a. TR → R → TR（正反验证）

print("--- 2a. TR ↔ R 正反验证 ---")

# 构造一个绕 Z 转 90°、平移 (100, 200, 300) mm 的位姿矩阵

cos90 = math.cos(math.pi / 2)

sin90 = math.sin(math.pi / 2)

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

]

R = VectorDouble()

tr_out = VectorDouble()

ret = get_tr2r(sock, tr_in, R)

print_result("get_tr2r (TR→R)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_matrix("输入位姿矩阵 (4x4, R无量纲, T:mm)", tr_in, 4, 4)

print_matrix("中间旋转矩阵 (3x3, 无量纲)", R, 3, 3)

ret = get_r2tr(sock, R, tr_out)

print_result("get_r2tr (R→TR)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_matrix("还原位姿矩阵 (4x4, R无量纲, T:mm)", tr_out, 4, 4)

# 说明: get_tr2r 只提取旋转矩阵，平移 T 在此丢弃；

# get_r2tr 的输入不含平移，还原出的 TR 平移部分恒为 0。

# SDK 无 "R+T 构造 TR" 接口，完整位姿矩阵需由调用方自行拼装 T。

print(" 注: get_tr2r 仅输出旋转矩阵，平移 T 不参与往返；")

print(" 还原位姿矩阵的平移部分为 0，需调用方另行拼装。")

# 对比旋转部分（TR→R→TR 链路仅保证旋转信息无损）

# 注意: 行主序 4x4 中旋转块位于 0,1,2, 4,5,6, 8,9,10，

# 位置 3/7/11 是平移列，不能按连续 0..8 遍历。

if len(tr_out) >= 16 and len(tr_in) >= 16:

    rot_idx = [0, 1, 2, 4, 5, 6, 8, 9, 10]

    r_err = 0.0

    for k in rot_idx:

        r_err += abs(tr_out[k] - tr_in[k])

    print(" 旋转矩阵误差 (应接近0): " + str(r_err))

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

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

功能介绍：将 3×3 旋转矩阵补全平移项，生成标准 4×4 齐次位姿（TR）矩阵；默认平移分量全部置 0，矩阵采用行主序存储，平移量单位为 mm，调用前需先建立机械臂 Socket通信连接。

参数：

socketFd：已连接的句柄。
r_matrix：被转换的旋转矩阵。
tr_res：接收位姿矩阵结果。

返回值：

0：成功
非 0：失败

使用示例：

Python

# ============================================================

# 2. 位姿矩阵 ↔ 旋转矩阵 --- 正反验证

# ============================================================

# 单位说明:

#   旋转矩阵 R (3×3)     --- 无量纲

#   平移向量 T [Tx,Ty,Tz] --- 毫米 mm

#   位姿矩阵 TR (4×4)  --- R 部分无量纲，T 部分单位 mm

# 位姿矩阵 (行主序 16 元素):

#   [Rxx Rxy Rxz Tx]  第 0 行

#   [Ryx Ryy Ryz Ty]  第 1 行

#   [Rzx Rzy Rzz Tz]  第 2 行

#   [  0   0   0  1]  第 3 行

print_separator("位姿矩阵 ↔ 旋转矩阵")

# 2a. TR → R → TR（正反验证）

print("--- 2a. TR ↔ R 正反验证 ---")

# 构造一个绕 Z 转 90°、平移 (100, 200, 300) mm 的位姿矩阵

cos90 = math.cos(math.pi / 2)

sin90 = math.sin(math.pi / 2)

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

]

R = VectorDouble()

tr_out = VectorDouble()

ret = get_tr2r(sock, tr_in, R)

print_result("get_tr2r (TR→R)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_matrix("输入位姿矩阵 (4x4, R无量纲, T:mm)", tr_in, 4, 4)

print_matrix("中间旋转矩阵 (3x3, 无量纲)", R, 3, 3)

ret = get_r2tr(sock, R, tr_out)

print_result("get_r2tr (R→TR)", ret)

if ret != SUCCESS:

    sys.exit(-1)

print_matrix("还原位姿矩阵 (4x4, R无量纲, T:mm)", tr_out, 4, 4)

# 说明: get_tr2r 只提取旋转矩阵，平移 T 在此丢弃；

# get_r2tr 的输入不含平移，还原出的 TR 平移部分恒为 0。

# SDK 无 "R+T 构造 TR" 接口，完整位姿矩阵需由调用方自行拼装 T。

print("  注: get_tr2r 仅输出旋转矩阵，平移 T 不参与往返；")

print("      还原位姿矩阵的平移部分为 0，需调用方另行拼装。")

# 对比旋转部分（TR→R→TR 链路仅保证旋转信息无损）

# 注意: 行主序 4x4 中旋转块位于 0,1,2, 4,5,6, 8,9,10，

#       位置 3/7/11 是平移列，不能按连续 0..8 遍历。

if len(tr_out) >= 16 and len(tr_in) >= 16:

    rot_idx = [0, 1, 2, 4, 5, 6, 8, 9, 10]

    r_err = 0.0

    for k in rot_idx:

        r_err += abs(tr_out[k] - tr_in[k])

    print("  旋转矩阵误差 (应接近0): " + str(r_err))