伺服状态与系统参数设置接口

头文件：include/tl_interface.h

1. clear_error - 清除系统错误/清除关节错误代码

方法：clear_error(SOCKETFD socketFd);

功能描述：复位控制器的当前报警状态（清除红色错误）。在机械臂发生碰撞、超限位等错误后，排除物理故障后调用此接口清除报警。清错后需要手动进行下电操作，释放控制器的占用状态才可以继续上电（清错后不能直接上电，先下电再上电）。

参数：

socketFd：已连接的句柄。

返回值：

0：成功
非 0：失败

使用示例：

Python

# 1. 先清除控制器错误和报警，再下电复位确保状态同步

clear_error(socket_fd)

time.sleep(0.3)

set_servo_poweroff(socket_fd)

time.sleep(0.3)

2. set_servo_state - 设置伺服就绪状态(使能)

方法：set_servo_state(SOCKETFD socketFd, int state);

功能描述：控制电机抱闸与使能。state=1=使能（解锁抱闸），state=0=去使能（抱闸锁死）。

参数：

socketFd：已连接的句柄。
state：1使能，0去使能。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

# 建立连接

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

# 设置伺服就绪（READY=使能抱闸）

ret = tl.set_servo_state(socket_fd, tl.ServoState.READY)

if ret == 0:

    print("  [信息] 伺服就绪设置成功")

else:

    print(f"[ERROR] 设置伺服就绪失败: {ret}")

3. set_servo_poweron - 设置伺服上电状态(先使能)

方法：set_servo_poweron(SOCKETFD socketFd);

功能描述：机器人上电。

参数：

socketFd：已连接的句柄。

返回值：

0：成功
非 0：失败

使用示例：

Python

def enable_servo(socket_fd):

    """使能上电 ------
先清错，再检查伺服状态，报警则报错退出，最后上电"""

    print("--- 检查伺服状态 ---")

    # 1. 先清除控制器错误和报警，再下电复位确保状态同步

    ret = clear_error(socket_fd)

    time.sleep(0.3)

    set_servo_poweroff(socket_fd)

    time.sleep(0.3)

    # 2. 检查伺服状态

    state = -1

    ret, state = get_servo_state(socket_fd, state)

    if ret != SUCCESS:

        print(f"[ERROR] 获取伺服状态失败: {ret}")

        return

    print(f"  [信息] 当前伺服状态: {state} (0=停止 1=就绪 2=报警 3=运行)")

    if state == 2:

        print("[ERROR]伺服处于报警状态，无法上电，请排查报警原因后重试")

        return

    if state == 3:

        print("[INFO] 机械臂已在使能上电状态")

        return

    # 3. 上电流程

    if state == 0:

        print("  [信息] 伺服处于停止状态，设置就绪...")

        ret = set_servo_state(socket_fd, 1)

        if ret != SUCCESS and ret != OPERATION_NOT_ALLOWED:

            print(f"[ERROR] 设置伺服就绪失败: {ret}")

            return

        time.sleep(0.5)

    print("  [信息] 上电使能...")

    ret = set_servo_poweron(socket_fd)

    if ret != SUCCESS:

        print(f"[ERROR] 上电失败: {ret}")

        return

    time.sleep(2)

    # 4. 确认结果

    _, state = get_servo_state(socket_fd, state)

    if state == 3:

        print("[INFO] 机械臂使能上电成功（servo_state = " + str(state) + "）")

    else:

        print("[ERROR] 机械臂使能上电失败（servo_state = " + str(state) + "）")

4. set_servo_poweroff - 设置伺服下电状态

方法：set_servo_poweroff(SOCKETFD socketFd);

功能描述：机器人下电。

参数：

socketFd：已连接的句柄。

返回值：

0：成功
非 0：失败

使用示例：

Python

def disable_servo(socket_fd):

    """使能伺服下电"""

    state = -1

    _, state = get_servo_state(socket_fd, state)

    if state == 1:

        print("[INFO] 机械臂已使能下电")

    elif state == 3:

        set_servo_poweroff(socket_fd)

        _, state = get_servo_state(socket_fd, state)

        print("[INFO] 机械臂使能下电成功（servo_state = " + str(state) + "）")

        return

    print("[INFO] 机械臂使能下电失败（servo_state = " + str(state) + "）")

5. set_speed - 设置速度

方法：set_speed(SOCKETFD socketFd, int speed);

功能描述：设置当前模式的速度。

参数：

socketFd：已连接的句柄。
speed :速度大小0-100。

返回值：

0：成功
非 0：失败

使用示例：

Python

