foodforthought-cli 0.2.8__py3-none-any.whl → 0.3.0__py3-none-any.whl

ate/drivers/mechdog.py

@@ -36,6 +36,7 @@ from ..interfaces.types import (
     RobotMode,
     BatteryState,
     IMUReading,
+    Image,
 )
 from ..interfaces.base import (
     RobotInterface,
@@ -45,6 +46,9 @@ from ..interfaces.base import (
 )
 from ..interfaces.locomotion import QuadrupedLocomotion
 from ..interfaces.body import BodyPoseInterface
+from ..interfaces.perception import CameraInterface
+from ..interfaces.sensors import DistanceSensorInterface, DistanceReading, DistanceSensorType
+from .wifi_camera import WiFiCamera, WiFiCameraConfig
 
 
 # =============================================================================
@@ -70,6 +74,30 @@ HEIGHT_DEFAULT = 0.12
 MAX_STRIDE_MM = 100   # Maximum stride in mm
 MAX_TURN_DEG = 30     # Maximum turn rate in degrees
 
+# Arm servo IDs (for models with arm attachment)
+# CORRECTED 2026-01-03: Discovered via vision-based servo identification
+# Vision diff analysis showed:
+# - Servos 1,2,4,5,6,7 are LEG servos (large movement, body tilt)
+# - Servos 8,9 are ARM servos (medium movement, arm only)
+# - Servo 11 is GRIPPER (small, localized movement at tip)
+SERVO_GRIPPER = 11    # Gripper servo (open/close)
+SERVO_SHOULDER = 8    # Shoulder/arm raise-lower
+SERVO_ELBOW = 9       # Elbow extend/retract
+
+# Calibrated arm servo positions (PWM microseconds: 500-2500)
+# Discovered empirically 2026-01-03 via systematic testing
+GRIPPER_OPEN = 500        # Gripper jaws spread wide
+GRIPPER_CLOSED = 2400     # Gripper jaws clamped
+GRIPPER_NEUTRAL = 1500    # Gripper partially open
+
+SHOULDER_UP = 2200        # Arm raised up (servo 4 high)
+SHOULDER_DOWN = 800       # Arm lowered for pickup (servo 4 low)
+SHOULDER_NEUTRAL = 1500   # Arm at middle height
+
+ELBOW_RETRACTED = 2000    # Arm pulled in toward body (servo 5 high)
+ELBOW_EXTENDED = 800      # Arm reaching forward (servo 5 low)
+ELBOW_NEUTRAL = 1500      # Arm at middle extension
+
 
 @dataclass
 class MechDogConfig:
@@ -78,10 +106,21 @@ class MechDogConfig:
     baud_rate: int = 115200
     timeout: float = 2.0
     has_arm: bool = False
+    # Camera configuration
     has_camera: bool = False
-
-
-class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, RobotInterface):
+    camera_ip: str = ""           # IP of visual module (e.g., "192.168.1.100")
+    camera_port: int = 80         # HTTP port
+    camera_stream_port: int = 81  # MJPEG stream port
+    # Ultrasonic sensor (HC-SR04 style on ESP32)
+    has_ultrasonic: bool = True   # MechDog has built-in ultrasonic
+    # BLE configuration (alternative to USB serial)
+    use_ble: bool = False         # Use BLE instead of serial
+    ble_address: str = ""         # BLE device address (MAC on Windows/Linux, UUID on macOS)
+    ble_service_uuid: str = "0000ffe0-0000-1000-8000-00805f9b34fb"  # HM-10/ESP32 BLE serial
+    ble_char_uuid: str = "0000ffe1-0000-1000-8000-00805f9b34fb"     # RX/TX characteristic
+
+
+class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, CameraInterface, DistanceSensorInterface, RobotInterface):
     """
     Driver for HiWonder MechDog quadruped robot.
 
@@ -89,6 +128,8 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
     - QuadrupedLocomotion: walk, turn, stand, sit, etc.
     - BodyPoseInterface: height, roll, pitch, yaw control
     - SafetyInterface: emergency stop, battery monitoring
+    - CameraInterface: image capture from visual module (optional)
+    - DistanceSensorInterface: ultrasonic distance sensing
     - RobotInterface: connection lifecycle, status
 
     Example:
@@ -99,6 +140,9 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         dog.walk(Vector3.forward(), speed=0.3)
         dog.set_body_height(0.10)
 
+        # If camera is configured:
+        image = dog.get_image()
+
         dog.disconnect()
     """
 
@@ -107,11 +151,12 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         Initialize MechDog driver.
 
         Args:
-            port: Serial port for MechDog connection
+            port: Serial port or BLE address for MechDog connection
             config: Optional configuration
         """
         self.config = config or MechDogConfig(port=port)
         self._serial: Optional[serial.Serial] = None
+        self._ble_transport = None  # BLE transport when use_ble=True
         self._connected = False
         self._estopped = False
         self._current_gait = GaitType.WALK
@@ -125,6 +170,23 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         self._pose = Pose.identity()
         self._velocity = Twist.zero()
 
+        # Camera (visual module)
+        self._camera: Optional[WiFiCamera] = None
+        if self.config.has_camera and self.config.camera_ip:
+            camera_config = WiFiCameraConfig(
+                ip=self.config.camera_ip,
+                port=self.config.camera_port,
+                stream_port=self.config.camera_stream_port,
+            )
+            self._camera = WiFiCamera(camera_config)
+
+    @property
+    def _transport(self):
+        """Get the active transport (serial or BLE)."""
+        if self.config.use_ble and self._ble_transport:
+            return self._ble_transport
+        return self._serial
+
     # =========================================================================
     # RobotInterface implementation
     # =========================================================================
@@ -144,6 +206,9 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         if self.config.has_camera:
             capabilities.add(Capability.CAMERA)
 
+        if self.config.has_ultrasonic:
+            capabilities.add(Capability.DISTANCE_SENSOR)
+
         return RobotInfo(
             name="MechDog",
             model="hiwonder_mechdog",
@@ -159,7 +224,58 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         )
 
     def connect(self) -> ActionResult:
-        """Connect to MechDog via serial."""
+        """Connect to MechDog via serial or BLE."""
+        if self.config.use_ble:
+            return self._connect_ble()
+        return self._connect_serial()
+
+    def _connect_ble(self) -> ActionResult:
+        """Connect to MechDog via Bluetooth Low Energy."""
+        try:
+            from .ble_transport import BLETransport
+        except ImportError:
+            return ActionResult.error("BLE transport requires bleak. Run: pip install bleak")
+
+        if not self.config.ble_address:
+            return ActionResult.error("BLE address not configured. Set config.ble_address")
+
+        try:
+            self._ble_transport = BLETransport(
+                address=self.config.ble_address,
+                service_uuid=self.config.ble_service_uuid,
+                char_uuid=self.config.ble_char_uuid,
+                timeout=self.config.timeout,
+            )
+
+            if not self._ble_transport.connect():
+                return ActionResult.error("Failed to connect to BLE device")
+
+            time.sleep(0.5)
+
+            # Enter friendly REPL mode
+            self._ble_transport.write(b'\x03')  # Ctrl+C to interrupt
+            time.sleep(0.3)
+            self._ble_transport.write(b'\x02')  # Ctrl+B for friendly REPL
+            time.sleep(0.5)
+            self._ble_transport.reset_input_buffer()
+
+            # Initialize MechDog library
+            result = self._send_command("from HW_MechDog import MechDog")
+            if "Error" in result or "Traceback" in result:
+                return ActionResult.error(f"Failed to import HW_MechDog: {result}")
+
+            result = self._send_command("dog = MechDog()")
+            if "Error" in result or "Traceback" in result:
+                return ActionResult.error(f"Failed to create MechDog: {result}")
+
+            self._connected = True
+            return ActionResult.ok(f"Connected to MechDog via BLE ({self.config.ble_address})")
+
+        except Exception as e:
+            return ActionResult.error(f"BLE connection failed: {e}")
+
+    def _connect_serial(self) -> ActionResult:
+        """Connect to MechDog via USB serial."""
         if not HAS_SERIAL:
             return ActionResult.error("pyserial not installed. Run: pip install pyserial")
 
@@ -195,22 +311,42 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
 
     def disconnect(self) -> ActionResult:
         """Disconnect from MechDog."""
+        try:
+            # Stop any movement first
+            self.stop()
+        except Exception:
+            pass
+
+        # Close BLE transport if active
+        if self._ble_transport:
+            try:
+                self._ble_transport.close()
+            except Exception:
+                pass
+            finally:
+                self._ble_transport = None
+
+        # Close serial if active
         if self._serial:
             try:
-                # Stop any movement first
-                self.stop()
                 self._serial.close()
             except Exception:
                 pass
             finally:
                 self._serial = None
-                self._connected = False
 
+        self._connected = False
         return ActionResult.ok("Disconnected")
 
     def is_connected(self) -> bool:
         """Check connection status."""
-        return self._connected and self._serial is not None and self._serial.is_open
+        if not self._connected:
+            return False
+
+        if self.config.use_ble:
+            return self._ble_transport is not None and self._ble_transport.is_open
+
+        return self._serial is not None and self._serial.is_open
 
     def get_status(self) -> RobotStatus:
         """Get robot status."""
@@ -239,7 +375,7 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         self._estopped = True
         self._is_moving = False
 
-        if self._serial:
+        if self._transport:
             # Send stop command
             self._send_command("dog.move(0, 0)")
 
@@ -259,6 +395,119 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         # MechDog doesn't report battery state via REPL
         return None
 
+    # =========================================================================
+    # CameraInterface implementation
+    # =========================================================================
+
+    def get_image(self) -> Image:
+        """
+        Capture image from visual module.
+
+        Returns:
+            Image with RGB data, or empty Image if camera not configured
+        """
+        if not self._camera:
+            return Image(
+                data=b"",
+                width=0,
+                height=0,
+                format="error",
+                encoding="none",
+                timestamp=time.time(),
+                metadata={"error": "Camera not configured"},
+            )
+
+        return self._camera.get_image()
+
+    def get_resolution(self) -> Tuple[int, int]:
+        """Get camera resolution."""
+        if not self._camera:
+            return (0, 0)
+        return self._camera.get_resolution()
+
+    def get_intrinsics(self) -> dict:
+        """Get camera intrinsic parameters."""
+        if not self._camera:
+            return {}
+        return self._camera.get_intrinsics()
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID for camera."""
+        return "mechdog_camera_link"
+
+    def has_camera(self) -> bool:
+        """Check if camera is available."""
+        return self._camera is not None
+
+    def is_camera_connected(self) -> bool:
+        """Check if camera is reachable."""
+        if not self._camera:
+            return False
+        return self._camera.is_connected()
+
+    # =========================================================================
+    # DistanceSensorInterface implementation
+    # =========================================================================
+
+    def get_distance(self) -> DistanceReading:
+        """
+        Read distance from ultrasonic sensor.
+
+        MechDog's ultrasonic sensor is mounted on the front.
+        Returns distance in meters.
+        """
+        if not self._check_connection():
+            return DistanceReading.invalid()
+
+        if not self.config.has_ultrasonic:
+            return DistanceReading.invalid()
+
+        try:
+            # MechDog ultrasonic reading via HW_MechDog library
+            # The sensor returns distance in centimeters
+            result = self._send_command("print(dog.read_ultrasonic())")
+
+            # Parse result - typically returns just a number
+            import re
+            match = re.search(r'(\d+\.?\d*)', result.split('\n')[-2] if '\n' in result else result)
+            if match:
+                distance_cm = float(match.group(1))
+                distance_m = distance_cm / 100.0
+
+                # Validate reading is in range
+                if distance_m < self.get_min_range() or distance_m > self.get_max_range():
+                    return DistanceReading(
+                        distance=distance_m,
+                        valid=False,
+                        sensor_type=DistanceSensorType.ULTRASONIC,
+                        timestamp=time.time(),
+                        confidence=0.5,
+                    )
+
+                return DistanceReading(
+                    distance=distance_m,
+                    valid=True,
+                    sensor_type=DistanceSensorType.ULTRASONIC,
+                    timestamp=time.time(),
+                    confidence=0.9,
+                )
+        except Exception:
+            pass
+
+        return DistanceReading.invalid()
+
+    def get_min_range(self) -> float:
+        """Get minimum measurable range (2cm for HC-SR04)."""
+        return 0.02  # 2cm
+
+    def get_max_range(self) -> float:
+        """Get maximum measurable range (4m for HC-SR04)."""
+        return 4.0  # 4m
+
+    def get_sensor_type(self) -> DistanceSensorType:
+        """Get the type of distance sensor."""
+        return DistanceSensorType.ULTRASONIC
+
     # =========================================================================
     # QuadrupedLocomotion implementation
     # =========================================================================
