运动控制示例

概述

本示例提供一个基于 UDP 协议的 Python 运动控制脚本，演示如何通过 UDP 指令控制 X30 机器人完成完整的运动流程：站立 → 力控 → 踏步前进 → 停止 → 趴下。

本示例实现了以下基本功能，便于用户熟悉：

• X30 UDP 指令协议的简单指令与轴指令格式
• 运动状态机的状态切换流程
• 如何通过 Python 实现状态监听与安全保护机制

前置条件

1. 网络连通：开发主机与 X30 运动主机处于同一网段（默认运动主机 IP 为 192.168.1.103）
2. Python 环境：开发主机已安装 Python 3.6+
3. 安全准备：
   • 确保机器人周围 5 米内 无障碍物
   • 建议将机器人悬挂在调试架上
   • 确认已熟悉急停操作（按 E 键触发软急停）

运动状态机

X30 的运动控制遵循以下状态切换流程：

graph LR
    A[趴下] -->|起立指令| B[初始站立]
    B -->|力控指令| C[力控站立]
    C -->|开始运动| D[踏步]
    D -->|轴指令| D
    D -->|停止运动| C
    C -->|趴下指令| A

状态码	状态名称	说明
0	趴下	机器人处于趴下状态
1	正在起立	起立过渡中
2	初始站立	站立完成，尚未启用外力适应
3	力控站立	启用力控，可适应外部推力
4	踏步	进入踏步状态，可接收速度指令
5	正在趴下	趴下过渡中
6	软急停/摔倒	触发急停或检测到摔倒

UDP 指令协议

简单指令格式

简单指令为 12 字节，用于状态切换类操作：

[code(4B)] [value(4B)] [type(4B)]

• code：指令码（uint32，小端序）
• value：参数值（简单指令为 0）
• type：指令类型（简单指令为 0）

本示例使用的简单指令码：

指令码	功能	状态切换
0x21010202	起立/趴下切换	趴下 ↔ 初始站立
0x2101020A	力控模式	初始站立 → 力控站立
0x21010201	开始/停止运动	力控站立 ↔ 踏步
0x21010C0E	软急停	任意状态 → 趴下（关节保护）

轴指令格式

轴指令用于在踏步状态下控制运动速度：

[code(4B)] [axis_value(4B)] [type(4B)]

• code：轴指令码，如 0x21010130（左摇杆 Y 轴，前进/后退）
• axis_value：速度值，换算公式为 v = (y / 32767) × v_max，行走步态 v_max = 1.5 m/s
• 轴指令需以 50Hz 频率持续发送

示例：前进 0.2 m/s → y = 0.2 / 1.5 × 32767 ≈ 4369

脚本使用方法

第一步：配置运动主机网络

在运动主机的 /home/ysc/jy_exe/conf/network.toml 中，确保已添加开发主机的 IP 和监听端口：

[udp_status]
ip = '192.168.1.105'
target_port = 43897
local_port = 43893
ips = ["192.168.1.105"]      # 开发主机的 IP 地址
ports = [43897]              # 开发主机的监听端口

修改后需重启运动服务：

cd /home/ysc/jy_exe/scripts
./restart.sh

第二步：修改脚本配置

打开 robot_control.py，根据实际网络环境修改以下配置：

MOTION_HOST_IP = "192.168.1.103"     # 运动主机 IP
MOTION_HOST_PORT = 43893             # 运动主机指令端口
STATUS_LISTEN_PORT = 43897           # 本机状态监听端口

第三步：运行脚本

python robot_control.py

脚本启动后会显示安全确认提示，输入 y 确认开始执行。

执行流程

脚本自动执行以下 5 个步骤：

步骤	操作	指令码	说明
Step 1	站立	0x21010202	发送起立指令，等待状态变为初始站立
Step 2	力控模式	0x2101020A	初始站立 → 力控站立
Step 3	开始运动	0x21010201	力控站立 → 踏步，先原地踏步，再前进 0.2m/s 持续 3 秒
Step 4	停止运动	0x21010201	踏步 → 力控站立
Step 5	趴下	0x21010202	力控站立 → 趴下

每个步骤都会实时显示当前运动状态，并等待状态切换完成后再进入下一步。

安全机制

脚本内置了多重安全保护：