test_speeds = [50, 80, 30]

for spd in test_speeds:

    print("\n[步骤 5] 设置全局速度: " + str(spd) + "%")

    ret = set_speed(sock, spd)

    print("set_speed", ret)

    time.sleep(0.2)

    current_speed = -1

    ret, current_speed = get_speed(sock, current_speed)

    if ret == SUCCESS:

        print("  [信息] 读取确认: 当前速度 = " + str(current_speed) + "%")

6. restore_default_param_DH - 恢复机械臂默认DH参数

方法：restore_default_param_DH(SOCKETFD socketFd, int robotNum);

功能描述：恢复机械臂默认DH参数。

参数：

socketFd：已连接的句柄。
robotNum：机器人编号(1-4)。

返回值：

0：成功
非 0：失败
DHparam DH：参数

使用示例：

Python

# 该功能不建议测试，请在厂家指导下使用

def demo_restore_default_param_DH(sock):

    """恢复机械臂默认DH参数"""

    print("恢复默认DH参数")

    robot_num = 1  # TODO: 根据实际机器人编号调整（1-4）

    ret = restore_default_param_DH(sock, robot_num)

    print("restore_default_param_DH", ret)

    print()

7. set_cartesian_params - 设置机器人笛卡尔参数

方法：set_cartesian_params(SOCKETFD socketFd, const CartesianParam& param);

功能描述：配置笛卡尔空间运动动力学约束参数，限定末端执行器最大直线速度、直线加速度、旋转角速度与角加速度，实现轨迹平滑度与运行安全管控。

参数：

socketFd：已连接的句柄。
param 包含笛卡尔空间运动参数的结构体。

返回值：

0：成功。
非 0：失败

使用示例：

Python

 # 以下功能不建议测试，请在厂家指导下使用

def demo_set_cartesian_params(sock):

    """设置笛卡尔参数（最大线速度/线加速度/角速度）"""

    print("设置笛卡尔参数")

    print("  [信息] 以下数值需根据真实机械臂手册调整")

    param = CartesianParam()

    param.maxSpeed = 500.0                # TODO: 最大线速度(mm/s)

    param.maxAcc = 1000.0                 # TODO: 最大线加速度(mm/s²)

    param.maxDec = 1000.0                 # TODO: 最大线减速度(mm/s²)

    param.maxAttitudeVel = 100.0          # TODO: 最大姿态角速度(°/s)

    param.speedLimitMode = 0              # TODO: 速度限制方式（0=位姿，1=位置）

    ret = set_cartesian_params(sock, param)

    print("set_cartesian_params", ret)

    print()

8. restore_network_factory_settings - 恢复网络出厂设置

方法：restore_network_factory_settings(SOCKETFD socketFd);

功能描述：执行控制器网络参数恢复出厂操作，重置通讯 IP 地址为默认固定地址，属于高危配置操作。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非 0：失败

使用示例：

Python

def demo_restore_network_factory_settings(sock):

    """恢复网络出厂设置（危险操作，会重置所有网络参数并可能断连）"""

    print("恢复网络出厂设置")

    print("  [警告] 恢复网络出厂设置会重置控制器所有网络参数！")

    print("  [警告] 执行后需要重新配置网络才能连接！")

    ret = restore_network_factory_settings(sock)

    print("restore_network_factory_settings", ret)

    print()

9. set_robot_joint_param - 设置当前机器人关节参数

方法：set_robot_joint_param(SOCKETFD socketFd, int id, RobotJointParam& param);

功能描述：对指定单关节进行运动与硬限位参数配置，设定各关节最大运行速度、加速度及正负向机械位置软限位，约束关节运动边界保障本体运行安全。

参数：

socketFd：已连接的句柄。
id：关节参数1,61,6。
param：结构体参数。

返回值：

0：成功。
非 0：失败

使用示例：

Python

 # 以下功能不建议测试，请在厂家指导下使用

def demo_set_robot_joint_param(sock):

    """设置关节参数（最大速度/加速度/位置限位）"""

    print("设置关节参数")

    print("  [信息] 遍历设置关节1~6的参数，以下数值需根据真实机械臂手册调整")

    for axis_id in range(1, 7):

        param = RobotJointParam()

        # --- 运动学参数（不可随意修改，需按实际减速比/编码器设置）---

        param.reduceRatio = 100.0          # TODO: 减速比

        param.encoderResolution = 17       # TODO: 编码器分辨率

        param.motorDirection = 1           # TODO: 电机方向（1=正向，-1=反向）

        # --- 位置限位（单位：度）---

        param.maxPos = 180.0               # TODO: 轴正限位(°)

        param.minPos = -180.0              # TODO: 轴反限位(°)

        # --- 速度参数（单位：度/秒）---

        param.ratedRotSpeed = 100.0        # TODO: 电机额定转速

        param.maxRotSpeed = 150.0          # TODO: 电机最大转速

        # --- 加减速度 ---

        param.maxAcc = 100.0               # TODO: 最大加速度

        param.maxDec = 100.0               # TODO: 最大减速度

        ret = set_robot_joint_param(sock, axis_id, param)

        print("set_robot_joint_param (关节%d)" % axis_id, ret)

    print()

