Unitree b1 manipulation pose integration

dimos/robot/unitree_webrtc/unitree_b1/README.md

@@ -61,10 +61,8 @@ The B1 robot runs Ubuntu with the following requirements:
 
 ```bash
 # Navigate to build directory
-cd build
-
-# Run with sudo for memory locking (required for real-time control)
-sudo ./joystick_server
+cd Unitree/sdk/unitree_legged_sdk_B1/build/
+./joystick_server
 
 # You should see:
 # UDP Unitree B1 Joystick Control Server
@@ -199,4 +197,4 @@ External Machine (Client)          B1 Robot (Server)
 
 ## License
 
-Copyright 2025 Dimensional Inc. Licensed under Apache License 2.0.
+Copyright 2025 Dimensional Inc. Licensed under Apache License 2.0.

dimos/robot/unitree_webrtc/unitree_b1/b1_command.py

@@ -51,17 +51,26 @@ def from_twist(cls, twist, mode: int = 2):
         Returns:
             B1Command configured for the given Twist
         """
-        # Only apply velocities in walk mode
-        if mode == 2:
+        if mode == 2:  # WALK mode - velocity control
             return cls(
-                lx=-twist.angular.z,  # ROS rotation → B1 turn (negated for correct direction)
+                lx=-twist.angular.z,  # ROS yaw → 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
+                ry=0.0,  # No pitch control in walk mode
+                mode=mode,
+            )
+        elif mode == 1:  # STAND mode - body pose control
+            # Map Twist pose controls to B1 joystick axes
+            # G1 cpp server maps: ly→bodyHeight, lx→euler[2], rx→euler[0], ry→euler[1]
+            return cls(
+                lx=-twist.angular.z,  # ROS yaw → B1 yaw (euler[2])
+                ly=twist.linear.z,  # ROS height → B1 bodyHeight
+                rx=-twist.angular.x,  # ROS roll → B1 roll (euler[0])
+                ry=twist.angular.y,  # ROS pitch → B1 pitch (euler[1])
                 mode=mode,
             )
         else:
-            # In non-walk modes, don't apply velocities
+            # IDLE mode - no controls
             return cls(mode=mode)
 
     def to_bytes(self) -> bytes:

dimos/robot/unitree_webrtc/unitree_b1/connection.py

@@ -207,8 +207,8 @@ def idle(self):
         return True
 
     @rpc
-    def stand(self):
-        """Set robot to stand mode."""
+    def pose(self):
+        """Set robot to stand/pose mode for reaching ground objects with manipulator."""
         self.set_mode(1)
         return True
 

dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py

@@ -76,8 +76,8 @@ def _pygame_loop(self):
 
         print("JoystickModule started - Focus pygame window to control")
         print("Controls:")
-        print("  WASD = Movement/Turning")
-        print("  IJKL = Strafing/Pitch (in appropriate modes)")
+        print("  Walk Mode: WASD = Move/Turn, JL = Strafe")
+        print("  Stand Mode: WASD = Height/Yaw, JL = Roll, IK = Pitch")
         print("  1/2/0 = Stand/Walk/Idle modes")
         print("  Space/Q = Emergency Stop")
         print("  ESC = Quit (or use Ctrl+C)")
@@ -133,25 +133,50 @@ def _pygame_loop(self):
             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:
+            # Apply controls based on mode
+            if self.current_mode == 2:  # WALK mode - movement control
                 # 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
+                # Turning (A/D)
                 if pygame.K_a in self.keys_held:
-                    twist.angular.z = 1.0  # Turn left (positive angular.z)
+                    twist.angular.z = 1.0  # Turn left
                 if pygame.K_d in self.keys_held:
-                    twist.angular.z = -1.0  # Turn right (negative angular.z)
+                    twist.angular.z = -1.0  # Turn right
 
-                # Strafing (J/L) - note the inversion for correct direction
+                # Strafing (J/L)
                 if pygame.K_j in self.keys_held:
-                    twist.linear.y = 1.0  # Strafe left (positive linear.y)
+                    twist.linear.y = 1.0  # Strafe left
                 if pygame.K_l in self.keys_held:
-                    twist.linear.y = -1.0  # Strafe right (negative linear.y)
+                    twist.linear.y = -1.0  # Strafe right
+
+            elif self.current_mode == 1:  # STAND mode - body pose control
+                # Height control (W/S) - use linear.z for body height
+                if pygame.K_w in self.keys_held:
+                    twist.linear.z = 1.0  # Raise body
+                if pygame.K_s in self.keys_held:
+                    twist.linear.z = -1.0  # Lower body
+
+                # Yaw control (A/D) - use angular.z for body yaw
+                if pygame.K_a in self.keys_held:
+                    twist.angular.z = 1.0  # Rotate body left
+                if pygame.K_d in self.keys_held:
+                    twist.angular.z = -1.0  # Rotate body right
+
+                # Roll control (J/L) - use angular.x for body roll
+                if pygame.K_j in self.keys_held:
+                    twist.angular.x = 1.0  # Roll left
+                if pygame.K_l in self.keys_held:
+                    twist.angular.x = -1.0  # Roll right
+
+                # Pitch control (I/K) - use angular.y for body pitch
+                if pygame.K_i in self.keys_held:
+                    twist.angular.y = 1.0  # Pitch forward
+                if pygame.K_k in self.keys_held:
+                    twist.angular.y = -1.0  # Pitch backward
 
             # Always publish twist at 50Hz (matching working client behavior)
             self.twist_out.publish(twist)
@@ -185,10 +210,12 @@ def _update_display(self, twist):
         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}",
-            "",
+            f"Linear X: {twist.linear.x:+.2f}",
+            f"Linear Y: {twist.linear.y:+.2f}",
+            f"Linear Z: {twist.linear.z:+.2f}",
+            f"Angular X: {twist.angular.x:+.2f}",
+            f"Angular Y: {twist.angular.y:+.2f}",
+            f"Angular Z: {twist.angular.z:+.2f}",
             "Keys: " + ", ".join([pygame.key.name(k).upper() for k in self.keys_held if k < 256]),
         ]
 
@@ -199,8 +226,14 @@ def _update_display(self, twist):
                 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:
+        if (
+            twist.linear.x != 0
+            or twist.linear.y != 0
+            or twist.linear.z != 0
+            or twist.angular.x != 0
+            or twist.angular.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