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

ate/interfaces/perception.py

@@ -0,0 +1,362 @@
+"""
+Perception interfaces for robot sensors.
+
+Supports:
+- RGB cameras
+- Depth cameras (stereo, ToF, structured light)
+- LiDAR (2D and 3D)
+- IMU
+- Force/Torque sensors
+
+Design principle: Return raw sensor data in standard formats.
+Processing (object detection, SLAM, etc.) happens at higher levels.
+"""
+
+from abc import ABC, abstractmethod
+from typing import Optional, List, Callable, Tuple
+from enum import Enum, auto
+
+from .types import (
+    Vector3,
+    Quaternion,
+    Pose,
+    Image,
+    DepthImage,
+    PointCloud,
+    IMUReading,
+    ForceTorqueReading,
+    ActionResult,
+)
+
+
+class CameraInterface(ABC):
+    """
+    Interface for RGB cameras.
+
+    Can be:
+    - Fixed mount (e.g., head camera)
+    - On end-effector (e.g., wrist camera)
+    - External (e.g., Intel RealSense)
+    """
+
+    @abstractmethod
+    def get_image(self) -> Image:
+        """
+        Capture current frame.
+
+        Returns:
+            Image with RGB data
+        """
+        pass
+
+    @abstractmethod
+    def get_resolution(self) -> Tuple[int, int]:
+        """
+        Get camera resolution.
+
+        Returns:
+            (width, height) in pixels
+        """
+        pass
+
+    @abstractmethod
+    def get_intrinsics(self) -> dict:
+        """
+        Get camera intrinsic parameters.
+
+        Returns:
+            Dict with:
+                - fx, fy: focal lengths
+                - cx, cy: principal point
+                - distortion: distortion coefficients
+        """
+        pass
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID for this camera."""
+        return "camera"
+
+    def set_resolution(self, width: int, height: int) -> ActionResult:
+        """Set camera resolution."""
+        return ActionResult.error("Resolution change not supported")
+
+    def set_exposure(self, exposure_ms: float) -> ActionResult:
+        """Set exposure time in milliseconds."""
+        return ActionResult.error("Exposure control not supported")
+
+    # =========================================================================
+    # Streaming (optional)
+    # =========================================================================
+
+    def start_streaming(self, callback: Callable[[Image], None]) -> ActionResult:
+        """
+        Start continuous image streaming.
+
+        Args:
+            callback: Function called with each new frame
+        """
+        return ActionResult.error("Streaming not supported")
+
+    def stop_streaming(self) -> ActionResult:
+        """Stop image streaming."""
+        return ActionResult.error("Streaming not supported")
+
+    def get_fps(self) -> float:
+        """Get current frame rate."""
+        return 0.0
+
+
+class DepthCameraInterface(CameraInterface):
+    """
+    Interface for depth cameras.
+
+    Extends CameraInterface with depth-specific methods.
+    Implemented by: RealSense, Kinect, stereo cameras, ToF sensors.
+    """
+
+    @abstractmethod
+    def get_depth_image(self) -> DepthImage:
+        """
+        Capture current depth frame.
+
+        Returns:
+            DepthImage with depth values
+        """
+        pass
+
+    @abstractmethod
+    def get_point_cloud(self) -> PointCloud:
+        """
+        Get 3D point cloud from depth data.
+
+        Returns:
+            PointCloud in camera frame
+        """
+        pass
+
+    def get_rgbd(self) -> Tuple[Image, DepthImage]:
+        """
+        Get aligned RGB and depth images.
+
+        Returns:
+            (rgb_image, depth_image) tuple
+        """
+        return (self.get_image(), self.get_depth_image())
+
+    def get_depth_range(self) -> Tuple[float, float]:
+        """
+        Get valid depth range.
+
+        Returns:
+            (min_depth, max_depth) in meters
+        """
+        return (0.1, 10.0)
+
+    def point_to_3d(self, x: int, y: int) -> Optional[Vector3]:
+        """
+        Convert pixel coordinates to 3D point.
+
+        Args:
+            x, y: Pixel coordinates
+
+        Returns:
+            Vector3 in camera frame, or None if invalid depth
+        """
+        depth = self.get_depth_image()
+        # Implementation depends on depth format
+        return None
+
+
+class LidarInterface(ABC):
+    """
+    Interface for LiDAR sensors.
+
+    Supports:
+    - 2D LiDAR (RPLIDAR, Hokuyo)
+    - 3D LiDAR (Velodyne, Ouster, Livox)
+    """
+
+    @abstractmethod
+    def get_scan(self) -> PointCloud:
+        """
+        Get current LiDAR scan.
+
+        Returns:
+            PointCloud in sensor frame
+        """
+        pass
+
+    @abstractmethod
+    def get_range(self) -> Tuple[float, float]:
+        """
+        Get measurement range.
+
+        Returns:
+            (min_range, max_range) in meters
+        """
+        pass
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID for this LiDAR."""
+        return "lidar"
+
+    def is_3d(self) -> bool:
+        """Check if this is a 3D LiDAR."""
+        return True
+
+    def get_angular_resolution(self) -> float:
+        """Get angular resolution in radians."""
+        return 0.01  # ~0.5 degrees default
+
+    # =========================================================================
+    # 2D LiDAR specific
+    # =========================================================================
+
+    def get_scan_2d(self) -> List[Tuple[float, float]]:
+        """
+        Get 2D scan as list of (angle, distance) pairs.
+
+        Returns:
+            List of (angle_rad, distance_m) tuples
+        """
+        # Default: project 3D to 2D
+        cloud = self.get_scan()
+        scan_2d = []
+        import math
+        for point in cloud.points:
+            angle = math.atan2(point.y, point.x)
+            distance = math.sqrt(point.x**2 + point.y**2)
+            scan_2d.append((angle, distance))
+        return scan_2d
+
+
+class IMUInterface(ABC):
+    """
+    Interface for Inertial Measurement Units.
+
+    Provides:
+    - Orientation (from gyro integration or sensor fusion)
+    - Angular velocity (from gyroscope)
+    - Linear acceleration (from accelerometer)
+    """
+
+    @abstractmethod
+    def get_reading(self) -> IMUReading:
+        """
+        Get current IMU reading.
+
+        Returns:
+            IMUReading with orientation, angular velocity, acceleration
+        """
+        pass
+
+    @abstractmethod
+    def get_orientation(self) -> Quaternion:
+        """
+        Get current orientation.
+
+        Returns:
+            Quaternion representing orientation
+        """
+        pass
+
+    def get_euler(self) -> Tuple[float, float, float]:
+        """
+        Get orientation as Euler angles.
+
+        Returns:
+            (roll, pitch, yaw) in radians
+        """
+        return self.get_orientation().to_euler()
+
+    def get_angular_velocity(self) -> Vector3:
+        """
+        Get angular velocity.
+
+        Returns:
+            Vector3 in rad/s
+        """
+        return self.get_reading().angular_velocity
+
+    def get_linear_acceleration(self) -> Vector3:
+        """
+        Get linear acceleration.
+
+        Returns:
+            Vector3 in m/s² (includes gravity)
+        """
+        return self.get_reading().linear_acceleration
+
+    def calibrate(self) -> ActionResult:
+        """
+        Calibrate IMU (set current orientation as reference).
+
+        Robot should be stationary and level.
+        """
+        return ActionResult.error("Calibration not supported")
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID for this IMU."""
+        return "imu"
+
+
+class ForceTorqueInterface(ABC):
+    """
+    Interface for force-torque sensors.
+
+    Typically mounted at wrist (between arm and gripper).
+    """
+
+    @abstractmethod
+    def get_reading(self) -> ForceTorqueReading:
+        """
+        Get current force-torque reading.
+
+        Returns:
+            ForceTorqueReading with force and torque vectors
+        """
+        pass
+
+    @abstractmethod
+    def get_force(self) -> Vector3:
+        """
+        Get force vector.
+
+        Returns:
+            Vector3 in Newtons
+        """
+        pass
+
+    @abstractmethod
+    def get_torque(self) -> Vector3:
+        """
+        Get torque vector.
+
+        Returns:
+            Vector3 in Nm
+        """
+        pass
+
+    def zero(self) -> ActionResult:
+        """
+        Zero the sensor (set current reading as offset).
+
+        Call when no external load is applied.
+        """
+        return ActionResult.error("Zeroing not supported")
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID for this sensor."""
+        return "ft_sensor"
+
+    def is_in_contact(self, threshold: float = 1.0) -> bool:
+        """
+        Check if sensor detects contact.
+
+        Args:
+            threshold: Force threshold in Newtons
+
+        Returns:
+            True if force magnitude exceeds threshold
+        """
+        return self.get_force().magnitude() > threshold

