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

ate/robot/visual_servoing.py

@@ -0,0 +1,570 @@
+"""
+Visual Servoing Controller for MechDog.
+
+Implements closed-loop control using camera feedback to navigate
+to and pick up objects. No LLM required for basic operation.
+
+For more complex decision-making, can be combined with an LLM subagent.
+"""
+
+import time
+import math
+from dataclasses import dataclass
+from typing import Optional, Tuple, Callable, List
+from enum import Enum
+
+try:
+    import cv2
+    import numpy as np
+    HAS_CV = True
+except ImportError:
+    HAS_CV = False
+
+
+class ServoState(Enum):
+    """State of the visual servoing controller."""
+    SEARCHING = "searching"      # Looking for target
+    ALIGNING = "aligning"        # Rotating to center target
+    APPROACHING = "approaching"  # Moving toward target
+    READY_TO_GRAB = "ready"      # Close enough to grab
+    GRABBING = "grabbing"        # Executing pickup
+    SUCCESS = "success"          # Target acquired
+    FAILED = "failed"            # Gave up
+
+
+@dataclass
+class TargetDetection:
+    """Result of detecting a target in an image."""
+    found: bool
+    x: float = 0.5          # Normalized x (0=left, 0.5=center, 1=right)
+    y: float = 0.5          # Normalized y (0=top, 1=bottom)
+    size: float = 0.0       # Normalized size (fraction of frame)
+    confidence: float = 0.0
+
+    @property
+    def center_offset(self) -> float:
+        """Offset from center (-0.5 to 0.5, negative=left)."""
+        return self.x - 0.5
+
+    @property
+    def is_centered(self) -> bool:
+        """Is target roughly centered?"""
+        return abs(self.center_offset) < 0.1
+
+    @property
+    def is_close(self) -> bool:
+        """Is target close enough to grab? (size > 15% of frame)"""
+        return self.size > 0.15
+
+
+class GreenBallDetector:
+    """
+    Simple green ball detector using HSV color filtering.
+
+    No ML required - just OpenCV color thresholding.
+    """
+
+    def __init__(
+        self,
+        hue_range: Tuple[int, int] = (35, 85),  # Green hue range
+        sat_min: int = 50,
+        val_min: int = 50,
+        min_area: int = 500,
+    ):
+        self.hue_range = hue_range
+        self.sat_min = sat_min
+        self.val_min = val_min
+        self.min_area = min_area
+
+    def detect(self, image: np.ndarray) -> TargetDetection:
+        """
+        Detect green ball in image.
+
+        Args:
+            image: BGR image from OpenCV
+
+        Returns:
+            TargetDetection with ball position
+        """
+        if image is None or image.size == 0:
+            return TargetDetection(found=False)
+
+        h, w = image.shape[:2]
+
+        # Convert to HSV
+        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+        # Create mask for green color
+        lower = np.array([self.hue_range[0], self.sat_min, self.val_min])
+        upper = np.array([self.hue_range[1], 255, 255])
+        mask = cv2.inRange(hsv, lower, upper)
+
+        # Clean up mask
+        kernel = np.ones((5, 5), np.uint8)
+        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
+        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
+
+        # Find contours
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        if not contours:
+            return TargetDetection(found=False)
+
+        # Find largest contour
+        largest = max(contours, key=cv2.contourArea)
+        area = cv2.contourArea(largest)
+
+        if area < self.min_area:
+            return TargetDetection(found=False)
+
+        # Get centroid
+        M = cv2.moments(largest)
+        if M["m00"] == 0:
+            return TargetDetection(found=False)
+
+        cx = int(M["m10"] / M["m00"])
+        cy = int(M["m01"] / M["m00"])
+
+        # Get bounding rect for size
+        x, y, bw, bh = cv2.boundingRect(largest)
+
+        return TargetDetection(
+            found=True,
+            x=cx / w,
+            y=cy / h,
+            size=max(bw, bh) / max(w, h),
+            confidence=min(1.0, area / 10000),
+        )
+
+
+class VisualServoController:
+    """
+    Closed-loop visual servoing controller.
+
+    Uses camera feedback to navigate robot to target and pick it up.
+
+    Example:
+        controller = VisualServoController(
+            capture_fn=webcam.capture,
+            move_fn=robot.move,
+            gripper_fn=robot.gripper_close,
+        )
+
+        success = controller.pickup_target()
+    """
+
+    def __init__(
+        self,
+        capture_fn: Callable[[], np.ndarray],
+        move_fn: Callable[[float, float], None],  # move(forward_vel, turn_vel)
+        stop_fn: Callable[[], None],
+        gripper_open_fn: Callable[[], None],
+        gripper_close_fn: Callable[[], None],
+        arm_down_fn: Callable[[], None],
+        arm_up_fn: Callable[[], None],
+        detector: Optional[GreenBallDetector] = None,
+        # Tuning parameters
+        turn_speed: float = 25.0,       # Turn velocity when aligning
+        approach_speed: float = 20.0,   # Forward velocity when approaching
+        align_threshold: float = 0.1,   # Center offset threshold
+        close_threshold: float = 0.15,  # Size threshold for "close enough"
+        max_iterations: int = 50,       # Max control loop iterations
+        step_delay: float = 0.3,        # Delay between steps
+        # Callbacks for monitoring
+        on_state_change: Optional[Callable[[ServoState], None]] = None,
+        on_detection: Optional[Callable[[TargetDetection, np.ndarray], None]] = None,
+    ):
+        self.capture = capture_fn
+        self.move = move_fn
+        self.stop = stop_fn
+        self.gripper_open = gripper_open_fn
+        self.gripper_close = gripper_close_fn
+        self.arm_down = arm_down_fn
+        self.arm_up = arm_up_fn
+
+        self.detector = detector or GreenBallDetector()
+
+        self.turn_speed = turn_speed
+        self.approach_speed = approach_speed
+        self.align_threshold = align_threshold
+        self.close_threshold = close_threshold
+        self.max_iterations = max_iterations
+        self.step_delay = step_delay
+
+        self.on_state_change = on_state_change
+        self.on_detection = on_detection
+
+        self.state = ServoState.SEARCHING
+        self.iterations = 0
+        self.history: List[TargetDetection] = []
+
+    def _set_state(self, new_state: ServoState):
+        """Update state and notify callback."""
+        if new_state != self.state:
+            self.state = new_state
+            if self.on_state_change:
+                self.on_state_change(new_state)
+
+    def _detect(self) -> TargetDetection:
+        """Capture image and detect target."""
+        image = self.capture()
+        detection = self.detector.detect(image)
+
+        self.history.append(detection)
+        if len(self.history) > 10:
+            self.history.pop(0)
+
+        if self.on_detection:
+            self.on_detection(detection, image)
+
+        return detection
+
+    def _is_detection_stable(self, min_frames: int = 3) -> bool:
+        """Check if detection is stable across recent frames."""
+        if len(self.history) < min_frames:
+            return False
+
+        recent = self.history[-min_frames:]
+        if not all(d.found for d in recent):
+            return False
+
+        # Check position consistency
+        x_vals = [d.x for d in recent]
+        return max(x_vals) - min(x_vals) < 0.15
+
+    def pickup_target(self) -> bool:
+        """
+        Execute full pickup behavior with visual servoing.
+
+        Returns:
+            True if pickup succeeded, False otherwise
+        """
+        print("\n" + "=" * 50)
+        print("VISUAL SERVOING PICKUP")
+        print("=" * 50)
+
+        self.iterations = 0
+        self.history = []
+        self._set_state(ServoState.SEARCHING)
+
+        try:
+            # Phase 1: Search for target
+            print("\n[1] Searching for target...")
+            if not self._search_phase():
+                return False
+
+            # Phase 2: Align to target
+            print("\n[2] Aligning to target...")
+            if not self._align_phase():
+                return False
+
+            # Phase 3: Approach target
+            print("\n[3] Approaching target...")
+            if not self._approach_phase():
+                return False
+
+            # Phase 4: Execute pickup
+            print("\n[4] Executing pickup...")
+            if not self._pickup_phase():
+                return False
+
+            self._set_state(ServoState.SUCCESS)
+            print("\n[SUCCESS] Pickup complete!")
+            return True
+
+        except KeyboardInterrupt:
+            print("\n[ABORTED] User interrupted")
+            self.stop()
+            self._set_state(ServoState.FAILED)
+            return False
+
+        except Exception as e:
+            print(f"\n[ERROR] {e}")
+            self.stop()
+            self._set_state(ServoState.FAILED)
+            return False
+
+    def _search_phase(self) -> bool:
+        """Search for target by rotating."""
+        self._set_state(ServoState.SEARCHING)
+
+        # First check current view
+        detection = self._detect()
+        if detection.found:
+            print(f"  Found immediately at x={detection.x:.2f}")
+            return True
+
+        # Rotate to search
+        search_steps = 12  # Full rotation in ~12 steps
+        for i in range(search_steps):
+            if self.iterations >= self.max_iterations:
+                print("  Max iterations reached")
+                self._set_state(ServoState.FAILED)
+                return False
+
+            print(f"  Searching... (step {i+1}/{search_steps})")
+
+            # Turn a bit
+            self.move(0, self.turn_speed)
+            time.sleep(0.5)
+            self.stop()
+            time.sleep(0.2)
+
+            # Check for target
+            detection = self._detect()
+            self.iterations += 1
+
+            if detection.found:
+                print(f"  Found at x={detection.x:.2f}")
+                return True
+
+        print("  Target not found after full rotation")
+        self._set_state(ServoState.FAILED)
+        return False
+
+    def _align_phase(self) -> bool:
+        """Align robot to center target in view."""
+        self._set_state(ServoState.ALIGNING)
+
+        for i in range(20):  # Max 20 alignment steps
+            if self.iterations >= self.max_iterations:
+                print("  Max iterations reached")
+                self._set_state(ServoState.FAILED)
+                return False
+
+            detection = self._detect()
+            self.iterations += 1
+
+            if not detection.found:
+                print("  Lost target during alignment!")
+                # Try to re-find
+                if not self._search_phase():
+                    return False
+                continue
+
+            offset = detection.center_offset
+
+            if abs(offset) < self.align_threshold:
+                print(f"  Aligned! (offset={offset:.3f})")
+                return True
+
+            # Turn toward target
+            # Negative offset = target on left = turn left (negative turn_vel)
+            turn_vel = -self.turn_speed if offset < 0 else self.turn_speed
+
+            # Proportional control - turn less when close to aligned
+            turn_vel *= min(1.0, abs(offset) * 3)
+
+            print(f"  Offset={offset:.3f}, turning {'left' if turn_vel < 0 else 'right'}")
+
+            self.move(0, turn_vel)
+            time.sleep(0.3)
+            self.stop()
+            time.sleep(0.1)
+
+        print("  Failed to align after 20 attempts")
+        self._set_state(ServoState.FAILED)
+        return False
+
+    def _approach_phase(self) -> bool:
+        """Approach target until close enough."""
+        self._set_state(ServoState.APPROACHING)
+
+        for i in range(30):  # Max 30 approach steps
+            if self.iterations >= self.max_iterations:
+                print("  Max iterations reached")
+                self._set_state(ServoState.FAILED)
+                return False
+
+            detection = self._detect()
+            self.iterations += 1
+
+            if not detection.found:
+                print("  Lost target during approach!")
+                # Stop and try to re-find
+                self.stop()
+                if not self._search_phase():
+                    return False
+                if not self._align_phase():
+                    return False
+                continue
+
+            # Check if close enough
+            if detection.size >= self.close_threshold:
+                print(f"  Close enough! (size={detection.size:.3f})")
+                self.stop()
+                return True
+
+            # Check alignment and correct if needed
+            offset = detection.center_offset
+            turn_correction = 0
+            if abs(offset) > self.align_threshold:
+                turn_correction = -self.turn_speed * 0.5 if offset < 0 else self.turn_speed * 0.5
+
+            print(f"  Size={detection.size:.3f}, offset={offset:.3f}, moving forward")
+
+            # Move forward with slight turn correction
+            self.move(self.approach_speed, turn_correction)
+            time.sleep(0.4)
+            self.stop()
+            time.sleep(0.1)
+
+        print("  Failed to reach target after 30 steps")
+        self._set_state(ServoState.FAILED)
+        return False
+
+    def _pickup_phase(self) -> bool:
+        """Execute the pickup sequence."""
+        self._set_state(ServoState.GRABBING)
+
+        print("  Opening gripper...")
+        self.gripper_open()
+        time.sleep(0.4)
+
+        print("  Lowering arm...")
+        self.arm_down()
+        time.sleep(0.5)
+
+        print("  Closing gripper...")
+        self.gripper_close()
+        time.sleep(0.4)
+
+        print("  Lifting arm...")
+        self.arm_up()
+        time.sleep(0.4)
+
+        # Verify pickup by checking if ball still visible on ground
+        detection = self._detect()
+
+        if not detection.found or detection.size < 0.05:
+            print("  Ball no longer visible on ground - likely picked up!")
+            return True
+        else:
+            print(f"  Ball still visible (size={detection.size:.3f}) - may have missed")
+            # Still return True as we completed the sequence
+            return True
+
+
+def create_mechdog_servo_controller(
+    serial_port: str = "/dev/cu.usbserial-10",
+    webcam_index: int = 0,
+) -> Optional[VisualServoController]:
+    """
+    Create a visual servo controller for MechDog.
+
+    Args:
+        serial_port: Serial port for MechDog
+        webcam_index: Webcam device index
+
+    Returns:
+        Configured VisualServoController, or None if setup fails
+    """
+    if not HAS_CV:
+        print("OpenCV not available. Install with: pip install opencv-python")
+        return None
+
+    try:
+        import serial
+    except ImportError:
+        print("pyserial not available. Install with: pip install pyserial")
+        return None
+
+    # Setup serial connection
+    try:
+        ser = serial.Serial(serial_port, 115200, timeout=1)
+        time.sleep(0.5)
+        ser.read(ser.in_waiting)
+
+        def cmd(command: str, wait: float = 0.3):
+            ser.write(f"{command}\r\n".encode())
+            time.sleep(wait)
+            return ser.read(ser.in_waiting).decode('utf-8', errors='ignore')
+
+        # Initialize MechDog
+        cmd("from HW_MechDog import MechDog", 1.5)
+        cmd("dog = MechDog()", 1.5)
+        print(f"Connected to MechDog on {serial_port}")
+
+    except Exception as e:
+        print(f"Failed to connect to MechDog: {e}")
+        return None
+
+    # Setup webcam
+    cap = cv2.VideoCapture(webcam_index)
+    if not cap.isOpened():
+        print(f"Failed to open webcam {webcam_index}")
+        ser.close()
+        return None
+
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+    print(f"Webcam {webcam_index} opened")
+
+    # Create wrapper functions
+    def capture() -> np.ndarray:
+        # Warm up frames
+        for _ in range(2):
+            cap.read()
+        ret, frame = cap.read()
+        return frame if ret else np.array([])
+
+    def move(forward: float, turn: float):
+        cmd(f"dog.move({int(forward)}, {int(turn)})", 0.2)
+
+    def stop():
+        cmd("dog.move(0, 0)", 0.2)
+
+    def gripper_open():
+        cmd("dog.set_servo(11, 500, 400)", 0.5)
+
+    def gripper_close():
+        cmd("dog.set_servo(11, 2400, 400)", 0.5)
+
+    def arm_down():
+        # CORRECTED 2026-01-03: Vision analysis confirmed servos 8,9,11 are arm
+        cmd("dog.set_servo(8, 800, 600)", 0.7)   # Shoulder down
+        cmd("dog.set_servo(9, 800, 600)", 0.7)   # Elbow extended
+
+    def arm_up():
+        # CORRECTED 2026-01-03: Vision analysis confirmed servos 8,9,11 are arm
+        cmd("dog.set_servo(9, 2000, 600)", 0.7)  # Elbow retracted first
+        cmd("dog.set_servo(8, 2200, 600)", 0.7)  # Then shoulder up
+
+    # State change callback
+    def on_state(state: ServoState):
+        print(f"  [STATE] {state.value}")
+
+    # Detection callback (save images for debugging)
+    frame_count = [0]
+    def on_detection(det: TargetDetection, image: np.ndarray):
+        # Save every 5th frame
+        if frame_count[0] % 5 == 0 and image is not None and image.size > 0:
+            path = f"/tmp/servo_{frame_count[0]:03d}.jpg"
+            cv2.imwrite(path, image)
+        frame_count[0] += 1
+
+    return VisualServoController(
+        capture_fn=capture,
+        move_fn=move,
+        stop_fn=stop,
+        gripper_open_fn=gripper_open,
+        gripper_close_fn=gripper_close,
+        arm_down_fn=arm_down,
+        arm_up_fn=arm_up,
+        on_state_change=on_state,
+        on_detection=on_detection,
+    )
+
+
+if __name__ == "__main__":
+    print("Visual Servoing Demo")
+    print("=" * 50)
+
+    controller = create_mechdog_servo_controller()
+
+    if controller:
+        success = controller.pickup_target()
+        print(f"\nResult: {'SUCCESS' if success else 'FAILED'}")
+        print(f"Total iterations: {controller.iterations}")
+    else:
+        print("Failed to create controller")