Motion Control Example

Overview

This example provides a Python motion control script based on the UDP protocol, demonstrating how to control the X30 robot through UDP commands to complete a full motion sequence: Stand Up → Force Control → Stepping & Walking → Stop → Lie Down.

This example implements the following basic functions to help users get familiar with:

• Simple command and axis command formats of the X30 UDP protocol
• Motion state machine transition flow
• How to implement status monitoring and safety protection mechanisms in Python

Prerequisites

1. Network connectivity: The development host and the X30 motion host are on the same network segment (default motion host IP: 192.168.1.103)
2. Python environment: Python 3.6+ installed on the development host
3. Safety preparation:
   • Ensure no obstacles within 5 meters of the robot
   • It is recommended to suspend the robot on a debugging frame
   • Confirm familiarity with emergency stop operation (press E to trigger soft e-stop)

Motion State Machine

The X30 motion control follows this state transition flow:

graph LR
    A[Lie Down] -->|Stand Command| B[Initial Stand]
    B -->|Force Control| C[Force Stand]
    C -->|Start Move| D[Stepping]
    D -->|Axis Cmd| D
    D -->|Stop Move| C
    C -->|Lie Down| A

State Code	State Name	Description
0	Lie Down	Robot is in lying position
1	Standing Up	Standing transition in progress
2	Initial Stand	Standing complete, external force adaptation not yet enabled
3	Force Stand	Force control enabled, can adapt to external pushes
4	Stepping	Stepping state, can receive velocity commands
5	Lying Down	Lying transition in progress
6	Soft E-Stop/Fall	E-stop triggered or fall detected

UDP Command Protocol

Simple Command Format

Simple commands are 12 bytes, used for state transition operations:

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

• code: Command code (uint32, little-endian)
• value: Parameter value (0 for simple commands)
• type: Command type (0 for simple commands)

Simple command codes used in this example:

Command Code	Function	State Transition
0x21010202	Stand/Lie Toggle	Lie Down ↔ Initial Stand
0x2101020A	Force Control	Initial Stand → Force Stand
0x21010201	Start/Stop Movement	Force Stand ↔ Stepping
0x21010C0E	Soft E-Stop	Any State → Lie Down (joint protection)

Axis Command Format

Axis commands are used to control movement velocity in the stepping state:

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

• code: Axis command code, e.g., 0x21010130 (left joystick Y-axis, forward/backward)
• axis_value: Velocity value, conversion formula: v = (y / 32767) × v_max, walking gait v_max = 1.5 m/s
• Axis commands must be sent continuously at 50Hz

Example: Forward 0.2 m/s → y = 0.2 / 1.5 × 32767 ≈ 4369

Script Usage

Step 1: Configure Motion Host Network

In the motion host’s /home/ysc/jy_exe/conf/network.toml, ensure the development host’s IP and listening port are added:

[udp_status]
ip = '192.168.1.105'
target_port = 43897
local_port = 43893
ips = ["192.168.1.105"]      # Development host IP address
ports = [43897]              # Development host listening port

Restart the motion service after modification:

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

Step 2: Modify Script Configuration

Open robot_control.py and modify the following configuration according to your network environment:

MOTION_HOST_IP = "192.168.1.103"     # Motion host IP
MOTION_HOST_PORT = 43893             # Motion host command port
STATUS_LISTEN_PORT = 43897           # Local status listening port

Step 3: Run the Script

python robot_control.py

After startup, the script will display a safety confirmation prompt. Enter y to confirm and start execution.

Execution Flow

The script automatically executes the following 5 steps:

Step	Operation	Command Code	Description
Step 1	Stand Up	0x21010202	Send stand command, wait for Initial Stand state
Step 2	Force Control	0x2101020A	Initial Stand → Force Stand
Step 3	Start Movement	0x21010201	Force Stand → Stepping, step in place then forward at 0.2m/s for 3 seconds
Step 4	Stop Movement	0x21010201	Stepping → Force Stand
Step 5	Lie Down	0x21010202	Force Stand → Lie Down

Each step displays the current motion state in real-time and waits for the state transition to complete before proceeding to the next step.

Safety Mechanisms

The script includes multiple safety protections:

1. Soft E-Stop (E key): Press E at any time to immediately send the soft e-stop command 0x21010C0E. The robot will immediately lie down and enter joint protection state
2. Safe Exit (Q key): Press Q to stop stepping first, then send the lie down command for a safe sequential exit
3. Startup Confirmation: The script requires manual y input to start, preventing accidental operation
4. Connection Detection: After startup, the script checks whether status data from the motion host can be received. If no data is received within 3 seconds, it prompts to check network configuration
5. Ctrl+C Exit: Supports standard keyboard interrupt exit

Core Code Description

Command Building

def build_simple_command(code):
    """Build simple command packet (12 bytes)"""
    return struct.pack("<III", code, 0, 0)

def send_axis_cmd(sock, code, value, duration=0, repeat_per_sec=50):
    """Send axis command, supports continuous sending"""
    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)

Status Monitoring

A background thread continuously monitors status data reported by the motion host (0x1009 message), parsing the basic_state field to obtain the current motion state:

def status_monitor(sock):
    """Background status monitoring thread"""
    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]  # First byte is motion state

Cross-Platform Keyboard Detection

The script supports both Windows (msvcrt) and Linux (termios + select) platforms for non-blocking keyboard input, ensuring the emergency stop key is available on both platforms.

Full Code

::: details Click to expand full code

"""
绝影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()

:::