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

ate/robot/primitives.py

@@ -0,0 +1,614 @@
+"""
+Robot Skill Primitives - Building blocks for complex behaviors.
+
+Primitives are the lowest-level reusable skills:
+- Single servo movements or simple transitions
+- Parameterized for flexibility
+- Composable into higher-level behaviors
+
+Skill Hierarchy:
+  Primitive → Compound → Behavior
+
+  Primitive: gripper_open(), arm_up()
+  Compound:  pickup() = [arm_down, gripper_open, gripper_close, arm_up]
+  Behavior:  fetch() = detect + approach + pickup + carry + place
+"""
+
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Callable, Any, Union
+from enum import Enum
+from pathlib import Path
+import json
+
+
+class SkillType(Enum):
+    """Classification of skill complexity."""
+    PRIMITIVE = "primitive"   # Single action (open gripper)
+    COMPOUND = "compound"     # Sequence of primitives (pickup)
+    BEHAVIOR = "behavior"     # Complex with perception/logic (fetch)
+
+
+class HardwareRequirement(Enum):
+    """Hardware capabilities required by a skill."""
+    GRIPPER = "gripper"
+    ARM = "arm"
+    LEGS = "legs"
+    CAMERA = "camera"
+    DISTANCE_SENSOR = "distance_sensor"
+
+
+@dataclass
+class SkillParameter:
+    """Parameter definition for a skill."""
+    name: str
+    param_type: str  # "float", "int", "bool", "str"
+    default: Any
+    min_value: Optional[float] = None
+    max_value: Optional[float] = None
+    description: str = ""
+
+
+@dataclass
+class Primitive:
+    """A primitive skill - the building block."""
+    name: str
+    description: str
+    servo_targets: Dict[int, int]  # servo_id -> target position
+    duration_ms: int = 500
+    parameters: List[SkillParameter] = field(default_factory=list)
+    hardware: List[HardwareRequirement] = field(default_factory=list)
+
+    def to_dict(self) -> Dict:
+        return {
+            "name": self.name,
+            "skill_type": SkillType.PRIMITIVE.value,
+            "description": self.description,
+            "servo_targets": self.servo_targets,
+            "duration_ms": self.duration_ms,
+            "parameters": [
+                {"name": p.name, "type": p.param_type, "default": p.default}
+                for p in self.parameters
+            ],
+            "hardware": [h.value for h in self.hardware],
+        }
+
+
+@dataclass
+class CompoundSkill:
+    """A compound skill - sequence of primitives."""
+    name: str
+    description: str
+    steps: List[str]  # Names of primitives to execute
+    wait_between_ms: int = 200
+    parameters: List[SkillParameter] = field(default_factory=list)
+    hardware: List[HardwareRequirement] = field(default_factory=list)
+
+    def to_dict(self) -> Dict:
+        return {
+            "name": self.name,
+            "skill_type": SkillType.COMPOUND.value,
+            "description": self.description,
+            "steps": self.steps,
+            "wait_between_ms": self.wait_between_ms,
+            "parameters": [
+                {"name": p.name, "type": p.param_type, "default": p.default}
+                for p in self.parameters
+            ],
+            "hardware": [h.value for h in self.hardware],
+        }
+
+
+@dataclass
+class Behavior:
+    """A behavior - complex skill with perception and logic."""
+    name: str
+    description: str
+    requires_perception: bool = True
+    perception_hook: Optional[str] = None  # Function name for detection
+    verify_hook: Optional[str] = None  # Function to verify success
+    steps: List[Union[str, Dict]] = field(default_factory=list)  # Primitives or conditions
+    parameters: List[SkillParameter] = field(default_factory=list)
+    hardware: List[HardwareRequirement] = field(default_factory=list)
+
+    def to_dict(self) -> Dict:
+        return {
+            "name": self.name,
+            "skill_type": SkillType.BEHAVIOR.value,
+            "description": self.description,
+            "requires_perception": self.requires_perception,
+            "perception_hook": self.perception_hook,
+            "verify_hook": self.verify_hook,
+            "steps": self.steps,
+            "parameters": [
+                {"name": p.name, "type": p.param_type, "default": p.default}
+                for p in self.parameters
+            ],
+            "hardware": [h.value for h in self.hardware],
+        }
+
+
+# =============================================================================
+# MechDog Primitives - Based on known servo configuration
+# =============================================================================
+
+# Servo mapping for HiWonder MechDog
+MECHDOG_SERVOS = {
+    # Legs (quadruped)
+    1: {"name": "front_right_hip", "group": "front_right_leg"},
+    2: {"name": "front_right_thigh", "group": "front_right_leg"},
+    3: {"name": "front_left_hip", "group": "front_left_leg"},
+    4: {"name": "front_left_thigh", "group": "front_left_leg"},
+    5: {"name": "rear_right_hip", "group": "rear_right_leg"},
+    6: {"name": "rear_right_thigh", "group": "rear_right_leg"},
+    7: {"name": "rear_left_hip", "group": "rear_left_leg"},
+    8: {"name": "rear_left_thigh", "group": "rear_left_leg"},
+    # Arm (optional attachment)
+    9: {"name": "arm_shoulder", "group": "arm"},
+    10: {"name": "arm_elbow", "group": "arm"},
+    11: {"name": "gripper", "group": "arm"},
+}
+
+# Known positions from calibration
+MECHDOG_POSITIONS = {
+    # Standing pose (from previous discovery)
+    "stand": {1: 2096, 2: 1621, 3: 2170, 4: 1611, 5: 904, 6: 1379, 7: 830, 8: 1389},
+
+    # Arm positions
+    "arm_up": {9: 500, 10: 1500, 11: 2500},
+    "arm_down": {9: 1800, 10: 1200, 11: 2500},
+    "arm_forward": {9: 1200, 10: 1500, 11: 2500},
+
+    # Gripper
+    "gripper_open": {11: 2500},
+    "gripper_closed": {11: 200},
+
+    # Body tilts (adjusting leg positions)
+    "tilt_forward": {
+        1: 2096, 2: 1900,  # Front right - extend
+        3: 2170, 4: 1900,  # Front left - extend
+        5: 904, 6: 1100,   # Rear right - retract
+        7: 830, 8: 1100,   # Rear left - retract
+    },
+    "tilt_back": {
+        1: 2096, 2: 1400,  # Front right - retract
+        3: 2170, 4: 1400,  # Front left - retract
+        5: 904, 6: 1600,   # Rear right - extend
+        7: 830, 8: 1600,   # Rear left - extend
+    },
+}
+
+
+def create_mechdog_primitives() -> Dict[str, Primitive]:
+    """Create the primitive library for MechDog."""
+    primitives = {}
+
+    # Gripper primitives
+    primitives["gripper_open"] = Primitive(
+        name="gripper_open",
+        description="Open the gripper fully",
+        servo_targets={11: 2500},
+        duration_ms=400,
+        hardware=[HardwareRequirement.GRIPPER],
+    )
+
+    primitives["gripper_close"] = Primitive(
+        name="gripper_close",
+        description="Close the gripper fully",
+        servo_targets={11: 200},
+        duration_ms=400,
+        hardware=[HardwareRequirement.GRIPPER],
+    )
+
+    primitives["gripper_half"] = Primitive(
+        name="gripper_half",
+        description="Gripper at half position",
+        servo_targets={11: 1350},
+        duration_ms=300,
+        hardware=[HardwareRequirement.GRIPPER],
+    )
+
+    # Arm primitives
+    primitives["arm_up"] = Primitive(
+        name="arm_up",
+        description="Raise arm to up position",
+        servo_targets={9: 500, 10: 1500},
+        duration_ms=500,
+        hardware=[HardwareRequirement.ARM],
+    )
+
+    primitives["arm_down"] = Primitive(
+        name="arm_down",
+        description="Lower arm to down/reaching position",
+        servo_targets={9: 1800, 10: 1200},
+        duration_ms=600,
+        hardware=[HardwareRequirement.ARM],
+    )
+
+    primitives["arm_forward"] = Primitive(
+        name="arm_forward",
+        description="Extend arm forward",
+        servo_targets={9: 1200, 10: 1500},
+        duration_ms=500,
+        hardware=[HardwareRequirement.ARM],
+    )
+
+    primitives["arm_carry"] = Primitive(
+        name="arm_carry",
+        description="Arm in carrying position (up and tucked)",
+        servo_targets={9: 800, 10: 1800},
+        duration_ms=500,
+        hardware=[HardwareRequirement.ARM],
+    )
+
+    # Body tilt primitives
+    primitives["tilt_forward"] = Primitive(
+        name="tilt_forward",
+        description="Tilt body forward (front legs extend, rear retract)",
+        servo_targets=MECHDOG_POSITIONS["tilt_forward"],
+        duration_ms=600,
+        hardware=[HardwareRequirement.LEGS],
+    )
+
+    primitives["tilt_back"] = Primitive(
+        name="tilt_back",
+        description="Tilt body backward",
+        servo_targets=MECHDOG_POSITIONS["tilt_back"],
+        duration_ms=600,
+        hardware=[HardwareRequirement.LEGS],
+    )
+
+    primitives["stand"] = Primitive(
+        name="stand",
+        description="Return to neutral standing position",
+        servo_targets=MECHDOG_POSITIONS["stand"],
+        duration_ms=500,
+        hardware=[HardwareRequirement.LEGS],
+    )
+
+    # Combined arm+gripper primitives
+    primitives["ready_to_grab"] = Primitive(
+        name="ready_to_grab",
+        description="Arm down with gripper open, ready to grab",
+        servo_targets={9: 1800, 10: 1200, 11: 2500},
+        duration_ms=600,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    primitives["grab_and_lift"] = Primitive(
+        name="grab_and_lift",
+        description="Close gripper and lift arm",
+        servo_targets={9: 800, 10: 1800, 11: 200},
+        duration_ms=600,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    return primitives
+
+
+def create_mechdog_compounds(primitives: Dict[str, Primitive]) -> Dict[str, CompoundSkill]:
+    """Create compound skills from primitives."""
+    compounds = {}
+
+    # Pickup sequence
+    compounds["pickup"] = CompoundSkill(
+        name="pickup",
+        description="Pick up an object from the ground",
+        steps=["arm_up", "gripper_open", "arm_down", "gripper_close", "arm_carry"],
+        wait_between_ms=200,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    # Place sequence
+    compounds["place"] = CompoundSkill(
+        name="place",
+        description="Place held object on the ground",
+        steps=["arm_down", "gripper_open", "arm_up"],
+        wait_between_ms=200,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    # Pickup with tilt (for objects further away)
+    compounds["tilt_pickup"] = CompoundSkill(
+        name="tilt_pickup",
+        description="Tilt forward and pick up object",
+        steps=["tilt_forward", "ready_to_grab", "gripper_close", "arm_carry", "stand"],
+        wait_between_ms=300,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER, HardwareRequirement.LEGS],
+    )
+
+    # Wave greeting
+    compounds["wave"] = CompoundSkill(
+        name="wave",
+        description="Wave the arm as a greeting",
+        steps=["arm_up", "gripper_open", "arm_forward", "arm_up", "arm_forward", "arm_up"],
+        wait_between_ms=150,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    # Show object (lift and present)
+    compounds["present"] = CompoundSkill(
+        name="present",
+        description="Present a held object",
+        steps=["arm_forward", "gripper_half", "arm_up"],
+        wait_between_ms=300,
+        hardware=[HardwareRequirement.ARM, HardwareRequirement.GRIPPER],
+    )
+
+    return compounds
+
+
+def create_mechdog_behaviors() -> Dict[str, Behavior]:
+    """Create behaviors that integrate perception."""
+    behaviors = {}
+
+    # Fetch behavior - find, approach, pickup, return
+    behaviors["fetch"] = Behavior(
+        name="fetch",
+        description="Find an object, approach it, pick it up, and return",
+        requires_perception=True,
+        perception_hook="detect_target",
+        verify_hook="verify_holding_object",
+        steps=[
+            {"action": "detect", "target": "object"},
+            {"action": "while", "condition": "target.distance > 0.15", "do": "approach"},
+            {"action": "skill", "name": "tilt_pickup"},
+            {"action": "verify", "check": "holding_object"},
+            {"action": "skill", "name": "stand"},
+        ],
+        hardware=[
+            HardwareRequirement.ARM,
+            HardwareRequirement.GRIPPER,
+            HardwareRequirement.LEGS,
+            HardwareRequirement.CAMERA,
+        ],
+    )
+
+    # Approach and pickup green ball
+    behaviors["pickup_green_ball"] = Behavior(
+        name="pickup_green_ball",
+        description="Detect green ball, approach, and pick it up",
+        requires_perception=True,
+        perception_hook="detect_green_object",
+        verify_hook="verify_gripper_closed",
+        steps=[
+            {"action": "detect", "target": "green_ball", "color": "green"},
+            {"action": "align", "target": "green_ball"},
+            {"action": "while", "condition": "distance > 0.2", "do": "step_forward"},
+            {"action": "skill", "name": "tilt_forward"},
+            {"action": "skill", "name": "ready_to_grab"},
+            {"action": "wait_ms", "duration": 300},
+            {"action": "skill", "name": "gripper_close"},
+            {"action": "wait_ms", "duration": 200},
+            {"action": "skill", "name": "arm_carry"},
+            {"action": "skill", "name": "stand"},
+            {"action": "verify", "check": "gripper_closed"},
+        ],
+        hardware=[
+            HardwareRequirement.ARM,
+            HardwareRequirement.GRIPPER,
+            HardwareRequirement.LEGS,
+            HardwareRequirement.CAMERA,
+        ],
+    )
+
+    # Scan environment
+    behaviors["scan"] = Behavior(
+        name="scan",
+        description="Scan environment for objects",
+        requires_perception=True,
+        perception_hook="detect_all_objects",
+        steps=[
+            {"action": "skill", "name": "stand"},
+            {"action": "detect", "target": "all"},
+            {"action": "return", "data": "detected_objects"},
+        ],
+        hardware=[HardwareRequirement.CAMERA],
+    )
+
+    return behaviors
+
+
+class PrimitiveLibrary:
+    """
+    Manages the complete skill library for a robot.
+
+    Provides:
+    - Primitive execution
+    - Compound skill sequencing
+    - Behavior orchestration with perception
+    """
+
+    def __init__(self, robot_interface=None):
+        self.robot = robot_interface
+        self.primitives = create_mechdog_primitives()
+        self.compounds = create_mechdog_compounds(self.primitives)
+        self.behaviors = create_mechdog_behaviors()
+
+    def list_all(self) -> Dict[str, List[str]]:
+        """List all available skills by type."""
+        return {
+            "primitives": list(self.primitives.keys()),
+            "compounds": list(self.compounds.keys()),
+            "behaviors": list(self.behaviors.keys()),
+        }
+
+    def get_skill(self, name: str) -> Optional[Union[Primitive, CompoundSkill, Behavior]]:
+        """Get a skill by name."""
+        if name in self.primitives:
+            return self.primitives[name]
+        if name in self.compounds:
+            return self.compounds[name]
+        if name in self.behaviors:
+            return self.behaviors[name]
+        return None
+
+    def execute_primitive(self, name: str, speed_factor: float = 1.0) -> bool:
+        """Execute a primitive skill."""
+        if not self.robot:
+            print(f"No robot connected - would execute: {name}")
+            return False
+
+        prim = self.primitives.get(name)
+        if not prim:
+            print(f"Unknown primitive: {name}")
+            return False
+
+        duration = int(prim.duration_ms / speed_factor)
+        for servo_id, position in prim.servo_targets.items():
+            self.robot.set_servo(servo_id, position, duration)
+
+        time.sleep(duration / 1000 + 0.05)
+        return True
+
+    def execute_compound(self, name: str, speed_factor: float = 1.0) -> bool:
+        """Execute a compound skill."""
+        compound = self.compounds.get(name)
+        if not compound:
+            print(f"Unknown compound: {name}")
+            return False
+
+        print(f"Executing compound: {name}")
+        for step_name in compound.steps:
+            print(f"  Step: {step_name}")
+            if not self.execute_primitive(step_name, speed_factor):
+                return False
+            time.sleep(compound.wait_between_ms / 1000)
+
+        return True
+
+    def execute(self, name: str, speed_factor: float = 1.0) -> bool:
+        """Execute any skill by name."""
+        if name in self.primitives:
+            return self.execute_primitive(name, speed_factor)
+        if name in self.compounds:
+            return self.execute_compound(name, speed_factor)
+        if name in self.behaviors:
+            print(f"Behavior execution requires perception - use execute_behavior()")
+            return False
+
+        print(f"Unknown skill: {name}")
+        return False
+
+    def export_all(self) -> Dict:
+        """Export all skills as serializable dict."""
+        return {
+            "primitives": {name: p.to_dict() for name, p in self.primitives.items()},
+            "compounds": {name: c.to_dict() for name, c in self.compounds.items()},
+            "behaviors": {name: b.to_dict() for name, b in self.behaviors.items()},
+        }
+
+    def save(self, path: Path):
+        """Save library to JSON file."""
+        with open(path, 'w') as f:
+            json.dump(self.export_all(), f, indent=2)
+
+    def generate_python_code(self, include_behaviors: bool = False) -> str:
+        """Generate Python code for all skills."""
+        lines = [
+            '"""',
+            'Auto-generated MechDog skill primitives.',
+            '',
+            'Usage:',
+            '    from mechdog_skills import *',
+            '    ',
+            '    # Execute primitives',
+            '    gripper_open(robot)',
+            '    arm_down(robot)',
+            '    ',
+            '    # Execute compounds',
+            '    pickup(robot)',
+            '    tilt_pickup(robot)',
+            '"""',
+            '',
+            'import time',
+            'from typing import Optional',
+            '',
+            '',
+            '# =============================================================================',
+            '# Primitives',
+            '# =============================================================================',
+            '',
+        ]
+
+        # Generate primitive functions
+        for name, prim in self.primitives.items():
+            lines.append(f'def {name}(robot, speed_factor: float = 1.0) -> bool:')
+            lines.append(f'    """{prim.description}"""')
+            lines.append(f'    duration = int({prim.duration_ms} / speed_factor)')
+            for servo_id, pos in prim.servo_targets.items():
+                servo_info = MECHDOG_SERVOS.get(servo_id, {})
+                servo_name = servo_info.get("name", f"servo_{servo_id}")
+                lines.append(f'    robot.set_servo({servo_id}, {pos}, duration)  # {servo_name}')
+            lines.append(f'    time.sleep(duration / 1000 + 0.05)')
+            lines.append(f'    return True')
+            lines.append('')
+            lines.append('')
+
+        # Generate compound functions
+        lines.append('# =============================================================================')
+        lines.append('# Compound Skills')
+        lines.append('# =============================================================================')
+        lines.append('')
+
+        for name, compound in self.compounds.items():
+            lines.append(f'def {name}(robot, speed_factor: float = 1.0) -> bool:')
+            lines.append(f'    """{compound.description}"""')
+            for step in compound.steps:
+                lines.append(f'    {step}(robot, speed_factor)')
+                lines.append(f'    time.sleep({compound.wait_between_ms} / 1000)')
+            lines.append(f'    return True')
+            lines.append('')
+            lines.append('')
+
+        if include_behaviors:
+            lines.append('# =============================================================================')
+            lines.append('# Behaviors (require perception)')
+            lines.append('# =============================================================================')
+            lines.append('')
+
+            for name, behavior in self.behaviors.items():
+                lines.append(f'def {name}(robot, detector, speed_factor: float = 1.0) -> bool:')
+                lines.append(f'    """')
+                lines.append(f'    {behavior.description}')
+                lines.append(f'    ')
+                lines.append(f'    Requires: {", ".join(h.value for h in behavior.hardware)}')
+                lines.append(f'    """')
+                lines.append(f'    # TODO: Implement perception-based behavior')
+                lines.append(f'    # Steps: {behavior.steps}')
+                lines.append(f'    raise NotImplementedError("Behavior requires perception integration")')
+                lines.append('')
+                lines.append('')
+
+        return '\n'.join(lines)
+
+
+# =============================================================================
+# CLI integration
+# =============================================================================
+
+def list_primitives():
+    """List all available primitives."""
+    lib = PrimitiveLibrary()
+    all_skills = lib.list_all()
+
+    print("\n=== PRIMITIVES ===")
+    for name in all_skills["primitives"]:
+        prim = lib.primitives[name]
+        print(f"  {name}: {prim.description}")
+
+    print("\n=== COMPOUNDS ===")
+    for name in all_skills["compounds"]:
+        comp = lib.compounds[name]
+        print(f"  {name}: {comp.description}")
+        print(f"    Steps: {' → '.join(comp.steps)}")
+
+    print("\n=== BEHAVIORS ===")
+    for name in all_skills["behaviors"]:
+        beh = lib.behaviors[name]
+        print(f"  {name}: {beh.description}")
+        print(f"    Requires: {', '.join(h.value for h in beh.hardware)}")
+
+
+if __name__ == "__main__":
+    list_primitives()