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

ate/interfaces/__init__.py

@@ -0,0 +1,171 @@
+"""
+FoodforThought Robot Interfaces
+
+Abstract interfaces that define capabilities for any robot.
+Implementations are hardware-specific, but skills are written against these interfaces.
+
+Architecture:
+    RobotInterface (base)
+    ├── Capabilities (mixins)
+    │   ├── Locomotion
+    │   │   ├── QuadrupedLocomotion
+    │   │   ├── BipedLocomotion
+    │   │   ├── WheeledLocomotion
+    │   │   └── AerialLocomotion
+    │   ├── Manipulation
+    │   │   ├── ArmInterface
+    │   │   └── GripperInterface
+    │   ├── Perception
+    │   │   ├── CameraInterface
+    │   │   ├── DepthCameraInterface
+    │   │   ├── LidarInterface
+    │   │   └── IMUInterface
+    │   └── Body
+    │       └── BodyPoseInterface
+    └── Safety
+        └── SafetyInterface
+
+Example:
+    # MechDog implements:
+    class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface):
+        ...
+
+    # Spot with arm implements:
+    class SpotDriver(QuadrupedLocomotion, BodyPoseInterface, ArmInterface,
+                     GripperInterface, CameraInterface, LidarInterface, SafetyInterface):
+        ...
+
+    # Skills are written against interfaces:
+    def pick_and_place(robot: QuadrupedLocomotion & GripperInterface, target: Vector3):
+        robot.walk_to(target)
+        robot.lower_body(height=0.1)
+        robot.grasp()
+        ...
+"""
+
+from .types import (
+    Vector3,
+    Quaternion,
+    Pose,
+    Twist,
+    JointState,
+    JointLimits,
+    Image,
+    DepthImage,
+    PointCloud,
+    IMUReading,
+    ForceTorqueReading,
+    BatteryState,
+    RobotStatus,
+    GaitType,
+    GripperState,
+    GripperStatus,
+    ActionResult,
+)
+
+from .base import (
+    RobotInterface,
+    SafetyInterface,
+    Capability,
+    RobotInfo,
+)
+
+from .locomotion import (
+    LocomotionInterface,
+    QuadrupedLocomotion,
+    BipedLocomotion,
+    WheeledLocomotion,
+    AerialLocomotion,
+)
+
+from .manipulation import (
+    ArmInterface,
+    GripperInterface,
+    DualArmInterface,
+)
+
+from .perception import (
+    CameraInterface,
+    DepthCameraInterface,
+    LidarInterface,
+    IMUInterface,
+    ForceTorqueInterface,
+)
+
+from .body import (
+    BodyPoseInterface,
+)
+
+from .detection import (
+    BoundingBox,
+    Detection,
+    DetectionResult,
+    ObjectDetectionInterface,
+    TrashDetectionInterface,
+)
+
+from .navigation import (
+    NavigationState,
+    NavigationGoal,
+    NavigationStatus,
+    Waypoint,
+    NavigationInterface,
+    SimpleNavigationInterface,
+)
+
+__all__ = [
+    # Types
+    "Vector3",
+    "Quaternion",
+    "Pose",
+    "Twist",
+    "JointState",
+    "JointLimits",
+    "Image",
+    "DepthImage",
+    "PointCloud",
+    "IMUReading",
+    "ForceTorqueReading",
+    "BatteryState",
+    "RobotStatus",
+    "GaitType",
+    "GripperState",
+    "GripperStatus",
+    "ActionResult",
+    # Base
+    "RobotInterface",
+    "SafetyInterface",
+    "Capability",
+    "RobotInfo",
+    # Locomotion
+    "LocomotionInterface",
+    "QuadrupedLocomotion",
+    "BipedLocomotion",
+    "WheeledLocomotion",
+    "AerialLocomotion",
+    # Manipulation
+    "ArmInterface",
+    "GripperInterface",
+    "DualArmInterface",
+    # Perception
+    "CameraInterface",
+    "DepthCameraInterface",
+    "LidarInterface",
+    "IMUInterface",
+    "ForceTorqueInterface",
+    # Body
+    "BodyPoseInterface",
+    # Detection (higher-level)
+    "BoundingBox",
+    "Detection",
+    "DetectionResult",
+    "ObjectDetectionInterface",
+    "TrashDetectionInterface",
+    # Navigation (higher-level)
+    "NavigationState",
+    "NavigationGoal",
+    "NavigationStatus",
+    "Waypoint",
+    "NavigationInterface",
+    "SimpleNavigationInterface",
+]

ate/interfaces/base.py