10. set_controller_ip - 配置控制器有线网口IP

方法：set_controller_ip(SOCKETFD socketFd, std::string name, std::string address, std::string gateway, std::string dns);

功能描述：配置控制器有线网口 IP。

参数：

socketFd：已连接的句柄。
name：网卡设备名称。
address：ip地址。
gateway：网关地址。
dns：DNS服务器地址。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

# 建立连接

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

# 危险操作：修改控制器 IP 会立即生效并导致 TCP 断连，请确认新 IP 与电脑在同一网段

# TODO: 根据实际网络环境修改以下参数

name = "eth0"            # 网口名称

address = "192.168.1.14"  # 目标 IP 地址

gateway = "192.168.1.1"   # 网关地址

dns = "8.8.8.8"           # DNS 服务器

ret = tl.set_controller_ip(socket_fd, name, address, gateway, dns)

if ret == 0:

    print("set_controller_ip 成功")

else:

    print(f"set_controller_ip 失败，错误码：{ret}")

11. set_robot_dh_param - 设置机器人 DH 参数

方法：set_robot_dh_param(socketFd, param);

功能描述：设置当前机器人 DH 参数。

参数：

socketFd：已连接的句柄。
param：RobotDHParam 结构体（alpha[6] 1-6轴DH参数alpha 单位 deg；a[6] 1-6轴DH参数a 单位 mm；theta[6] 1-6轴DH参数theta 单位 deg；d[6] 1-6轴DH参数d 单位 mm；eulerAngle 欧拉角模式；mountingAngle 安装角度单位 deg）。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

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

param = RobotDHParam()

param.alpha = [0, 0, 0, 0, 0, 0]

ret = tl.set_robot_dh_param(socket_fd, param)

12. set_zero_pos_deviation - 设置零位偏差

方法：set_zero_pos_deviation(socketFd, axis, shift);

功能描述：设置零点偏移。

参数：

socketFd：已连接的句柄。
axis：轴号（1-7）。
shift：偏移量（-360 < shift < 360）。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

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

ret = tl.set_zero_pos_deviation(socket_fd, 1, 0.5)

13. set_axis_zero_position - 标定当前状态下的零位/指定关节当前位置为零位

方法：set_axis_zero_position(SOCKETFD socketFd, int axis);

功能描述：将当前关节位置记录为零位。axis=0 表示所有轴，axis=1~6 表示单轴。

**警告：**仅在机械臂物理对齐零刻度线后调用，否则将导致坐标系错乱，引发撞机风险！

参数：

socketFd：已连接的句柄。
axis：轴号，1~7 代表第 1~7 轴，0 表示全部轴。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

import time

# 建立连接

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

# 设置所有轴原点

axis = 0 # 0表示全部关节

ret = tl.set_axis_zero_position(socket_fd, axis)

print(f"设置轴原点返回值(ret) = {ret}")

time.sleep(1)

14. set_receive_error_or_warnning_message_callback - 设置错误警告回调

方法：set_receive_error_or_warnning_message_callback(SOCKETFD socketFd,void(*function) (int messageType, const char* message, int messageCode));

功能描述：注册异步错误/警告回调函数。当控制器产生错误或警告时，会异步调用该回调。

参数：

socketFd：已连接的句柄。
function：指向回调函数的指针。该回调函数在接收到错误或警告信息时被调用。

返回值：

0：成功
非 0：失败

使用示例：

Python

import tl_interface as tl

# 建立连接

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

# 定义错误/警告回调（在独立线程触发，不能做耗时操作）

def error_warning_callback(message_type, message, code):

    print(f"[回调] type={message_type} message={message} code={code}")

# 注册回调

ret = tl.set_receive_error_or_warnning_message_callback(socket_fd, error_warning_callback)

if ret == 0:

    print(" [成功] 回调注册成功，后续收到错误/警告会自动触发")

else:

    print(f" [失败] 回调注册失败，错误码：{ret}")