点动使用示例

# /**

#  * ex_teach_control.py

#  * @brief 示教控制相关接口

#  * @attention

#  *   - 运行模式: 示教模式

#  *   - 开始前需: connect_robot → set_servo_state(1) → set_servo_poweron

#  *   - 退出前需: set_servo_poweroff → disconnect_robot

#  *   - 确保机械臂供电正常、网络通信正常

#  * @note 运行步骤

#  *       运行: python3 ex_teach_control.py

#  */

import sys

import os

import platform

# 基于脚本所在目录构建 SDK Python 绑定的绝对路径

_script_dir = os.path.dirname(os.path.abspath(__file__))

# 自动检测平台，添加对应 SDK Python 绑定路径

if platform.system() == "Windows":

    sys.path.append(os.path.join(_script_dir, "../../lib/windows/python"))

elif platform.machine() in ("aarch64", "arm64"):

    sys.path.append(os.path.join(_script_dir, "../../lib/linux/arm64/python"))

else:

    sys.path.append(os.path.join(_script_dir, "../../lib/linux/x86/python"))

# Windows: 将 tl_host.dll 所在目录加入 DLL 搜索路径

if platform.system() == "Windows":

    _dll_dir = os.path.join(_script_dir, "../../lib/windows")

    os.add_dll_directory(_dll_dir)

from tl_interface import *

import time

# ----工具函数----

def print_result(msg, ret):

    """打印操作结果"""

    if ret == SUCCESS:

        print("  [成功] " + msg)

    else:

        print("  [失败] " + msg + "，错误码: " + str(ret))

def print_separator(title):

    """打印分隔标题"""

    print("\n========== " + title + " ==========")

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) + "）")

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

    else:

        pass

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

def demo_set_current_teach_sensitivity(socket_fd):

    """设置电流环拖动示教灵敏度"""

    print("[INFO] **********设置电流环拖动示教灵敏度**********")

    friction_offsets = VectorDouble()

    friction_offsets.append(1)   # J1

    friction_offsets.append(1)   # J2

    friction_offsets.append(1)   # J3

    friction_offsets.append(1)   # J4

    friction_offsets.append(1)   # J5

    friction_offsets.append(1)   # J6

    ret = set_current_teach_sensitivity(socket_fd, friction_offsets)

    if ret != SUCCESS:

        print("[ERROR] 设置电流环拖动示教灵敏度失败，错误码: " + str(ret))

        return

    print("[INFO] 设置电流环拖动示教灵敏度成功")

    print()

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

# 主程序

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

if __name__ == "__main__":

    # 定义IP地址和端口

    ip = "192.168.1.13"

    port = "6001"

    # 获取 SDK 版本

    version = get_library_version()

    print("[INFO] SDK 版本: " + version)

    # 连接控制器

    print("[INFO] 连接控制器...")

    socket_fd = connect_robot(ip, port)

    if socket_fd < 0:

        print("[ERROR] 连接失败，程序退出!")

        sys.exit(-1)

    print("[INFO] 连接成功 (socket=" + str(socket_fd) + ")")

    # 使能上电

    enable_servo(socket_fd)

    # ----切换为示教模式----

    # 示教点动需要在示教模式下执行（0=示教）

    print("--- 切换示教模式 ---")

    ret = set_current_mode(socket_fd, 0)

    if ret != SUCCESS:

        print("[ERROR] 切换示教模式失败，程序退出!")

        sys.exit(-1)

    print("[INFO] 已切换为示教模式")

    # ----示教点动（注意安全）----

    # ⚠️  robot_start_jogging 会直接驱动机器人运动，运行前确保安全区域无人

    print("[INFO] **********示教点动**********")

    axis = 1

    direction = True

    ret = robot_start_jogging(socket_fd, axis, direction)

    if ret != SUCCESS:

        print("[ERROR] 示教点动失败，程序退出!")

        sys.exit(-1)

    print("[INFO] 示教点动成功")

    time.sleep(5)

    print("[INFO] **********停止点动**********")

    axis = 1

    ret = robot_stop_jogging(arm.socket_fd, axis)

    if ret != SUCCESS:

        print("[ERROR] 停止点动失败，程序退出!")

        sys.exit(-1)

    print("[INFO] 停止点动成功")

    # ----设置电流环拖动示教灵敏度----

    demo_set_current_teach_sensitivity(socket_fd)

    # 使能下电

    disable_servo(socket_fd)

    # 断开控制器连接

    print("[INFO] 断开控制器连接...")

    ret = disconnect_robot(socket_fd)

    if ret != SUCCESS:

        print("[ERROR] 断开连接失败，程序退出!")

        sys.exit(-1)

    print("[INFO] 断开连接成功")