@@ -0,0 +1,271 @@
+"""
+Base interfaces that all robots must implement.
+
+Every robot, regardless of type, has:
+1. Identity - what it is, what it can do
+2. Safety - emergency stop, status monitoring
+3. Lifecycle - connect, disconnect, ready state
+"""
+
+from abc import ABC, abstractmethod
+from dataclasses import dataclass, field
+from typing import List, Optional, Set, Type, TYPE_CHECKING
+from enum import Enum, auto
+
+from .types import RobotStatus, BatteryState, ActionResult
+
+if TYPE_CHECKING:
+    from .locomotion import LocomotionInterface
+    from .manipulation import ArmInterface, GripperInterface
+    from .perception import CameraInterface, LidarInterface, IMUInterface
+
+
+class Capability(Enum):
+    """
+    Capabilities a robot can have.
+    Used for runtime capability checking and skill compatibility.
+    """
+    # Locomotion
+    QUADRUPED = auto()
+    BIPED = auto()
+    WHEELED = auto()
+    AERIAL = auto()
+    AQUATIC = auto()
+
+    # Manipulation
+    ARM = auto()
+    DUAL_ARM = auto()
+    GRIPPER = auto()
+    SUCTION = auto()
+
+    # Perception
+    CAMERA = auto()
+    DEPTH_CAMERA = auto()
+    LIDAR = auto()
+    IMU = auto()
+    FORCE_TORQUE = auto()
+    TACTILE = auto()
+
+    # Body control
+    BODY_POSE = auto()
+
+    # Communication
+    AUDIO_INPUT = auto()
+    AUDIO_OUTPUT = auto()
+
+
+@dataclass
+class RobotInfo:
+    """
+    Static information about a robot.
+    Used for compatibility checking and UI display.
+    """
+    # Identity
+    name: str                           # Human-readable name
+    model: str                          # Model identifier (e.g., "hiwonder_mechdog")
+    manufacturer: str                   # Manufacturer name
+    archetype: str                      # Primary type: "quadruped", "humanoid", "arm", etc.
+
+    # Capabilities
+    capabilities: Set[Capability] = field(default_factory=set)
+
+    # Physical properties
+    mass: Optional[float] = None        # kg
+    payload_capacity: Optional[float] = None  # kg
+    reach: Optional[float] = None       # meters (for arms)
+    dimensions: Optional[tuple] = None  # (length, width, height) in meters
+
+    # Workspace limits (in base frame)
+    workspace_min: Optional[tuple] = None  # (x, y, z) min bounds
+    workspace_max: Optional[tuple] = None  # (x, y, z) max bounds
+
+    # Performance
+    max_speed: Optional[float] = None   # m/s
+    battery_capacity: Optional[float] = None  # Wh
+
+    # Metadata
+    firmware_version: Optional[str] = None
+    serial_number: Optional[str] = None
+    description: str = ""
+
+    def has_capability(self, cap: Capability) -> bool:
+        return cap in self.capabilities
+
+    def has_all_capabilities(self, caps: Set[Capability]) -> bool:
+        return caps.issubset(self.capabilities)
+
+
+class RobotInterface(ABC):
+    """
+    Base interface that ALL robots must implement.
+
+    This provides:
+    - Identity and capability information
+    - Connection lifecycle
+    - Status monitoring
+
+    Specific capabilities (locomotion, manipulation, etc.) are added via mixins.
+    """
+
+    @abstractmethod
+    def get_info(self) -> RobotInfo:
+        """
+        Get static information about this robot.
+
+        Returns:
+            RobotInfo with name, model, capabilities, etc.
+        """
+        pass
+
+    @abstractmethod
+    def connect(self) -> ActionResult:
+        """
+        Establish connection to the robot hardware.
+
+        Returns:
+            ActionResult indicating success/failure
+        """
+        pass
+
+    @abstractmethod
+    def disconnect(self) -> ActionResult:
+        """
+        Safely disconnect from the robot hardware.
+
+        Returns:
+            ActionResult indicating success/failure
+        """
+        pass
+
+    @abstractmethod
+    def is_connected(self) -> bool:
+        """
+        Check if currently connected to the robot.
+
+        Returns:
+            True if connected and communication is working
+        """
+        pass
+
+    @abstractmethod
+    def get_status(self) -> RobotStatus:
+        """
+        Get current robot status.
+
+        Returns:
+            RobotStatus with mode, errors, battery, etc.
+        """
+        pass
+
+    def has_capability(self, cap: Capability) -> bool:
+        """Check if robot has a specific capability."""
+        return self.get_info().has_capability(cap)
+
+    def get_capabilities(self) -> Set[Capability]:
+        """Get all capabilities of this robot."""
+        return self.get_info().capabilities
+
+
+class SafetyInterface(ABC):
+    """
+    Safety interface that all robots should implement.
+
+    Provides emergency stop and safety monitoring.
+    """
+
+    @abstractmethod
+    def emergency_stop(self) -> ActionResult:
+        """
+        Immediately stop all motion and enter safe state.
+
+        This should:
+        - Stop all actuators immediately
+        - Disable motor power if possible
+        - Set robot to ESTOPPED mode
+
+        Returns:
+            ActionResult (should rarely fail)
+        """
+        pass
+
+    @abstractmethod
+    def reset_emergency_stop(self) -> ActionResult:
+        """
+        Clear emergency stop state and return to ready.
+
+        Returns:
+            ActionResult indicating if reset was successful
+        """
+        pass
+
+    @abstractmethod
+    def is_estopped(self) -> bool:
+        """
+        Check if emergency stop is active.
+
+        Returns:
+            True if robot is in emergency stop state
+        """
+        pass
+
+    @abstractmethod
+    def get_battery_state(self) -> Optional[BatteryState]:
+        """
+        Get current battery status.
+
+        Returns:
+            BatteryState or None if no battery
+        """
+        pass
+
+    def check_safety(self) -> List[str]:
+        """
+        Run safety checks and return list of issues.
+
+        Returns:
+            List of safety warning/error messages (empty if all OK)
+        """
+        issues = []
+
+        battery = self.get_battery_state()
+        if battery:
+            if battery.percentage < 0.1:
+                issues.append(f"Critical battery: {battery.percentage*100:.0f}%")
+            elif battery.percentage < 0.2:
+                issues.append(f"Low battery: {battery.percentage*100:.0f}%")
+
+        if self.is_estopped():
+            issues.append("Emergency stop is active")
+
+        return issues
+
+
+# =============================================================================
+# Utility functions for capability checking
+# =============================================================================
+
+def requires_capabilities(*caps: Capability):
+    """
+    Decorator to mark that a skill requires certain capabilities.
+
+    Usage:
+        @requires_capabilities(Capability.QUADRUPED, Capability.GRIPPER)
+        def pick_up_object(robot, target):
+            ...
+    """
+    def decorator(func):
+        func._required_capabilities = set(caps)
+        return func
+    return decorator
+
+
+def check_robot_compatibility(robot: RobotInterface, required: Set[Capability]) -> List[str]:
+    """
+    Check if a robot is compatible with required capabilities.
+
+    Returns:
+        List of missing capabilities (empty if compatible)
+    """
+    robot_caps = robot.get_capabilities()
+    missing = required - robot_caps
+    return [cap.name for cap in missing]

