Unitree b1 integration with continuous cmd_vel Twist interface, joystick control for testing, C++ UDP server for onboard B1

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

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"]

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.

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"]

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