1. 软急停（E 键）：任何时候按 E 键，立即发送软急停指令 0x21010C0E，机器人会立即趴下并进入关节保护状态
2. 安全退出（Q 键）：按 Q 键，脚本会先停止踏步、再发送趴下指令，按顺序安全退出
3. 启动确认：脚本启动时需要手动输入 y 确认，防止误操作
4. 连接检测：启动后会检测是否能收到运动主机的状态数据，若 3 秒内无数据会提示检查网络配置
5. Ctrl+C 退出：支持标准的键盘中断退出

核心代码说明

指令构建

def build_simple_command(code):
    """构建简单指令包（12字节）"""
    return struct.pack("<III", code, 0, 0)

def send_axis_cmd(sock, code, value, duration=0, repeat_per_sec=50):
    """发送轴指令，支持持续发送"""
    packet = struct.pack("<III", code, value & 0xFFFFFFFF, 0)
    if duration <= 0:
        sock.sendto(packet, target)
    else:
        count = int(duration * repeat_per_sec)
        interval = 1.0 / repeat_per_sec
        for _ in range(count):
            sock.sendto(packet, target)
            time.sleep(interval)

状态监听

后台线程持续监听运动主机上报的状态数据（0x1009 消息），解析 basic_state 字段获取当前运动状态：

def status_monitor(sock):
    """后台状态监听线程"""
    while not _estop_triggered.is_set():
        data, addr = sock.recvfrom(1024)
        code = struct.unpack_from("<I", data, 0)[0]
        if code == 0x1009:
            basic_state = data[12]  # 首个字节为运动状态

跨平台键盘检测

脚本同时支持 Windows（msvcrt）和 Linux（termios + select）平台的非阻塞键盘输入，确保急停按键在两个平台上均可用。

完整代码

::: details 点击展开完整代码

"""
绝影X30巡检机器人运动控制脚本（UDP版）

通过UDP协议向运动主机发送指令控制机器人：站立 → 趴下

协议要点：
  - 运动主机地址：192.168.1.103:43893
  - 指令格式：12字节简单指令 (code + paramters_size + type)
  - 指令码前2位 0x21 = 手柄/遥控终端发送源

安全机制：
  - 按 E 键随时触发软急停（0x21010C0E）
  - 按 Ctrl+C 安全退出
"""

import socket
import struct
import sys
import time
import threading

# 跨平台非阻塞键盘检测
try:
    import msvcrt  # Windows
    def kbhit():
        return msvcrt.kbhit()
    def getch():
        return msvcrt.getch().decode('utf-8', errors='ignore')
except ImportError:
    import select
    import termios
    import tty
    _fd = sys.stdin.fileno()
    _old_settings = termios.tcgetattr(_fd)
    def kbhit():
        dr, _, _ = select.select([sys.stdin], [], [], 0)
        return bool(dr)
    def getch():
        tty.setraw(sys.stdin)
        ch = sys.stdin.read(1)
        termios.tcsetattr(_fd, termios.TCSADRAIN, _old_settings)
        return ch

# ======================== 配置 ========================
MOTION_HOST_IP = "192.168.1.103"
MOTION_HOST_PORT = 43893
STATUS_LISTEN_PORT = 43897

# UDP 简单指令码定义
CMD_STAND_LIE_TOGGLE = 0x21010202   # 起立/趴下
CMD_FORCE_CONTROL    = 0x2101020A   # 力控模式
CMD_START_STOP_MOVE  = 0x21010201   # 开始运动/停止运动
CMD_SOFT_ESTOP       = 0x21010C0E   # 软急停

# 轴指令码
CMD_LEFT_JOYSTICK_Y  = 0x21010130   # 左摇杆Y轴（前进/后退）

# 速度参数
AXIS_FORWARD_VALUE = 4369            # 前进 0.2m/s
AXIS_STOP_VALUE = 0                  # 停止

# 运动状态
STATE_LIE_DOWN = 0
STATE_STANDING_UP = 1
STATE_STAND_INIT = 2
STATE_STAND_FORCE = 3
STATE_STEPPING = 4
STATE_LYING_DOWN = 5
STATE_SOFT_ESTOP = 6

STATE_NAMES = {
    0: "趴下", 1: "正在起立", 2: "初始站立", 3: "力控站立",
    4: "踏步", 5: "正在趴下", 6: "软急停/摔倒", 16: "RL",
}

# ======================== 全局安全状态 ========================
_estop_triggered = threading.Event()
_latest_state = -1
_state_lock = threading.Lock()
_data_received = False


# ======================== 协议层 ========================

def build_simple_command(code):
    """构建简单指令包（12字节）"""
    return struct.pack("<III", code, 0, 0)


