diff --git a/dimos/robot/unitree_webrtc/unitree_b1/README.md b/dimos/robot/unitree_webrtc/unitree_b1/README.md index ebe59681e1..040a0a6da9 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/README.md +++ b/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. \ No newline at end of file +Copyright 2025 Dimensional Inc. Licensed under Apache License 2.0. diff --git a/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py b/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py index f17fa18882..ac56978e72 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/b1_command.py +++ b/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: diff --git a/dimos/robot/unitree_webrtc/unitree_b1/connection.py b/dimos/robot/unitree_webrtc/unitree_b1/connection.py index 56f684fc98..9cbcd2f7d7 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/connection.py +++ b/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 diff --git a/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py b/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py index f56eeb9b72..e8857e31e2 100644 --- a/dimos/robot/unitree_webrtc/unitree_b1/joystick_module.py +++ b/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