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

ate/robot/servo_mapper.py

@@ -0,0 +1,1153 @@
+"""
+Agentic Servo Mapper - Automatically discover and map servo capabilities.
+
+Uses an LLM agent to systematically explore servos and generate primitive skills.
+Optionally uses camera feedback to verify movements and understand what each
+servo controls.
+
+Architecture:
+- ServoProbe: Low-level servo testing and measurement
+- ServoMapperAgent: LLM-guided exploration and analysis
+- PrimitiveGenerator: Creates skill definitions from discovered mappings
+
+Usage:
+    from ate.robot.servo_mapper import run_servo_mapping
+
+    # Map all servos and generate primitives
+    primitives = run_servo_mapping(
+        serial_port="/dev/cu.usbserial-10",
+        num_servos=13,
+        use_camera=True,
+    )
+"""
+
+import time
+import json
+from dataclasses import dataclass, field, asdict
+from typing import Optional, List, Dict, Any, Callable, Tuple
+from enum import Enum
+from pathlib import Path
+
+try:
+    import cv2
+    import numpy as np
+    HAS_CV = True
+except ImportError:
+    HAS_CV = False
+
+try:
+    from ..llm_proxy import LLMProxy, LLMProxyError
+    HAS_LLM_PROXY = True
+except ImportError:
+    HAS_LLM_PROXY = False
+
+
+# ============================================================================
+# Data Classes
+# ============================================================================
+
+class ServoType(Enum):
+    """Classification of servo function."""
+    UNKNOWN = "unknown"
+    LEG_HIP = "leg_hip"           # Hip rotation (forward/back)
+    LEG_SHOULDER = "leg_shoulder"  # Upper leg (lift)
+    LEG_KNEE = "leg_knee"          # Lower leg (extend/retract)
+    ARM_SHOULDER = "arm_shoulder"  # Arm base rotation
+    ARM_ELBOW = "arm_elbow"        # Arm extension
+    GRIPPER = "gripper"            # End effector
+    HEAD_PAN = "head_pan"          # Head left/right
+    HEAD_TILT = "head_tilt"        # Head up/down
+    TAIL = "tail"                  # Tail wagging
+
+
+@dataclass
+class ServoRange:
+    """Discovered range for a servo."""
+    min_pwm: int = 500       # Minimum safe PWM value
+    max_pwm: int = 2500      # Maximum safe PWM value
+    center_pwm: int = 1500   # Center/neutral position
+    safe_min: int = 500      # Safe minimum (may be narrower than hardware)
+    safe_max: int = 2500     # Safe maximum
+
+    def to_dict(self) -> Dict:
+        return asdict(self)
+
+
+@dataclass
+class NamedPosition:
+    """A named position for a servo."""
+    name: str               # e.g., "GRIPPER_OPEN"
+    pwm_value: int          # PWM value for this position
+    description: str = ""   # Human description
+
+
+@dataclass
+class ServoMapping:
+    """Complete mapping for a single servo."""
+    servo_id: int
+    servo_type: ServoType = ServoType.UNKNOWN
+    body_part: str = ""              # e.g., "front_left_leg", "gripper"
+    joint_name: str = ""             # e.g., "hip", "elbow"
+    range: ServoRange = field(default_factory=ServoRange)
+    named_positions: List[NamedPosition] = field(default_factory=list)
+    observations: List[str] = field(default_factory=list)
+    confidence: float = 0.0          # Confidence in mapping (0-1)
+
+    def to_dict(self) -> Dict:
+        return {
+            "servo_id": self.servo_id,
+            "servo_type": self.servo_type.value,
+            "body_part": self.body_part,
+            "joint_name": self.joint_name,
+            "range": self.range.to_dict(),
+            "named_positions": [
+                {"name": p.name, "pwm": p.pwm_value, "description": p.description}
+                for p in self.named_positions
+            ],
+            "observations": self.observations,
+            "confidence": self.confidence,
+        }
+
+
+@dataclass
+class ProbeResult:
+    """Result of probing a servo at a position."""
+    servo_id: int
+    pwm_value: int
+    response_time_ms: float
+    moved: bool              # Whether robot responded
+    image_before: Optional[np.ndarray] = None
+    image_after: Optional[np.ndarray] = None
+    pixel_change: float = 0.0  # Amount of visual change (0-1)
+    error: Optional[str] = None
+
+
+# ============================================================================
+# Servo Probe - Low-level testing
+# ============================================================================
+
+class ServoProbe:
+    """
+    Low-level servo probing for discovering capabilities.
+
+    Safely tests servos through their range and measures responses.
+    """
+
+    def __init__(
+        self,
+        send_command: Callable[[str, float], str],
+        capture_fn: Optional[Callable[[], np.ndarray]] = None,
+        safe_delay: float = 0.5,
+    ):
+        """
+        Initialize probe.
+
+        Args:
+            send_command: Function to send commands to robot (cmd, wait) -> response
+            capture_fn: Optional function to capture camera images
+            safe_delay: Delay between movements for safety
+        """
+        self.send = send_command
+        self.capture = capture_fn
+        self.safe_delay = safe_delay
+
+    def probe_servo(
+        self,
+        servo_id: int,
+        pwm_value: int,
+        duration_ms: int = 500,
+    ) -> ProbeResult:
+        """
+        Move a servo to a position and measure the result.
+
+        Args:
+            servo_id: Servo ID to test
+            pwm_value: PWM value to send (500-2500)
+            duration_ms: Movement duration in milliseconds
+
+        Returns:
+            ProbeResult with timing and visual change data
+        """
+        # Capture before image
+        image_before = None
+        if self.capture:
+            try:
+                image_before = self.capture()
+            except Exception:
+                pass
+
+        # Send command and measure response time
+        start_time = time.time()
+        try:
+            response = self.send(
+                f"dog.set_servo({servo_id}, {pwm_value}, {duration_ms})",
+                duration_ms / 1000 + 0.2
+            )
+            response_time = (time.time() - start_time) * 1000
+            moved = True
+            error = None
+        except Exception as e:
+            response_time = 0
+            moved = False
+            error = str(e)
+
+        # Wait for movement to complete
+        time.sleep(self.safe_delay)
+
+        # Capture after image
+        image_after = None
+        pixel_change = 0.0
+        if self.capture and image_before is not None:
+            try:
+                image_after = self.capture()
+                # Calculate visual difference
+                if image_after is not None and image_after.size > 0:
+                    diff = cv2.absdiff(image_before, image_after)
+                    pixel_change = np.mean(diff) / 255.0
+            except Exception:
+                pass
+
+        return ProbeResult(
+            servo_id=servo_id,
+            pwm_value=pwm_value,
+            response_time_ms=response_time,
+            moved=moved,
+            image_before=image_before,
+            image_after=image_after,
+            pixel_change=pixel_change,
+            error=error,
+        )
+
+    def find_range(
+        self,
+        servo_id: int,
+        start_pwm: int = 1500,
+        step: int = 200,
+        min_bound: int = 500,
+        max_bound: int = 2500,
+    ) -> ServoRange:
+        """
+        Find the safe range of motion for a servo.
+
+        Starts from center and expands outward, looking for movement limits.
+
+        Args:
+            servo_id: Servo to test
+            start_pwm: Starting position (center)
+            step: Step size for probing
+            min_bound: Minimum PWM to test
+            max_bound: Maximum PWM to test
+
+        Returns:
+            ServoRange with discovered limits
+        """
+        print(f"  Finding range for servo {servo_id}...")
+
+        # Move to center first
+        self.probe_servo(servo_id, start_pwm)
+
+        # Without camera, we can't detect limits - just probe key positions
+        has_camera = self.capture is not None
+
+        if has_camera:
+            # Find minimum with visual feedback
+            safe_min = start_pwm
+            current = start_pwm - step
+            while current >= min_bound:
+                result = self.probe_servo(servo_id, current)
+                if not result.moved or result.pixel_change < 0.001:
+                    break
+                safe_min = current
+                current -= step
+
+            # Move back to center
+            self.probe_servo(servo_id, start_pwm)
+
+            # Find maximum with visual feedback
+            safe_max = start_pwm
+            current = start_pwm + step
+            while current <= max_bound:
+                result = self.probe_servo(servo_id, current)
+                if not result.moved or result.pixel_change < 0.001:
+                    break
+                safe_max = current
+                current += step
+        else:
+            # Without camera, probe the full range with key positions
+            # Use known safe ranges for MechDog servos
+            safe_min = min_bound
+            safe_max = max_bound
+
+            # Probe min, center, max to ensure servo responds
+            self.probe_servo(servo_id, min_bound, duration_ms=800)
+            self.probe_servo(servo_id, start_pwm, duration_ms=500)
+            self.probe_servo(servo_id, max_bound, duration_ms=800)
+
+        # Return to center
+        self.probe_servo(servo_id, start_pwm)
+
+        return ServoRange(
+            min_pwm=min_bound,
+            max_pwm=max_bound,
+            center_pwm=start_pwm,
+            safe_min=safe_min,
+            safe_max=safe_max,
+        )
+
+    def sweep_servo(
+        self,
+        servo_id: int,
+        pwm_values: List[int],
+        save_images: bool = False,
+        output_dir: str = "/tmp/servo_sweep",
+    ) -> List[ProbeResult]:
+        """
+        Sweep a servo through multiple positions.
+
+        Args:
+            servo_id: Servo to sweep
+            pwm_values: List of PWM values to test
+            save_images: Whether to save images
+            output_dir: Directory for saved images
+
+        Returns:
+            List of ProbeResults
+        """
+        results = []
+
+        if save_images:
+            Path(output_dir).mkdir(parents=True, exist_ok=True)
+
+        for i, pwm in enumerate(pwm_values):
+            result = self.probe_servo(servo_id, pwm)
+            results.append(result)
+
+            if save_images and result.image_after is not None:
+                path = f"{output_dir}/servo{servo_id}_pwm{pwm}.jpg"
+                cv2.imwrite(path, result.image_after)
+
+            print(f"    PWM {pwm}: change={result.pixel_change:.3f}")
+
+        return results
+
+
+# ============================================================================
+# Servo Mapper Agent - LLM-guided exploration
+# ============================================================================
+
+class ServoMapperAgent:
+    """
+    LLM-powered agent for discovering servo mappings.
+
+    Uses the LLM to:
+    - Decide which servos to test
+    - Analyze camera images to understand servo function
+    - Generate named positions and primitive skills
+    """
+
+    SYSTEM_PROMPT = """You are a robotics engineer analyzing servo motors on a quadruped robot with an arm attachment.
+
+The robot has 13 servos:
+- Servos 1-10: Likely leg servos (4 legs x ~2-3 joints each)
+- Servos 11-13: Likely arm servos (gripper, shoulder, elbow)
+
+Your task is to analyze servo test data and determine:
+1. What body part each servo controls
+2. What type of joint it is (hip, knee, shoulder, gripper, etc.)
+3. Named positions (e.g., GRIPPER_OPEN=500, GRIPPER_CLOSED=2400)
+4. Safe operating range
+
+For each servo, you'll receive:
+- Servo ID
+- PWM range tested (500-2500 typically)
+- Visual change measurements at each position
+- Optional camera images showing before/after
+
+Respond with JSON:
+{
+    "servo_id": 11,
+    "body_part": "arm",
+    "joint_name": "gripper",
+    "servo_type": "gripper",
+    "named_positions": [
+        {"name": "OPEN", "pwm": 500, "description": "Gripper fully open"},
+        {"name": "CLOSED", "pwm": 2400, "description": "Gripper fully closed"}
+    ],
+    "safe_range": {"min": 500, "max": 2500},
+    "confidence": 0.9,
+    "reasoning": "Large visual change between extremes, typical gripper behavior"
+}
+
+Servo types: leg_hip, leg_shoulder, leg_knee, arm_shoulder, arm_elbow, gripper, head_pan, head_tilt, tail, unknown
+
+Common patterns:
+- Gripper: Large visual change, positions at extremes
+- Leg joints: Moderate change, affects robot height/stance
+- Arm joints: Affects arm position relative to body"""
+
+    def __init__(self, use_proxy: bool = True):
+        """
+        Initialize agent.
+
+        Args:
+            use_proxy: Use FoodforThought proxy for metering
+        """
+        self.proxy = None
+
+        if use_proxy and HAS_LLM_PROXY:
+            try:
+                self.proxy = LLMProxy()
+                if not self.proxy.is_authenticated():
+                    print("  [Agent] Not logged in - run 'ate login'")
+                    self.proxy = None
+            except Exception as e:
+                print(f"  [Agent] Proxy error: {e}")
+
+        if not self.proxy:
+            print("  [Agent] No LLM available - using heuristic analysis")
+
+    def analyze_servo(
+        self,
+        servo_id: int,
+        sweep_results: List[ProbeResult],
+        servo_range: ServoRange,
+    ) -> ServoMapping:
+        """
+        Analyze sweep data to determine servo function.
+
+        Args:
+            servo_id: Servo being analyzed
+            sweep_results: Results from sweep test
+            servo_range: Discovered range
+
+        Returns:
+            ServoMapping with inferred function
+        """
+        if self.proxy:
+            return self._llm_analyze(servo_id, sweep_results, servo_range)
+        else:
+            return self._heuristic_analyze(servo_id, sweep_results, servo_range)
+
+    def _llm_analyze(
+        self,
+        servo_id: int,
+        sweep_results: List[ProbeResult],
+        servo_range: ServoRange,
+    ) -> ServoMapping:
+        """Use LLM to analyze servo data."""
+
+        # Build context
+        sweep_data = [
+            {"pwm": r.pwm_value, "visual_change": round(r.pixel_change, 4)}
+            for r in sweep_results
+        ]
+
+        context = {
+            "servo_id": servo_id,
+            "range": {
+                "tested_min": servo_range.safe_min,
+                "tested_max": servo_range.safe_max,
+                "center": servo_range.center_pwm,
+            },
+            "sweep_data": sweep_data,
+            "total_visual_change": sum(r.pixel_change for r in sweep_results),
+        }
+
+        prompt = f"""Analyze servo {servo_id}:
+
+{json.dumps(context, indent=2)}
+
+Based on this data, what does this servo control? Provide your analysis as JSON."""
+
+        try:
+            response = self.proxy.chat(
+                messages=[{"role": "user", "content": prompt}],
+                model="claude-3-5-haiku-20241022",
+                max_tokens=500,
+                system=self.SYSTEM_PROMPT,
+            )
+
+            # Parse response
+            text = response.content.strip()
+
+            # Extract JSON - handle various formats
+            data = None
+
+            # Try direct parse first
+            try:
+                data = json.loads(text)
+            except json.JSONDecodeError:
+                pass
+
+            # Try extracting from markdown code block
+            if data is None and "```" in text:
+                parts = text.split("```")
+                for part in parts[1:]:  # Skip text before first ```
+                    part = part.strip()
+                    if part.startswith("json"):
+                        part = part[4:].strip()
+                    # Find the end of the JSON (before next ```)
+                    if part.endswith("```"):
+                        part = part[:-3]
+                    try:
+                        data = json.loads(part.strip())
+                        break
+                    except json.JSONDecodeError:
+                        continue
+
+            # Try finding JSON object in text
+            if data is None:
+                import re
+                json_match = re.search(r'\{[^{}]*(?:\{[^{}]*\}[^{}]*)*\}', text, re.DOTALL)
+                if json_match:
+                    try:
+                        data = json.loads(json_match.group())
+                    except json.JSONDecodeError:
+                        pass
+
+            if data is None:
+                raise ValueError("Could not extract JSON from response")
+
+            # Build mapping
+            mapping = ServoMapping(
+                servo_id=servo_id,
+                servo_type=ServoType(data.get("servo_type", "unknown")),
+                body_part=data.get("body_part", ""),
+                joint_name=data.get("joint_name", ""),
+                range=servo_range,
+                confidence=data.get("confidence", 0.5),
+                observations=[data.get("reasoning", "")],
+            )
+
+            # Add named positions
+            for pos in data.get("named_positions", []):
+                mapping.named_positions.append(NamedPosition(
+                    name=pos["name"],
+                    pwm_value=pos["pwm"],
+                    description=pos.get("description", ""),
+                ))
+
+            return mapping
+
+        except Exception as e:
+            print(f"  [Agent] LLM error: {e}")
+            return self._heuristic_analyze(servo_id, sweep_results, servo_range)
+
+    def _heuristic_analyze(
+        self,
+        servo_id: int,
+        sweep_results: List[ProbeResult],
+        servo_range: ServoRange,
+    ) -> ServoMapping:
+        """Fallback heuristic analysis."""
+
+        # Classify based on servo ID patterns
+        # Common MechDog layout:
+        # 1-3: Front left leg, 4-6: Front right leg
+        # 7-9: Back left leg, 10: Back right (or partial)
+        # 11: Gripper, 12: Shoulder, 13: Elbow
+
+        mapping = ServoMapping(servo_id=servo_id, range=servo_range)
+
+        if servo_id == 11:
+            mapping.servo_type = ServoType.GRIPPER
+            mapping.body_part = "arm"
+            mapping.joint_name = "gripper"
+            mapping.named_positions = [
+                NamedPosition("OPEN", servo_range.safe_min, "Gripper open"),
+                NamedPosition("CLOSED", servo_range.safe_max, "Gripper closed"),
+            ]
+            mapping.confidence = 0.9
+
+        elif servo_id == 12:
+            mapping.servo_type = ServoType.ARM_SHOULDER
+            mapping.body_part = "arm"
+            mapping.joint_name = "shoulder"
+            mapping.named_positions = [
+                NamedPosition("UP", servo_range.safe_min, "Arm raised"),
+                NamedPosition("DOWN", servo_range.safe_max, "Arm lowered"),
+            ]
+            mapping.confidence = 0.8
+
+        elif servo_id == 13:
+            mapping.servo_type = ServoType.ARM_ELBOW
+            mapping.body_part = "arm"
+            mapping.joint_name = "elbow"
+            mapping.named_positions = [
+                NamedPosition("RETRACTED", servo_range.center_pwm + 100, "Elbow bent"),
+                NamedPosition("EXTENDED", servo_range.safe_min, "Elbow straight"),
+            ]
+            mapping.confidence = 0.8
+
+        elif servo_id <= 10:
+            # Leg servo - determine which leg and joint
+            leg_num = (servo_id - 1) // 3
+            joint_num = (servo_id - 1) % 3
+
+            leg_names = ["front_left", "front_right", "back_left", "back_right"]
+            joint_types = [ServoType.LEG_HIP, ServoType.LEG_SHOULDER, ServoType.LEG_KNEE]
+            joint_names = ["hip", "shoulder", "knee"]
+
+            if leg_num < 4:
+                mapping.body_part = leg_names[leg_num] + "_leg"
+                mapping.servo_type = joint_types[joint_num] if joint_num < 3 else ServoType.UNKNOWN
+                mapping.joint_name = joint_names[joint_num] if joint_num < 3 else "unknown"
+                mapping.confidence = 0.6
+
+        # Add generic positions if none set
+        if not mapping.named_positions:
+            mapping.named_positions = [
+                NamedPosition("MIN", servo_range.safe_min, "Minimum position"),
+                NamedPosition("CENTER", servo_range.center_pwm, "Center position"),
+                NamedPosition("MAX", servo_range.safe_max, "Maximum position"),
+            ]
+
+        return mapping
+
+
+# ============================================================================
+# Primitive Generator
+# ============================================================================
+
+class PrimitiveGenerator:
+    """
+    Generate primitive skill definitions from servo mappings.
+    """
+
+    def __init__(self, robot_name: str = "mechdog"):
+        self.robot_name = robot_name
+
+    def generate_primitives(
+        self,
+        mappings: List[ServoMapping],
+    ) -> Dict[str, Any]:
+        """
+        Generate primitive skill definitions from mappings.
+
+        Args:
+            mappings: List of servo mappings
+
+        Returns:
+            Dictionary of primitive definitions
+        """
+        primitives = {
+            "robot": self.robot_name,
+            "generated_at": time.strftime("%Y-%m-%d %H:%M:%S"),
+            "servo_count": len(mappings),
+            "constants": {},
+            "primitives": [],
+        }
+
+        # Generate constants for named positions
+        for mapping in mappings:
+            prefix = f"SERVO_{mapping.servo_id}"
+            if mapping.body_part and mapping.joint_name:
+                prefix = f"{mapping.body_part.upper()}_{mapping.joint_name.upper()}"
+
+            for pos in mapping.named_positions:
+                const_name = f"{prefix}_{pos.name}"
+                primitives["constants"][const_name] = pos.pwm_value
+
+        # Generate primitive functions
+        for mapping in mappings:
+            self._add_primitives_for_servo(primitives, mapping)
+
+        # Add compound primitives
+        self._add_compound_primitives(primitives, mappings)
+
+        return primitives
+
+    def _add_primitives_for_servo(
+        self,
+        primitives: Dict,
+        mapping: ServoMapping,
+    ):
+        """Add primitive functions for a single servo."""
+
+        for pos in mapping.named_positions:
+            name = f"{mapping.body_part}_{mapping.joint_name}_{pos.name}".lower()
+            name = name.replace("__", "_").strip("_")
+
+            if not name:
+                name = f"servo_{mapping.servo_id}_{pos.name}".lower()
+
+            primitive = {
+                "name": name,
+                "description": pos.description or f"Move {mapping.joint_name} to {pos.name}",
+                "servo_id": mapping.servo_id,
+                "pwm_value": pos.pwm_value,
+                "body_part": mapping.body_part,
+                "joint": mapping.joint_name,
+                "code": f"dog.set_servo({mapping.servo_id}, {pos.pwm_value}, {{duration}})",
+            }
+            primitives["primitives"].append(primitive)
+
+    def _add_compound_primitives(
+        self,
+        primitives: Dict,
+        mappings: List[ServoMapping],
+    ):
+        """Add compound primitives that combine multiple servos."""
+
+        # Find arm servos
+        gripper = next((m for m in mappings if m.servo_type == ServoType.GRIPPER), None)
+        shoulder = next((m for m in mappings if m.servo_type == ServoType.ARM_SHOULDER), None)
+        elbow = next((m for m in mappings if m.servo_type == ServoType.ARM_ELBOW), None)
+
+        if gripper and shoulder and elbow:
+            # Arm up compound
+            primitives["primitives"].append({
+                "name": "arm_up",
+                "description": "Raise arm to upright position",
+                "compound": True,
+                "steps": [
+                    {"servo_id": elbow.servo_id, "position": "RETRACTED"},
+                    {"servo_id": shoulder.servo_id, "position": "UP"},
+                ],
+            })
+
+            # Arm down compound
+            primitives["primitives"].append({
+                "name": "arm_down",
+                "description": "Lower arm for pickup",
+                "compound": True,
+                "steps": [
+                    {"servo_id": shoulder.servo_id, "position": "DOWN"},
+                    {"servo_id": elbow.servo_id, "position": "EXTENDED"},
+                ],
+            })
+
+            # Grab sequence
+            primitives["primitives"].append({
+                "name": "grab",
+                "description": "Complete grab sequence",
+                "compound": True,
+                "steps": [
+                    {"primitive": "arm_gripper_open"},
+                    {"primitive": "arm_down"},
+                    {"wait": 0.5},
+                    {"primitive": "arm_gripper_closed"},
+                    {"primitive": "arm_up"},
+                ],
+            })
+
+    def generate_python_code(
+        self,
+        mappings: List[ServoMapping],
+    ) -> str:
+        """
+        Generate Python code for primitives.
+
+        Args:
+            mappings: Servo mappings
+
+        Returns:
+            Python code string
+        """
+        lines = [
+            '"""',
+            f'Auto-generated primitives for {self.robot_name}.',
+            f'Generated: {time.strftime("%Y-%m-%d %H:%M:%S")}',
+            '"""',
+            '',
+            '# Servo Constants',
+        ]
+
+        for mapping in mappings:
+            prefix = f"SERVO_{mapping.servo_id}"
+            if mapping.body_part and mapping.joint_name:
+                prefix = f"{mapping.body_part.upper()}_{mapping.joint_name.upper()}"
+
+            lines.append(f"# {mapping.body_part} {mapping.joint_name} (servo {mapping.servo_id})")
+            for pos in mapping.named_positions:
+                const_name = f"{prefix}_{pos.name}".replace("__", "_")
+                lines.append(f"{const_name} = {pos.pwm_value}")
+            lines.append("")
+
+        lines.extend([
+            '',
+            '# Primitive Functions',
+            '',
+        ])
+
+        for mapping in mappings:
+            for pos in mapping.named_positions:
+                func_name = f"{mapping.body_part}_{mapping.joint_name}_{pos.name}".lower()
+                func_name = func_name.replace("__", "_").strip("_")
+                if not func_name:
+                    func_name = f"servo_{mapping.servo_id}_{pos.name}".lower()
+
+                lines.extend([
+                    f"def {func_name}(dog, duration_ms=500):",
+                    f'    """{pos.description or f"Move {mapping.joint_name} to {pos.name}"}"""',
+                    f"    dog.set_servo({mapping.servo_id}, {pos.pwm_value}, duration_ms)",
+                    "",
+                ])
+
+        # Add compound functions
+        gripper = next((m for m in mappings if m.servo_type == ServoType.GRIPPER), None)
+        shoulder = next((m for m in mappings if m.servo_type == ServoType.ARM_SHOULDER), None)
+        elbow = next((m for m in mappings if m.servo_type == ServoType.ARM_ELBOW), None)
+
+        if gripper and shoulder and elbow:
+            lines.extend([
+                "",
+                "# Compound Primitives",
+                "",
+                "def arm_up(dog, duration_ms=600):",
+                '    """Raise arm to upright position."""',
+            ])
+
+            # Find positions
+            elbow_ret = next((p for p in elbow.named_positions if "RETRACT" in p.name.upper()), None)
+            shoulder_up = next((p for p in shoulder.named_positions if "UP" in p.name.upper()), None)
+
+            if elbow_ret and shoulder_up:
+                lines.extend([
+                    f"    dog.set_servo({elbow.servo_id}, {elbow_ret.pwm_value}, duration_ms)",
+                    f"    dog.set_servo({shoulder.servo_id}, {shoulder_up.pwm_value}, duration_ms)",
+                ])
+
+            lines.extend([
+                "",
+                "def arm_down(dog, duration_ms=600):",
+                '    """Lower arm for pickup."""',
+            ])
+
+            shoulder_down = next((p for p in shoulder.named_positions if "DOWN" in p.name.upper()), None)
+            elbow_ext = next((p for p in elbow.named_positions if "EXTEND" in p.name.upper()), None)
+
+            if shoulder_down and elbow_ext:
+                lines.extend([
+                    f"    dog.set_servo({shoulder.servo_id}, {shoulder_down.pwm_value}, duration_ms)",
+                    f"    dog.set_servo({elbow.servo_id}, {elbow_ext.pwm_value}, duration_ms)",
+                ])
+
+            gripper_open = next((p for p in gripper.named_positions if "OPEN" in p.name.upper()), None)
+            gripper_close = next((p for p in gripper.named_positions if "CLOSE" in p.name.upper()), None)
+
+            if gripper_open and gripper_close:
+                lines.extend([
+                    "",
+                    "def grab_sequence(dog):",
+                    '    """Complete grab sequence: open, lower, close, raise."""',
+                    "    import time",
+                    f"    dog.set_servo({gripper.servo_id}, {gripper_open.pwm_value}, 400)  # Open gripper",
+                    "    time.sleep(0.4)",
+                    "    arm_down(dog, 600)",
+                    "    time.sleep(0.6)",
+                    f"    dog.set_servo({gripper.servo_id}, {gripper_close.pwm_value}, 400)  # Close gripper",
+                    "    time.sleep(0.4)",
+                    "    arm_up(dog, 600)",
+                ])
+
+        return "\n".join(lines)
+
+
+# ============================================================================
+# Main Entry Point
+# ============================================================================
+
+def run_servo_mapping(
+    serial_port: str = "/dev/cu.usbserial-10",
+    num_servos: int = 13,
+    use_camera: bool = False,
+    camera_index: int = 0,
+    wifi_camera_ip: Optional[str] = None,
+    use_proxy: bool = True,
+    output_file: Optional[str] = None,
+    save_images: bool = False,
+) -> Dict[str, Any]:
+    """
+    Run servo mapping to discover and document all servos.
+
+    Args:
+        serial_port: Robot serial port
+        num_servos: Number of servos to probe
+        use_camera: Use camera for visual verification
+        camera_index: Webcam index (if use_camera=True)
+        wifi_camera_ip: WiFi camera IP (alternative to webcam)
+        use_proxy: Route LLM through FoodforThought for metering
+        output_file: Path to save results
+        save_images: Save images during sweep
+
+    Returns:
+        Dictionary with mappings and generated primitives
+    """
+    print("\n" + "=" * 60)
+    print("SERVO MAPPER - Automated Primitive Discovery")
+    print("=" * 60)
+
+    # Setup serial connection
+    try:
+        import serial
+    except ImportError:
+        print("Error: pyserial required. Install with: pip install pyserial")
+        return {}
+
+    print(f"\nConnecting to robot at {serial_port}...")
+    try:
+        ser = serial.Serial(serial_port, 115200, timeout=1)
+        time.sleep(0.5)
+        ser.read(ser.in_waiting)
+
+        def cmd(command: str, wait: float = 0.3) -> str:
+            ser.write(f"{command}\r\n".encode())
+            time.sleep(wait)
+            return ser.read(ser.in_waiting).decode('utf-8', errors='ignore')
+
+        # Initialize MechDog
+        cmd("from HW_MechDog import MechDog", 1.5)
+        cmd("dog = MechDog()", 1.5)
+        print("Connected!")
+
+    except Exception as e:
+        print(f"Failed to connect: {e}")
+        return {}
+
+    # Setup camera if requested
+    capture_fn = None
+    cap = None
+
+    if use_camera:
+        if wifi_camera_ip:
+            print(f"Using WiFi camera at {wifi_camera_ip}...")
+            import requests
+            from PIL import Image as PILImage
+            import io
+
+            def wifi_capture():
+                try:
+                    response = requests.get(
+                        f"http://{wifi_camera_ip}/capture",
+                        timeout=2
+                    )
+                    pil_img = PILImage.open(io.BytesIO(response.content))
+                    frame = np.array(pil_img.convert("RGB"))
+                    return cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+                except Exception:
+                    return np.array([])
+
+            capture_fn = wifi_capture
+
+        elif HAS_CV:
+            print(f"Opening webcam {camera_index}...")
+            cap = cv2.VideoCapture(camera_index)
+            if cap.isOpened():
+                cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+                cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+
+                def webcam_capture():
+                    for _ in range(2):
+                        cap.read()
+                    ret, frame = cap.read()
+                    return frame if ret else np.array([])
+
+                capture_fn = webcam_capture
+                print("Camera ready!")
+            else:
+                print("Warning: Could not open camera")
+
+    # Create probe and agent
+    probe = ServoProbe(cmd, capture_fn)
+    agent = ServoMapperAgent(use_proxy=use_proxy)
+
+    # Map each servo
+    mappings = []
+    print(f"\nMapping {num_servos} servos...")
+
+    for servo_id in range(1, num_servos + 1):
+        print(f"\n--- Servo {servo_id}/{num_servos} ---")
+
+        # Find range
+        servo_range = probe.find_range(servo_id)
+        print(f"  Range: {servo_range.safe_min} - {servo_range.safe_max}")
+
+        # Sweep through range
+        sweep_values = list(range(
+            servo_range.safe_min,
+            servo_range.safe_max + 1,
+            (servo_range.safe_max - servo_range.safe_min) // 5 or 100
+        ))
+
+        sweep_results = probe.sweep_servo(
+            servo_id,
+            sweep_values,
+            save_images=save_images,
+        )
+
+        # Analyze with agent
+        mapping = agent.analyze_servo(servo_id, sweep_results, servo_range)
+        mappings.append(mapping)
+
+        print(f"  Identified: {mapping.body_part} {mapping.joint_name} ({mapping.servo_type.value})")
+        print(f"  Confidence: {mapping.confidence:.0%}")
+
+        # Return to center
+        probe.probe_servo(servo_id, servo_range.center_pwm)
+
+    # Generate primitives
+    print("\n" + "=" * 60)
+    print("GENERATING PRIMITIVES")
+    print("=" * 60)
+
+    generator = PrimitiveGenerator("mechdog")
+    primitives = generator.generate_primitives(mappings)
+    python_code = generator.generate_python_code(mappings)
+
+    # Print summary
+    print(f"\nDiscovered {len(primitives['primitives'])} primitives:")
+    for p in primitives["primitives"]:
+        print(f"  - {p['name']}")
+
+    print(f"\nConstants defined: {len(primitives['constants'])}")
+
+    # Save results
+    results = {
+        "mappings": [m.to_dict() for m in mappings],
+        "primitives": primitives,
+        "python_code": python_code,
+    }
+
+    if output_file:
+        output_path = Path(output_file)
+
+        # Save JSON
+        with open(output_path.with_suffix(".json"), "w") as f:
+            json.dump(results, f, indent=2)
+        print(f"\nSaved JSON to {output_path.with_suffix('.json')}")
+
+        # Save Python code
+        with open(output_path.with_suffix(".py"), "w") as f:
+            f.write(python_code)
+        print(f"Saved Python to {output_path.with_suffix('.py')}")
+
+    # Cleanup
+    if cap:
+        cap.release()
+    ser.close()
+
+    print("\n" + "=" * 60)
+    print("MAPPING COMPLETE")
+    print("=" * 60)
+
+    return results
+
+
+def upload_servo_mapping(
+    results: Dict[str, Any],
+    output_file: str,
+    project_id: Optional[str] = None,
+) -> Dict[str, Any]:
+    """
+    Upload servo mapping results to FoodforThought.
+
+    Creates artifacts:
+    - Calibration (raw stage, calibration type): servo mappings and ranges
+    - Generated Skills (skill stage, code type): Python primitives
+
+    Args:
+        results: Results from run_servo_mapping()
+        output_file: Output file path (used for naming)
+        project_id: Optional project ID (creates new if not provided)
+
+    Returns:
+        Dict with project_id and artifact list
+    """
+    from .skill_upload import SkillLibraryUploader
+
+    uploader = SkillLibraryUploader()
+
+    # Extract robot name from output file
+    robot_name = Path(output_file).stem.replace("servo_mappings_", "")
+    if not robot_name:
+        robot_name = "mechdog"
+
+    # Get or create project
+    if not project_id:
+        project_id = uploader.get_or_create_project(
+            f"{robot_name.replace('_', ' ').title()} Servo Mapping",
+            f"Automated servo discovery and primitive generation for {robot_name}",
+        )
+
+    result = {
+        "project_id": project_id,
+        "artifacts": [],
+    }
+
+    # 1. Upload calibration/mapping data as raw + calibration type
+    mappings = results.get("mappings", [])
+    primitives = results.get("primitives", {})
+
+    calibration_response = uploader._request("POST", "/artifacts", json={
+        "projectId": project_id,
+        "name": f"Servo Mapping ({len(mappings)} servos)",
+        "stage": "raw",
+        "type": "calibration",
+        "metadata": {
+            "robot_name": robot_name,
+            "servo_count": len(mappings),
+            "mappings": mappings,
+            "constants": primitives.get("constants", {}),
+            "generator": "ate_servo_mapper_v1",
+            "generated_at": primitives.get("generated_at", time.strftime("%Y-%m-%d %H:%M:%S")),
+            "source": "agentic_servo_mapper",
+        },
+    })
+    calibration_artifact_id = calibration_response.get("artifact", {}).get("id")
+    result["artifacts"].append({
+        "id": calibration_artifact_id,
+        "stage": "raw",
+        "name": f"Servo Mapping ({len(mappings)} servos)",
+    })
+
+    # 2. Upload generated Python code as skill artifact
+    python_code = results.get("python_code", "")
+    if python_code:
+        primitives_list = primitives.get("primitives", [])
+        skill_response = uploader._request("POST", "/artifacts", json={
+            "projectId": project_id,
+            "name": f"Generated Primitives ({len(primitives_list)} functions)",
+            "stage": "skill",
+            "type": "code",
+            "parentArtifactId": calibration_artifact_id,
+            "transformationType": "generation",
+            "transformationNotes": "Python primitives auto-generated from servo mapping via LLM analysis",
+            "metadata": {
+                "robot_name": robot_name,
+                "skill_type": "primitive",
+                "primitive_count": len(primitives_list),
+                "constant_count": len(primitives.get("constants", {})),
+                "code": python_code,
+                "primitives": primitives_list,
+                "hardware_requirements": ["servos"],
+                "source": "agentic_servo_mapper",
+                "generator": "ate_primitive_generator_v1",
+            },
+        })
+        result["artifacts"].append({
+            "id": skill_response.get("artifact", {}).get("id"),
+            "stage": "skill",
+            "name": f"Generated Primitives ({len(primitives_list)} functions)",
+        })
+
+    return result
+
+
+if __name__ == "__main__":
+    import sys
+
+    output = sys.argv[1] if len(sys.argv) > 1 else "/tmp/mechdog_primitives"
+    use_cam = "--camera" in sys.argv
+
+    results = run_servo_mapping(
+        use_camera=use_cam,
+        output_file=output,
+        save_images=use_cam,
+    )
+
+    if results.get("python_code"):
+        print("\n" + "=" * 60)
+        print("GENERATED PYTHON CODE:")
+        print("=" * 60)
+        print(results["python_code"])