def send_axis_cmd(sock, code, value, duration=0, repeat_per_sec=50):
    """发送轴指令"""
    packet = struct.pack("<III", code, value & 0xFFFFFFFF, 0)
    target = (MOTION_HOST_IP, MOTION_HOST_PORT)
    if duration <= 0:
        sock.sendto(packet, target)
    else:
        count = int(duration * repeat_per_sec)
        interval = 1.0 / repeat_per_sec
        for _ in range(count):
            sock.sendto(packet, target)
            time.sleep(interval)


def send_cmd(sock, code, repeat=5):
    """发送简单指令，默认重复发5次"""
    packet = build_simple_command(code)
    for i in range(repeat):
        sock.sendto(packet, (MOTION_HOST_IP, MOTION_HOST_PORT))
        if i < repeat - 1:
            time.sleep(0.1)


def status_monitor(sock):
    """后台状态监听线程"""
    global _latest_state, _data_received
    while not _estop_triggered.is_set():
        try:
            sock.settimeout(1.0)
            data, addr = sock.recvfrom(1024)
            _data_received = True
            if len(data) >= 13:
                code = struct.unpack_from("<I", data, 0)[0]
                if code == 0x1009:
                    basic_state = data[12]
                    with _state_lock:
                        _latest_state = basic_state
        except socket.timeout:
            continue
        except Exception:
            continue


def get_current_state():
    """获取最新的运动状态"""
    with _state_lock:
        return _latest_state


def emergency_stop(sock):
    """紧急停止"""
    print("\n" + "!" * 50)
    print("  软急停已触发！(0x21010C0E)")
    print("!" * 50)
    send_cmd(sock, CMD_SOFT_ESTOP, repeat=5)
    _estop_triggered.set()


def keyboard_monitor(sock):
    """后台键盘监听线程"""
    while not _estop_triggered.is_set():
        if kbhit():
            key = getch().upper()
            if key == 'E':
                emergency_stop(sock)
                return
            elif key == 'Q':
                print("\n[退出] 用户请求安全退出...")
                send_cmd(sock, CMD_START_STOP_MOVE)
                time.sleep(0.5)
                send_cmd(sock, CMD_STAND_LIE_TOGGLE)
                _estop_triggered.set()
                return
        time.sleep(0.05)


# ======================== 主流程 ========================

