六维力传感器查询接口

头文件：include/tl_interface.h

1. set_six_dimensional_force_communication_params - 配置六维力传感器数据

方法：set_six_dimensional_force_communication_params(SOCKETFD socketFd, SixDimensionalForceCommunicationParams& param);

功能描述：配置机械臂末端六维力传感器。

参数：

socketFd：已连接的句柄。
commParams：输入参数，用于存储六维力传感器通讯参数。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 1. 设置六维力传感器通讯参数

commParams = SixDimensionalForceCommunicationParams()

commParams.sensorCommunicationType = 0 # 0: EtherCAT

commParams.startupAutoConnectSensor = True # 启动自动连接传感器

commParams.sensorDragEnable = True # 传感器拖拽使能

commParams.etherCat_mapNum = 1 # EtherCAT 参数

commParams.YDirection = -1 # y反方向

commParams.ZDirection = -1 # z反方向

ret = set_six_dimensional_force_communication_params(socketFd, commParams)

if ret == SUCCESS:

    print("设置六维力传感器通讯参数成功!")

else:

    print(f"设置六维力传感器通讯参数失败! ret = {ret}")

2. get_six_dimensional_force_communication_params - 获取六维力传感器通讯参数

方法：get_six_dimensional_force_communication_params(socketFd, params);

功能描述：获取六维力传感器通讯参数。

参数：

socketFd：已连接的句柄。
params：SixDimensionalForceCommunicationParams 输出参数（sensorCommunicationType 通讯类型；sensorDragEnable 传感器拖拽使能；originDataInitialPara 原始数据初始参数；startupAutoConnectSensor 启动自动连接传感器；YDirection Y 方向；ZDirection Z 方向；etherCat_mapNum EtherCAT 等）。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

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

params = SixDimensionalForceCommunicationParams()

ret = tl.get_six_dimensional_force_communication_params(socket_fd, params)

3. get_sensor_base_param - 获取六维力传感器的基础参数

方法：get_sensor_base_param(SOCKETFD socketFd, SensorBaseParam& baseParam);

功能说明：获取六维力传感器的基础参数（质量、质心、标零状态）。

参数：

socketFd：已连接的句柄。
baseParam 用于接收传感器基础参数的结构体。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 查询标定后的基础参数

base_param = SensorBaseParam()

ret = get_sensor_base_param(socket_fd, base_param)

if ret == SUCCESS:

    print("  传感器质量: " + str(base_param.sensorMass))

    print("  质心 - X: " + str(base_param.sensorMassCenterX)

          + ", Y: " + str(base_param.sensorMassCenterY)

          + ", Z: " + str(base_param.sensorMassCenterZ))

4. mark_base_sensor - 执行六维力传感器标定

方法：mark_base_sensor(SOCKETFD socketFd, bool& success);

功能描述：完成六维力传感器零点偏移与正交耦合误差校准，消除自重及安装偏差带来的力信号零漂，保证后续力控采集数据精度。

参数：

socketFd：已连接的句柄。

返回值：

0：成功
非 0：失败

使用示例：

Python

# ----标定六维力传感器零位/重心----

# 操作前请确保:

#   1. 机械臂末端无负载或已安装标准负载

#   2. 机械臂处于竖直姿态（关节各轴在合理位置）

#   3. 周围安全，标定过程中机械臂会缓慢运动

print("[INFO]**********标定六维力传感器零位/重心**********")

print("[INFO] 即将执行六维力传感器标定，请确认:")

print("  1. 机械臂末端无负载（或已安装标准负载）")

print("  2. 机械臂周围安全无人")

confirm = input("  是否继续? (y/n): ")

if confirm.lower() == 'y':

    success = False

    ret, success = mark_base_sensor(socket_fd, success)

    if ret != SUCCESS:

        print("[ERROR] 标定六维力传感器失败，错误码: " + str(ret))

    elif success:

        print("[INFO] 标定六维力传感器成功")

        # 查询标定后的基础参数

        base_param = SensorBaseParam()

        ret = get_sensor_base_param(socket_fd, base_param)

        if ret == SUCCESS:

            print("  传感器质量: " + str(base_param.sensorMass))

            print("  质心 - X: " + str(base_param.sensorMassCenterX)

                  + ", Y: " + str(base_param.sensorMassCenterY)

                  + ", Z: " + str(base_param.sensorMassCenterZ))

            print("  标零状态: " + ("已标零" if base_param.saveZero else "未标零"))

    else:

        print("[ERROR] 标定六维力传感器失败，标定结果为 false")

else:

    print("  [信息] 用户取消标定")

print("")

5. get_sensor_6d_data - 获取六维力传感器数据

方法：get_sensor_6d_data(socketFd, sensorData);

功能描述：获取六维力传感器数据。

参数：

socketFd：已连接的句柄。
sensorData：输出参数，用于存储力和力矩的结构体。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 2. 获取六维力传感器数据

sensorData = Sensor6DData()

ret = get_sensor_6d_data(socketFd, sensorData)

if ret == SUCCESS:

    print("获取六维力传感器数据成功!")

    print(f"传感器连接状态: {'已连接' if sensorData.sensorConnected else '未连接'}")

    print("--- 传感器原始数据 ---")

    print(f"Fx = {sensorData.fxData:.3f} N")

    print(f"Fy = {sensorData.fyData:.3f} N")

    print(f"Fz = {sensorData.fzData:.3f} N")

    print(f"Mx = {sensorData.mxData:.3f} N·m")

    print(f"My = {sensorData.myData:.3f} N·m")

    print(f"Mz = {sensorData.mzData:.3f} N·m")

    print("--- 去皮后数据 ---")

    print(f"Fx = {sensorData.fxDataSubBase:.3f} N")

    print(f"Fy = {sensorData.fyDataSubBase:.3f} N")

    print(f"Fz = {sensorData.fzDataSubBase:.3f} N")

    print(f"Mx = {sensorData.mxDataSubBase:.3f} N·m")

    print(f"My = {sensorData.myDataSubBase:.3f} N·m")

    print(f"Mz = {sensorData.mzDataSubBase:.3f} N·m")

else:

    print(f"获取六维力传感器数据失败! ret = {ret}")

6. get_payload_param_by_sensor - 获取六维力传感器负载参数

方法：get_payload_param_by_sensor(socketFd, param);

功能描述：获取传感器负载参数。

参数：

socketFd：已连接的句柄。
param：PayloadParamBySensor 输出参数（payloadMass 负载质量 kg；payloadMassCenterX/Y/Z 负载质心 mm）。

返回值：

tuple：(ret, param)
0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

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

param = PayloadParamBySensor()

ret, param = tl.get_payload_param_by_sensor(socket_fd, param)