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

ate/robot/locomotion_calibration.py

@@ -0,0 +1,508 @@
+"""
+Locomotion Characterization for Mobile Robots.
+
+This module provides tools to characterize the relationship between
+motor commands and actual robot displacement. This is the missing
+piece that connects "move forward 100mm" to actual motor commands.
+
+The key insight: vendor APIs like `dog.move(vel, turn)` are velocity-based
+with unknown relationships to actual displacement. This module measures
+those relationships and stores them for later use.
+
+Usage:
+    from ate.robot.locomotion_calibration import LocomotionCalibrator
+
+    # Setup with ArUco marker on ground
+    calibrator = LocomotionCalibrator(driver, camera)
+
+    # Run characterization
+    results = calibrator.characterize_locomotion()
+
+    # Now we know: move(50, 0) for 1 sec = 35mm forward
+
+    # Use for planning
+    time_needed = calibrator.time_for_distance(100)  # mm
+    driver.move(50, 0)
+    time.sleep(time_needed)
+    driver.stop()
+
+Pain Points This Solves:
+1. "move(50, 0) for how long to go 100mm?" - Now we can calculate
+2. "turn(30) for how long to rotate 45°?" - Now we can calculate
+3. Planning becomes possible - decompose into known steps
+4. Drift detection - compare expected vs actual position
+"""
+
+import time
+import json
+import math
+from dataclasses import dataclass, asdict
+from typing import Optional, Tuple, List, Callable
+from pathlib import Path
+
+
+@dataclass
+class LocomotionParameters:
+    """
+    Characterized locomotion parameters for a robot.
+
+    These are measured empirically by commanding motion and
+    observing actual displacement via external reference (ArUco marker).
+    """
+    # Forward motion
+    forward_mm_per_sec_at_50: float = 0.0  # mm/s at move(50, 0)
+    forward_variance: float = 0.0          # Standard deviation in mm
+
+    # Backward motion (may differ from forward)
+    backward_mm_per_sec_at_50: float = 0.0
+
+    # Turn motion
+    turn_deg_per_sec_at_30: float = 0.0    # deg/s at move(0, 30)
+    turn_variance: float = 0.0              # Standard deviation in degrees
+
+    # Relationship (assumed linear for simplicity)
+    # actual_vel = command_vel * scale_factor
+    forward_scale: float = 1.0
+    turn_scale: float = 1.0
+
+    # Calibration metadata
+    surface_type: str = "unknown"          # e.g., "hardwood", "carpet", "tile"
+    battery_level: Optional[float] = None  # Battery % at calibration
+    calibrated_at: str = ""
+
+    def time_for_forward_mm(self, distance_mm: float, command_vel: int = 50) -> float:
+        """Calculate time needed to move forward given distance."""
+        if self.forward_mm_per_sec_at_50 <= 0:
+            raise ValueError("Locomotion not characterized - run characterize_locomotion() first")
+
+        # Scale velocity to match calibration baseline
+        vel_ratio = command_vel / 50.0
+        mm_per_sec = self.forward_mm_per_sec_at_50 * vel_ratio * self.forward_scale
+
+        return distance_mm / mm_per_sec
+
+    def time_for_turn_deg(self, angle_deg: float, command_vel: int = 30) -> float:
+        """Calculate time needed to turn given angle."""
+        if self.turn_deg_per_sec_at_30 <= 0:
+            raise ValueError("Locomotion not characterized - run characterize_locomotion() first")
+
+        vel_ratio = command_vel / 30.0
+        deg_per_sec = self.turn_deg_per_sec_at_30 * vel_ratio * self.turn_scale
+
+        return abs(angle_deg) / deg_per_sec
+
+    def to_dict(self) -> dict:
+        return asdict(self)
+
+    @classmethod
+    def from_dict(cls, data: dict) -> 'LocomotionParameters':
+        return cls(**{k: v for k, v in data.items() if k in cls.__dataclass_fields__})
+
+
+class LocomotionCalibrator:
+    """
+    Characterizes locomotion parameters using visual reference.
+
+    Requires:
+    - Robot driver with move() command
+    - Camera that can see ArUco marker on ground
+    - ArUco marker placed on floor in robot's path
+
+    The process:
+    1. Detect marker position (reference point)
+    2. Command robot to move
+    3. Detect new marker position
+    4. Calculate actual displacement
+    5. Store velocity-to-displacement relationship
+    """
+
+    def __init__(
+        self,
+        driver,
+        get_frame: Callable,
+        marker_size_mm: float = 50.0,
+        marker_id: int = 0,
+    ):
+        """
+        Initialize locomotion calibrator.
+
+        Args:
+            driver: Robot driver with move() and stop() methods
+            get_frame: Function that returns camera frame (numpy array)
+            marker_size_mm: Physical size of ArUco marker
+            marker_id: ArUco marker ID to track
+        """
+        self.driver = driver
+        self.get_frame = get_frame
+        self.marker_size_mm = marker_size_mm
+        self.marker_id = marker_id
+
+        # ArUco detection setup
+        try:
+            import cv2
+            self._cv2 = cv2
+            self._aruco = cv2.aruco
+            self._aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+            self._aruco_params = cv2.aruco.DetectorParameters()
+            self._detector = cv2.aruco.ArucoDetector(self._aruco_dict, self._aruco_params)
+        except ImportError:
+            raise ImportError("OpenCV with ArUco support required")
+
+        self.params = LocomotionParameters()
+
+    def detect_marker(self) -> Optional[Tuple[float, float, float]]:
+        """
+        Detect ArUco marker and return (x, y, angle) in image coordinates.
+
+        Returns:
+            (center_x, center_y, rotation_deg) or None if not found
+        """
+        frame = self.get_frame()
+        if frame is None:
+            return None
+
+        gray = self._cv2.cvtColor(frame, self._cv2.COLOR_BGR2GRAY)
+        corners, ids, rejected = self._detector.detectMarkers(gray)
+
+        if ids is None or self.marker_id not in ids.flatten():
+            return None
+
+        idx = list(ids.flatten()).index(self.marker_id)
+        corner = corners[idx][0]
+
+        # Calculate center
+        center_x = corner[:, 0].mean()
+        center_y = corner[:, 1].mean()
+
+        # Calculate rotation from marker orientation
+        dx = corner[1, 0] - corner[0, 0]
+        dy = corner[1, 1] - corner[0, 1]
+        angle = math.degrees(math.atan2(dy, dx))
+
+        return (center_x, center_y, angle)
+
+    def measure_marker_displacement(
+        self,
+        move_cmd: Tuple[int, int],
+        duration_sec: float,
+    ) -> Optional[Tuple[float, float, float]]:
+        """
+        Measure actual displacement from a move command.
+
+        Args:
+            move_cmd: (forward_vel, turn_vel) to send to robot
+            duration_sec: How long to move
+
+        Returns:
+            (delta_x_pixels, delta_y_pixels, delta_angle_deg) or None
+        """
+        # Get initial position
+        initial = self.detect_marker()
+        if initial is None:
+            print("ERROR: Cannot see marker before move")
+            return None
+
+        # Execute move
+        forward, turn = move_cmd
+        if hasattr(self.driver, '_send_command'):
+            self.driver._send_command(f"dog.move({forward}, {turn})", wait=0.1)
+        else:
+            self.driver.move(forward, turn)
+
+        time.sleep(duration_sec)
+
+        # Stop
+        if hasattr(self.driver, '_send_command'):
+            self.driver._send_command("dog.move(0, 0)", wait=0.1)
+        else:
+            self.driver.stop()
+
+        time.sleep(0.3)  # Let robot settle
+
+        # Get final position
+        final = self.detect_marker()
+        if final is None:
+            print("ERROR: Cannot see marker after move")
+            return None
+
+        # Calculate displacement
+        dx = final[0] - initial[0]
+        dy = final[1] - initial[1]
+        dangle = final[2] - initial[2]
+
+        return (dx, dy, dangle)
+
+    def pixels_to_mm(self, pixels: float, frame) -> float:
+        """
+        Convert pixel displacement to mm using marker size as reference.
+
+        This requires seeing the marker to calibrate the pixel-to-mm ratio.
+        """
+        # Detect marker to get its size in pixels
+        gray = self._cv2.cvtColor(frame, self._cv2.COLOR_BGR2GRAY)
+        corners, ids, _ = self._detector.detectMarkers(gray)
+
+        if ids is None or self.marker_id not in ids.flatten():
+            raise ValueError("Marker not visible - cannot calibrate pixel scale")
+
+        idx = list(ids.flatten()).index(self.marker_id)
+        corner = corners[idx][0]
+
+        # Marker size in pixels (average of width and height)
+        marker_pixels = (
+            math.sqrt((corner[1, 0] - corner[0, 0])**2 + (corner[1, 1] - corner[0, 1])**2) +
+            math.sqrt((corner[2, 0] - corner[1, 0])**2 + (corner[2, 1] - corner[1, 1])**2)
+        ) / 2
+
+        # Pixels per mm
+        scale = marker_pixels / self.marker_size_mm
+
+        return pixels / scale
+
+    def characterize_forward(self, trials: int = 3) -> float:
+        """
+        Characterize forward motion.
+
+        Returns:
+            mm/sec at move(50, 0)
+        """
+        print("\n=== Characterizing Forward Motion ===")
+        print("Place ArUco marker on floor in front of robot")
+        print("Robot will move forward 3 times")
+
+        measurements = []
+
+        for trial in range(trials):
+            print(f"\nTrial {trial + 1}/{trials}")
+
+            # Get reference frame for pixel calibration
+            frame = self.get_frame()
+
+            # Move forward for 1 second at velocity 50
+            result = self.measure_marker_displacement((50, 0), 1.0)
+
+            if result is None:
+                print(f"  Trial failed - marker lost")
+                continue
+
+            dx, dy, _ = result
+
+            # Convert to mm (marker moves opposite to robot)
+            displacement_mm = -self.pixels_to_mm(dy, frame)  # Y in image = forward
+            measurements.append(displacement_mm)
+
+            print(f"  Displacement: {displacement_mm:.1f} mm")
+
+        if not measurements:
+            raise RuntimeError("All trials failed")
+
+        avg = sum(measurements) / len(measurements)
+        variance = sum((m - avg)**2 for m in measurements) / len(measurements)
+
+        print(f"\nResult: {avg:.1f} mm/s (±{math.sqrt(variance):.1f})")
+
+        self.params.forward_mm_per_sec_at_50 = avg
+        self.params.forward_variance = math.sqrt(variance)
+
+        return avg
+
+    def characterize_turn(self, trials: int = 3) -> float:
+        """
+        Characterize turn motion.
+
+        Returns:
+            deg/sec at move(0, 30)
+        """
+        print("\n=== Characterizing Turn Motion ===")
+        print("Place ArUco marker on floor beside robot")
+        print("Robot will turn in place 3 times")
+
+        measurements = []
+
+        for trial in range(trials):
+            print(f"\nTrial {trial + 1}/{trials}")
+
+            # Turn for 1 second at velocity 30
+            result = self.measure_marker_displacement((0, 30), 1.0)
+
+            if result is None:
+                print(f"  Trial failed - marker lost")
+                continue
+
+            _, _, dangle = result
+            measurements.append(abs(dangle))
+
+            print(f"  Rotation: {abs(dangle):.1f} deg")
+
+        if not measurements:
+            raise RuntimeError("All trials failed")
+
+        avg = sum(measurements) / len(measurements)
+        variance = sum((m - avg)**2 for m in measurements) / len(measurements)
+
+        print(f"\nResult: {avg:.1f} deg/s (±{math.sqrt(variance):.1f})")
+
+        self.params.turn_deg_per_sec_at_30 = avg
+        self.params.turn_variance = math.sqrt(variance)
+
+        return avg
+
+    def characterize_locomotion(
+        self,
+        surface_type: str = "unknown"
+    ) -> LocomotionParameters:
+        """
+        Run full locomotion characterization.
+
+        Returns:
+            LocomotionParameters with measured values
+        """
+        print("\n" + "=" * 60)
+        print("LOCOMOTION CHARACTERIZATION")
+        print("=" * 60)
+        print(f"Surface: {surface_type}")
+        print("This process will move the robot and measure displacement")
+        print("Ensure ArUco marker is visible on floor")
+
+        self.characterize_forward()
+        self.characterize_turn()
+
+        self.params.surface_type = surface_type
+        self.params.calibrated_at = time.strftime("%Y-%m-%dT%H:%M:%S")
+
+        print("\n" + "=" * 60)
+        print("CHARACTERIZATION COMPLETE")
+        print("=" * 60)
+        print(f"Forward: {self.params.forward_mm_per_sec_at_50:.1f} mm/s at vel=50")
+        print(f"Turn: {self.params.turn_deg_per_sec_at_30:.1f} deg/s at vel=30")
+
+        return self.params
+
+    def save_to_calibration(self, calibration_path: Path):
+        """
+        Add locomotion parameters to existing calibration file.
+        """
+        if calibration_path.exists():
+            with open(calibration_path) as f:
+                data = json.load(f)
+        else:
+            data = {}
+
+        data["locomotion"] = self.params.to_dict()
+
+        with open(calibration_path, 'w') as f:
+            json.dump(data, f, indent=2)
+
+        print(f"Saved locomotion parameters to {calibration_path}")
+
+    @classmethod
+    def load_from_calibration(cls, calibration_path: Path) -> LocomotionParameters:
+        """
+        Load locomotion parameters from calibration file.
+        """
+        with open(calibration_path) as f:
+            data = json.load(f)
+
+        if "locomotion" not in data:
+            raise ValueError("Calibration file has no locomotion parameters")
+
+        return LocomotionParameters.from_dict(data["locomotion"])
+
+
+# =============================================================================
+# Convenience functions for ate CLI
+# =============================================================================
+
+def characterize_mechdog(
+    serial_port: str = "/dev/cu.usbserial-10",
+    webcam_index: int = 0,
+    marker_size_mm: float = 50.0,
+    surface_type: str = "desk",
+) -> LocomotionParameters:
+    """
+    Run locomotion characterization for MechDog.
+
+    Requires:
+    - MechDog connected via USB
+    - Webcam viewing ArUco marker on floor
+    - ArUco marker (ID 0, 50mm) placed in front of robot
+
+    Returns:
+        LocomotionParameters with measured values
+    """
+    import cv2
+    import serial
+
+    # Setup serial
+    print(f"Connecting to MechDog on {serial_port}...")
+    ser = serial.Serial(serial_port, 115200, timeout=1)
+    time.sleep(0.5)
+    ser.read(ser.in_waiting)
+
+    def cmd(command: str, wait: float = 0.5):
+        ser.write(f"{command}\r\n".encode())
+        time.sleep(wait)
+        return ser.read(ser.in_waiting).decode('utf-8', errors='ignore')
+
+    cmd("from HW_MechDog import MechDog", 1.5)
+    cmd("dog = MechDog()", 1.5)
+    print("Connected!")
+
+    # Setup webcam
+    print(f"Opening webcam {webcam_index}...")
+    cap = cv2.VideoCapture(webcam_index)
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+    print("Ready!")
+
+    # Create simple driver wrapper
+    class SimpleDriver:
+        def _send_command(self, command, wait=0.1):
+            cmd(command, wait)
+
+    def get_frame():
+        for _ in range(2):
+            cap.read()
+        ret, frame = cap.read()
+        return frame if ret else None
+
+    # Run characterization
+    calibrator = LocomotionCalibrator(
+        SimpleDriver(),
+        get_frame,
+        marker_size_mm=marker_size_mm,
+    )
+
+    try:
+        params = calibrator.characterize_locomotion(surface_type)
+        return params
+    finally:
+        ser.close()
+        cap.release()
+
+
+if __name__ == "__main__":
+    print("Locomotion Characterization Demo")
+    print("================================")
+    print()
+    print("This will characterize your MechDog's locomotion.")
+    print("Requirements:")
+    print("  1. MechDog connected via USB")
+    print("  2. Webcam viewing floor area")
+    print("  3. ArUco marker (ID 0, 50mm) on floor")
+    print()
+
+    try:
+        params = characterize_mechdog()
+
+        print("\n" + "=" * 40)
+        print("To move forward 100mm:")
+        t = params.time_for_forward_mm(100)
+        print(f"  dog.move(50, 0) for {t:.2f} seconds")
+
+        print("\nTo turn 45 degrees:")
+        t = params.time_for_turn_deg(45)
+        print(f"  dog.move(0, 30) for {t:.2f} seconds")
+
+    except Exception as e:
+        print(f"Error: {e}")