def main():
    print("=" * 50)
    print("绝影X30 运动控制脚本（UDP版）")
    print("=" * 50)
    print()
    print(f"  运动主机: {MOTION_HOST_IP}:{MOTION_HOST_PORT}")
    print(f"  状态监听端口: {STATUS_LISTEN_PORT}")
    print()
    print("  安全提示：")
    print("  - 按 [E] 键 -> 软急停")
    print("  - 按 [Q] 键 -> 安全退出")
    print("  - 按 [Ctrl+C] -> 退出")
    print()

    confirm = input("确认已做好安全准备，开始执行？(y/N): ").strip().upper()
    if confirm != 'Y':
        print("已取消。")
        return

    cmd_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    status_sock = None
    try:
        status_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        status_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        status_sock.bind(('', STATUS_LISTEN_PORT))
    except OSError as e:
        print(f"[提示] 状态监听端口不可用: {e}")

    if status_sock:
        print(f"\n[监听] 状态监听已开启，端口: {STATUS_LISTEN_PORT}")

    if status_sock:
        t = threading.Thread(target=status_monitor, args=(status_sock,), daemon=True)
        t.start()

    _estop_triggered.clear()
    monitor = threading.Thread(target=keyboard_monitor, args=(cmd_sock,), daemon=True)
    monitor.start()
    print("[监控] 键盘监听已启动（按 E 急停，按 Q 安全退出）")

    # 检查是否收到状态数据
    if status_sock:
        print("\n[检测] 等待运动主机状态数据（3秒）...")
        time.sleep(3)
        if not _data_received:
            print("[警告] 未收到任何状态数据！")
            print("  请检查运动主机 network.toml 配置")
            cont = input("  是否继续执行控制？(y/N): ").strip().upper()
            if cont != 'Y':
                cmd_sock.close()
                if status_sock:
                    status_sock.close()
                return
        else:
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            print(f"[检测] 已收到状态数据，当前状态: {state_name}")

    try:
        # ==================== Step 1: 站立 ====================
        print(f"\n--- Step 1: 站立 (0x21010202) ---")
        send_cmd(cmd_sock, CMD_STAND_LIE_TOGGLE)
        print("  已发送，等待机器人起立...")

        for i in range(40):
            time.sleep(0.5)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            if state in (STATE_STAND_INIT, STATE_STAND_FORCE, STATE_STEPPING):
                print(f"  [状态] {state_name} - 起立完成")
                break
            if i % 4 == 3:
                print(f"  [等待中... {i+1}/40] 当前状态: {state_name}")

        time.sleep(2)

        # ==================== Step 2: 力控 ====================
        print(f"\n--- Step 2: 力控模式 (0x2101020A) ---")
        send_cmd(cmd_sock, CMD_FORCE_CONTROL)
        print("  已发送，等待进入力控站立...")

        for i in range(20):
            time.sleep(0.5)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            if state == STATE_STAND_FORCE:
                print(f"  [状态] {state_name} - 力控站立完成")
                break
            if i % 4 == 3:
                print(f"  [等待中... {i+1}/20] 当前状态: {state_name}")

        time.sleep(1)

        # ==================== Step 3: 开始运动 ====================
        print(f"\n--- Step 3: 开始运动 (0x21010201) ---")

        for i in range(30):
            packet = build_simple_command(CMD_START_STOP_MOVE)
            cmd_sock.sendto(packet, (MOTION_HOST_IP, MOTION_HOST_PORT))
            time.sleep(0.5)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            if state == STATE_STEPPING:
                print(f"  [状态] {state_name} - 已开始踏步（第{i+1}次发送后）")
                break
            if i % 4 == 0:
                print(f"  [发送中... {i+1}/30] 当前状态: {state_name}")

        # 原地踏步 + 前进
        STEP_DURATION = 13   # 总踏步时间（秒）
        FORWARD_TIME = 3     # 前进持续时间（秒）
        FORWARD_SPEED = 0.2  # 前进速度（m/s）

        print(f"\n  原地踏步中... ({STEP_DURATION} 秒)")

        # 先原地踏步一段时间
        for i in range(STEP_DURATION // 2):
            time.sleep(1)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            print(f"  [踏步 {i+1}/{STEP_DURATION}] 状态: {state_name}")

        # 前进（50Hz 发送轴指令）
        print(f"\n  [前进] 速度={FORWARD_SPEED}m/s，持续={FORWARD_TIME}秒")
        send_axis_cmd(cmd_sock, CMD_LEFT_JOYSTICK_Y, AXIS_FORWARD_VALUE, duration=FORWARD_TIME)
        print("  [前进] 完成")

        # 停止前进
        send_axis_cmd(cmd_sock, CMD_LEFT_JOYSTICK_Y, AXIS_STOP_VALUE)
        print("  [停止] 速度已清零")

        # 继续原地踏步
        for i in range(STEP_DURATION // 2, STEP_DURATION):
            time.sleep(1)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            print(f"  [踏步 {i+1}/{STEP_DURATION}] 状态: {state_name}")

        # ==================== Step 4: 停止运动 ====================
        print(f"\n--- Step 4: 停止运动 (0x21010201) ---")
        send_cmd(cmd_sock, CMD_START_STOP_MOVE)
        print("  已发送，等待停止踏步...")

        for i in range(20):
            time.sleep(0.5)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            if state in (STATE_STAND_INIT, STATE_STAND_FORCE):
                print(f"  [状态] {state_name} - 踏步已停止")
                break
            if i % 4 == 3:
                print(f"  [等待中... {i+1}/20] 当前状态: {state_name}")

        time.sleep(2)

        # ==================== Step 5: 趴下 ====================
        print(f"\n--- Step 5: 趴下 (0x21010202) ---")
        send_cmd(cmd_sock, CMD_STAND_LIE_TOGGLE)
        print("  已发送，等待机器人趴下...")

        for i in range(40):
            time.sleep(0.5)
            state = get_current_state()
            state_name = STATE_NAMES.get(state, f"未知({state})")
            if state == STATE_LIE_DOWN:
                print(f"  [状态] {state_name} - 趴下完成")
                break
            if i % 4 == 3:
                print(f"  [等待中... {i+1}/40] 当前状态: {state_name}")

        print("\n" + "=" * 50)
        print("全部动作执行完毕！")
        print("=" * 50)

    except KeyboardInterrupt:
        print("\n\n[中断] 用户中断")
    except Exception as e:
        print(f"\n[错误] {e}")
    finally:
        _estop_triggered.set()
        cmd_sock.close()
        if status_sock:
            status_sock.close()
        print("[完成] 已关闭所有连接")


if __name__ == "__main__":
    main()

:::