ate/interfaces/body.py

@@ -0,0 +1,267 @@
+"""
+Body pose interfaces for robots with adjustable body position/orientation.
+
+Primarily for legged robots that can:
+- Adjust body height
+- Tilt body (roll, pitch, yaw)
+- Shift body position relative to feet
+
+These are independent of locomotion - you can adjust pose while standing still.
+"""
+
+from abc import ABC, abstractmethod
+from typing import Optional, Tuple
+
+from .types import (
+    Vector3,
+    Quaternion,
+    Pose,
+    ActionResult,
+)
+
+
+class BodyPoseInterface(ABC):
+    """
+    Interface for controlling robot body pose.
+
+    Implemented by quadrupeds, bipeds, and other robots that can adjust
+    their body position/orientation independent of locomotion.
+
+    Coordinate frame:
+    - Body origin is typically at the center of the robot
+    - Z+ is up, X+ is forward, Y+ is left
+    """
+
+    # =========================================================================
+    # Body height control
+    # =========================================================================
+
+    @abstractmethod
+    def set_body_height(self, height: float) -> ActionResult:
+        """
+        Set body height above ground.
+
+        Args:
+            height: Height in meters (from nominal ground level)
+
+        Returns:
+            ActionResult
+        """
+        pass
+
+    @abstractmethod
+    def get_body_height(self) -> float:
+        """
+        Get current body height.
+
+        Returns:
+            Height in meters
+        """
+        pass
+
+    def get_height_limits(self) -> Tuple[float, float]:
+        """
+        Get allowed body height range.
+
+        Returns:
+            (min_height, max_height) in meters
+        """
+        return (0.05, 0.30)  # Default for small quadrupeds
+
+    # =========================================================================
+    # Body orientation control
+    # =========================================================================
+
+    @abstractmethod
+    def set_body_orientation(
+        self,
+        roll: float = 0.0,
+        pitch: float = 0.0,
+        yaw: float = 0.0
+    ) -> ActionResult:
+        """
+        Set body orientation (tilt).
+
+        Args:
+            roll: Roll angle in radians (positive = right side down)
+            pitch: Pitch angle in radians (positive = nose down)
+            yaw: Yaw angle in radians (positive = turn left)
+
+        Returns:
+            ActionResult
+        """
+        pass
+
+    @abstractmethod
+    def get_body_orientation(self) -> Tuple[float, float, float]:
+        """
+        Get current body orientation.
+
+        Returns:
+            (roll, pitch, yaw) in radians
+        """
+        pass
+
+    def get_orientation_limits(self) -> Tuple[Tuple[float, float], Tuple[float, float], Tuple[float, float]]:
+        """
+        Get allowed orientation ranges.
+
+        Returns:
+            ((roll_min, roll_max), (pitch_min, pitch_max), (yaw_min, yaw_max))
+            All in radians
+        """
+        import math
+        return (
+            (-math.pi/6, math.pi/6),   # ±30° roll
+            (-math.pi/6, math.pi/6),   # ±30° pitch
+            (-math.pi/4, math.pi/4),   # ±45° yaw
+        )
+
+    # =========================================================================
+    # Body position control
+    # =========================================================================
+
+    def set_body_position(self, offset: Vector3) -> ActionResult:
+        """
+        Shift body position relative to feet.
+
+        This moves the body while keeping feet planted.
+
+        Args:
+            offset: Position offset from center (x=forward, y=left, z=up)
+
+        Returns:
+            ActionResult
+        """
+        return ActionResult.error("Body position shift not supported")
+
+    def get_body_position(self) -> Vector3:
+        """
+        Get current body position offset.
+
+        Returns:
+            Vector3 offset from center
+        """
+        return Vector3.zero()
+
+    # =========================================================================
+    # Combined pose control
+    # =========================================================================
+
+    @abstractmethod
+    def set_body_pose(
+        self,
+        height: Optional[float] = None,
+        roll: Optional[float] = None,
+        pitch: Optional[float] = None,
+        yaw: Optional[float] = None,
+        x_offset: Optional[float] = None,
+        y_offset: Optional[float] = None
+    ) -> ActionResult:
+        """
+        Set multiple body pose parameters at once.
+
+        Args:
+            height: Body height (None = keep current)
+            roll, pitch, yaw: Orientation (None = keep current)
+            x_offset, y_offset: Body position offset (None = keep current)
+
+        Returns:
+            ActionResult
+        """
+        pass
+
+    def get_body_pose(self) -> dict:
+        """
+        Get complete body pose.
+
+        Returns:
+            Dict with height, roll, pitch, yaw, x_offset, y_offset
+        """
+        roll, pitch, yaw = self.get_body_orientation()
+        pos = self.get_body_position()
+        return {
+            "height": self.get_body_height(),
+            "roll": roll,
+            "pitch": pitch,
+            "yaw": yaw,
+            "x_offset": pos.x,
+            "y_offset": pos.y,
+        }
+
+    # =========================================================================
+    # Preset poses
+    # =========================================================================
+
+    def reset_pose(self) -> ActionResult:
+        """
+        Reset to default/neutral body pose.
+
+        Returns:
+            ActionResult
+        """
+        height_min, height_max = self.get_height_limits()
+        default_height = (height_min + height_max) / 2
+        return self.set_body_pose(
+            height=default_height,
+            roll=0.0,
+            pitch=0.0,
+            yaw=0.0,
+            x_offset=0.0,
+            y_offset=0.0
+        )
+
+    def lower_body(self, height: Optional[float] = None) -> ActionResult:
+        """
+        Lower body (for grasping objects on ground).
+
+        Args:
+            height: Target height (None = minimum safe height)
+
+        Returns:
+            ActionResult
+        """
+        if height is None:
+            height = self.get_height_limits()[0] + 0.02  # 2cm above min
+        return self.set_body_height(height)
+
+    def raise_body(self, height: Optional[float] = None) -> ActionResult:
+        """
+        Raise body to maximum comfortable height.
+
+        Args:
+            height: Target height (None = default standing height)
+
+        Returns:
+            ActionResult
+        """
+        if height is None:
+            height_min, height_max = self.get_height_limits()
+            height = (height_min + height_max) / 2
+        return self.set_body_height(height)
+
+    def look_at(self, direction: Vector3) -> ActionResult:
+        """
+        Tilt body to "look" in a direction.
+
+        Adjusts pitch and yaw to point forward axis toward direction.
+
+        Args:
+            direction: Direction to look (in robot base frame)
+
+        Returns:
+            ActionResult
+        """
+        import math
+
+        # Calculate required yaw and pitch
+        yaw = math.atan2(direction.y, direction.x)
+        horizontal_dist = math.sqrt(direction.x**2 + direction.y**2)
+        pitch = -math.atan2(direction.z, horizontal_dist)
+
+        # Clamp to limits
+        roll_lim, pitch_lim, yaw_lim = self.get_orientation_limits()
+        pitch = max(pitch_lim[0], min(pitch_lim[1], pitch))
+        yaw = max(yaw_lim[0], min(yaw_lim[1], yaw))
+
+        return self.set_body_orientation(roll=0.0, pitch=pitch, yaw=yaw)