@@ -294,12 +543,13 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         if self._estopped:
             return ActionResult.error("Emergency stop active")
 
-        # Convert direction and speed to MechDog stride/angle
-        # MechDog uses: move(stride_mm, angle_deg)
-        # stride: -100 to +100 (negative = backward)
-        # angle: -30 to +30 (negative = turn right)
+        # Convert direction and speed to MechDog velocity control
+        # MechDog uses: move(forward_vel, turn_vel)
+        # forward_vel: -100 to +100 (negative = backward)
+        # turn_vel: -100 to +100 (negative = turn left, positive = turn right)
+        # Note: This is velocity-based, NOT distance-based!
 
-        # Calculate stride from speed (simple mapping)
+        # Calculate forward velocity from speed (simple mapping)
         stride = int(min(MAX_STRIDE_MM, max(-MAX_STRIDE_MM, speed * 200)))
 
         # Apply direction
@@ -553,18 +803,21 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
 
     def _check_connection(self) -> bool:
         """Check if connected and ready."""
-        return self._connected and self._serial is not None and not self._estopped
+        if self._estopped:
+            return False
+        return self._connected and self._transport is not None
 
     def _send_command(self, cmd: str, wait: float = 0.5) -> str:
         """Send command to MicroPython REPL and get response."""
-        if not self._serial:
+        transport = self._transport
+        if not transport:
             return ""
 
         try:
-            self._serial.reset_input_buffer()
-            self._serial.write(f"{cmd}\r\n".encode())
+            transport.reset_input_buffer()
+            transport.write(f"{cmd}\r\n".encode())
             time.sleep(wait)
-            response = self._serial.read(4000).decode('utf-8', errors='ignore')
+            response = transport.read(4000).decode('utf-8', errors='ignore')
             return response
         except Exception as e:
             return f"Error: {e}"
@@ -597,10 +850,93 @@ class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface, Rob
         self._send_command(f"dog.set_servo({servo_id}, {position}, {time_ms})")
         return ActionResult.ok(f"Servo {servo_id} set to {position}")
 
-    def run_action(self, action_name: str) -> ActionResult:
-        """Run a pre-programmed action."""
+    def run_action(self, action_id: int) -> ActionResult:
+        """
+        Run a pre-programmed action by ID.
+
+        Args:
+            action_id: Integer ID of the action to run
+
+        Note: Action IDs must be discovered experimentally as
+        the MechDog firmware doesn't document available actions.
+        Common IDs found: 0-15 (varies by firmware version)
+        """
+        if not self._check_connection():
+            return ActionResult.error("Not connected")
+
+        self._send_command(f"dog.action_run({action_id})")
+        return ActionResult.ok(f"Running action ID: {action_id}")
+
+    def wait_action(self) -> ActionResult:
+        """Wait for current action to complete."""
         if not self._check_connection():
             return ActionResult.error("Not connected")
 
-        self._send_command(f"dog.action_run('{action_name}')")
-        return ActionResult.ok(f"Running action: {action_name}")
+        self._send_command("dog.wait_action_over()")
+        return ActionResult.ok("Action complete")
+
+    def stop_action(self) -> ActionResult:
+        """Stop current action."""
+        if not self._check_connection():
+            return ActionResult.error("Not connected")
+
+        self._send_command("dog.stop_action()")
+        return ActionResult.ok("Action stopped")
+
+    # =========================================================================
+    # Arm control methods (for models with arm attachment)
+    # =========================================================================
+
+    def gripper_open(self, time_ms: int = 400) -> ActionResult:
+        """Open gripper to grab position."""
+        return self.set_servo(SERVO_GRIPPER, GRIPPER_OPEN, time_ms)
+
+    def gripper_close(self, time_ms: int = 400) -> ActionResult:
+        """Close gripper to grasp object."""
+        return self.set_servo(SERVO_GRIPPER, GRIPPER_CLOSED, time_ms)
+
+    def arm_up(self, time_ms: int = 600) -> ActionResult:
+        """Raise arm to carry position."""
+        self.set_servo(SERVO_ELBOW, ELBOW_RETRACTED, time_ms)
+        return self.set_servo(SERVO_SHOULDER, SHOULDER_UP, time_ms)
+
+    def arm_down(self, time_ms: int = 600) -> ActionResult:
+        """Lower arm to pickup position."""
+        self.set_servo(SERVO_SHOULDER, SHOULDER_DOWN, time_ms)
+        return self.set_servo(SERVO_ELBOW, ELBOW_EXTENDED, time_ms)
+
+    def arm_neutral(self, time_ms: int = 500) -> ActionResult:
+        """Move arm to neutral position."""
+        self.set_servo(SERVO_GRIPPER, GRIPPER_NEUTRAL, time_ms)
+        self.set_servo(SERVO_SHOULDER, SHOULDER_NEUTRAL, time_ms)
+        return self.set_servo(SERVO_ELBOW, ELBOW_NEUTRAL, time_ms)
+
+    def pickup_sequence(self, grab_delay: float = 0.3) -> ActionResult:
+        """
+        Execute pickup sequence: lower arm, close gripper, lift.
+
+        Args:
+            grab_delay: Seconds to wait between lowering and closing gripper
+
+        Returns:
+            ActionResult indicating success/failure
+        """
+        if not self.config.has_arm:
+            return ActionResult.error("Robot does not have arm attachment")
+
+        # Open gripper
+        self.gripper_open()
+        time.sleep(0.3)
+
+        # Lower arm
+        self.arm_down()
+        time.sleep(grab_delay)
+
+        # Close gripper
+        self.gripper_close()
+        time.sleep(0.3)
+
+        # Lift arm
+        self.arm_up()
+
+        return ActionResult.ok("Pickup sequence complete")