From e61e228150681aa3788c20042f990113b9f102bf Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sat, 6 Sep 2025 06:34:58 -0700
Subject: [PATCH 1/9] Working UDP joystick streaming client to B1 C++ server

---
 .../unitree_b1/joystick_client_udp.py         | 338 ++++++++++++++++
 .../unitree_b1/joystick_server_udp.cpp        | 366 ++++++++++++++++++
 2 files changed, 704 insertions(+)
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py b/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py
new file mode 100644
index 0000000000..6eabd9a669
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py
@@ -0,0 +1,338 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+UDP Joystick Control Client for Unitree B1 Robot
+Sends joystick commands via UDP datagrams - no fragmentation issues!
+"""
+
+import socket
+import struct
+import sys
+import time
+import termios
+import tty
+import select
+import signal
+
+
+class NetworkJoystickCmd:
+    """Structure matching the C++ NetworkJoystickCmd"""
+
+    # Format: 4 floats (lx, ly, rx, ry), 1 uint16 (buttons), 1 uint8 (mode)
+    # Little-endian byte order to match x86/ARM
+    FORMAT = "<ffffHB"  # 19 bytes total
+    SIZE = struct.calcsize(FORMAT)
+
+    def __init__(self):
+        self.lx = 0.0  # left stick x (-1 to 1)
+        self.ly = 0.0  # left stick y (-1 to 1)
+        self.rx = 0.0  # right stick x (-1 to 1)
+        self.ry = 0.0  # right stick y (-1 to 1)
+        self.buttons = 0  # button states (uint16)
+        self.mode = 0  # control mode (uint8)
+
+    def pack(self):
+        """Pack the structure into bytes matching C++ memory layout"""
+        return struct.pack(self.FORMAT, self.lx, self.ly, self.rx, self.ry, self.buttons, self.mode)
+
+    def reset_sticks(self):
+        """Reset stick values (they don't hold position)"""
+        self.lx = 0.0
+        self.ly = 0.0
+        self.rx = 0.0
+        self.ry = 0.0
+
+
+class JoystickClient:
+    """UDP client for sending joystick commands to the robot server"""
+
+    def __init__(self, server_ip, server_port, test_mode=False, verbose=False):
+        self.server_ip = server_ip
+        self.server_port = server_port
+        self.socket = None
+        self.old_settings = None
+        self.running = False
+        self.active_keys = set()  # Track all currently active keys
+        self.test_mode = test_mode
+        self.verbose = verbose  # Show packet details when running real
+        self.packet_count = 0
+
+    def connect(self):
+        """Create UDP socket (no actual connection needed)"""
+        try:
+            # Create UDP socket
+            self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+            # Set socket to non-blocking for potential future enhancements
+            # self.socket.setblocking(0)
+
+            if self.test_mode:
+                print(f"TEST MODE: Created UDP socket")
+                print(f"Will send to: {self.server_ip}:{self.server_port}")
+                print(f"Packet size: {NetworkJoystickCmd.SIZE} bytes")
+            else:
+                print(f"UDP client ready")
+                print(f"Target server: {self.server_ip}:{self.server_port}")
+                print(f"Packet size: {NetworkJoystickCmd.SIZE} bytes")
+                print(f"Protocol: UDP (fire-and-forget)")
+
+            return True
+        except Exception as e:
+            print(f"Failed to create UDP socket: {e}")
+            if self.socket:
+                self.socket.close()
+                self.socket = None
+            return False
+
+    def disconnect(self):
+        """Close the UDP socket"""
+        if self.socket:
+            self.socket.close()
+            self.socket = None
+
+    def send_command(self, cmd):
+        """Send a joystick command via UDP datagram"""
+        if not self.socket and not self.test_mode:
+            return False
+
+        try:
+            data = cmd.pack()
+            self.packet_count += 1
+
+            # Verify packet size
+            if len(data) != 19:
+                print(f"ERROR: Packet size is {len(data)}, expected 19")
+                return False
+
+            if self.test_mode:
+                # Test mode - clean output
+                if cmd.lx != 0 or cmd.ly != 0 or cmd.rx != 0 or cmd.ry != 0:
+                    # Movement active
+                    sys.stdout.write(
+                        f"\r[Moving] Mode: {cmd.mode} | LX: {cmd.lx:+.1f} LY: {cmd.ly:+.1f} | RX: {cmd.rx:+.1f} RY: {cmd.ry:+.1f}     "
+                    )
+                    sys.stdout.flush()
+                elif self.packet_count % 50 == 0:
+                    # Idle status update
+                    sys.stdout.write(
+                        f"\r[Idle] Mode: {cmd.mode} | Packets: {self.packet_count}     "
+                    )
+                    sys.stdout.flush()
+                return True
+
+            # Send UDP datagram (fire and forget)
+            bytes_sent = self.socket.sendto(data, (self.server_ip, self.server_port))
+
+            # Show real-time status when verbose mode is enabled (same format as test mode)
+            if self.verbose:
+                if cmd.lx != 0 or cmd.ly != 0 or cmd.rx != 0 or cmd.ry != 0:
+                    # Movement active
+                    sys.stdout.write(
+                        f"\r[Moving] Mode: {cmd.mode} | LX: {cmd.lx:+.1f} LY: {cmd.ly:+.1f} | RX: {cmd.rx:+.1f} RY: {cmd.ry:+.1f}     "
+                    )
+                    sys.stdout.flush()
+                elif self.packet_count % 50 == 0:
+                    # Idle status update
+                    sys.stdout.write(
+                        f"\r[Idle] Mode: {cmd.mode} | Packets: {self.packet_count}     "
+                    )
+                    sys.stdout.flush()
+            elif self.packet_count % 50 == 0:
+                # Non-verbose: just show packet count every second
+                print(f"Sent {self.packet_count} packets ({self.packet_count * 0.02:.1f}s)")
+
+            return bytes_sent == NetworkJoystickCmd.SIZE
+
+        except Exception as e:
+            print(f"\nFailed to send UDP packet: {e}")
+            return False
+
+    def set_nonblocking_input(self):
+        """Set terminal to non-blocking input mode"""
+        self.old_settings = termios.tcgetattr(sys.stdin)
+        tty.setraw(sys.stdin.fileno())
+
+    def restore_input(self):
+        """Restore terminal settings"""
+        if self.old_settings:
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
+
+    def get_key(self, timeout=0):
+        """Get a single keypress without blocking"""
+        if select.select([sys.stdin], [], [], timeout)[0]:
+            key = sys.stdin.read(1)
+            # Check for Ctrl+C (ASCII code 3)
+            if ord(key) == 3:
+                print("\nCtrl+C pressed - exiting...")
+                self.running = False
+                return None
+            return key
+        return None
+
+    def run_keyboard_control(self):
+        """Main control loop with keyboard input"""
+        self.set_nonblocking_input()
+        self.running = True
+
+        cmd = NetworkJoystickCmd()
+
+        print("\n" + "=" * 50)
+        print("UDP Keyboard Control (No TCP fragmentation!)")
+        print("=" * 50)
+        print("Modes: 0=Idle, 1=Stand, 2=Walk, 6=Recovery")
+        print("WASD = Movement/Turning (left stick)")
+        print("IJKL = Strafing/Pitch (right stick)")
+        print("Space = Emergency Stop")
+        print("Q = Quit")
+        print("\nHOLD keys for continuous movement")
+        print("Multiple keys work together (e.g., W+A = forward + turn)")
+        print(f"\nCurrent mode: {cmd.mode}")
+
+        try:
+            while self.running:
+                # Check for any key presses
+                key = self.get_key()
+                if not self.running:  # Ctrl+C was pressed in get_key()
+                    break
+                key_lower = key.lower() if key else None
+
+                if key:
+                    # Mode selection (immediate, not held)
+                    if key == "0":
+                        cmd.mode = 0
+                        print(f"\rMode: Idle     ", end="", flush=True)
+                    elif key == "1":
+                        cmd.mode = 1
+                        print(f"\rMode: Stand    ", end="", flush=True)
+                    elif key == "2":
+                        cmd.mode = 2
+                        print(f"\rMode: Walk     ", end="", flush=True)
+                    elif key == "6":
+                        cmd.mode = 6
+                        print(f"\rMode: Recovery ", end="", flush=True)
+                    elif key_lower == "q":
+                        print("\nQuitting...")
+                        self.running = False
+                        break  # Break immediately
+                    elif key == " ":  # Space bar = emergency stop
+                        cmd.mode = 0
+                        print(f"\rEMERGENCY STOP ", end="", flush=True)
+
+                # Reset stick values EVERY frame
+                cmd.reset_sticks()
+
+                # Only apply movement for CURRENT key press (no toggle, no persistence)
+                if key_lower == "w":
+                    cmd.ly = 1.0  # Forward
+                if key_lower == "s":
+                    cmd.ly = -1.0  # Backward
+                if key_lower == "a":
+                    cmd.lx = -1.0  # Turn/yaw left
+                if key_lower == "d":
+                    cmd.lx = 1.0  # Turn/yaw right
+                if key_lower == "i":
+                    cmd.ry = 1.0  # Pitch up
+                if key_lower == "k":
+                    cmd.ry = -1.0  # Pitch down
+                if key_lower == "j":
+                    cmd.rx = -1.0  # Roll/strafe left
+                if key_lower == "l":
+                    cmd.rx = 1.0  # Roll/strafe right
+
+                # Send UDP packet at 50Hz
+                if not self.send_command(cmd):
+                    if not self.test_mode:
+                        print("\nFailed to send UDP packet")
+                        break
+
+                # Maintain 50Hz update rate (20ms between packets)
+                time.sleep(0.020)
+
+        except KeyboardInterrupt:
+            print("\nCtrl+C received, shutting down...")
+        except Exception as e:
+            print(f"\nError: {e}")
+        finally:
+            # Send final idle command for safety
+            print("\nSending idle command...")
+            final_cmd = NetworkJoystickCmd()
+            final_cmd.mode = 0
+            for _ in range(5):  # Send multiple times to ensure receipt
+                self.send_command(final_cmd)
+                time.sleep(0.02)
+
+            self.restore_input()
+            print("Terminal restored")
+            print(f"Total packets sent: {self.packet_count}")
+
+    def signal_handler(self, signum, frame):
+        """Handle Ctrl+C gracefully"""
+        self.running = False
+
+
+def main():
+    # Check for test mode
+    test_mode = "--test" in sys.argv
+    if test_mode:
+        sys.argv.remove("--test")
+        # Use dummy values if not provided
+        if len(sys.argv) < 3:
+            sys.argv.extend(["127.0.0.1", "9090"])
+
+    # Check for verbose mode
+    verbose = "--verbose" in sys.argv or "-v" in sys.argv
+    if "--verbose" in sys.argv:
+        sys.argv.remove("--verbose")
+    if "-v" in sys.argv:
+        sys.argv.remove("-v")
+
+    if len(sys.argv) < 3:
+        print(f"Usage: {sys.argv[0]} [--test] [--verbose|-v] <server_ip> <server_port>")
+        print(f"Example: {sys.argv[0]} 192.168.123.220 9090")
+        print(f"Verbose: {sys.argv[0]} 192.168.123.220 9090 --verbose")
+        print(f"Test mode: {sys.argv[0]} --test")
+        return 1
+
+    server_ip = sys.argv[1]
+    server_port = int(sys.argv[2])
+
+    print(f"\n{'=' * 50}")
+    print(f"UDP Joystick Client for Unitree B1")
+    if verbose:
+        print(f"VERBOSE MODE: Showing all packet details")
+    print(f"{'=' * 50}")
+
+    client = JoystickClient(server_ip, server_port, test_mode=test_mode, verbose=verbose)
+
+    # Set up signal handler for clean shutdown
+    signal.signal(signal.SIGINT, client.signal_handler)
+    signal.signal(signal.SIGTERM, client.signal_handler)
+
+    if not client.connect():
+        return 1
+
+    try:
+        client.run_keyboard_control()
+    finally:
+        client.disconnect()
+        if not test_mode:
+            print("UDP socket closed")
+
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp b/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp
new file mode 100644
index 0000000000..423182f5a1
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp
@@ -0,0 +1,366 @@
+/*****************************************************************
+ UDP Joystick Control Server for Unitree B1 Robot
+ With timeout protection and guaranteed packet boundaries
+******************************************************************/
+
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
+#include <math.h>
+#include <iostream>
+#include <unistd.h>
+#include <string.h>
+#include <thread>
+#include <mutex>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <signal.h>
+#include <chrono>
+#include <errno.h>
+
+using namespace UNITREE_LEGGED_SDK;
+
+// Joystick command structure received over network
+struct NetworkJoystickCmd {
+    float lx;  // left stick x (-1 to 1)
+    float ly;  // left stick y (-1 to 1)
+    float rx;  // right stick x (-1 to 1)
+    float ry;  // right stick y (-1 to 1)
+    uint16_t buttons;  // button states
+    uint8_t mode;  // control mode
+};
+
+class JoystickServer {
+public:
+    JoystickServer(uint8_t level, int server_port) : 
+        safe(LeggedType::B1),
+        udp(level, 8090, "192.168.123.220", 8082),
+        server_port_(server_port),
+        running_(false) {
+        udp.InitCmdData(cmd);
+        memset(&joystick_cmd_, 0, sizeof(joystick_cmd_));
+        joystick_cmd_.mode = 0;  // Start in idle mode
+        last_packet_time_ = std::chrono::steady_clock::now();
+    }
+
+    void Start();
+    void Stop();
+
+private:
+    void UDPRecv();
+    void UDPSend();
+    void RobotControl();
+    void NetworkServerThread();
+    void ParseJoystickCommand(const NetworkJoystickCmd& net_cmd);
+    void CheckTimeout();
+
+    Safety safe;
+    UDP udp;
+    HighCmd cmd = {0};
+    HighState state = {0};
+    
+    NetworkJoystickCmd joystick_cmd_;
+    std::mutex cmd_mutex_;
+    
+    int server_port_;
+    int server_socket_;
+    bool running_;
+    std::thread server_thread_;
+    
+    // Client tracking for debug
+    struct sockaddr_in last_client_addr_;
+    bool has_client_ = false;
+    
+    // SAFETY: Timeout tracking
+    std::chrono::steady_clock::time_point last_packet_time_;
+    const int PACKET_TIMEOUT_MS = 100;  // Stop if no packet for 100ms
+    
+    float dt = 0.002;
+    
+    // Control parameters
+    const float MAX_FORWARD_SPEED = 0.3f;   // m/s
+    const float MAX_SIDE_SPEED = 0.3f;     // m/s  
+    const float MAX_YAW_SPEED = 0.3f;      // rad/s
+    const float MAX_BODY_HEIGHT = 0.1f;     // m
+    const float MAX_EULER_ANGLE = 0.3f;     // rad
+    const float DEADZONE = 0.0f;            // joystick deadzone
+};
+
+void JoystickServer::Start() {
+    running_ = true;
+    
+    // Start network server thread
+    server_thread_ = std::thread(&JoystickServer::NetworkServerThread, this);
+    
+    // Initialize environment
+    InitEnvironment();
+    
+    // Start control loops
+    LoopFunc loop_control("control_loop", dt, boost::bind(&JoystickServer::RobotControl, this));
+    LoopFunc loop_udpSend("udp_send", dt, 3, boost::bind(&JoystickServer::UDPSend, this));
+    LoopFunc loop_udpRecv("udp_recv", dt, 3, boost::bind(&JoystickServer::UDPRecv, this));
+
+    loop_udpSend.start();
+    loop_udpRecv.start();
+    loop_control.start();
+
+    std::cout << "UDP Joystick server started on port " << server_port_ << std::endl;
+    std::cout << "Timeout protection: " << PACKET_TIMEOUT_MS << "ms" << std::endl;
+    std::cout << "Expected packet size: 19 bytes" << std::endl;
+    std::cout << "Robot control loops started" << std::endl;
+    
+    // Keep running
+    while (running_) {
+        sleep(1);
+    }
+}
+
+void JoystickServer::Stop() {
+    running_ = false;
+    close(server_socket_);
+    if (server_thread_.joinable()) {
+        server_thread_.join();
+    }
+}
+
+void JoystickServer::NetworkServerThread() {
+    // Create UDP socket
+    server_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
+    if (server_socket_ < 0) {
+        std::cerr << "Failed to create UDP socket" << std::endl;
+        return;
+    }
+    
+    // Allow socket reuse
+    int opt = 1;
+    setsockopt(server_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+    
+    // Bind socket
+    struct sockaddr_in server_addr;
+    server_addr.sin_family = AF_INET;
+    server_addr.sin_addr.s_addr = INADDR_ANY;
+    server_addr.sin_port = htons(server_port_);
+    
+    if (bind(server_socket_, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+        std::cerr << "Failed to bind UDP socket to port " << server_port_ << std::endl;
+        close(server_socket_);
+        return;
+    }
+    
+    std::cout << "UDP server listening on port " << server_port_ << std::endl;
+    std::cout << "Waiting for joystick packets..." << std::endl;
+    
+    NetworkJoystickCmd net_cmd;
+    struct sockaddr_in client_addr;
+    socklen_t client_len;
+    
+    while (running_) {
+        client_len = sizeof(client_addr);
+        
+        // Receive UDP datagram (blocks until packet arrives)
+        ssize_t bytes = recvfrom(server_socket_, &net_cmd, sizeof(net_cmd), 
+                                  0, (struct sockaddr*)&client_addr, &client_len);
+        
+        if (bytes == 19) {
+            // Perfect packet size from Python client
+            if (!has_client_) {
+                std::cout << "Client connected from " << inet_ntoa(client_addr.sin_addr) 
+                          << ":" << ntohs(client_addr.sin_port) << std::endl;
+                has_client_ = true;
+                last_client_addr_ = client_addr;
+            }
+            ParseJoystickCommand(net_cmd);
+        } else if (bytes == sizeof(NetworkJoystickCmd)) {
+            // C++ client with padding (20 bytes)
+            if (!has_client_) {
+                std::cout << "C++ Client connected from " << inet_ntoa(client_addr.sin_addr) 
+                          << ":" << ntohs(client_addr.sin_port) << std::endl;
+                has_client_ = true;
+                last_client_addr_ = client_addr;
+            }
+            ParseJoystickCommand(net_cmd);
+        } else if (bytes > 0) {
+            // Wrong packet size - ignore but log
+            static int error_count = 0;
+            if (error_count++ < 5) {  // Only log first 5 errors
+                std::cerr << "Ignored packet with wrong size: " << bytes 
+                          << " bytes (expected 19)" << std::endl;
+            }
+        }
+        // Note: recvfrom returns -1 on error, which we ignore
+    }
+}
+
+void JoystickServer::ParseJoystickCommand(const NetworkJoystickCmd& net_cmd) {
+    std::lock_guard<std::mutex> lock(cmd_mutex_);
+    joystick_cmd_ = net_cmd;
+    
+    // SAFETY: Update timestamp for timeout tracking
+    last_packet_time_ = std::chrono::steady_clock::now();
+    
+    // Apply deadzone to analog sticks
+    if (fabs(joystick_cmd_.lx) < DEADZONE) joystick_cmd_.lx = 0;
+    if (fabs(joystick_cmd_.ly) < DEADZONE) joystick_cmd_.ly = 0;
+    if (fabs(joystick_cmd_.rx) < DEADZONE) joystick_cmd_.rx = 0;
+    if (fabs(joystick_cmd_.ry) < DEADZONE) joystick_cmd_.ry = 0;
+}
+
+void JoystickServer::CheckTimeout() {
+    auto now = std::chrono::steady_clock::now();
+    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+        now - last_packet_time_).count();
+    
+    static bool timeout_printed = false;
+    
+    if (elapsed > PACKET_TIMEOUT_MS) {
+        joystick_cmd_.lx = 0;
+        joystick_cmd_.ly = 0;
+        joystick_cmd_.rx = 0;
+        joystick_cmd_.ry = 0;
+        joystick_cmd_.buttons = 0;
+        
+        if (!timeout_printed) {
+            std::cout << "SAFETY: Packet timeout - stopping movement!" << std::endl;
+            timeout_printed = true;
+        }
+    } else {
+        // Reset flag when packets resume
+        if (timeout_printed) {
+            std::cout << "Packets resumed - control restored" << std::endl;
+            timeout_printed = false;
+        }
+    }
+}
+
+void JoystickServer::UDPRecv() {
+    udp.Recv();
+}
+
+void JoystickServer::UDPSend() {
+    udp.Send();
+}
+
+void JoystickServer::RobotControl() {
+    udp.GetRecv(state);
+    
+    // SAFETY: Check for packet timeout
+    NetworkJoystickCmd current_cmd;
+    {
+        std::lock_guard<std::mutex> lock(cmd_mutex_);
+        CheckTimeout();  // This may zero movement if timeout
+        current_cmd = joystick_cmd_;
+    }
+    
+    cmd.mode = 0;
+    cmd.gaitType = 0;
+    cmd.speedLevel = 0;
+    cmd.footRaiseHeight = 0;
+    cmd.bodyHeight = 0;
+    cmd.euler[0] = 0;
+    cmd.euler[1] = 0;
+    cmd.euler[2] = 0;
+    cmd.velocity[0] = 0.0f;
+    cmd.velocity[1] = 0.0f;
+    cmd.yawSpeed = 0.0f;
+    cmd.reserve = 0;
+    
+    // Set mode from joystick
+    cmd.mode = current_cmd.mode;
+    
+    // Map joystick to robot control based on mode
+    switch (current_cmd.mode) {
+        case 0:  // Idle
+            // Robot stops
+            break;
+            
+        case 1:  // Force stand with body control
+            // Left stick controls body height and yaw
+            cmd.bodyHeight = current_cmd.ly * MAX_BODY_HEIGHT;
+            cmd.euler[2] = current_cmd.lx * MAX_EULER_ANGLE;
+            
+            // Right stick controls pitch and roll
+            cmd.euler[1] = current_cmd.ry * MAX_EULER_ANGLE;
+            cmd.euler[0] = current_cmd.rx * MAX_EULER_ANGLE;
+            break;
+            
+        case 2:  // Walk mode
+            cmd.velocity[0] = current_cmd.ly * MAX_FORWARD_SPEED;
+            cmd.yawSpeed = -current_cmd.lx * MAX_YAW_SPEED;
+            cmd.velocity[1] = -current_cmd.rx * MAX_SIDE_SPEED;
+            
+            // Check button states for gait type
+            if (current_cmd.buttons & 0x0001) {  // Button A
+                cmd.gaitType = 0;  // Trot
+            } else if (current_cmd.buttons & 0x0002) {  // Button B  
+                cmd.gaitType = 1;  // Trot running
+            } else if (current_cmd.buttons & 0x0004) {  // Button X
+                cmd.gaitType = 2;  // Climb mode
+            } else if (current_cmd.buttons & 0x0008) {  // Button Y
+                cmd.gaitType = 3;  // Trot obstacle
+            }
+            break;
+            
+        case 5:  // Damping mode
+        case 6:  // Recovery stand up
+            break;
+            
+        default:
+            cmd.mode = 0;  // Default to idle for safety
+            break;
+    }
+    
+    // Debug output
+    static int counter = 0;
+    if (counter++ % 500 == 0) {  // Print every second
+        auto now = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+            now - last_packet_time_).count();
+        
+        std::cout << "Mode: " << (int)cmd.mode 
+                  << " Vel: [" << cmd.velocity[0] << ", " << cmd.velocity[1] << "]"
+                  << " Yaw: " << cmd.yawSpeed
+                  << " Last packet: " << elapsed << "ms ago"
+                  << " IMU: " << state.imu.rpy[2] << std::endl;
+    }
+    
+    udp.SetSend(cmd);
+}
+
+// Signal handler for clean shutdown
+JoystickServer* g_server = nullptr;
+
+void signal_handler(int sig) {
+    if (g_server) {
+        std::cout << "\nShutting down server..." << std::endl;
+        g_server->Stop();
+    }
+    exit(0);
+}
+
+int main(int argc, char* argv[]) {
+    int port = 9090;  // Default port
+    
+    if (argc > 1) {
+        port = atoi(argv[1]);
+    }
+    
+    std::cout << "UDP Unitree B1 Joystick Control Server" << std::endl;
+    std::cout << "Communication level: HIGH-level" << std::endl;
+    std::cout << "Protocol: UDP (datagram)" << std::endl;
+    std::cout << "Server port: " << port << std::endl;
+    std::cout << "Packet size: 19 bytes (Python) or 20 bytes (C++)" << std::endl;
+    std::cout << "Update rate: 50Hz expected" << std::endl;
+    std::cout << "WARNING: Make sure the robot is standing on the ground." << std::endl;
+    std::cout << "Press Enter to continue..." << std::endl;
+    std::cin.ignore();
+    
+    JoystickServer server(HIGHLEVEL, port);
+    g_server = &server;
+    
+    // Set up signal handler
+    signal(SIGINT, signal_handler);
+    signal(SIGTERM, signal_handler);
+    
+    server.Start();
+    
+    return 0;
+}
\ No newline at end of file

From e68fc4692ca65b3f8938b1df50811da30ca047ac Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sat, 6 Sep 2025 17:02:41 -0700
Subject: [PATCH 2/9] added additional safety handling to c++ UDP cmd_vel
 server

---
 .../unitree_b1/joystick_server_udp.cpp               | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp b/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp
index 423182f5a1..56e2b29412 100644
--- a/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp
+++ b/dimos/robot/unitree_webrtc/unitree_b1/joystick_server_udp.cpp
@@ -77,9 +77,9 @@ class JoystickServer {
     float dt = 0.002;
     
     // Control parameters
-    const float MAX_FORWARD_SPEED = 0.3f;   // m/s
-    const float MAX_SIDE_SPEED = 0.3f;     // m/s  
-    const float MAX_YAW_SPEED = 0.3f;      // rad/s
+    const float MAX_FORWARD_SPEED = 0.2f;   // m/s
+    const float MAX_SIDE_SPEED = 0.2f;     // m/s  
+    const float MAX_YAW_SPEED = 0.2f;      // rad/s
     const float MAX_BODY_HEIGHT = 0.1f;     // m
     const float MAX_EULER_ANGLE = 0.3f;     // rad
     const float DEADZONE = 0.0f;            // joystick deadzone
@@ -283,9 +283,9 @@ void JoystickServer::RobotControl() {
             break;
             
         case 2:  // Walk mode
-            cmd.velocity[0] = current_cmd.ly * MAX_FORWARD_SPEED;
-            cmd.yawSpeed = -current_cmd.lx * MAX_YAW_SPEED;
-            cmd.velocity[1] = -current_cmd.rx * MAX_SIDE_SPEED;
+            cmd.velocity[0] = std::clamp(current_cmd.ly * MAX_FORWARD_SPEED, -MAX_FORWARD_SPEED, MAX_FORWARD_SPEED);
+            cmd.yawSpeed = std::clamp(-current_cmd.lx * MAX_YAW_SPEED, -MAX_YAW_SPEED, MAX_YAW_SPEED);
+            cmd.velocity[1] = std::clamp(-current_cmd.rx * MAX_SIDE_SPEED, -MAX_SIDE_SPEED, MAX_SIDE_SPEED);
             
             // Check button states for gait type
             if (current_cmd.buttons & 0x0001) {  // Button A

From 5be2a55727f247803848d32cdf4935d46905c7ec Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sat, 6 Sep 2025 23:04:26 -0700
Subject: [PATCH 3/9] Unitree B1 integrated with Twist cmd_vel stream LCM input
 and UDP connection client

---
 dimos/msgs/std_msgs/Int32.py                  |  31 +++
 dimos/msgs/std_msgs/__init__.py               |   3 +-
 .../unitree_webrtc/unitree_b1/__init__.py     |   8 +
 .../unitree_webrtc/unitree_b1/b1_command.py   |  82 ++++++
 .../unitree_webrtc/unitree_b1/connection.py   | 242 ++++++++++++++++
 .../unitree_b1/joystick_module.py             | 233 ++++++++++++++++
 .../unitree_webrtc/unitree_b1/unitree_b1.py   | 262 ++++++++++++++++++
 7 files changed, 860 insertions(+), 1 deletion(-)
 create mode 100644 dimos/msgs/std_msgs/Int32.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/__init__.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/b1_command.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/connection.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py

diff --git a/dimos/msgs/std_msgs/Int32.py b/dimos/msgs/std_msgs/Int32.py
new file mode 100644
index 0000000000..910d7c375e
--- /dev/null
+++ b/dimos/msgs/std_msgs/Int32.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Copyright 2025 Dimensional Inc.
+
+"""Int32 message type."""
+
+from typing import ClassVar
+from dimos_lcm.std_msgs import Int32 as LCMInt32
+
+
+class Int32(LCMInt32):
+    """ROS-compatible Int32 message."""
+
+    msg_name: ClassVar[str] = "std_msgs.Int32"
+
+    def __init__(self, data: int = 0):
+        """Initialize Int32 with data value."""
+        self.data = data
diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py
index a58b74a2f2..3ed93fa77d 100644
--- a/dimos/msgs/std_msgs/__init__.py
+++ b/dimos/msgs/std_msgs/__init__.py
@@ -13,5 +13,6 @@
 # limitations under the License.
 
 from .Header import Header
+from .Int32 import Int32
 
-__all__ = ["Header"]
+__all__ = ["Header", "Int32"]
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/__init__.py b/dimos/robot/unitree_webrtc/unitree_b1/__init__.py
new file mode 100644
index 0000000000..e6e5a0f04a
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/__init__.py
@@ -0,0 +1,8 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+
+"""Unitree B1 robot module."""
+
+from .unitree_b1 import UnitreeB1
+
+__all__ = ["UnitreeB1"]
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py b/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py
new file mode 100644
index 0000000000..f17fa18882
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Copyright 2025 Dimensional Inc.
+
+"""Internal B1 command structure for UDP communication."""
+
+from pydantic import BaseModel, Field
+from typing import Optional
+import struct
+
+
+class B1Command(BaseModel):
+    """Internal B1 robot command matching UDP packet structure.
+
+    This is an internal type - external interfaces use standard Twist messages.
+    """
+
+    # Direct joystick values matching C++ NetworkJoystickCmd struct
+    lx: float = Field(default=0.0, ge=-1.0, le=1.0)  # Turn velocity (left stick X)
+    ly: float = Field(default=0.0, ge=-1.0, le=1.0)  # Forward/back velocity (left stick Y)
+    rx: float = Field(default=0.0, ge=-1.0, le=1.0)  # Strafe velocity (right stick X)
+    ry: float = Field(default=0.0, ge=-1.0, le=1.0)  # Pitch/height adjustment (right stick Y)
+    buttons: int = Field(default=0, ge=0, le=65535)  # Button states (uint16)
+    mode: int = Field(
+        default=0, ge=0, le=255
+    )  # Control mode (uint8): 0=idle, 1=stand, 2=walk, 6=recovery
+
+    @classmethod
+    def from_twist(cls, twist, mode: int = 2):
+        """Create B1Command from standard ROS Twist message.
+
+        This is the key integration point for navigation and planning.
+
+        Args:
+            twist: ROS Twist message with linear and angular velocities
+            mode: Robot mode (default is walk mode for navigation)
+
+        Returns:
+            B1Command configured for the given Twist
+        """
+        # Only apply velocities in walk mode
+        if mode == 2:
+            return cls(
+                lx=-twist.angular.z,  # ROS rotation → B1 turn (negated for correct direction)
+                ly=twist.linear.x,  # ROS forward → B1 forward
+                rx=-twist.linear.y,  # ROS lateral → B1 strafe (negated for correct direction)
+                ry=0.0,  # No pitch control from Twist
+                mode=mode,
+            )
+        else:
+            # In non-walk modes, don't apply velocities
+            return cls(mode=mode)
+
+    def to_bytes(self) -> bytes:
+        """Pack to 19-byte UDP packet matching C++ struct.
+
+        Format: 4 floats + uint16 + uint8 = 19 bytes (little-endian)
+        """
+        return struct.pack("<ffffHB", self.lx, self.ly, self.rx, self.ry, self.buttons, self.mode)
+
+    def __str__(self) -> str:
+        """Human-readable representation."""
+        mode_names = {0: "IDLE", 1: "STAND", 2: "WALK", 6: "RECOVERY"}
+        mode_str = mode_names.get(self.mode, f"MODE_{self.mode}")
+
+        if self.lx != 0 or self.ly != 0 or self.rx != 0 or self.ry != 0:
+            return f"B1Cmd[{mode_str}] LX:{self.lx:+.2f} LY:{self.ly:+.2f} RX:{self.rx:+.2f} RY:{self.ry:+.2f}"
+        else:
+            return f"B1Cmd[{mode_str}] (idle)"
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/connection.py b/dimos/robot/unitree_webrtc/unitree_b1/connection.py
new file mode 100644
index 0000000000..2a769001eb
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/connection.py
@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Copyright 2025 Dimensional Inc.
+
+"""B1 Connection Module that accepts standard Twist commands and converts to UDP packets."""
+
+import socket
+import threading
+import time
+from typing import Optional
+
+from dimos.core import In, Module, rpc
+from dimos.msgs.geometry_msgs import Twist
+from dimos.msgs.std_msgs import Int32
+from .b1_command import B1Command
+
+
+class B1ConnectionModule(Module):
+    """UDP connection module for B1 robot with standard Twist interface.
+
+    Accepts standard ROS Twist messages on /cmd_vel and mode changes on /b1/mode,
+    internally converts to B1Command format, and sends UDP packets at 50Hz.
+    """
+
+    # Module inputs
+    cmd_vel: In[Twist] = None  # Standard velocity commands
+    mode_cmd: In[Int32] = None  # Mode changes
+
+    def __init__(
+        self, ip: str = "192.168.12.1", port: int = 9090, test_mode: bool = False, *args, **kwargs
+    ):
+        """Initialize B1 connection module.
+
+        Args:
+            ip: Robot IP address
+            port: UDP port for joystick server
+            test_mode: If True, print commands instead of sending UDP
+        """
+        Module.__init__(self, *args, **kwargs)
+
+        self.ip = ip
+        self.port = port
+        self.test_mode = test_mode
+        self.current_mode = 0  # Start in IDLE mode for safety
+
+        # Internal state as B1Command
+        self._current_cmd = B1Command(mode=0)
+
+        # Thread control
+        self.running = False
+        self.send_thread = None
+
+        # UDP socket (created in start)
+        self.socket = None
+
+        # Packet counter for debugging
+        self.packet_count = 0
+
+    @rpc
+    def start(self):
+        """Start the connection and subscribe to command streams."""
+
+        # Setup UDP socket (unless in test mode)
+        if not self.test_mode:
+            self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            print(f"B1 Connection started - UDP to {self.ip}:{self.port} at 50Hz")
+        else:
+            print(f"[TEST MODE] B1 Connection started - would send to {self.ip}:{self.port}")
+
+        # Subscribe to input streams
+        if self.cmd_vel:
+            self.cmd_vel.subscribe(self.handle_twist)
+        if self.mode_cmd:
+            self.mode_cmd.subscribe(self.handle_mode)
+
+        # Start 50Hz sending thread
+        self.running = True
+        self.send_thread = threading.Thread(target=self._send_loop, daemon=True)
+        self.send_thread.start()
+
+        return True
+
+    @rpc
+    def stop(self):
+        """Stop the connection and send stop commands."""
+        # Send stop command multiple times for safety
+        self.set_mode(0)  # IDLE mode
+        self._current_cmd = B1Command(mode=0)  # Zero all velocities
+
+        # Send multiple stop packets to ensure receipt
+        if not self.test_mode and self.socket:
+            stop_cmd = B1Command(mode=0)
+            for _ in range(5):
+                data = stop_cmd.to_bytes()
+                self.socket.sendto(data, (self.ip, self.port))
+                time.sleep(0.02)
+
+        # Stop thread
+        self.running = False
+        if self.send_thread:
+            self.send_thread.join(timeout=0.5)
+
+        # Close socket
+        if self.socket:
+            self.socket.close()
+            self.socket = None
+
+        return True
+
+    def handle_twist(self, twist: Twist):
+        """Handle standard Twist message and convert to B1Command.
+
+        This is called automatically when messages arrive on cmd_vel input.
+        """
+        if self.test_mode:
+            print(
+                f"[TEST] Received Twist: linear=({twist.linear.x:.2f}, {twist.linear.y:.2f}), angular.z={twist.angular.z:.2f}"
+            )
+        # Convert Twist to B1Command using current mode
+        self._current_cmd = B1Command.from_twist(twist, self.current_mode)
+
+    def handle_mode(self, mode_msg: Int32):
+        """Handle mode change message.
+
+        This is called automatically when messages arrive on mode_cmd input.
+        """
+        if self.test_mode:
+            print(f"[TEST] Received mode change: {mode_msg.data}")
+        self.set_mode(mode_msg.data)
+
+    @rpc
+    def set_mode(self, mode: int):
+        """Set robot mode (0=idle, 1=stand, 2=walk, 6=recovery)."""
+        self.current_mode = mode
+        self._current_cmd.mode = mode
+
+        # Clear velocities when not in walk mode
+        if mode != 2:
+            self._current_cmd.lx = 0.0
+            self._current_cmd.ly = 0.0
+            self._current_cmd.rx = 0.0
+            self._current_cmd.ry = 0.0
+
+        if self.test_mode:
+            mode_names = {0: "IDLE", 1: "STAND", 2: "WALK", 6: "RECOVERY"}
+            print(f"[TEST] Mode changed to: {mode_names.get(mode, mode)}")
+
+        return True
+
+    def _send_loop(self):
+        """Continuously send current command at 50Hz."""
+        while self.running:
+            try:
+                if self.test_mode:
+                    # Test mode - print status periodically
+                    if self.packet_count % 50 == 0:  # Every second
+                        print(f"[TEST] {self._current_cmd} | Packets: {self.packet_count}")
+                else:
+                    # Real mode - send UDP packet
+                    if self.socket:
+                        data = self._current_cmd.to_bytes()
+                        self.socket.sendto(data, (self.ip, self.port))
+
+                self.packet_count += 1
+
+                # Maintain 50Hz rate (20ms between packets)
+                time.sleep(0.020)
+
+            except Exception as e:
+                if self.running:
+                    print(f"Send error: {e}")
+
+    # Direct RPC methods for mode control
+    @rpc
+    def idle(self):
+        """Set robot to idle mode."""
+        self.set_mode(0)
+        return True
+
+    @rpc
+    def stand(self):
+        """Set robot to stand mode."""
+        self.set_mode(1)
+        return True
+
+    @rpc
+    def walk(self):
+        """Set robot to walk mode."""
+        self.set_mode(2)
+        return True
+
+    @rpc
+    def recovery(self):
+        """Set robot to recovery mode."""
+        self.set_mode(6)
+        return True
+
+    @rpc
+    def move(self, twist: Twist, duration: float = 0.0):
+        """Direct RPC method for sending Twist commands.
+
+        Args:
+            twist: Velocity command
+            duration: Not used, kept for compatibility
+        """
+        self.handle_twist(twist)
+        return True
+
+    def cleanup(self):
+        """Clean up resources when module is destroyed."""
+        self.stop()
+
+
+class TestB1ConnectionModule(B1ConnectionModule):
+    """Test connection module that prints commands instead of sending UDP."""
+
+    def __init__(self, ip: str = "127.0.0.1", port: int = 9090, *args, **kwargs):
+        """Initialize test connection without creating socket."""
+        super().__init__(ip, port, test_mode=True, *args, **kwargs)
+
+    def _send_loop(self):
+        """Override to provide better test output."""
+        while self.running:
+            # Print current state every 0.5 seconds (not every packet)
+            if self.packet_count % 25 == 0:
+                print(f"[TEST] {self._current_cmd} | Count: {self.packet_count}")
+
+            self.packet_count += 1
+            time.sleep(0.020)
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py b/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
new file mode 100644
index 0000000000..45e3dd05d7
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
@@ -0,0 +1,233 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Copyright 2025 Dimensional Inc.
+
+"""Pygame Joystick Module for testing B1 control via LCM."""
+
+import os
+import threading
+
+# Force X11 driver to avoid OpenGL threading issues
+os.environ["SDL_VIDEODRIVER"] = "x11"
+
+from dimos.core import Module, Out, rpc
+from dimos.msgs.geometry_msgs import Twist, Vector3
+from dimos.msgs.std_msgs import Int32
+
+
+class JoystickModule(Module):
+    """Pygame-based joystick control module for B1 testing.
+
+    Outputs standard Twist messages on /cmd_vel and mode changes on /b1/mode.
+    This allows testing the same interface that navigation will use.
+    """
+
+    twist_out: Out[Twist] = None  # Standard velocity commands
+    mode_out: Out[Int32] = None  # Mode changes
+
+    def __init__(self, *args, **kwargs):
+        Module.__init__(self, *args, **kwargs)
+        self.pygame_ready = False
+        self.running = False
+        self.current_mode = 2  # Start in walk mode
+
+    @rpc
+    def start(self):
+        """Initialize pygame and start control loop."""
+        try:
+            import pygame
+        except ImportError:
+            print("ERROR: pygame not installed. Install with: pip install pygame")
+            return False
+
+        self.keys_held = set()
+        self.pygame_ready = True
+        self.running = True
+
+        # Start pygame loop in background thread - ALL pygame ops will happen there
+        self._thread = threading.Thread(target=self._pygame_loop, daemon=True)
+        self._thread.start()
+
+        return True
+
+    def _pygame_loop(self):
+        """Main pygame event loop - ALL pygame operations happen here."""
+        import pygame
+
+        # Initialize pygame and create display IN THIS THREAD
+        pygame.init()
+        self.screen = pygame.display.set_mode((500, 400), pygame.SWSURFACE)
+        pygame.display.set_caption("B1 Joystick Control (LCM)")
+        self.clock = pygame.time.Clock()
+        self.font = pygame.font.Font(None, 24)
+
+        print("JoystickModule started - Focus pygame window to control")
+        print("Controls:")
+        print("  WASD = Movement/Turning")
+        print("  IJKL = Strafing/Pitch (in appropriate modes)")
+        print("  1/2/0 = Stand/Walk/Idle modes")
+        print("  Space = Emergency Stop")
+        print("  Q/ESC = Quit")
+
+        last_twist = Twist()
+
+        while self.running and self.pygame_ready:
+            # Handle pygame events
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    self.running = False
+                elif event.type == pygame.KEYDOWN:
+                    self.keys_held.add(event.key)
+
+                    # Mode changes - publish to mode_out for connection module
+                    if event.key == pygame.K_0:
+                        self.current_mode = 0
+                        mode_msg = Int32()
+                        mode_msg.data = 0
+                        self.mode_out.publish(mode_msg)
+                        print("Mode: IDLE")
+                    elif event.key == pygame.K_1:
+                        self.current_mode = 1
+                        mode_msg = Int32()
+                        mode_msg.data = 1
+                        self.mode_out.publish(mode_msg)
+                        print("Mode: STAND")
+                    elif event.key == pygame.K_2:
+                        self.current_mode = 2
+                        mode_msg = Int32()
+                        mode_msg.data = 2
+                        self.mode_out.publish(mode_msg)
+                        print("Mode: WALK")
+                    elif event.key == pygame.K_SPACE:
+                        self.keys_held.clear()
+                        # Send IDLE mode for emergency stop
+                        self.current_mode = 0
+                        mode_msg = Int32()
+                        mode_msg.data = 0
+                        self.mode_out.publish(mode_msg)
+                        # Also send zero twist
+                        stop_twist = Twist()
+                        stop_twist.linear = Vector3(0, 0, 0)
+                        stop_twist.angular = Vector3(0, 0, 0)
+                        self.twist_out.publish(stop_twist)
+                        print("EMERGENCY STOP!")
+                    elif event.key == pygame.K_ESCAPE or event.key == pygame.K_q:
+                        self.running = False
+
+                elif event.type == pygame.KEYUP:
+                    self.keys_held.discard(event.key)
+
+            # Generate Twist message from held keys
+            twist = Twist()
+            twist.linear = Vector3(0, 0, 0)
+            twist.angular = Vector3(0, 0, 0)
+
+            # Only apply movement in walk mode (mode 2)
+            if self.current_mode == 2:
+                # Forward/backward (W/S)
+                if pygame.K_w in self.keys_held:
+                    twist.linear.x = 1.0  # Forward
+                if pygame.K_s in self.keys_held:
+                    twist.linear.x = -1.0  # Backward
+
+                # Turning (A/D) - note the inversion for correct direction
+                if pygame.K_a in self.keys_held:
+                    twist.angular.z = 1.0  # Turn left (positive angular.z)
+                if pygame.K_d in self.keys_held:
+                    twist.angular.z = -1.0  # Turn right (negative angular.z)
+
+                # Strafing (J/L) - note the inversion for correct direction
+                if pygame.K_j in self.keys_held:
+                    twist.linear.y = 1.0  # Strafe left (positive linear.y)
+                if pygame.K_l in self.keys_held:
+                    twist.linear.y = -1.0  # Strafe right (negative linear.y)
+
+            # Always publish twist at 50Hz (matching working client behavior)
+            self.twist_out.publish(twist)
+            last_twist = twist
+
+            # Update pygame display
+            self._update_display(twist)
+
+            # Maintain 50Hz rate
+            self.clock.tick(50)
+
+        pygame.quit()
+        print("JoystickModule stopped")
+
+    def _update_display(self, twist):
+        """Update pygame window with current status."""
+        import pygame
+
+        self.screen.fill((30, 30, 30))
+
+        # Mode display
+        y_pos = 20
+        mode_text = ["IDLE", "STAND", "WALK"][self.current_mode if self.current_mode < 3 else 0]
+        mode_color = (
+            (0, 255, 0)
+            if self.current_mode == 2
+            else (255, 255, 0)
+            if self.current_mode == 1
+            else (100, 100, 100)
+        )
+
+        texts = [
+            f"Mode: {mode_text}",
+            "",
+            f"Linear X (Forward/Back): {twist.linear.x:+.2f}",
+            f"Linear Y (Strafe L/R): {twist.linear.y:+.2f}",
+            f"Angular Z (Turn L/R): {twist.angular.z:+.2f}",
+            "",
+            "Keys: " + ", ".join([pygame.key.name(k).upper() for k in self.keys_held if k < 256]),
+        ]
+
+        for i, text in enumerate(texts):
+            if text:
+                color = mode_color if i == 0 else (255, 255, 255)
+                surf = self.font.render(text, True, color)
+                self.screen.blit(surf, (20, y_pos))
+            y_pos += 30
+
+        # Movement indicator
+        if twist.linear.x != 0 or twist.linear.y != 0 or twist.angular.z != 0:
+            pygame.draw.circle(self.screen, (255, 0, 0), (450, 30), 15)  # Red = moving
+        else:
+            pygame.draw.circle(self.screen, (0, 255, 0), (450, 30), 15)  # Green = stopped
+
+        # Instructions at bottom
+        y_pos = 300
+        help_texts = ["WASD: Move | JL: Strafe | 1/2/0: Modes", "Space: Stop | Q/ESC: Quit"]
+        for text in help_texts:
+            surf = self.font.render(text, True, (150, 150, 150))
+            self.screen.blit(surf, (20, y_pos))
+            y_pos += 25
+
+        pygame.display.flip()
+
+    @rpc
+    def stop(self):
+        """Stop the joystick module."""
+        self.running = False
+        # Send stop command
+        stop_twist = Twist()
+        self.twist_out.publish(stop_twist)
+        return True
+
+    def cleanup(self):
+        """Clean up pygame resources."""
+        self.running = False
+        self.pygame_ready = False
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
new file mode 100644
index 0000000000..7d7e49a8f1
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
@@ -0,0 +1,262 @@
+#!/usr/bin/env python3
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Copyright 2025 Dimensional Inc.
+
+"""
+Unitree B1 quadruped robot with simplified UDP control.
+Uses standard Twist interface for velocity commands.
+"""
+
+import os
+import logging
+from typing import Optional
+
+from dimos import core
+from dimos.msgs.geometry_msgs import Twist
+from dimos.msgs.std_msgs import Int32
+from dimos.protocol.pubsub.lcmpubsub import LCM
+from dimos.robot.robot import Robot
+from dimos.robot.unitree_webrtc.unitree_b1.connection import (
+    B1ConnectionModule,
+    TestB1ConnectionModule,
+)
+from dimos.skills.skills import SkillLibrary
+from dimos.types.robot_capabilities import RobotCapability
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger("dimos.robot.unitree_webrtc.unitree_b1", level=logging.INFO)
+
+
+class UnitreeB1(Robot):
+    """Unitree B1 quadruped robot with UDP control.
+
+    Simplified architecture:
+    - Connection module handles Twist → B1Command conversion
+    - Standard /cmd_vel interface for navigation compatibility
+    - Optional joystick module for testing
+    """
+
+    def __init__(
+        self,
+        ip: str = "192.168.123.14",
+        port: int = 9090,
+        output_dir: str = None,
+        skill_library: Optional[SkillLibrary] = None,
+        enable_joystick: bool = False,
+        test_mode: bool = False,
+    ):
+        """Initialize the B1 robot.
+
+        Args:
+            ip: Robot IP address (or server running joystick_server_udp)
+            port: UDP port for joystick server (default 9090)
+            output_dir: Directory for saving outputs
+            skill_library: Skill library instance (optional)
+            enable_joystick: Enable pygame joystick control module
+            test_mode: Test mode - print commands instead of sending UDP
+        """
+        super().__init__()
+        self.ip = ip
+        self.port = port
+        self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output")
+        self.enable_joystick = enable_joystick
+        self.test_mode = test_mode
+
+        # Initialize with basic locomotion capability
+        self.capabilities = [RobotCapability.LOCOMOTION]
+
+        # Skills (will be added in future)
+        self.skill_library = skill_library
+
+        # Module references
+        self.connection = None
+        self.joystick = None
+
+        # Ensure output directory exists
+        os.makedirs(self.output_dir, exist_ok=True)
+        logger.info(f"Robot outputs will be saved to: {self.output_dir}")
+
+        # Initialize DimOS
+        logger.info("Initializing DimOS...")
+        self.dimos = core.start(2)
+
+        # Deploy modules
+        logger.info("Deploying connection module...")
+        self._deploy_connection()
+        if self.enable_joystick:
+            self._deploy_joystick()
+
+        # Start modules
+        self._start_modules()
+
+        logger.info(f"UnitreeB1 initialized - UDP control to {self.ip}:{self.port}")
+        if self.enable_joystick:
+            logger.info("Pygame joystick module enabled for testing")
+
+    def _deploy_connection(self):
+        """Deploy and configure the connection module."""
+        if self.test_mode:
+            self.connection = self.dimos.deploy(TestB1ConnectionModule, self.ip, self.port)
+        else:
+            self.connection = self.dimos.deploy(
+                B1ConnectionModule, self.ip, self.port, self.test_mode
+            )
+
+        # Configure LCM transports for standard interfaces
+        self.connection.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist)
+        self.connection.mode_cmd.transport = core.LCMTransport("/b1/mode", Int32)
+
+    def _deploy_joystick(self):
+        """Deploy and configure the joystick module for testing."""
+        from dimos.robot.unitree_webrtc.unitree_b1.joystick_module import JoystickModule
+
+        self.joystick = self.dimos.deploy(JoystickModule)
+
+        # Connect joystick outputs to standard topics
+        self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist)
+        self.joystick.mode_out.transport = core.LCMTransport("/b1/mode", Int32)
+
+        logger.info("Joystick module deployed - pygame window will open")
+
+    def _start_modules(self):
+        """Start all deployed modules."""
+        # Start connection (includes starting 50Hz sending thread)
+        self.connection.start()
+
+        # Start in IDLE mode for safety
+        self.connection.idle()
+        logger.info("B1 started in IDLE mode (safety)")
+
+        # Start joystick if enabled
+        if self.joystick:
+            self.joystick.start()
+
+    # Robot control methods (standard interface)
+    def move(self, twist: Twist, duration: float = 0.0):
+        """Send movement command to robot using standard Twist.
+
+        Args:
+            twist: Twist message with linear and angular velocities
+            duration: How long to move (not used for B1)
+        """
+        if self.connection:
+            self.connection.move(twist, duration)
+
+    def stop(self):
+        """Stop all robot movement."""
+        if self.connection:
+            self.connection.stop()
+
+    def stand(self):
+        """Put robot in stand mode."""
+        if self.connection:
+            self.connection.stand()
+            logger.info("B1 switched to STAND mode")
+
+    def walk(self):
+        """Put robot in walk mode."""
+        if self.connection:
+            self.connection.walk()
+            logger.info("B1 switched to WALK mode")
+
+    def idle(self):
+        """Put robot in idle mode."""
+        if self.connection:
+            self.connection.idle()
+            logger.info("B1 switched to IDLE mode")
+
+    def cleanup(self):
+        """Clean up robot resources."""
+        logger.info("Cleaning up B1 robot...")
+
+        # Stop robot movement
+        self.stop()
+
+        # Clean up modules
+        if self.connection:
+            self.connection.cleanup()
+
+        logger.info("B1 cleanup complete")
+
+    def __del__(self):
+        """Destructor to ensure cleanup."""
+        self.cleanup()
+
+
+def main():
+    """Main entry point for testing B1 robot."""
+    import argparse
+
+    parser = argparse.ArgumentParser(description="Unitree B1 Robot Control")
+    parser.add_argument("--ip", default="192.168.12.1", help="Robot IP address")
+    parser.add_argument("--port", type=int, default=9090, help="UDP port")
+    parser.add_argument("--joystick", action="store_true", help="Enable pygame joystick control")
+    parser.add_argument("--output-dir", help="Output directory for logs/data")
+    parser.add_argument(
+        "--test", action="store_true", help="Test mode - print commands instead of UDP"
+    )
+
+    args = parser.parse_args()
+
+    # Create robot instance
+    robot = UnitreeB1(
+        ip=args.ip,
+        port=args.port,
+        output_dir=args.output_dir,
+        enable_joystick=args.joystick,
+        test_mode=args.test,
+    )
+
+    try:
+        if args.joystick:
+            print("\n" + "=" * 50)
+            print("B1 JOYSTICK CONTROL")
+            print("=" * 50)
+            print("Focus the pygame window to control")
+            print("Press keys in pygame window:")
+            print("  0/1/2 = Idle/Stand/Walk modes")
+            print("  WASD = Move/Turn")
+            print("  JL = Strafe")
+            print("  Space = Emergency Stop")
+            print("  Q/ESC = Quit")
+            print("=" * 50 + "\n")
+
+            # Keep running until interrupted
+            import time
+
+            while True:
+                time.sleep(1)
+        else:
+            # Manual control example
+            print("\nB1 Robot ready for commands")
+            print("Use robot.idle(), robot.stand(), robot.walk() to change modes")
+            print("Use robot.move(Twist(...)) to send velocity commands")
+            print("Press Ctrl+C to exit\n")
+
+            # Keep running
+            import time
+
+            while True:
+                time.sleep(1)
+
+    except KeyboardInterrupt:
+        print("\nShutting down...")
+    finally:
+        robot.cleanup()
+
+
+if __name__ == "__main__":
+    main()

From aadd5a66a4ba6e300ae7138c622babba87e3cc53 Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 04:29:21 -0700
Subject: [PATCH 4/9] Increased robot UDP control safety with apcket freshness
 checks

---
 .../unitree_webrtc/unitree_b1/connection.py   | 82 +++++++++++++------
 1 file changed, 57 insertions(+), 25 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/connection.py b/dimos/robot/unitree_webrtc/unitree_b1/connection.py
index 2a769001eb..56f684fc98 100644
--- a/dimos/robot/unitree_webrtc/unitree_b1/connection.py
+++ b/dimos/robot/unitree_webrtc/unitree_b1/connection.py
@@ -55,19 +55,15 @@ def __init__(
         self.port = port
         self.test_mode = test_mode
         self.current_mode = 0  # Start in IDLE mode for safety
-
         # Internal state as B1Command
         self._current_cmd = B1Command(mode=0)
-
         # Thread control
         self.running = False
         self.send_thread = None
-
-        # UDP socket (created in start)
         self.socket = None
-
-        # Packet counter for debugging
         self.packet_count = 0
+        self.last_command_time = time.time()
+        self.command_timeout = 0.1  # 100ms timeout matching C++ server
 
     @rpc
     def start(self):
@@ -96,11 +92,10 @@ def start(self):
     @rpc
     def stop(self):
         """Stop the connection and send stop commands."""
-        # Send stop command multiple times for safety
-        self.set_mode(0)  # IDLE mode
+        self.set_mode(0)  # IDLE
         self._current_cmd = B1Command(mode=0)  # Zero all velocities
 
-        # Send multiple stop packets to ensure receipt
+        # Send multiple stop packets
         if not self.test_mode and self.socket:
             stop_cmd = B1Command(mode=0)
             for _ in range(5):
@@ -108,12 +103,10 @@ def stop(self):
                 self.socket.sendto(data, (self.ip, self.port))
                 time.sleep(0.02)
 
-        # Stop thread
         self.running = False
         if self.send_thread:
             self.send_thread.join(timeout=0.5)
 
-        # Close socket
         if self.socket:
             self.socket.close()
             self.socket = None
@@ -129,8 +122,9 @@ def handle_twist(self, twist: Twist):
             print(
                 f"[TEST] Received Twist: linear=({twist.linear.x:.2f}, {twist.linear.y:.2f}), angular.z={twist.angular.z:.2f}"
             )
-        # Convert Twist to B1Command using current mode
+        # Convert Twist to B1Command
         self._current_cmd = B1Command.from_twist(twist, self.current_mode)
+        self.last_command_time = time.time()
 
     def handle_mode(self, mode_msg: Int32):
         """Handle mode change message.
@@ -161,18 +155,41 @@ def set_mode(self, mode: int):
         return True
 
     def _send_loop(self):
-        """Continuously send current command at 50Hz."""
+        """Continuously send current command at 50Hz with safety timeout."""
+        timeout_warned = False
+
         while self.running:
             try:
-                if self.test_mode:
-                    # Test mode - print status periodically
-                    if self.packet_count % 50 == 0:  # Every second
-                        print(f"[TEST] {self._current_cmd} | Packets: {self.packet_count}")
+                # Safety check: If no command received recently, send zeros
+                time_since_last_cmd = time.time() - self.last_command_time
+
+                if time_since_last_cmd > self.command_timeout:
+                    # Command is stale - send zero velocities for safety
+                    if not timeout_warned:
+                        if self.test_mode:
+                            print(
+                                f"[TEST] Command timeout ({time_since_last_cmd:.1f}s) - sending zeros"
+                            )
+                        timeout_warned = True
+
+                    # Create safe idle command
+                    safe_cmd = B1Command(mode=self.current_mode)
+                    safe_cmd.lx = 0.0
+                    safe_cmd.ly = 0.0
+                    safe_cmd.rx = 0.0
+                    safe_cmd.ry = 0.0
+                    cmd_to_send = safe_cmd
                 else:
-                    # Real mode - send UDP packet
-                    if self.socket:
-                        data = self._current_cmd.to_bytes()
-                        self.socket.sendto(data, (self.ip, self.port))
+                    # Send command if fresh
+                    if timeout_warned:
+                        if self.test_mode:
+                            print("[TEST] Commands resumed - control restored")
+                        timeout_warned = False
+                    cmd_to_send = self._current_cmd
+
+                if self.socket:
+                    data = cmd_to_send.to_bytes()
+                    self.socket.sendto(data, (self.ip, self.port))
 
                 self.packet_count += 1
 
@@ -183,7 +200,6 @@ def _send_loop(self):
                 if self.running:
                     print(f"Send error: {e}")
 
-    # Direct RPC methods for mode control
     @rpc
     def idle(self):
         """Set robot to idle mode."""
@@ -232,11 +248,27 @@ def __init__(self, ip: str = "127.0.0.1", port: int = 9090, *args, **kwargs):
         super().__init__(ip, port, test_mode=True, *args, **kwargs)
 
     def _send_loop(self):
-        """Override to provide better test output."""
+        """Override to provide better test output with timeout detection."""
+        timeout_warned = False
+
         while self.running:
-            # Print current state every 0.5 seconds (not every packet)
+            time_since_last_cmd = time.time() - self.last_command_time
+            is_timeout = time_since_last_cmd > self.command_timeout
+
+            # Show timeout transitions
+            if is_timeout and not timeout_warned:
+                print(f"[TEST] Command timeout! Sending zeros after {time_since_last_cmd:.1f}s")
+                timeout_warned = True
+            elif not is_timeout and timeout_warned:
+                print("[TEST] Commands resumed - control restored")
+                timeout_warned = False
+
+            # Print current state every 0.5 seconds
             if self.packet_count % 25 == 0:
-                print(f"[TEST] {self._current_cmd} | Count: {self.packet_count}")
+                if is_timeout:
+                    print(f"[TEST] B1Cmd[ZEROS] (timeout) | Count: {self.packet_count}")
+                else:
+                    print(f"[TEST] {self._current_cmd} | Count: {self.packet_count}")
 
             self.packet_count += 1
             time.sleep(0.020)

From 69c5e8b0713ae06f1b3371a5fda8d166eea1f303 Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 04:56:28 -0700
Subject: [PATCH 5/9] Final cleanup

---
 .../unitree_b1/joystick_module.py             | 18 ++---
 .../unitree_webrtc/unitree_b1/unitree_b1.py   | 67 ++++++-------------
 2 files changed, 28 insertions(+), 57 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py b/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
index 45e3dd05d7..f56eeb9b72 100644
--- a/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
+++ b/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py
@@ -42,7 +42,7 @@ def __init__(self, *args, **kwargs):
         Module.__init__(self, *args, **kwargs)
         self.pygame_ready = False
         self.running = False
-        self.current_mode = 2  # Start in walk mode
+        self.current_mode = 0  # Start in IDLE mode for safety
 
     @rpc
     def start(self):
@@ -79,13 +79,10 @@ def _pygame_loop(self):
         print("  WASD = Movement/Turning")
         print("  IJKL = Strafing/Pitch (in appropriate modes)")
         print("  1/2/0 = Stand/Walk/Idle modes")
-        print("  Space = Emergency Stop")
-        print("  Q/ESC = Quit")
-
-        last_twist = Twist()
+        print("  Space/Q = Emergency Stop")
+        print("  ESC = Quit (or use Ctrl+C)")
 
         while self.running and self.pygame_ready:
-            # Handle pygame events
             for event in pygame.event.get():
                 if event.type == pygame.QUIT:
                     self.running = False
@@ -111,7 +108,7 @@ def _pygame_loop(self):
                         mode_msg.data = 2
                         self.mode_out.publish(mode_msg)
                         print("Mode: WALK")
-                    elif event.key == pygame.K_SPACE:
+                    elif event.key == pygame.K_SPACE or event.key == pygame.K_q:
                         self.keys_held.clear()
                         # Send IDLE mode for emergency stop
                         self.current_mode = 0
@@ -124,7 +121,8 @@ def _pygame_loop(self):
                         stop_twist.angular = Vector3(0, 0, 0)
                         self.twist_out.publish(stop_twist)
                         print("EMERGENCY STOP!")
-                    elif event.key == pygame.K_ESCAPE or event.key == pygame.K_q:
+                    elif event.key == pygame.K_ESCAPE:
+                        # ESC still quits for development convenience
                         self.running = False
 
                 elif event.type == pygame.KEYUP:
@@ -157,7 +155,6 @@ def _pygame_loop(self):
 
             # Always publish twist at 50Hz (matching working client behavior)
             self.twist_out.publish(twist)
-            last_twist = twist
 
             # Update pygame display
             self._update_display(twist)
@@ -208,9 +205,8 @@ def _update_display(self, twist):
         else:
             pygame.draw.circle(self.screen, (0, 255, 0), (450, 30), 15)  # Green = stopped
 
-        # Instructions at bottom
         y_pos = 300
-        help_texts = ["WASD: Move | JL: Strafe | 1/2/0: Modes", "Space: Stop | Q/ESC: Quit"]
+        help_texts = ["WASD: Move | JL: Strafe | 1/2/0: Modes", "Space/Q: E-Stop | ESC: Quit"]
         for text in help_texts:
             surf = self.font.render(text, True, (150, 150, 150))
             self.screen.blit(surf, (20, y_pos))
diff --git a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
index 7d7e49a8f1..afeb8dfcc9 100644
--- a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
+++ b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
@@ -74,76 +74,49 @@ def __init__(
         self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output")
         self.enable_joystick = enable_joystick
         self.test_mode = test_mode
-
-        # Initialize with basic locomotion capability
         self.capabilities = [RobotCapability.LOCOMOTION]
-
-        # Skills (will be added in future)
-        self.skill_library = skill_library
-
-        # Module references
         self.connection = None
         self.joystick = None
 
-        # Ensure output directory exists
         os.makedirs(self.output_dir, exist_ok=True)
         logger.info(f"Robot outputs will be saved to: {self.output_dir}")
 
-        # Initialize DimOS
+    def start(self):
+        """Start the B1 robot - initialize DimOS, deploy modules, and start them."""
+
         logger.info("Initializing DimOS...")
         self.dimos = core.start(2)
 
-        # Deploy modules
         logger.info("Deploying connection module...")
-        self._deploy_connection()
-        if self.enable_joystick:
-            self._deploy_joystick()
-
-        # Start modules
-        self._start_modules()
-
-        logger.info(f"UnitreeB1 initialized - UDP control to {self.ip}:{self.port}")
-        if self.enable_joystick:
-            logger.info("Pygame joystick module enabled for testing")
-
-    def _deploy_connection(self):
-        """Deploy and configure the connection module."""
         if self.test_mode:
             self.connection = self.dimos.deploy(TestB1ConnectionModule, self.ip, self.port)
         else:
-            self.connection = self.dimos.deploy(
-                B1ConnectionModule, self.ip, self.port, self.test_mode
-            )
+            self.connection = self.dimos.deploy(B1ConnectionModule, self.ip, self.port)
 
-        # Configure LCM transports for standard interfaces
+        # Configure LCM transports for connection
         self.connection.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist)
         self.connection.mode_cmd.transport = core.LCMTransport("/b1/mode", Int32)
 
-    def _deploy_joystick(self):
-        """Deploy and configure the joystick module for testing."""
-        from dimos.robot.unitree_webrtc.unitree_b1.joystick_module import JoystickModule
-
-        self.joystick = self.dimos.deploy(JoystickModule)
-
-        # Connect joystick outputs to standard topics
-        self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist)
-        self.joystick.mode_out.transport = core.LCMTransport("/b1/mode", Int32)
+        # Deploy joystick move_vel control
+        if self.enable_joystick:
+            from dimos.robot.unitree_webrtc.unitree_b1.joystick_module import JoystickModule
 
-        logger.info("Joystick module deployed - pygame window will open")
+            self.joystick = self.dimos.deploy(JoystickModule)
+            self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist)
+            self.joystick.mode_out.transport = core.LCMTransport("/b1/mode", Int32)
+            logger.info("Joystick module deployed - pygame window will open")
 
-    def _start_modules(self):
-        """Start all deployed modules."""
-        # Start connection (includes starting 50Hz sending thread)
         self.connection.start()
-
-        # Start in IDLE mode for safety
-        self.connection.idle()
+        self.connection.idle()  # Start in IDLE mode for safety
         logger.info("B1 started in IDLE mode (safety)")
 
-        # Start joystick if enabled
         if self.joystick:
             self.joystick.start()
 
+        logger.info(f"UnitreeB1 initialized - UDP control to {self.ip}:{self.port}")
+        if self.enable_joystick:
+            logger.info("Pygame joystick module enabled for testing")
+
     # Robot control methods (standard interface)
     def move(self, twist: Twist, duration: float = 0.0):
         """Send movement command to robot using standard Twist.
@@ -220,6 +193,8 @@ def main():
         test_mode=args.test,
     )
 
+    robot.start()
+
     try:
         if args.joystick:
             print("\n" + "=" * 50)
@@ -230,8 +205,8 @@ def main():
             print("  0/1/2 = Idle/Stand/Walk modes")
             print("  WASD = Move/Turn")
             print("  JL = Strafe")
-            print("  Space = Emergency Stop")
-            print("  Q/ESC = Quit")
+            print("  Space/Q = Emergency Stop")
+            print("  ESC = Quit pygame (then Ctrl+C to exit)")
             print("=" * 50 + "\n")
 
             # Keep running until interrupted

From 83166680588d0c727837e2b8a9e9b444d626b664 Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 04:59:44 -0700
Subject: [PATCH 6/9] Deleted old udp client

---
 .../unitree_b1/joystick_client_udp.py         | 338 ------------------
 1 file changed, 338 deletions(-)
 delete mode 100644 dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py b/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py
deleted file mode 100644
index 6eabd9a669..0000000000
--- a/dimos/robot/unitree_webrtc/unitree_b1/joystick_client_udp.py
+++ /dev/null
@@ -1,338 +0,0 @@
-#!/usr/bin/env python3
-# Copyright 2025 Dimensional Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-"""
-UDP Joystick Control Client for Unitree B1 Robot
-Sends joystick commands via UDP datagrams - no fragmentation issues!
-"""
-
-import socket
-import struct
-import sys
-import time
-import termios
-import tty
-import select
-import signal
-
-
-class NetworkJoystickCmd:
-    """Structure matching the C++ NetworkJoystickCmd"""
-
-    # Format: 4 floats (lx, ly, rx, ry), 1 uint16 (buttons), 1 uint8 (mode)
-    # Little-endian byte order to match x86/ARM
-    FORMAT = "<ffffHB"  # 19 bytes total
-    SIZE = struct.calcsize(FORMAT)
-
-    def __init__(self):
-        self.lx = 0.0  # left stick x (-1 to 1)
-        self.ly = 0.0  # left stick y (-1 to 1)
-        self.rx = 0.0  # right stick x (-1 to 1)
-        self.ry = 0.0  # right stick y (-1 to 1)
-        self.buttons = 0  # button states (uint16)
-        self.mode = 0  # control mode (uint8)
-
-    def pack(self):
-        """Pack the structure into bytes matching C++ memory layout"""
-        return struct.pack(self.FORMAT, self.lx, self.ly, self.rx, self.ry, self.buttons, self.mode)
-
-    def reset_sticks(self):
-        """Reset stick values (they don't hold position)"""
-        self.lx = 0.0
-        self.ly = 0.0
-        self.rx = 0.0
-        self.ry = 0.0
-
-
-class JoystickClient:
-    """UDP client for sending joystick commands to the robot server"""
-
-    def __init__(self, server_ip, server_port, test_mode=False, verbose=False):
-        self.server_ip = server_ip
-        self.server_port = server_port
-        self.socket = None
-        self.old_settings = None
-        self.running = False
-        self.active_keys = set()  # Track all currently active keys
-        self.test_mode = test_mode
-        self.verbose = verbose  # Show packet details when running real
-        self.packet_count = 0
-
-    def connect(self):
-        """Create UDP socket (no actual connection needed)"""
-        try:
-            # Create UDP socket
-            self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-
-            # Set socket to non-blocking for potential future enhancements
-            # self.socket.setblocking(0)
-
-            if self.test_mode:
-                print(f"TEST MODE: Created UDP socket")
-                print(f"Will send to: {self.server_ip}:{self.server_port}")
-                print(f"Packet size: {NetworkJoystickCmd.SIZE} bytes")
-            else:
-                print(f"UDP client ready")
-                print(f"Target server: {self.server_ip}:{self.server_port}")
-                print(f"Packet size: {NetworkJoystickCmd.SIZE} bytes")
-                print(f"Protocol: UDP (fire-and-forget)")
-
-            return True
-        except Exception as e:
-            print(f"Failed to create UDP socket: {e}")
-            if self.socket:
-                self.socket.close()
-                self.socket = None
-            return False
-
-    def disconnect(self):
-        """Close the UDP socket"""
-        if self.socket:
-            self.socket.close()
-            self.socket = None
-
-    def send_command(self, cmd):
-        """Send a joystick command via UDP datagram"""
-        if not self.socket and not self.test_mode:
-            return False
-
-        try:
-            data = cmd.pack()
-            self.packet_count += 1
-
-            # Verify packet size
-            if len(data) != 19:
-                print(f"ERROR: Packet size is {len(data)}, expected 19")
-                return False
-
-            if self.test_mode:
-                # Test mode - clean output
-                if cmd.lx != 0 or cmd.ly != 0 or cmd.rx != 0 or cmd.ry != 0:
-                    # Movement active
-                    sys.stdout.write(
-                        f"\r[Moving] Mode: {cmd.mode} | LX: {cmd.lx:+.1f} LY: {cmd.ly:+.1f} | RX: {cmd.rx:+.1f} RY: {cmd.ry:+.1f}     "
-                    )
-                    sys.stdout.flush()
-                elif self.packet_count % 50 == 0:
-                    # Idle status update
-                    sys.stdout.write(
-                        f"\r[Idle] Mode: {cmd.mode} | Packets: {self.packet_count}     "
-                    )
-                    sys.stdout.flush()
-                return True
-
-            # Send UDP datagram (fire and forget)
-            bytes_sent = self.socket.sendto(data, (self.server_ip, self.server_port))
-
-            # Show real-time status when verbose mode is enabled (same format as test mode)
-            if self.verbose:
-                if cmd.lx != 0 or cmd.ly != 0 or cmd.rx != 0 or cmd.ry != 0:
-                    # Movement active
-                    sys.stdout.write(
-                        f"\r[Moving] Mode: {cmd.mode} | LX: {cmd.lx:+.1f} LY: {cmd.ly:+.1f} | RX: {cmd.rx:+.1f} RY: {cmd.ry:+.1f}     "
-                    )
-                    sys.stdout.flush()
-                elif self.packet_count % 50 == 0:
-                    # Idle status update
-                    sys.stdout.write(
-                        f"\r[Idle] Mode: {cmd.mode} | Packets: {self.packet_count}     "
-                    )
-                    sys.stdout.flush()
-            elif self.packet_count % 50 == 0:
-                # Non-verbose: just show packet count every second
-                print(f"Sent {self.packet_count} packets ({self.packet_count * 0.02:.1f}s)")
-
-            return bytes_sent == NetworkJoystickCmd.SIZE
-
-        except Exception as e:
-            print(f"\nFailed to send UDP packet: {e}")
-            return False
-
-    def set_nonblocking_input(self):
-        """Set terminal to non-blocking input mode"""
-        self.old_settings = termios.tcgetattr(sys.stdin)
-        tty.setraw(sys.stdin.fileno())
-
-    def restore_input(self):
-        """Restore terminal settings"""
-        if self.old_settings:
-            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
-
-    def get_key(self, timeout=0):
-        """Get a single keypress without blocking"""
-        if select.select([sys.stdin], [], [], timeout)[0]:
-            key = sys.stdin.read(1)
-            # Check for Ctrl+C (ASCII code 3)
-            if ord(key) == 3:
-                print("\nCtrl+C pressed - exiting...")
-                self.running = False
-                return None
-            return key
-        return None
-
-    def run_keyboard_control(self):
-        """Main control loop with keyboard input"""
-        self.set_nonblocking_input()
-        self.running = True
-
-        cmd = NetworkJoystickCmd()
-
-        print("\n" + "=" * 50)
-        print("UDP Keyboard Control (No TCP fragmentation!)")
-        print("=" * 50)
-        print("Modes: 0=Idle, 1=Stand, 2=Walk, 6=Recovery")
-        print("WASD = Movement/Turning (left stick)")
-        print("IJKL = Strafing/Pitch (right stick)")
-        print("Space = Emergency Stop")
-        print("Q = Quit")
-        print("\nHOLD keys for continuous movement")
-        print("Multiple keys work together (e.g., W+A = forward + turn)")
-        print(f"\nCurrent mode: {cmd.mode}")
-
-        try:
-            while self.running:
-                # Check for any key presses
-                key = self.get_key()
-                if not self.running:  # Ctrl+C was pressed in get_key()
-                    break
-                key_lower = key.lower() if key else None
-
-                if key:
-                    # Mode selection (immediate, not held)
-                    if key == "0":
-                        cmd.mode = 0
-                        print(f"\rMode: Idle     ", end="", flush=True)
-                    elif key == "1":
-                        cmd.mode = 1
-                        print(f"\rMode: Stand    ", end="", flush=True)
-                    elif key == "2":
-                        cmd.mode = 2
-                        print(f"\rMode: Walk     ", end="", flush=True)
-                    elif key == "6":
-                        cmd.mode = 6
-                        print(f"\rMode: Recovery ", end="", flush=True)
-                    elif key_lower == "q":
-                        print("\nQuitting...")
-                        self.running = False
-                        break  # Break immediately
-                    elif key == " ":  # Space bar = emergency stop
-                        cmd.mode = 0
-                        print(f"\rEMERGENCY STOP ", end="", flush=True)
-
-                # Reset stick values EVERY frame
-                cmd.reset_sticks()
-
-                # Only apply movement for CURRENT key press (no toggle, no persistence)
-                if key_lower == "w":
-                    cmd.ly = 1.0  # Forward
-                if key_lower == "s":
-                    cmd.ly = -1.0  # Backward
-                if key_lower == "a":
-                    cmd.lx = -1.0  # Turn/yaw left
-                if key_lower == "d":
-                    cmd.lx = 1.0  # Turn/yaw right
-                if key_lower == "i":
-                    cmd.ry = 1.0  # Pitch up
-                if key_lower == "k":
-                    cmd.ry = -1.0  # Pitch down
-                if key_lower == "j":
-                    cmd.rx = -1.0  # Roll/strafe left
-                if key_lower == "l":
-                    cmd.rx = 1.0  # Roll/strafe right
-
-                # Send UDP packet at 50Hz
-                if not self.send_command(cmd):
-                    if not self.test_mode:
-                        print("\nFailed to send UDP packet")
-                        break
-
-                # Maintain 50Hz update rate (20ms between packets)
-                time.sleep(0.020)
-
-        except KeyboardInterrupt:
-            print("\nCtrl+C received, shutting down...")
-        except Exception as e:
-            print(f"\nError: {e}")
-        finally:
-            # Send final idle command for safety
-            print("\nSending idle command...")
-            final_cmd = NetworkJoystickCmd()
-            final_cmd.mode = 0
-            for _ in range(5):  # Send multiple times to ensure receipt
-                self.send_command(final_cmd)
-                time.sleep(0.02)
-
-            self.restore_input()
-            print("Terminal restored")
-            print(f"Total packets sent: {self.packet_count}")
-
-    def signal_handler(self, signum, frame):
-        """Handle Ctrl+C gracefully"""
-        self.running = False
-
-
-def main():
-    # Check for test mode
-    test_mode = "--test" in sys.argv
-    if test_mode:
-        sys.argv.remove("--test")
-        # Use dummy values if not provided
-        if len(sys.argv) < 3:
-            sys.argv.extend(["127.0.0.1", "9090"])
-
-    # Check for verbose mode
-    verbose = "--verbose" in sys.argv or "-v" in sys.argv
-    if "--verbose" in sys.argv:
-        sys.argv.remove("--verbose")
-    if "-v" in sys.argv:
-        sys.argv.remove("-v")
-
-    if len(sys.argv) < 3:
-        print(f"Usage: {sys.argv[0]} [--test] [--verbose|-v] <server_ip> <server_port>")
-        print(f"Example: {sys.argv[0]} 192.168.123.220 9090")
-        print(f"Verbose: {sys.argv[0]} 192.168.123.220 9090 --verbose")
-        print(f"Test mode: {sys.argv[0]} --test")
-        return 1
-
-    server_ip = sys.argv[1]
-    server_port = int(sys.argv[2])
-
-    print(f"\n{'=' * 50}")
-    print(f"UDP Joystick Client for Unitree B1")
-    if verbose:
-        print(f"VERBOSE MODE: Showing all packet details")
-    print(f"{'=' * 50}")
-
-    client = JoystickClient(server_ip, server_port, test_mode=test_mode, verbose=verbose)
-
-    # Set up signal handler for clean shutdown
-    signal.signal(signal.SIGINT, client.signal_handler)
-    signal.signal(signal.SIGTERM, client.signal_handler)
-
-    if not client.connect():
-        return 1
-
-    try:
-        client.run_keyboard_control()
-    finally:
-        client.disconnect()
-        if not test_mode:
-            print("UDP socket closed")
-
-    return 0
-
-
-if __name__ == "__main__":
-    sys.exit(main())

From 8044d1b2f9e8e3ca145e92fd39078f77cce89949 Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 05:00:00 -0700
Subject: [PATCH 7/9] Added pygame to sim deps

---
 pyproject.toml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/pyproject.toml b/pyproject.toml
index 645da15a67..9da8a620d0 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -177,6 +177,7 @@ sim = [
     # Simulation
     "mujoco>=3.3.4",
     "playground>=0.0.5",
+    "pygame>=2.6.1",
 ]
 
 jetson-jp6-cuda126 = [

From 6f26681987b828f6aefd187ec022c19d4a547436 Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 05:01:09 -0700
Subject: [PATCH 8/9] Cleanup

---
 dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
index afeb8dfcc9..a81d75cc2f 100644
--- a/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
+++ b/dimos/robot/unitree_webrtc/unitree_b1/unitree_b1.py
@@ -184,7 +184,6 @@ def main():
 
     args = parser.parse_args()
 
-    # Create robot instance
     robot = UnitreeB1(
         ip=args.ip,
         port=args.port,
@@ -209,7 +208,6 @@ def main():
             print("  ESC = Quit pygame (then Ctrl+C to exit)")
             print("=" * 50 + "\n")
 
-            # Keep running until interrupted
             import time
 
             while True:
@@ -221,7 +219,6 @@ def main():
             print("Use robot.move(Twist(...)) to send velocity commands")
             print("Press Ctrl+C to exit\n")
 
-            # Keep running
             import time
 
             while True:

From bb602c3a87f17d8c28f0ab6535497d34bbff2c6b Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Sun, 7 Sep 2025 05:26:35 -0700
Subject: [PATCH 9/9] Added B1 readme

---
 .../robot/unitree_webrtc/unitree_b1/README.md | 202 ++++++++++++++++++
 1 file changed, 202 insertions(+)
 create mode 100644 dimos/robot/unitree_webrtc/unitree_b1/README.md

diff --git a/dimos/robot/unitree_webrtc/unitree_b1/README.md b/dimos/robot/unitree_webrtc/unitree_b1/README.md
new file mode 100644
index 0000000000..ebe59681e1
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/unitree_b1/README.md
@@ -0,0 +1,202 @@
+# Unitree B1 Dimensional Integration
+
+This module provides UDP-based control for the Unitree B1 quadruped robot with DimOS integration with ROS Twist cmd_vel interface.
+
+## Overview
+
+The system consists of two components:
+1. **Server Side**: C++ UDP server running on the B1's internal computer
+2. **Client Side**: Python control module running on external machine
+
+Key features:
+- 50Hz continuous UDP streaming
+- 100ms command timeout for automatic stop
+- Standard Twist velocity interface
+- Emergency stop (Space/Q keys)
+- IDLE/STAND/WALK mode control
+- Optional pygame joystick interface
+
+## Server Side Setup (B1 Internal Computer)
+
+### Prerequisites
+
+The B1 robot runs Ubuntu with the following requirements:
+- Unitree Legged SDK v3.8.3 for B1
+- Boost (>= 1.71.0)
+- CMake (>= 3.16.3)
+- g++ (>= 9.4.0)
+
+### Step 1: Connect to B1 Robot
+
+1. **Connect to B1's WiFi Access Point**:
+   - SSID: `Unitree_B1_XXXXX` (where XXXXX is your robot's ID)
+   - Password: `00000000` (8 zeros)
+
+2. **SSH into the B1**:
+   ```bash
+   ssh unitree@192.168.12.1
+   # Default password: 123
+   ```
+
+### Step 2: Build the UDP Server
+
+1. **Add joystick_server_udp.cpp to CMakeLists.txt**:
+   ```bash
+   # Edit the CMakeLists.txt in the unitree_legged_sdk_B1 directory
+   vim CMakeLists.txt
+   
+   # Add this line with the other add_executable statements:
+   add_executable(joystick_server example/joystick_server_udp.cpp)
+   target_link_libraries(joystick_server ${EXTRA_LIBS})```
+
+2. **Build the server**:
+   ```bash
+   mkdir build
+   cd build
+   cmake ../
+   make
+   ```
+
+### Step 3: Run the UDP Server
+
+```bash
+# Navigate to build directory
+cd build
+
+# Run with sudo for memory locking (required for real-time control)
+sudo ./joystick_server
+
+# You should see:
+# UDP Unitree B1 Joystick Control Server
+# Communication level: HIGH-level
+# Server port: 9090
+# WARNING: Make sure the robot is standing on the ground.
+# Press Enter to continue...
+```
+
+The server will now listen for UDP packets on port 9090 and control the B1 robot.
+
+### Server Safety Features
+
+- **100ms timeout**: Robot stops if no packets received for 100ms
+- **Packet validation**: Only accepts correctly formatted 19-byte packets
+- **Mode restrictions**: Velocities only applied in WALK mode
+- **Emergency stop**: Mode 0 (IDLE) stops all movement
+
+## Client Side Setup (External Machine)
+
+### Prerequisites
+
+- Python 3.10+
+- DimOS framework installed
+- pygame (optional, for joystick control)
+
+### Step 1: Install Dependencies
+
+```bash
+# Install Dimensional
+pip install -e .[cpu,sim]
+```
+
+### Step 2: Connect to B1 Network
+
+1. **Connect your machine to B1's WiFi**:
+   - SSID: `Unitree_B1_XXXXX`
+   - Password: `00000000`
+
+2. **Verify connection**:
+   ```bash
+   ping 192.168.12.1  # Should get responses
+   ```
+
+### Step 3: Run the Client
+
+#### With Joystick Control (Recommended for Testing)
+
+```bash
+python -m dimos.robot.unitree_webrtc.unitree_b1.unitree_b1 \
+    --ip 192.168.12.1 \
+    --port 9090 \
+    --joystick
+```
+
+**Joystick Controls**:
+- `0/1/2` - Switch between IDLE/STAND/WALK modes
+- `WASD` - Move forward/backward, turn left/right (only in WALK mode)
+- `JL` - Strafe left/right (only in WALK mode)
+- `Space/Q` - Emergency stop (switches to IDLE)
+- `ESC` - Quit pygame window
+- `Ctrl+C` - Exit program
+
+#### Test Mode (No Robot Required)
+
+```bash
+python -m dimos.robot.unitree_webrtc.unitree_b1.unitree_b1 \
+    --test \
+    --joystick
+```
+
+This prints commands instead of sending UDP packets - useful for development.
+
+## Safety Features
+
+### Client Side
+- **Command freshness tracking**: Stops sending if no new commands for 100ms
+- **Emergency stop**: Q or Space immediately sets IDLE mode
+- **Mode safety**: Movement only allowed in WALK mode
+- **Graceful shutdown**: Sends stop commands on exit
+
+### Server Side  
+- **Packet timeout**: Robot stops if no packets for 100ms
+- **Continuous monitoring**: Checks timeout before every control update
+- **Safe defaults**: Starts in IDLE mode
+- **Packet validation**: Rejects malformed packets
+
+## Architecture
+
+```
+External Machine (Client)          B1 Robot (Server)
+┌─────────────────────┐           ┌──────────────────┐
+│ Joystick Module     │           │                  │
+│ (pygame input)      │           │ joystick_server  │
+│         ↓           │           │    _udp.cpp      │
+│    Twist msg        │           │                  │
+│         ↓           │  WiFi AP  │                  │
+│ B1ConnectionModule  │◄─────────►│ UDP Port 9090    │
+│ (Twist → B1Command) │ 192.168.  │                  │
+│         ↓           │   12.1    │                  │
+│  UDP packets 50Hz   │           │ Unitree SDK      │
+└─────────────────────┘           └──────────────────┘
+```
+
+## Troubleshooting
+
+### Cannot connect to B1
+- Ensure WiFi connection to B1's AP
+- Check IP: should be `192.168.12.1`
+- Verify server is running: `ssh unitree@192.168.12.1`
+
+### Robot not responding
+- Verify server shows "Client connected" message
+- Check robot is in WALK mode (press '2')
+- Ensure no timeout messages in server output
+
+### Timeout issues
+- Check network latency: `ping 192.168.12.1`
+- Ensure 50Hz sending rate is maintained
+- Look for "Command timeout" messages
+
+### Emergency situations
+- Press Space or Q for immediate stop
+- Use Ctrl+C to exit cleanly
+- Robot auto-stops after 100ms without commands
+
+## Development Notes
+
+- Packets are 19 bytes: 4 floats + uint16 + uint8
+- Coordinate system: B1 uses different conventions, hence negations in `b1_command.py`
+- LCM topics: `/cmd_vel` for Twist, `/b1/mode` for Int32 mode changes
+
+## License
+
+Copyright 2025 Dimensional Inc. Licensed under Apache License 2.0.
\ No newline at end of file