First Commit

Deploy_Tienkung/udp_loopback/README.md

@@ -0,0 +1,66 @@
+# UDP Loopback
+
+独立于原 `rl_control_node.py` 的 localhost UDP 键盘测试目录。
+
+包含：
+
+- `udp_keyboard_sender.py`：从终端读取按键，编码 UDP 报文并发送
+- `udp_loopback_node.py`：接收 UDP 报文，解码事件并计算目标值
+- `protocol.py`：自定义协议和状态结构
+- `config/udp_loopback.yaml`：本地测试配置
+
+运行方式：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_loopback_node.py
+```
+
+另开一个终端：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_keyboard_sender.py
+```
+
+接回现有 FSM 的方式：
+
+1. 把 [dex_config.yaml](/home/meiqi/tienkung/Deploy_Tienkung/config/dex_config.yaml) 里的 `control_tool` 改成 `udp_loopback`
+2. 启动原控制链，例如：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 rl_control_node_sim.py
+```
+
+3. 再启动发送端：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_keyboard_sender.py
+```
+
+此时 UDP 接收结果会在接收侧被映射回现有 FSM 命令：
+
+- `pose_home -> gotoZERO`
+- `pose_hold -> gotoSTOP`
+- `mode_stride -> gotoWALKAMP`
+- `mode_dash -> gotoMYPOLICY`
+- `surge/sway/spin -> x/y/yaw speed command`
+
+当前事件码不是原工程里的 `gotoZERO` / `gotoSTOP` / `x_speed_command` 这一套，而是：
+
+- `pose_home`
+- `pose_hold`
+- `mode_stride`
+- `mode_dash`
+- `surge_up`
+- `surge_down`
+- `sway_left`
+- `sway_right`
+- `spin_left`
+- `spin_right`
+- `lift_up`
+- `lift_down`
+- `trim_reset`
+- `session_quit`

Deploy_Tienkung/udp_loopback/__init__.py

@@ -0,0 +1 @@
+"""Standalone localhost UDP keyboard loopback package."""

Deploy_Tienkung/udp_loopback/config/udp_loopback.yaml

@@ -0,0 +1,22 @@
+dt: 0.01
+
+sender:
+  target_host: 127.0.0.1
+  target_port: 31000
+  source_tag: local_keys
+
+receiver:
+  listen_host: 127.0.0.1
+  listen_port: 31000
+
+motion:
+  initial_lift: 0.89
+  min_lift: 0.65
+  max_lift: 0.90
+  lift_step: 0.05
+  surge_step: 0.10
+  sway_step: 0.10
+  spin_step: 0.10
+  max_surge: 1.00
+  max_sway: 0.50
+  max_spin: 0.50

Deploy_Tienkung/udp_loopback/protocol.py

@@ -0,0 +1,61 @@
+"""Protocol and state definitions for the standalone UDP loopback."""
+
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass, field
+import json
+import time
+from typing import Any, Dict
+
+
+@dataclass
+class InputEnvelope:
+    """Small UDP payload carrying one encoded keyboard event."""
+
+    seq_id: int
+    event_code: str
+    key_name: str
+    sent_at: float = field(default_factory=time.time)
+    drive_value: float = 1.0
+    source_tag: str = "local_keys"
+
+    def encode(self) -> bytes:
+        return json.dumps(asdict(self), separators=(",", ":")).encode("utf-8")
+
+    @classmethod
+    def decode(cls, payload: bytes) -> "InputEnvelope":
+        data = json.loads(payload.decode("utf-8"))
+        return cls(
+            seq_id=int(data["seq_id"]),
+            event_code=str(data["event_code"]),
+            key_name=str(data["key_name"]),
+            sent_at=float(data.get("sent_at", time.time())),
+            drive_value=float(data.get("drive_value", 1.0)),
+            source_tag=str(data.get("source_tag", "unknown")),
+        )
+
+
+@dataclass
+class MotionFrame:
+    """Receiver-side motion targets computed from decoded events."""
+
+    mode_tag: str = "pose_home"
+    relay_on: bool = True
+    surge_goal: float = 0.0
+    sway_goal: float = 0.0
+    spin_goal: float = 0.0
+    lift_goal: float = 0.89
+    last_event_code: str = "boot"
+    last_rx_time: float = 0.0
+
+    def as_dict(self) -> Dict[str, Any]:
+        return asdict(self)
+
+
+def format_motion_frame(frame: MotionFrame) -> str:
+    return (
+        f"mode={frame.mode_tag} relay_on={frame.relay_on} "
+        f"surge={frame.surge_goal:.2f} sway={frame.sway_goal:.2f} "
+        f"spin={frame.spin_goal:.2f} lift={frame.lift_goal:.2f} "
+        f"event={frame.last_event_code}"
+    )

