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

ate/robot/visual_servo_loop.py

@@ -0,0 +1,494 @@
+"""
+Visual Servoing Framework
+
+A reusable feedback loop that uses direction calibration to iteratively
+move toward a target. This is the "Model-Free Adaptive IBVS" approach
+recommended by research.
+
+The Problem This Solves:
+-----------------------
+Direction calibration tells us WHICH WAY to move, but not HOW FAR.
+Open-loop commands (set servo to X) fail if the target position is wrong.
+
+Visual servoing solves this by:
+1. Measure current distance to target
+2. Move a small step in the "toward" direction
+3. Measure new distance
+4. Repeat until close enough (or stuck)
+
+This is robot-agnostic and task-agnostic. Any task that needs to
+approach a visual target can use this framework.
+
+Usage:
+    from ate.robot.visual_servo_loop import VisualServoLoop, ServoConfig
+
+    # Configure servos with their direction calibration
+    servos = [
+        ServoConfig(id=9, toward="positive", min=800, max=2200, step=50),
+        ServoConfig(id=8, toward="negative", min=800, max=2200, step=30),
+    ]
+
+    # Create the loop
+    loop = VisualServoLoop(
+        servos=servos,
+        target_detector=detect_ball,  # function(frame) -> (x, y) or None
+        effector_detector=detect_gripper,  # function(frame) -> (x, y) or None
+        move_servo=robot.set_servo,  # function(id, position, duration)
+        capture_frame=camera.read,  # function() -> frame
+    )
+
+    # Run until converged or max iterations
+    result = loop.run(
+        threshold_px=50,  # Stop when within 50 pixels
+        max_iterations=20,
+    )
+"""
+
+import time
+import numpy as np
+from dataclasses import dataclass, field
+from typing import Callable, Optional, List, Dict, Tuple, Any
+from enum import Enum
+from datetime import datetime
+import json
+from pathlib import Path
+
+
+class ServoRole(Enum):
+    """Semantic role of a servo in approaching a target."""
+    HORIZONTAL = "horizontal"  # Moves effector left/right
+    VERTICAL = "vertical"      # Moves effector up/down
+    DEPTH = "depth"           # Moves effector forward/back
+    GRIPPER = "gripper"       # Opens/closes gripper
+    UNKNOWN = "unknown"
+
+
+@dataclass
+class ServoConfig:
+    """Configuration for a servo in the visual servo loop."""
+    id: int
+    toward_direction: str  # "positive" or "negative"
+    min_value: int = 500
+    max_value: int = 2500
+    current_value: int = 1500
+    step_size: int = 50  # PWM units per iteration
+    role: ServoRole = ServoRole.UNKNOWN
+    enabled: bool = True  # Can disable specific servos
+    confidence: float = 1.0  # From direction calibration
+
+
+@dataclass
+class ServoState:
+    """Tracks the state of a servo during servoing."""
+    config: ServoConfig
+    initial_value: int
+    current_value: int
+    moves_made: int = 0
+    last_delta: float = 0.0  # Last measured improvement
+    stuck_count: int = 0  # How many times we've made no progress
+
+
+@dataclass
+class ServoIteration:
+    """Record of a single iteration of the servo loop."""
+    iteration: int
+    timestamp: str
+    target_pos: Optional[Tuple[int, int]]
+    effector_pos: Optional[Tuple[int, int]]
+    distance_px: float
+    servo_moves: Dict[int, int]  # servo_id -> new_value
+    delta_distance: float  # Change from last iteration
+
+
+@dataclass
+class ServoResult:
+    """Result of a visual servo run."""
+    success: bool
+    reason: str  # "converged", "max_iterations", "target_lost", "stuck"
+    final_distance_px: float
+    iterations: int
+    total_time_s: float
+    history: List[ServoIteration] = field(default_factory=list)
+    servo_states: Dict[int, ServoState] = field(default_factory=dict)
+
+
+class VisualServoLoop:
+    """
+    Feedback loop that uses direction calibration to approach a target.
+
+    This is the core visual servoing framework. It's robot-agnostic -
+    you provide the functions to detect targets, move servos, and capture frames.
+    """
+
+    def __init__(
+        self,
+        servos: List[ServoConfig],
+        target_detector: Callable[[np.ndarray], Optional[Tuple[int, int]]],
+        effector_detector: Callable[[np.ndarray], Optional[Tuple[int, int]]],
+        move_servo: Callable[[int, int, int], None],
+        capture_frame: Callable[[], np.ndarray],
+        frame_delay: float = 0.3,  # Seconds to wait after move
+    ):
+        """
+        Initialize the visual servo loop.
+
+        Args:
+            servos: List of servo configurations with direction calibration
+            target_detector: Function that takes a frame and returns target (x, y) or None
+            effector_detector: Function that takes a frame and returns effector (x, y) or None
+            move_servo: Function to move a servo: (id, position, duration_ms)
+            capture_frame: Function to capture a camera frame
+            frame_delay: Seconds to wait after moving before capturing
+        """
+        self.servos = {s.id: s for s in servos}
+        self.target_detector = target_detector
+        self.effector_detector = effector_detector
+        self.move_servo = move_servo
+        self.capture_frame = capture_frame
+        self.frame_delay = frame_delay
+
+        # State tracking
+        self.states: Dict[int, ServoState] = {}
+        self.history: List[ServoIteration] = []
+
+        # Initialize states
+        for servo in servos:
+            self.states[servo.id] = ServoState(
+                config=servo,
+                initial_value=servo.current_value,
+                current_value=servo.current_value,
+            )
+
+    def distance(self, p1: Optional[Tuple], p2: Optional[Tuple]) -> float:
+        """Euclidean distance between two points."""
+        if p1 is None or p2 is None:
+            return float("inf")
+        return np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
+
+    def capture_state(self) -> Tuple[Optional[Tuple], Optional[Tuple], np.ndarray]:
+        """Capture frame and detect target/effector positions."""
+        frame = self.capture_frame()
+        target = self.target_detector(frame)
+        effector = self.effector_detector(frame)
+        return target, effector, frame
+
+    def move_toward(self, servo_id: int, amount: int = None) -> int:
+        """
+        Move a servo toward the target by one step.
+
+        Uses direction calibration to determine which way to move.
+        Returns the new servo position.
+        """
+        state = self.states[servo_id]
+        config = state.config
+
+        if not config.enabled:
+            return state.current_value
+
+        step = amount or config.step_size
+
+        # Use calibrated direction
+        if config.toward_direction == "positive":
+            new_value = state.current_value + step
+        else:
+            new_value = state.current_value - step
+
+        # Clamp to limits
+        new_value = max(config.min_value, min(config.max_value, new_value))
+
+        # Execute move
+        self.move_servo(servo_id, new_value, 300)
+        state.current_value = new_value
+        state.moves_made += 1
+
+        return new_value
+
+    def move_away(self, servo_id: int, amount: int = None) -> int:
+        """Move a servo away from the target by one step."""
+        state = self.states[servo_id]
+        config = state.config
+
+        if not config.enabled:
+            return state.current_value
+
+        step = amount or config.step_size
+
+        # Opposite of toward direction
+        if config.toward_direction == "positive":
+            new_value = state.current_value - step
+        else:
+            new_value = state.current_value + step
+
+        new_value = max(config.min_value, min(config.max_value, new_value))
+        self.move_servo(servo_id, new_value, 300)
+        state.current_value = new_value
+        state.moves_made += 1
+
+        return new_value
+
+    def run(
+        self,
+        threshold_px: float = 50.0,
+        max_iterations: int = 30,
+        stuck_threshold: int = 3,
+        verbose: bool = True,
+    ) -> ServoResult:
+        """
+        Run the visual servo loop until convergence or failure.
+
+        Args:
+            threshold_px: Stop when distance is below this (pixels)
+            max_iterations: Maximum iterations before giving up
+            stuck_threshold: Give up if no progress for this many iterations
+            verbose: Print progress
+
+        Returns:
+            ServoResult with success/failure and history
+        """
+        start_time = time.time()
+        self.history = []
+
+        if verbose:
+            print("\n" + "=" * 60)
+            print("VISUAL SERVO LOOP")
+            print("=" * 60)
+            print(f"Target threshold: {threshold_px}px")
+            print(f"Max iterations: {max_iterations}")
+            print(f"Servos: {list(self.servos.keys())}")
+            print()
+
+        # Initial state
+        target, effector, frame = self.capture_state()
+
+        if target is None:
+            return ServoResult(
+                success=False,
+                reason="target_lost",
+                final_distance_px=float("inf"),
+                iterations=0,
+                total_time_s=time.time() - start_time,
+                history=self.history,
+                servo_states=self.states,
+            )
+
+        if effector is None:
+            return ServoResult(
+                success=False,
+                reason="effector_lost",
+                final_distance_px=float("inf"),
+                iterations=0,
+                total_time_s=time.time() - start_time,
+                history=self.history,
+                servo_states=self.states,
+            )
+
+        current_distance = self.distance(effector, target)
+        if verbose:
+            print(f"Initial: target={target}, effector={effector}, dist={current_distance:.1f}px")
+
+        global_stuck_count = 0
+
+        for iteration in range(max_iterations):
+            # Check if we've converged
+            if current_distance < threshold_px:
+                if verbose:
+                    print(f"\n[CONVERGED] Distance {current_distance:.1f}px < threshold {threshold_px}px")
+
+                return ServoResult(
+                    success=True,
+                    reason="converged",
+                    final_distance_px=current_distance,
+                    iterations=iteration,
+                    total_time_s=time.time() - start_time,
+                    history=self.history,
+                    servo_states=self.states,
+                )
+
+            # Try moving each enabled servo toward target
+            moves_this_iteration = {}
+            best_improvement = 0
+            best_servo = None
+
+            for servo_id, state in self.states.items():
+                if not state.config.enabled:
+                    continue
+
+                # Store current position for potential rollback
+                original_value = state.current_value
+
+                # Try moving toward
+                new_value = self.move_toward(servo_id)
+                time.sleep(self.frame_delay)
+
+                # Measure result
+                new_target, new_effector, _ = self.capture_state()
+
+                if new_target is None or new_effector is None:
+                    # Lost tracking - rollback
+                    self.move_servo(servo_id, original_value, 200)
+                    state.current_value = original_value
+                    time.sleep(self.frame_delay)
+                    continue
+
+                new_distance = self.distance(new_effector, new_target)
+                improvement = current_distance - new_distance
+
+                if verbose:
+                    direction = "↓" if improvement > 0 else "↑"
+                    print(f"  Servo {servo_id}: {original_value}→{new_value}, "
+                          f"dist {current_distance:.1f}→{new_distance:.1f}px "
+                          f"({direction}{abs(improvement):.1f}px)")
+
+                if improvement > best_improvement:
+                    best_improvement = improvement
+                    best_servo = servo_id
+                    moves_this_iteration[servo_id] = new_value
+
+                # Rollback this move - we'll commit the best one
+                self.move_servo(servo_id, original_value, 200)
+                state.current_value = original_value
+                time.sleep(0.1)
+
+            # Commit the best move (if any improvement)
+            if best_servo is not None and best_improvement > 5:  # Minimum improvement threshold
+                state = self.states[best_servo]
+                new_value = moves_this_iteration[best_servo]
+                self.move_servo(best_servo, new_value, 300)
+                state.current_value = new_value
+                time.sleep(self.frame_delay)
+
+                # Update distance
+                target, effector, frame = self.capture_state()
+                if target and effector:
+                    current_distance = self.distance(effector, target)
+                    state.last_delta = best_improvement
+
+                global_stuck_count = 0
+
+                if verbose:
+                    print(f"  → Committed servo {best_servo} to {new_value}, "
+                          f"new dist={current_distance:.1f}px")
+            else:
+                global_stuck_count += 1
+                if verbose:
+                    print(f"  → No improvement found (stuck count: {global_stuck_count})")
+
+            # Record iteration
+            self.history.append(ServoIteration(
+                iteration=iteration,
+                timestamp=datetime.now().isoformat(),
+                target_pos=target,
+                effector_pos=effector,
+                distance_px=current_distance,
+                servo_moves=moves_this_iteration,
+                delta_distance=best_improvement,
+            ))
+
+            # Check if stuck
+            if global_stuck_count >= stuck_threshold:
+                if verbose:
+                    print(f"\n[STUCK] No progress for {stuck_threshold} iterations")
+
+                return ServoResult(
+                    success=False,
+                    reason="stuck",
+                    final_distance_px=current_distance,
+                    iterations=iteration,
+                    total_time_s=time.time() - start_time,
+                    history=self.history,
+                    servo_states=self.states,
+                )
+
+            # Re-acquire target for next iteration
+            target, effector, frame = self.capture_state()
+            if target is None:
+                if verbose:
+                    print("\n[TARGET LOST]")
+                return ServoResult(
+                    success=False,
+                    reason="target_lost",
+                    final_distance_px=current_distance,
+                    iterations=iteration,
+                    total_time_s=time.time() - start_time,
+                    history=self.history,
+                    servo_states=self.states,
+                )
+
+        # Max iterations reached
+        if verbose:
+            print(f"\n[MAX ITERATIONS] Reached {max_iterations} iterations")
+
+        return ServoResult(
+            success=False,
+            reason="max_iterations",
+            final_distance_px=current_distance,
+            iterations=max_iterations,
+            total_time_s=time.time() - start_time,
+            history=self.history,
+            servo_states=self.states,
+        )
+
+    def save_history(self, path: str) -> None:
+        """Save servo history to JSON for analysis."""
+        data = {
+            "timestamp": datetime.now().isoformat(),
+            "servos": {
+                str(sid): {
+                    "toward_direction": s.config.toward_direction,
+                    "initial_value": s.initial_value,
+                    "final_value": s.current_value,
+                    "moves_made": s.moves_made,
+                }
+                for sid, s in self.states.items()
+            },
+            "iterations": [
+                {
+                    "iteration": it.iteration,
+                    "timestamp": it.timestamp,
+                    "target_pos": it.target_pos,
+                    "effector_pos": it.effector_pos,
+                    "distance_px": it.distance_px,
+                    "delta_distance": it.delta_distance,
+                }
+                for it in self.history
+            ],
+        }
+
+        Path(path).parent.mkdir(parents=True, exist_ok=True)
+        with open(path, "w") as f:
+            json.dump(data, f, indent=2)
+
+
+def load_direction_calibration_for_servos(
+    robot_name: str,
+    servo_ids: List[int],
+) -> List[ServoConfig]:
+    """
+    Load direction calibration and create ServoConfig objects.
+
+    Convenience function to bridge direction calibration with servo loop.
+    """
+    from .direction_calibration import load_direction_calibration
+
+    calibration = load_direction_calibration(robot_name)
+    if not calibration:
+        raise ValueError(f"No direction calibration found for {robot_name}")
+
+    configs = []
+    for servo_id in servo_ids:
+        mapping = calibration.get(servo_id)
+        if mapping:
+            configs.append(ServoConfig(
+                id=servo_id,
+                toward_direction=mapping.toward_direction,
+                confidence=mapping.confidence,
+            ))
+        else:
+            # No calibration - default to positive
+            configs.append(ServoConfig(
+                id=servo_id,
+                toward_direction="positive",
+                confidence=0.0,
+            ))
+
+    return configs