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

ate/robot/behaviors.py

@@ -0,0 +1,493 @@
+"""
+Behavior Executor - Runs complex skills with perception integration.
+
+Behaviors are the highest level of skill:
+  - Integrate camera/detection
+  - Have loops and conditions
+  - Verify success/failure
+  - Handle recovery
+
+Example flow:
+  1. detect_green_ball() → returns target location
+  2. while distance > threshold: approach()
+  3. tilt_pickup()
+  4. verify_holding() → success/retry
+"""
+
+import time
+import json
+from dataclasses import dataclass
+from typing import Dict, List, Optional, Callable, Any, Tuple
+from pathlib import Path
+from enum import Enum
+
+from .primitives import PrimitiveLibrary, Behavior, HardwareRequirement
+
+
+class BehaviorResult(Enum):
+    """Result of behavior execution."""
+    SUCCESS = "success"
+    FAILED = "failed"
+    NO_TARGET = "no_target"
+    TIMEOUT = "timeout"
+    ABORTED = "aborted"
+
+
+@dataclass
+class DetectionResult:
+    """Result from object detection."""
+    found: bool
+    label: str = ""
+    confidence: float = 0.0
+    x: float = 0.0  # Normalized x position (0-1, center is 0.5)
+    y: float = 0.0  # Normalized y position
+    width: float = 0.0  # Normalized width
+    height: float = 0.0  # Normalized height
+    distance: Optional[float] = None  # Estimated distance in meters
+
+    @property
+    def center_offset(self) -> float:
+        """How far from center (negative=left, positive=right)."""
+        return self.x - 0.5
+
+    @property
+    def is_centered(self) -> bool:
+        """Is target roughly centered?"""
+        return abs(self.center_offset) < 0.15
+
+
+class BehaviorExecutor:
+    """
+    Executes complex behaviors with perception.
+
+    Coordinates:
+    - Detection (finding targets)
+    - Movement (primitives and locomotion)
+    - Verification (checking success)
+    """
+
+    def __init__(
+        self,
+        robot_interface,
+        camera_interface=None,
+        detector=None,
+    ):
+        self.robot = robot_interface
+        self.camera = camera_interface
+        self.detector = detector
+        self.primitives = PrimitiveLibrary(robot_interface)
+
+        # State
+        self.current_target: Optional[DetectionResult] = None
+        self.is_holding_object: bool = False
+
+        # Configuration
+        self.approach_distance = 0.15  # meters
+        self.alignment_threshold = 0.1  # normalized x offset
+        self.max_approach_steps = 20
+        self.step_delay = 0.5  # seconds between movement steps
+
+    def detect(self, target_type: str = "any", color: Optional[str] = None) -> DetectionResult:
+        """
+        Detect objects using the camera.
+
+        Args:
+            target_type: Type of object to find ("any", "ball", "cube", etc.)
+            color: Optional color filter ("green", "red", "blue")
+
+        Returns:
+            DetectionResult with target info
+        """
+        if not self.detector:
+            print("No detector configured - simulating detection")
+            # Return simulated result for testing
+            return DetectionResult(
+                found=True,
+                label=f"{color or ''} {target_type}".strip(),
+                confidence=0.85,
+                x=0.5,  # Centered
+                y=0.6,  # Lower half of frame
+                width=0.1,
+                height=0.1,
+                distance=0.3,  # 30cm away
+            )
+
+        # Real detection
+        frame = self.camera.capture() if self.camera else None
+        if frame is None:
+            return DetectionResult(found=False)
+
+        # Use detector to find objects
+        detections = self.detector.detect(frame)
+
+        # Filter by target type and color
+        for det in detections:
+            if target_type != "any" and det.label != target_type:
+                continue
+            if color and not det.label.lower().startswith(color.lower()):
+                continue
+
+            # Estimate distance based on bounding box size
+            # Larger box = closer object
+            estimated_distance = self._estimate_distance(det.width, det.height)
+
+            return DetectionResult(
+                found=True,
+                label=det.label,
+                confidence=det.confidence,
+                x=det.x,
+                y=det.y,
+                width=det.width,
+                height=det.height,
+                distance=estimated_distance,
+            )
+
+        return DetectionResult(found=False)
+
+    def _estimate_distance(self, width: float, height: float) -> float:
+        """
+        Estimate distance to object based on apparent size.
+
+        This is a rough approximation. For accurate distance,
+        use stereo vision, depth camera, or LIDAR.
+        """
+        # Assume a ball ~5cm diameter
+        # At 15cm distance, it would appear ~30% of frame width
+        # At 30cm distance, it would appear ~15% of frame width
+        apparent_size = max(width, height)
+        if apparent_size < 0.05:
+            return 1.0  # Far away
+        elif apparent_size > 0.4:
+            return 0.1  # Very close
+        else:
+            # Inverse relationship: distance ≈ k / size
+            return 0.15 / apparent_size
+
+    def align_to_target(self, target: DetectionResult) -> bool:
+        """
+        Rotate robot to center target in frame.
+
+        Returns True when aligned.
+        """
+        if target.is_centered:
+            return True
+
+        offset = target.center_offset
+        print(f"Aligning: offset = {offset:.2f}")
+
+        # Turn toward target
+        # Positive offset = target is on right = turn right
+        if offset > self.alignment_threshold:
+            self._turn_right()
+        elif offset < -self.alignment_threshold:
+            self._turn_left()
+
+        time.sleep(0.3)
+        return False
+
+    def approach_target(self, target: DetectionResult) -> bool:
+        """
+        Move toward target until within approach_distance.
+
+        Returns True when close enough.
+        """
+        if target.distance and target.distance <= self.approach_distance:
+            return True
+
+        print(f"Approaching: distance = {target.distance:.2f}m")
+        self._step_forward()
+        time.sleep(self.step_delay)
+        return False
+
+    def _step_forward(self):
+        """Take one step forward (placeholder for locomotion)."""
+        print("  [Step forward]")
+        # TODO: Integrate with locomotion primitives
+        # For now, we assume the robot has a walk command
+        if hasattr(self.robot, 'walk'):
+            self.robot.walk('forward', 1)
+
+    def _turn_left(self):
+        """Turn left (placeholder)."""
+        print("  [Turn left]")
+        if hasattr(self.robot, 'turn'):
+            self.robot.turn('left', 15)
+
+    def _turn_right(self):
+        """Turn right (placeholder)."""
+        print("  [Turn right]")
+        if hasattr(self.robot, 'turn'):
+            self.robot.turn('right', 15)
+
+    def verify_holding_object(self) -> bool:
+        """
+        Verify that gripper is holding an object.
+
+        Could use:
+        - Gripper position feedback
+        - Force sensor
+        - Visual detection of object in gripper
+        """
+        # For now, assume success if gripper closed
+        # TODO: Add actual verification
+        return self.is_holding_object
+
+    def execute_behavior(
+        self,
+        behavior_name: str,
+        timeout: float = 30.0,
+        **kwargs
+    ) -> BehaviorResult:
+        """
+        Execute a named behavior.
+
+        This is the main entry point for running complex skills.
+        """
+        behavior = self.primitives.behaviors.get(behavior_name)
+        if not behavior:
+            print(f"Unknown behavior: {behavior_name}")
+            return BehaviorResult.FAILED
+
+        print(f"\n=== Executing: {behavior_name} ===")
+        print(f"Description: {behavior.description}")
+        print(f"Steps: {len(behavior.steps)}")
+
+        start_time = time.time()
+
+        try:
+            for step in behavior.steps:
+                # Check timeout
+                if time.time() - start_time > timeout:
+                    print("Timeout!")
+                    return BehaviorResult.TIMEOUT
+
+                result = self._execute_step(step, **kwargs)
+                if result == BehaviorResult.FAILED:
+                    return result
+                if result == BehaviorResult.NO_TARGET:
+                    return result
+
+            return BehaviorResult.SUCCESS
+
+        except KeyboardInterrupt:
+            print("\nAborted by user")
+            return BehaviorResult.ABORTED
+
+        except Exception as e:
+            print(f"Error: {e}")
+            return BehaviorResult.FAILED
+
+    def _execute_step(self, step: Dict, **kwargs) -> BehaviorResult:
+        """Execute a single step in a behavior."""
+        action = step.get("action") if isinstance(step, dict) else step
+
+        if action == "detect":
+            target_type = step.get("target", "any")
+            color = step.get("color")
+            print(f"Detecting: {color or ''} {target_type}")
+
+            result = self.detect(target_type, color)
+            if not result.found:
+                print("  No target found")
+                return BehaviorResult.NO_TARGET
+
+            self.current_target = result
+            print(f"  Found: {result.label} at distance {result.distance:.2f}m")
+            return BehaviorResult.SUCCESS
+
+        elif action == "align":
+            if not self.current_target:
+                return BehaviorResult.NO_TARGET
+
+            print("Aligning to target...")
+            for _ in range(10):  # Max 10 alignment attempts
+                # Re-detect to get current position
+                self.current_target = self.detect(
+                    step.get("target", "any"),
+                    step.get("color")
+                )
+                if not self.current_target.found:
+                    return BehaviorResult.NO_TARGET
+                if self.align_to_target(self.current_target):
+                    print("  Aligned!")
+                    return BehaviorResult.SUCCESS
+
+            print("  Failed to align")
+            return BehaviorResult.FAILED
+
+        elif action == "while":
+            condition = step.get("condition", "")
+            do_action = step.get("do", "")
+            print(f"Loop: while {condition} do {do_action}")
+
+            for _ in range(self.max_approach_steps):
+                # Re-detect
+                self.current_target = self.detect()
+                if not self.current_target.found:
+                    return BehaviorResult.NO_TARGET
+
+                # Check condition
+                if "distance" in condition:
+                    threshold = float(condition.split(">")[-1].strip())
+                    if self.current_target.distance <= threshold:
+                        print("  Condition met")
+                        return BehaviorResult.SUCCESS
+
+                # Execute action
+                if do_action == "step_forward":
+                    self._step_forward()
+                elif do_action == "approach":
+                    self.approach_target(self.current_target)
+
+                time.sleep(0.3)
+
+            return BehaviorResult.TIMEOUT
+
+        elif action == "skill":
+            skill_name = step.get("name", "")
+            print(f"Executing skill: {skill_name}")
+            if self.primitives.execute(skill_name):
+                if "gripper_close" in skill_name:
+                    self.is_holding_object = True
+                elif "gripper_open" in skill_name:
+                    self.is_holding_object = False
+                return BehaviorResult.SUCCESS
+            return BehaviorResult.FAILED
+
+        elif action == "wait_ms":
+            duration = step.get("duration", 0)
+            print(f"Waiting {duration}ms")
+            time.sleep(duration / 1000)
+            return BehaviorResult.SUCCESS
+
+        elif action == "verify":
+            check = step.get("check", "")
+            print(f"Verifying: {check}")
+            if check == "gripper_closed" or check == "holding_object":
+                if self.verify_holding_object():
+                    print("  Verified!")
+                    return BehaviorResult.SUCCESS
+                print("  Verification failed")
+                return BehaviorResult.FAILED
+
+            return BehaviorResult.SUCCESS
+
+        elif action == "return":
+            # End of behavior with return data
+            return BehaviorResult.SUCCESS
+
+        else:
+            print(f"Unknown action: {action}")
+            return BehaviorResult.FAILED
+
+    def pickup_green_ball(self) -> BehaviorResult:
+        """
+        High-level behavior: Find and pick up a green ball.
+
+        This is the composed behavior the user requested.
+        """
+        print("\n" + "=" * 60)
+        print("BEHAVIOR: Pickup Green Ball")
+        print("=" * 60)
+
+        # Step 1: Detect green ball
+        print("\n[1] Detecting green ball...")
+        target = self.detect("ball", "green")
+        if not target.found:
+            print("    No green ball found!")
+            return BehaviorResult.NO_TARGET
+
+        print(f"    Found! Distance: {target.distance:.2f}m, Offset: {target.center_offset:.2f}")
+        self.current_target = target
+
+        # Step 2: Align to target
+        print("\n[2] Aligning to target...")
+        for _ in range(5):
+            target = self.detect("ball", "green")
+            if not target.found:
+                print("    Lost target!")
+                return BehaviorResult.NO_TARGET
+            if target.is_centered:
+                print("    Aligned!")
+                break
+            self.align_to_target(target)
+            time.sleep(0.3)
+
+        # Step 3: Approach until close enough
+        print("\n[3] Approaching target...")
+        for i in range(self.max_approach_steps):
+            target = self.detect("ball", "green")
+            if not target.found:
+                print("    Lost target!")
+                return BehaviorResult.NO_TARGET
+
+            if target.distance <= self.approach_distance:
+                print(f"    Close enough! ({target.distance:.2f}m)")
+                break
+
+            print(f"    Step {i+1}: distance = {target.distance:.2f}m")
+            self._step_forward()
+            time.sleep(self.step_delay)
+        else:
+            print("    Too many steps, giving up")
+            return BehaviorResult.TIMEOUT
+
+        # Step 4: Tilt forward and prepare
+        print("\n[4] Tilting forward...")
+        self.primitives.execute("tilt_forward")
+        time.sleep(0.3)
+
+        # Step 5: Ready to grab
+        print("\n[5] Preparing gripper...")
+        self.primitives.execute("ready_to_grab")
+        time.sleep(0.3)
+
+        # Step 6: Close gripper
+        print("\n[6] Grasping...")
+        self.primitives.execute("gripper_close")
+        self.is_holding_object = True
+        time.sleep(0.3)
+
+        # Step 7: Lift and stand
+        print("\n[7] Lifting and standing...")
+        self.primitives.execute("arm_carry")
+        time.sleep(0.2)
+        self.primitives.execute("stand")
+
+        # Step 8: Verify
+        print("\n[8] Verifying...")
+        if self.verify_holding_object():
+            print("    SUCCESS! Holding object.")
+            return BehaviorResult.SUCCESS
+        else:
+            print("    Gripper may be empty")
+            return BehaviorResult.FAILED
+
+
+def run_behavior_demo():
+    """Demo the behavior executor."""
+    print("=" * 60)
+    print("BEHAVIOR EXECUTOR DEMO")
+    print("=" * 60)
+
+    # Create executor without real robot (simulation mode)
+    executor = BehaviorExecutor(
+        robot_interface=None,
+        camera_interface=None,
+        detector=None,  # Will use simulated detection
+    )
+
+    # Show available behaviors
+    print("\nAvailable behaviors:")
+    for name, beh in executor.primitives.behaviors.items():
+        print(f"  - {name}: {beh.description}")
+
+    # Run the pickup_green_ball behavior
+    print("\n" + "-" * 60)
+    result = executor.pickup_green_ball()
+    print("\n" + "-" * 60)
+    print(f"Result: {result.value}")
+
+
+if __name__ == "__main__":
+    run_behavior_demo()