Deploy_Tienkung/udp_loopback/udp_fsm_controller.py

@@ -0,0 +1,219 @@
+"""Adapter that maps UDP loopback events onto the existing FSM control flag."""
+
+from __future__ import annotations
+
+import queue
+import socket
+import threading
+import time
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+from common.joystick import ControlFlag
+
+try:
+    from .protocol import InputEnvelope, MotionFrame, format_motion_frame
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope, MotionFrame, format_motion_frame
+
+
+class UDPFSMFlag(ControlFlag):
+    """FSM-facing flag produced from decoded UDP events."""
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.x_speed_command: float = 0.0
+        self.y_speed_command: float = 0.0
+        self.yaw_speed_command: float = 0.0
+        self.height_cmd: float = 0.89
+
+
+class UDPFSMController:
+    """Receive UDP packets and expose them as a ControlFlag-like interface."""
+
+    def __init__(self) -> None:
+        self.config: Dict[str, object] = {}
+        self.data_mutex = threading.Lock()
+
+        self._load_config()
+        self._init_data_structures()
+
+        self.recv_running = False
+        self.recv_thread: Optional[threading.Thread] = None
+        self.rx_socket: Optional[socket.socket] = None
+
+    def _load_config(self) -> None:
+        config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with config_path.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        receiver_cfg = self.config.get("receiver", {})
+        motion_cfg = self.config.get("motion", {})
+
+        self.listen_host = receiver_cfg.get("listen_host", "127.0.0.1")
+        self.listen_port = int(receiver_cfg.get("listen_port", 31000))
+
+        self.initial_lift = float(motion_cfg.get("initial_lift", 0.89))
+        self.lift_step = float(motion_cfg.get("lift_step", 0.05))
+        self.max_surge = float(motion_cfg.get("max_surge", 1.0))
+        self.max_sway = float(motion_cfg.get("max_sway", 0.5))
+        self.max_spin = float(motion_cfg.get("max_spin", 0.5))
+        self.max_lift = float(motion_cfg.get("max_lift", 0.90))
+        self.min_lift = float(motion_cfg.get("min_lift", 0.65))
+        self.surge_step = float(motion_cfg.get("surge_step", 0.1))
+        self.sway_step = float(motion_cfg.get("sway_step", 0.1))
+        self.spin_step = float(motion_cfg.get("spin_step", 0.1))
+
+    def _init_data_structures(self) -> None:
+        self.packet_queue: queue.Queue[InputEnvelope] = queue.Queue(maxsize=128)
+        self.motion_frame = MotionFrame(lift_goal=self.initial_lift)
+        self.udp_flag = UDPFSMFlag()
+        self.udp_flag.height_cmd = self.initial_lift
+        self.last_seq_id = -1
+
+    def start(self) -> None:
+        self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.rx_socket.bind((self.listen_host, self.listen_port))
+        self.rx_socket.settimeout(0.2)
+
+        self.recv_running = True
+        self.recv_thread = threading.Thread(target=self._recv_loop, daemon=True)
+        self.recv_thread.start()
+        print(
+            f"UDP FSM controller listening on {self.listen_host}:{self.listen_port}"
+        )
+
+    def stop(self) -> None:
+        self.recv_running = False
+        if self.recv_thread and self.recv_thread.is_alive():
+            self.recv_thread.join(timeout=1.0)
+        if self.rx_socket is not None:
+            self.rx_socket.close()
+            self.rx_socket = None
+        print("UDP FSM controller stopped")
+
+    def _recv_loop(self) -> None:
+        while self.recv_running:
+            try:
+                payload, _ = self.rx_socket.recvfrom(4096)
+            except socket.timeout:
+                continue
+            except OSError:
+                break
+
+            try:
+                packet = InputEnvelope.decode(payload)
+            except (ValueError, KeyError, TypeError, UnicodeDecodeError) as exc:
+                print(f"[udp_fsm] drop invalid payload: {exc}")
+                continue
+
+            try:
+                self.packet_queue.put_nowait(packet)
+            except queue.Full:
+                try:
+                    self.packet_queue.get_nowait()
+                    self.packet_queue.put_nowait(packet)
+                except queue.Empty:
+                    pass
+
+    def update_flag(self) -> None:
+        while not self.packet_queue.empty():
+            try:
+                packet = self.packet_queue.get_nowait()
+            except queue.Empty:
+                break
+            self._apply_packet(packet)
+
+    def get_udp_flag(self) -> UDPFSMFlag:
+        with self.data_mutex:
+            flag_copy = UDPFSMFlag()
+            flag_copy.__dict__.update(self.udp_flag.__dict__)
+            return flag_copy
+
+    def init(self) -> int:
+        print("UDP FSM controller initialized")
+        return 0
+
+    def _apply_packet(self, packet: InputEnvelope) -> None:
+        with self.data_mutex:
+            self.last_seq_id = packet.seq_id
+            self.motion_frame.last_event_code = packet.event_code
+            self.motion_frame.last_rx_time = packet.sent_at
+
+            event_code = packet.event_code
+            if event_code == "pose_home":
+                self.motion_frame.mode_tag = "pose_home"
+            elif event_code == "pose_hold":
+                self.motion_frame.mode_tag = "pose_hold"
+            elif event_code == "mode_stride":
+                self.motion_frame.mode_tag = "mode_stride"
+            elif event_code == "mode_dash":
+                self.motion_frame.mode_tag = "mode_dash"
+            elif event_code == "mode_xrun":
+                self.motion_frame.mode_tag = "mode_xrun"
+            elif event_code == "surge_up":
+                self.motion_frame.surge_goal = min(
+                    self.max_surge, self.motion_frame.surge_goal + self.surge_step
+                )
+            elif event_code == "surge_down":
+                self.motion_frame.surge_goal = max(
+                    -self.max_surge, self.motion_frame.surge_goal - self.surge_step
+                )
+            elif event_code == "sway_left":
+                self.motion_frame.sway_goal = max(
+                    -self.max_sway, self.motion_frame.sway_goal - self.sway_step
+                )
+            elif event_code == "sway_right":
+                self.motion_frame.sway_goal = min(
+                    self.max_sway, self.motion_frame.sway_goal + self.sway_step
+                )
+            elif event_code == "spin_left":
+                self.motion_frame.spin_goal = max(
+                    -self.max_spin, self.motion_frame.spin_goal - self.spin_step
+                )
+            elif event_code == "spin_right":
+                self.motion_frame.spin_goal = min(
+                    self.max_spin, self.motion_frame.spin_goal + self.spin_step
+                )
+            elif event_code == "lift_up":
+                self.motion_frame.lift_goal = min(
+                    self.max_lift, self.motion_frame.lift_goal + self.lift_step
+                )
+            elif event_code == "lift_down":
+                self.motion_frame.lift_goal = max(
+                    self.min_lift, self.motion_frame.lift_goal - self.lift_step
+                )
+            elif event_code == "trim_reset":
+                self.motion_frame.surge_goal = 0.0
+                self.motion_frame.sway_goal = 0.0
+                self.motion_frame.spin_goal = 0.0
+            elif event_code == "session_quit":
+                self.motion_frame.relay_on = False
+                self.motion_frame.mode_tag = "pose_hold"
+                self.recv_running = False
+
+            self._sync_motion_frame_to_flag()
+            print(
+                f"[udp_fsm] seq={packet.seq_id} key={packet.key_name} "
+                f"{format_motion_frame(self.motion_frame)} "
+                f"fsm={self.udp_flag.fsm_state_command}"
+            )
+
+    def _sync_motion_frame_to_flag(self) -> None:
+        mode_to_fsm_command = {
+            "pose_home": "gotoZERO",
+            "pose_hold": "gotoSTOP",
+            "mode_stride": "gotoWALKAMP",
+            "mode_dash": "gotoMYPOLICY",
+            "mode_xrun": "gotoXSIMRUN",
+        }
+        self.udp_flag.enable = self.motion_frame.relay_on
+        self.udp_flag.fsm_state_command = mode_to_fsm_command.get(
+            self.motion_frame.mode_tag, self.udp_flag.fsm_state_command
+        )
+        self.udp_flag.x_speed_command = self.motion_frame.surge_goal
+        self.udp_flag.y_speed_command = self.motion_frame.sway_goal
+        self.udp_flag.yaw_speed_command = self.motion_frame.spin_goal
+        self.udp_flag.height_cmd = self.motion_frame.lift_goal