ate/interfaces/sensors.py

@@ -0,0 +1,247 @@
+"""
+Sensor interfaces for robot perception.
+
+These interfaces abstract different sensor types so behaviors
+can work across robots with different sensor configurations.
+
+Design principle: A behavior that needs distance sensing can use
+ANY implementation of DistanceSensorInterface - ultrasonic, lidar,
+depth camera, or even visual estimation.
+"""
+
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from typing import Optional, List, Tuple
+from enum import Enum, auto
+
+from .types import ActionResult
+
+
+class DistanceSensorType(Enum):
+    """Type of distance sensor."""
+    ULTRASONIC = auto()      # HC-SR04, etc.
+    INFRARED = auto()        # IR proximity
+    LIDAR_SINGLE = auto()    # Single-point lidar
+    LIDAR_2D = auto()        # 2D scanning lidar
+    DEPTH_CAMERA = auto()    # Depth camera center point
+    VISUAL_ESTIMATION = auto()  # Estimated from RGB camera
+
+
+@dataclass
+class DistanceReading:
+    """A distance measurement."""
+    distance: float          # Distance in meters
+    valid: bool = True       # Whether reading is valid
+    sensor_type: DistanceSensorType = DistanceSensorType.ULTRASONIC
+    timestamp: float = 0.0   # Seconds since epoch
+    confidence: float = 1.0  # Confidence in reading (0-1)
+
+    @classmethod
+    def invalid(cls) -> "DistanceReading":
+        """Create an invalid reading."""
+        return cls(distance=0.0, valid=False)
+
+
+class DistanceSensorInterface(ABC):
+    """
+    Interface for distance/proximity sensing.
+
+    Can be implemented by:
+    - Ultrasonic sensors (HC-SR04, etc.)
+    - IR proximity sensors
+    - Single-point lidar
+    - Depth camera (center pixel or ROI average)
+    - Visual estimation (object size in image)
+
+    This abstraction allows behaviors like "approach target" to work
+    on any robot with any distance sensing capability.
+
+    Example:
+        # Works with any implementation
+        sensor: DistanceSensorInterface = robot.get_distance_sensor()
+
+        while True:
+            reading = sensor.get_distance()
+            if reading.valid and reading.distance < 0.15:
+                robot.stop()
+                break
+            robot.walk_forward(speed=0.1)
+    """
+
+    @abstractmethod
+    def get_distance(self) -> DistanceReading:
+        """
+        Get current distance reading.
+
+        Returns:
+            DistanceReading with distance in meters
+        """
+        pass
+
+    @abstractmethod
+    def get_min_range(self) -> float:
+        """
+        Get minimum measurable range in meters.
+
+        Returns below this distance are unreliable.
+        """
+        pass
+
+    @abstractmethod
+    def get_max_range(self) -> float:
+        """
+        Get maximum measurable range in meters.
+
+        Returns above this distance are unreliable.
+        """
+        pass
+
+    def get_sensor_type(self) -> DistanceSensorType:
+        """Get the type of distance sensor."""
+        return DistanceSensorType.ULTRASONIC  # Default
+
+    def is_obstacle_detected(self, threshold: float = 0.20) -> bool:
+        """
+        Check if an obstacle is within threshold distance.
+
+        Args:
+            threshold: Distance threshold in meters
+
+        Returns:
+            True if obstacle detected within threshold
+        """
+        reading = self.get_distance()
+        return reading.valid and reading.distance < threshold
+
+
+class VisualDistanceEstimator(DistanceSensorInterface):
+    """
+    Estimate distance from visual object size.
+
+    When no hardware distance sensor is available, we can estimate
+    distance based on the apparent size of a detected object.
+
+    This uses the pinhole camera model:
+        distance = (known_size * focal_length) / apparent_size
+
+    For unknown objects, we use heuristics based on object category.
+    """
+
+    # Typical object sizes in meters (width)
+    TYPICAL_SIZES = {
+        "bottle": 0.07,
+        "can": 0.065,
+        "wrapper": 0.10,
+        "paper": 0.20,
+        "plastic": 0.10,
+        "metal": 0.08,
+        "unknown": 0.10,
+    }
+
+    def __init__(
+        self,
+        image_width: int = 640,
+        image_height: int = 480,
+        fov_horizontal: float = 60.0,  # Degrees
+    ):
+        """
+        Initialize visual distance estimator.
+
+        Args:
+            image_width: Camera image width in pixels
+            image_height: Camera image height in pixels
+            fov_horizontal: Horizontal field of view in degrees
+        """
+        self.image_width = image_width
+        self.image_height = image_height
+        self.fov_horizontal = fov_horizontal
+
+        # Calculate focal length in pixels
+        import math
+        self.focal_length = image_width / (2 * math.tan(math.radians(fov_horizontal / 2)))
+
+        self._last_reading: Optional[DistanceReading] = None
+
+    def estimate_from_detection(
+        self,
+        bbox_width: int,
+        object_type: str = "unknown",
+        known_size: Optional[float] = None,
+    ) -> DistanceReading:
+        """
+        Estimate distance from detected object bounding box.
+
+        Args:
+            bbox_width: Width of bounding box in pixels
+            object_type: Type of object (for size lookup)
+            known_size: Known real-world size in meters (overrides lookup)
+
+        Returns:
+            DistanceReading with estimated distance
+        """
+        import time
+
+        if bbox_width <= 0:
+            return DistanceReading.invalid()
+
+        # Get object size
+        real_size = known_size or self.TYPICAL_SIZES.get(object_type, 0.10)
+
+        # Estimate distance using pinhole model
+        distance = (real_size * self.focal_length) / bbox_width
+
+        # Confidence decreases with smaller bounding boxes (more uncertainty)
+        confidence = min(1.0, bbox_width / 50.0)
+
+        reading = DistanceReading(
+            distance=distance,
+            valid=True,
+            sensor_type=DistanceSensorType.VISUAL_ESTIMATION,
+            timestamp=time.time(),
+            confidence=confidence,
+        )
+
+        self._last_reading = reading
+        return reading
+
+    def get_distance(self) -> DistanceReading:
+        """Get last estimated distance."""
+        if self._last_reading:
+            return self._last_reading
+        return DistanceReading.invalid()
+
+    def get_min_range(self) -> float:
+        return 0.1  # 10cm minimum
+
+    def get_max_range(self) -> float:
+        return 5.0  # 5m maximum (very rough beyond this)
+
+    def get_sensor_type(self) -> DistanceSensorType:
+        return DistanceSensorType.VISUAL_ESTIMATION
+
+
+@dataclass
+class ProximitySensorReading:
+    """Reading from proximity/bump sensors."""
+    triggered: bool          # Whether sensor is triggered
+    sensor_id: str           # Sensor identifier
+    timestamp: float = 0.0
+
+
+class ProximitySensorInterface(ABC):
+    """
+    Interface for proximity/bump sensors.
+
+    These are simple binary sensors that detect contact or very close proximity.
+    Common on wheeled robots and robot vacuums.
+    """
+
+    @abstractmethod
+    def get_proximity_sensors(self) -> List[ProximitySensorReading]:
+        """Get readings from all proximity sensors."""
+        pass
+
+    @abstractmethod
+    def is_any_triggered(self) -> bool:
+        """Check if any proximity sensor is triggered."""
+        pass