Deploy_Tienkung/udp_loopback/udp_keyboard_sender.py

@@ -0,0 +1,200 @@
+"""Keyboard sender that encodes key events and emits them over localhost UDP."""
+
+from __future__ import annotations
+
+import os
+import select
+import signal
+import socket
+import sys
+import termios
+import threading
+import tty
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+try:
+    from .protocol import InputEnvelope
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope
+
+
+class UDPKeyboardSender:
+    """Standalone keyboard sender for localhost UDP testing."""
+
+    def __init__(self) -> None:
+        self.config: Dict[str, object] = {}
+        self.seq_id = 0
+        self.running = False
+        self.socket: Optional[socket.socket] = None
+        self.input_thread: Optional[threading.Thread] = None
+        self.original_terminal_settings = None
+
+        self._load_config()
+        self._init_socket()
+        self._print_help()
+
+    def _load_config(self) -> None:
+        config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with config_path.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        sender_cfg = self.config.get("sender", {})
+        self.target_host = sender_cfg.get("target_host", "127.0.0.1")
+        self.target_port = int(sender_cfg.get("target_port", 31000))
+        self.source_tag = sender_cfg.get("source_tag", "local_keys")
+
+    def _init_socket(self) -> None:
+        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+    def _print_help(self) -> None:
+        print("UDP keyboard sender ready")
+        print(f"Target: {self.target_host}:{self.target_port}")
+        print("Keys:")
+        print("  z -> pose_home")
+        print("  c -> pose_hold")
+        print("  m -> mode_stride")
+        print("  p -> mode_dash")
+        print("  n -> mode_xrun")
+        print("  w/s -> surge +/-")
+        print("  a/d -> sway +/-")
+        print("  q/e -> spin +/-")
+        print("  Left/Right -> lift +/-")
+        print("  Up/Down -> surge +/-")
+        print("  r -> trim_reset")
+        print("  x -> session_quit")
+
+    def start(self) -> None:
+        self.running = True
+        self.input_thread = threading.Thread(target=self._input_loop, daemon=True)
+        self.input_thread.start()
+        print("UDP keyboard sender thread started")
+
+    def stop(self) -> None:
+        self.running = False
+        if self.input_thread and self.input_thread.is_alive():
+            self.input_thread.join(timeout=1.0)
+        if self.original_terminal_settings is not None:
+            try:
+                termios.tcsetattr(
+                    sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
+                )
+            except termios.error:
+                pass
+            self.original_terminal_settings = None
+        if self.socket is not None:
+            self.socket.close()
+            self.socket = None
+        print("UDP keyboard sender stopped")
+
+    def _input_loop(self) -> None:
+        self.original_terminal_settings = termios.tcgetattr(sys.stdin)
+        try:
+            tty.setraw(sys.stdin.fileno())
+            while self.running:
+                if select.select([sys.stdin], [], [], 0.1)[0]:
+                    key = sys.stdin.read(1)
+                    self._process_key(key)
+        except KeyboardInterrupt:
+            self._handle_ctrl_c()
+        finally:
+            if self.original_terminal_settings is not None:
+                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.original_terminal_settings)
+                self.original_terminal_settings = None
+
+    def _process_key(self, key: str) -> None:
+        event_map = {
+            "w": ("surge_up", "w"),
+            "s": ("surge_down", "s"),
+            "a": ("sway_left", "a"),
+            "d": ("sway_right", "d"),
+            "q": ("spin_left", "q"),
+            "e": ("spin_right", "e"),
+            "z": ("pose_home", "z"),
+            "c": ("pose_hold", "c"),
+            "m": ("mode_stride", "m"),
+            "p": ("mode_dash", "p"),
+            "n": ("mode_xrun", "n"),
+            "r": ("trim_reset", "r"),
+            "x": ("session_quit", "x"),
+        }
+
+        if key == "\x03":
+            self._handle_ctrl_c()
+            return
+
+        if key == "\x1b":
+            self._handle_arrow_key()
+            return
+
+        if key in event_map:
+            event_code, key_name = event_map[key]
+            self._send_event(event_code, key_name)
+
+    def _handle_arrow_key(self) -> None:
+        if not select.select([sys.stdin], [], [], 0.1)[0]:
+            return
+        key2 = sys.stdin.read(1)
+        if key2 != "[":
+            return
+        if not select.select([sys.stdin], [], [], 0.1)[0]:
+            return
+        key3 = sys.stdin.read(1)
+        arrow_map = {
+            "A": ("surge_up", "arrow_up"),
+            "B": ("surge_down", "arrow_down"),
+            "C": ("lift_down", "arrow_right"),
+            "D": ("lift_up", "arrow_left"),
+        }
+        if key3 in arrow_map:
+            event_code, key_name = arrow_map[key3]
+            self._send_event(event_code, key_name)
+
+    def _send_event(self, event_code: str, key_name: str) -> None:
+        if self.socket is None:
+            return
+        envelope = InputEnvelope(
+            seq_id=self.seq_id,
+            event_code=event_code,
+            key_name=key_name,
+            source_tag=self.source_tag,
+        )
+        self.seq_id += 1
+        self.socket.sendto(envelope.encode(), (self.target_host, self.target_port))
+        print(
+            f"sent seq={envelope.seq_id} event={envelope.event_code} "
+            f"key={envelope.key_name}"
+        )
+        if event_code == "session_quit":
+            self.running = False
+
+    def _handle_ctrl_c(self) -> None:
+        self.running = False
+        if self.original_terminal_settings is not None:
+            try:
+                termios.tcsetattr(
+                    sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
+                )
+            except termios.error:
+                pass
+            self.original_terminal_settings = None
+        os.kill(os.getpid(), signal.SIGINT)
+
+
+def main() -> None:
+    sender = UDPKeyboardSender()
+    sender.start()
+    try:
+        while sender.running:
+            if sender.input_thread is not None:
+                sender.input_thread.join(timeout=0.2)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        sender.stop()
+
+
+if __name__ == "__main__":
+    main()

Deploy_Tienkung/udp_loopback/udp_loopback_node.py

@@ -0,0 +1,227 @@
+"""Receiver-side localhost UDP loopback node with a control loop layout."""
+
+from __future__ import annotations
+
+import queue
+import socket
+import threading
+import time
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+try:
+    from .protocol import InputEnvelope, MotionFrame, format_motion_frame
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope, MotionFrame, format_motion_frame
+
+
+class UDPLoopbackNode:
+    """Standalone UDP receiver that mirrors rl_control_node.py structure."""
+
+    def __init__(self, debug: bool = False) -> None:
+        self.debug = debug
+        self.config: Dict[str, object] = {}
+
+        self._load_config()
+        self._init_data_structures()
+        self._init_udp_interfaces()
+        self._init_control_system()
+        self._start_threads()
+
+    def _load_config(self) -> None:
+        self.config_file = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with self.config_file.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        self.dt = float(self.config.get("dt", 0.01))
+        self.receiver_cfg = self.config.get("receiver", {})
+        self.motion_cfg = self.config.get("motion", {})
+
+        self.listen_host = self.receiver_cfg.get("listen_host", "127.0.0.1")
+        self.listen_port = int(self.receiver_cfg.get("listen_port", 31000))
+
+        self.initial_lift = float(self.motion_cfg.get("initial_lift", 0.89))
+        self.lift_step = float(self.motion_cfg.get("lift_step", 0.05))
+        self.max_surge = float(self.motion_cfg.get("max_surge", 1.0))
+        self.max_sway = float(self.motion_cfg.get("max_sway", 0.5))
+        self.max_spin = float(self.motion_cfg.get("max_spin", 0.5))
+        self.max_lift = float(self.motion_cfg.get("max_lift", 0.90))
+        self.min_lift = float(self.motion_cfg.get("min_lift", 0.65))
+        self.surge_step = float(self.motion_cfg.get("surge_step", 0.1))
+        self.sway_step = float(self.motion_cfg.get("sway_step", 0.1))
+        self.spin_step = float(self.motion_cfg.get("spin_step", 0.1))
+
+    def _init_data_structures(self) -> None:
+        self.packet_queue: queue.Queue[InputEnvelope] = queue.Queue(maxsize=128)
+        self.motion_frame = MotionFrame(lift_goal=self.initial_lift)
+        self.frame_lock = threading.Lock()
+        self.last_seq_id = -1
+
+    def _init_udp_interfaces(self) -> None:
+        self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.rx_socket.bind((self.listen_host, self.listen_port))
+        self.rx_socket.settimeout(0.2)
+
+    def _init_control_system(self) -> None:
+        self.control_running = False
+        self.recv_running = False
+        self.control_thread: Optional[threading.Thread] = None
+        self.recv_thread: Optional[threading.Thread] = None
+
+    def _start_threads(self) -> None:
+        self.recv_running = True
+        self.control_running = True
+
+        self.recv_thread = threading.Thread(target=self._udp_recv_loop, daemon=True)
+        self.recv_thread.start()
+
+        self.control_thread = threading.Thread(target=self._control_loop, daemon=True)
+        self.control_thread.start()
+
+        print(
+            f"UDP loopback node listening on {self.listen_host}:{self.listen_port} "
+            f"with dt={self.dt:.3f}s"
+        )
+
+    def _udp_recv_loop(self) -> None:
+        while self.recv_running:
+            try:
+                payload, _ = self.rx_socket.recvfrom(4096)
+            except socket.timeout:
+                continue
+            except OSError:
+                break
+
+            try:
+                packet = InputEnvelope.decode(payload)
+            except (ValueError, KeyError, TypeError, UnicodeDecodeError, yaml.YAMLError) as exc:
+                print(f"[udp_loopback] drop invalid payload: {exc}")
+                continue
+
+            try:
+                self.packet_queue.put_nowait(packet)
+            except queue.Full:
+                try:
+                    self.packet_queue.get_nowait()
+                    self.packet_queue.put_nowait(packet)
+                except queue.Empty:
+                    pass
+
+    def _control_loop(self) -> None:
+        print("UDP loopback control loop started")
+        while self.control_running:
+            loop_start = time.perf_counter()
+            self._process_udp_packets()
+            self._precise_sleep_until(loop_start + self.dt)
+        print("UDP loopback control loop ended")
+
+    def _process_udp_packets(self) -> None:
+        while not self.packet_queue.empty():
+            try:
+                packet = self.packet_queue.get_nowait()
+            except queue.Empty:
+                break
+            self._apply_packet(packet)
+
+    def _apply_packet(self, packet: InputEnvelope) -> None:
+        with self.frame_lock:
+            self.last_seq_id = packet.seq_id
+            self.motion_frame.last_event_code = packet.event_code
+            self.motion_frame.last_rx_time = packet.sent_at
+
+            event_code = packet.event_code
+            if event_code == "pose_home":
+                self.motion_frame.mode_tag = "pose_home"
+            elif event_code == "pose_hold":
+                self.motion_frame.mode_tag = "pose_hold"
+            elif event_code == "mode_stride":
+                self.motion_frame.mode_tag = "mode_stride"
+            elif event_code == "mode_dash":
+                self.motion_frame.mode_tag = "mode_dash"
+            elif event_code == "mode_xrun":
+                self.motion_frame.mode_tag = "mode_xrun"
+            elif event_code == "surge_up":
+                self.motion_frame.surge_goal = min(
+                    self.max_surge, self.motion_frame.surge_goal + self.surge_step
+                )
+            elif event_code == "surge_down":
+                self.motion_frame.surge_goal = max(
+                    -self.max_surge, self.motion_frame.surge_goal - self.surge_step
+                )
+            elif event_code == "sway_left":
+                self.motion_frame.sway_goal = max(
+                    -self.max_sway, self.motion_frame.sway_goal - self.sway_step
+                )
+            elif event_code == "sway_right":
+                self.motion_frame.sway_goal = min(
+                    self.max_sway, self.motion_frame.sway_goal + self.sway_step
+                )
+            elif event_code == "spin_left":
+                self.motion_frame.spin_goal = max(
+                    -self.max_spin, self.motion_frame.spin_goal - self.spin_step
+                )
+            elif event_code == "spin_right":
+                self.motion_frame.spin_goal = min(
+                    self.max_spin, self.motion_frame.spin_goal + self.spin_step
+                )
+            elif event_code == "lift_up":
+                self.motion_frame.lift_goal = min(
+                    self.max_lift, self.motion_frame.lift_goal + self.lift_step
+                )
+            elif event_code == "lift_down":
+                self.motion_frame.lift_goal = max(
+                    self.min_lift, self.motion_frame.lift_goal - self.lift_step
+                )
+            elif event_code == "trim_reset":
+                self.motion_frame.surge_goal = 0.0
+                self.motion_frame.sway_goal = 0.0
+                self.motion_frame.spin_goal = 0.0
+            elif event_code == "session_quit":
+                self.motion_frame.relay_on = False
+                self.motion_frame.mode_tag = "session_quit"
+                self.control_running = False
+                self.recv_running = False
+
+            print(
+                f"[udp_loopback] seq={packet.seq_id} key={packet.key_name} "
+                f"{format_motion_frame(self.motion_frame)}"
+            )
+
+    def _precise_sleep_until(self, target_time: float) -> None:
+        current_time = time.perf_counter()
+        sleep_time = target_time - current_time
+        if sleep_time <= 0:
+            return
+        if sleep_time > 0.003:
+            time.sleep(sleep_time * 0.9)
+            while time.perf_counter() < target_time:
+                pass
+        else:
+            while time.perf_counter() < target_time:
+                pass
+
+    def destroy(self) -> None:
+        self.control_running = False
+        self.recv_running = False
+        if self.control_thread and self.control_thread.is_alive():
+            self.control_thread.join(timeout=1.0)
+        if self.recv_thread and self.recv_thread.is_alive():
+            self.recv_thread.join(timeout=1.0)
+        self.rx_socket.close()
+
+
+def main() -> None:
+    node = UDPLoopbackNode(debug=False)
+    try:
+        while node.control_running:
+            time.sleep(0.2)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy()
+
+
+if __name__ == "__main__":
+    main()