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

ate/robot/visual_system_id.py

@@ -0,0 +1,906 @@
+#!/usr/bin/env python3
+"""
+Visual System Identification - Robot Self-Modeling via Webcam
+
+Instead of guessing what servos do, we MEASURE:
+1. Stick ArUco markers on each robot link
+2. Move motors one at a time
+3. Track how markers move relative to each other
+4. Infer joint types and parameters mathematically
+5. Generate URDF automatically
+
+This closes the sim-to-real gap by reverse-engineering
+the kinematic model from observation.
+
+Usage:
+    python -m ate.robot.visual_system_id --port /dev/cu.usbserial-10
+
+Prerequisites:
+    pip install opencv-contrib-python numpy
+    Print ArUco markers and attach to robot links
+"""
+
+import cv2
+import numpy as np
+import time
+import json
+import math
+from dataclasses import dataclass, field
+from typing import List, Dict, Optional, Tuple, Any
+from pathlib import Path
+from enum import Enum
+
+# Import unified JointType from shared types module
+from ate.robot.types import JointType
+
+
+@dataclass
+class MarkerPose:
+    """6DOF pose of an ArUco marker."""
+    marker_id: int
+    timestamp: float
+    position: np.ndarray      # [x, y, z] in camera frame (meters)
+    rotation: np.ndarray      # 3x3 rotation matrix
+    rvec: np.ndarray          # Rodrigues rotation vector
+    tvec: np.ndarray          # Translation vector
+
+    @property
+    def euler_angles(self) -> Tuple[float, float, float]:
+        """Get roll, pitch, yaw in degrees."""
+        # Extract Euler angles from rotation matrix
+        sy = math.sqrt(self.rotation[0,0]**2 + self.rotation[1,0]**2)
+        singular = sy < 1e-6
+
+        if not singular:
+            roll = math.atan2(self.rotation[2,1], self.rotation[2,2])
+            pitch = math.atan2(-self.rotation[2,0], sy)
+            yaw = math.atan2(self.rotation[1,0], self.rotation[0,0])
+        else:
+            roll = math.atan2(-self.rotation[1,2], self.rotation[1,1])
+            pitch = math.atan2(-self.rotation[2,0], sy)
+            yaw = 0
+
+        return (math.degrees(roll), math.degrees(pitch), math.degrees(yaw))
+
+    def to_dict(self) -> dict:
+        roll, pitch, yaw = self.euler_angles
+        return {
+            "marker_id": self.marker_id,
+            "timestamp": self.timestamp,
+            "position": self.position.tolist(),
+            "euler_deg": {"roll": roll, "pitch": pitch, "yaw": yaw},
+        }
+
+
+@dataclass
+class MotionSample:
+    """A single observation during motor motion."""
+    motor_id: int
+    pwm_value: int
+    marker_poses: Dict[int, MarkerPose]  # marker_id -> pose
+
+
+@dataclass
+class JointInference:
+    """Inferred properties of a joint."""
+    motor_id: int
+    joint_type: JointType
+    parent_marker: Optional[int]     # Marker on parent link
+    child_marker: Optional[int]      # Marker on child link
+    axis: Optional[np.ndarray]       # Joint axis (unit vector)
+    origin: Optional[np.ndarray]     # Joint origin in parent frame
+    range_min: float = 0.0           # Joint limit (radians or meters)
+    range_max: float = 0.0
+    confidence: float = 0.0
+
+    def to_dict(self) -> dict:
+        return {
+            "motor_id": self.motor_id,
+            "joint_type": self.joint_type.value,
+            "parent_marker": self.parent_marker,
+            "child_marker": self.child_marker,
+            "axis": self.axis.tolist() if self.axis is not None else None,
+            "origin": self.origin.tolist() if self.origin is not None else None,
+            "range_min_deg": math.degrees(self.range_min),
+            "range_max_deg": math.degrees(self.range_max),
+            "confidence": self.confidence,
+        }
+
+
+class ArUcoTracker:
+    """
+    Track ArUco markers and compute 6DOF poses.
+
+    ArUco markers are like QR codes but optimized for pose estimation.
+    Each marker has a unique ID and known geometry, allowing us to
+    compute its exact position and orientation in 3D space.
+    """
+
+    def __init__(
+        self,
+        camera_index: int = 0,
+        marker_size_meters: float = 0.03,  # 3cm markers
+        dictionary: int = cv2.aruco.DICT_4X4_50,
+    ):
+        self.camera_index = camera_index
+        self.marker_size = marker_size_meters
+
+        # ArUco detector setup
+        self.aruco_dict = cv2.aruco.getPredefinedDictionary(dictionary)
+        self.aruco_params = cv2.aruco.DetectorParameters()
+        self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
+
+        # Camera (will be initialized on connect)
+        self.cap = None
+        self.frame_size = (640, 480)
+
+        # Camera calibration (default - should be calibrated properly)
+        # These are approximate values for a typical webcam
+        self.camera_matrix = None
+        self.dist_coeffs = None
+        self._set_default_calibration()
+
+    def _set_default_calibration(self):
+        """Set default camera calibration (approximate)."""
+        # Approximate intrinsics for 640x480 webcam
+        fx = fy = 600  # Focal length in pixels
+        cx, cy = 320, 240  # Principal point
+
+        self.camera_matrix = np.array([
+            [fx, 0, cx],
+            [0, fy, cy],
+            [0, 0, 1]
+        ], dtype=np.float32)
+
+        self.dist_coeffs = np.zeros(5, dtype=np.float32)
+
+    def connect(self) -> bool:
+        """Open camera connection."""
+        self.cap = cv2.VideoCapture(self.camera_index)
+        if not self.cap.isOpened():
+            return False
+
+        # Set resolution
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+        # Read actual size
+        ret, frame = self.cap.read()
+        if ret:
+            self.frame_size = (frame.shape[1], frame.shape[0])
+            # Update principal point for actual resolution
+            self.camera_matrix[0, 2] = frame.shape[1] / 2
+            self.camera_matrix[1, 2] = frame.shape[0] / 2
+
+        return True
+
+    def disconnect(self):
+        """Close camera."""
+        if self.cap:
+            self.cap.release()
+            self.cap = None
+
+    def detect_markers(self, frame: Optional[np.ndarray] = None) -> Tuple[Dict[int, MarkerPose], np.ndarray]:
+        """
+        Detect all ArUco markers and compute their poses.
+
+        Returns:
+            Dict mapping marker_id -> MarkerPose
+            Annotated frame for visualization
+        """
+        if frame is None:
+            if not self.cap:
+                return {}, np.array([])
+            ret, frame = self.cap.read()
+            if not ret:
+                return {}, np.array([])
+
+        # Detect markers
+        corners, ids, rejected = self.detector.detectMarkers(frame)
+
+        poses = {}
+        annotated = frame.copy()
+
+        if ids is not None:
+            # Draw detected markers
+            cv2.aruco.drawDetectedMarkers(annotated, corners, ids)
+
+            # Estimate pose for each marker
+            for i, marker_id in enumerate(ids.flatten()):
+                # Get pose using solvePnP
+                rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
+                    corners[i:i+1],
+                    self.marker_size,
+                    self.camera_matrix,
+                    self.dist_coeffs
+                )
+
+                rvec = rvec[0][0]
+                tvec = tvec[0][0]
+
+                # Convert to rotation matrix
+                rotation, _ = cv2.Rodrigues(rvec)
+
+                pose = MarkerPose(
+                    marker_id=int(marker_id),
+                    timestamp=time.time(),
+                    position=tvec,
+                    rotation=rotation,
+                    rvec=rvec,
+                    tvec=tvec,
+                )
+                poses[int(marker_id)] = pose
+
+                # Draw axis on frame
+                cv2.drawFrameAxes(
+                    annotated,
+                    self.camera_matrix,
+                    self.dist_coeffs,
+                    rvec, tvec,
+                    self.marker_size * 0.5
+                )
+
+                # Add label
+                corner = corners[i][0][0]
+                cv2.putText(
+                    annotated,
+                    f"ID:{marker_id}",
+                    (int(corner[0]), int(corner[1]) - 10),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2
+                )
+
+        return poses, annotated
+
+    def get_frame(self) -> np.ndarray:
+        """Get current camera frame."""
+        if not self.cap:
+            return np.array([])
+        ret, frame = self.cap.read()
+        return frame if ret else np.array([])
+
+
+class MotionAnalyzer:
+    """
+    Analyze marker motion to infer joint properties.
+
+    The key insight: when a motor moves, markers on the child link
+    will move in a predictable pattern:
+    - Revolute joint: markers trace an arc
+    - Prismatic joint: markers trace a line
+    - Fixed: markers don't move relative to parent
+    """
+
+    def __init__(self, samples: List[MotionSample]):
+        self.samples = samples
+
+    def analyze_motion(self, reference_marker: int, moving_marker: int) -> JointInference:
+        """
+        Analyze how one marker moves relative to another.
+
+        Args:
+            reference_marker: Marker ID on parent link (should stay still)
+            moving_marker: Marker ID on child link (should move)
+
+        Returns:
+            JointInference with inferred joint type and parameters
+        """
+        if not self.samples:
+            return JointInference(
+                motor_id=0,
+                joint_type=JointType.UNKNOWN,
+                parent_marker=reference_marker,
+                child_marker=moving_marker,
+                axis=None,
+                origin=None,
+                confidence=0.0,
+            )
+
+        motor_id = self.samples[0].motor_id
+
+        # Extract positions of moving marker relative to reference
+        relative_positions = []
+        relative_rotations = []
+        pwm_values = []
+
+        for sample in self.samples:
+            if reference_marker not in sample.marker_poses:
+                continue
+            if moving_marker not in sample.marker_poses:
+                continue
+
+            ref_pose = sample.marker_poses[reference_marker]
+            mov_pose = sample.marker_poses[moving_marker]
+
+            # Transform moving marker to reference frame
+            # P_rel = R_ref^T * (P_mov - P_ref)
+            rel_pos = ref_pose.rotation.T @ (mov_pose.position - ref_pose.position)
+            rel_rot = ref_pose.rotation.T @ mov_pose.rotation
+
+            relative_positions.append(rel_pos)
+            relative_rotations.append(rel_rot)
+            pwm_values.append(sample.pwm_value)
+
+        if len(relative_positions) < 3:
+            return JointInference(
+                motor_id=motor_id,
+                joint_type=JointType.UNKNOWN,
+                parent_marker=reference_marker,
+                child_marker=moving_marker,
+                axis=None,
+                origin=None,
+                confidence=0.0,
+            )
+
+        positions = np.array(relative_positions)
+
+        # Analyze the trajectory
+        joint_type, axis, origin, confidence = self._classify_motion(positions)
+
+        # Calculate range of motion
+        range_min, range_max = self._calculate_range(positions, joint_type, axis, origin)
+
+        return JointInference(
+            motor_id=motor_id,
+            joint_type=joint_type,
+            parent_marker=reference_marker,
+            child_marker=moving_marker,
+            axis=axis,
+            origin=origin,
+            range_min=range_min,
+            range_max=range_max,
+            confidence=confidence,
+        )
+
+    def _classify_motion(self, positions: np.ndarray) -> Tuple[JointType, Optional[np.ndarray], Optional[np.ndarray], float]:
+        """
+        Classify motion type from trajectory.
+
+        Returns: (joint_type, axis, origin, confidence)
+        """
+        # Check if marker moved significantly
+        pos_range = np.ptp(positions, axis=0)  # Peak-to-peak range
+        total_motion = np.linalg.norm(pos_range)
+
+        if total_motion < 0.005:  # Less than 5mm motion
+            return JointType.FIXED, None, None, 0.9
+
+        # Try to fit a plane to the points
+        # For revolute joint, points should lie on a plane (the rotation plane)
+        centroid = np.mean(positions, axis=0)
+        centered = positions - centroid
+
+        # SVD to find principal components
+        U, S, Vt = np.linalg.svd(centered)
+
+        # The smallest singular value's direction is the plane normal
+        # For revolute joint, this should be the rotation axis
+        plane_normal = Vt[2]  # Normal to the plane of motion
+
+        # Check planarity: if 3rd singular value is small, points are planar
+        planarity = 1.0 - (S[2] / (S[0] + 1e-6))
+
+        if planarity > 0.9:  # Points are mostly planar
+            # Check if motion is circular (revolute) or linear (prismatic)
+
+            # Project to 2D on the plane
+            basis1 = Vt[0]
+            basis2 = Vt[1]
+            projected_2d = np.array([
+                [np.dot(p, basis1), np.dot(p, basis2)]
+                for p in centered
+            ])
+
+            # Fit circle to 2D points
+            circle_fit = self._fit_circle_2d(projected_2d)
+
+            if circle_fit is not None:
+                center_2d, radius, circle_error = circle_fit
+
+                # If circle fit is good, it's revolute
+                if circle_error < 0.1 * radius:  # Error less than 10% of radius
+                    # Reconstruct 3D center
+                    center_3d = centroid + center_2d[0] * basis1 + center_2d[1] * basis2
+
+                    return JointType.REVOLUTE, plane_normal, center_3d, planarity * 0.9
+
+            # Otherwise check for linear motion (prismatic)
+            # Principal direction should explain most variance
+            linearity = S[0] / (S[0] + S[1] + 1e-6)
+
+            if linearity > 0.9:
+                return JointType.PRISMATIC, Vt[0], centroid, linearity * 0.8
+
+        # Can't determine
+        return JointType.UNKNOWN, None, None, 0.3
+
+    def _fit_circle_2d(self, points: np.ndarray) -> Optional[Tuple[np.ndarray, float, float]]:
+        """
+        Fit a circle to 2D points.
+
+        Returns: (center, radius, fit_error) or None if fit fails
+        """
+        if len(points) < 3:
+            return None
+
+        # Use algebraic circle fit
+        # Circle equation: (x-a)^2 + (y-b)^2 = r^2
+        # Expanded: x^2 + y^2 - 2ax - 2by + (a^2 + b^2 - r^2) = 0
+        # Let c = a^2 + b^2 - r^2
+        # Then: x^2 + y^2 = 2ax + 2by - c
+
+        A = np.zeros((len(points), 3))
+        b = np.zeros(len(points))
+
+        for i, (x, y) in enumerate(points):
+            A[i] = [2*x, 2*y, 1]
+            b[i] = x*x + y*y
+
+        try:
+            # Solve least squares
+            result, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)
+
+            center = result[:2]
+            c = result[2]
+            radius_sq = center[0]**2 + center[1]**2 - c
+
+            if radius_sq < 0:
+                return None
+
+            radius = np.sqrt(radius_sq)
+
+            # Calculate fit error (RMS distance from circle)
+            distances = np.sqrt(np.sum((points - center)**2, axis=1))
+            error = np.sqrt(np.mean((distances - radius)**2))
+
+            return center, radius, error
+
+        except np.linalg.LinAlgError:
+            return None
+
+    def _calculate_range(
+        self,
+        positions: np.ndarray,
+        joint_type: JointType,
+        axis: Optional[np.ndarray],
+        origin: Optional[np.ndarray],
+    ) -> Tuple[float, float]:
+        """Calculate range of motion."""
+
+        if joint_type == JointType.REVOLUTE and axis is not None and origin is not None:
+            # Calculate angles around the axis
+            angles = []
+            for pos in positions:
+                # Vector from origin to position
+                vec = pos - origin
+                # Project out axis component
+                vec_proj = vec - np.dot(vec, axis) * axis
+                # Angle (arbitrary reference)
+                if len(angles) == 0:
+                    ref_vec = vec_proj / (np.linalg.norm(vec_proj) + 1e-6)
+                    angles.append(0.0)
+                else:
+                    vec_norm = vec_proj / (np.linalg.norm(vec_proj) + 1e-6)
+                    cos_angle = np.clip(np.dot(ref_vec, vec_norm), -1, 1)
+                    angle = np.arccos(cos_angle)
+                    # Determine sign using cross product
+                    cross = np.cross(ref_vec, vec_norm)
+                    if np.dot(cross, axis) < 0:
+                        angle = -angle
+                    angles.append(angle)
+
+            return min(angles), max(angles)
+
+        elif joint_type == JointType.PRISMATIC and axis is not None:
+            # Project positions onto axis
+            projections = [np.dot(pos, axis) for pos in positions]
+            return min(projections), max(projections)
+
+        return 0.0, 0.0
+
+
+class VisualSystemIdentifier:
+    """
+    Main class for visual system identification.
+
+    Workflow:
+    1. Connect camera and robot
+    2. Detect markers in initial frame
+    3. For each motor, perform "wiggle test"
+    4. Analyze motion patterns
+    5. Infer kinematic structure
+    6. Generate URDF
+    """
+
+    def __init__(
+        self,
+        robot_interface=None,
+        camera_index: int = 0,
+        marker_size_meters: float = 0.03,
+    ):
+        self.robot = robot_interface
+        self.tracker = ArUcoTracker(
+            camera_index=camera_index,
+            marker_size_meters=marker_size_meters,
+        )
+        self.results: Dict[int, JointInference] = {}
+
+    def connect(self) -> bool:
+        """Connect to camera."""
+        return self.tracker.connect()
+
+    def disconnect(self):
+        """Disconnect."""
+        self.tracker.disconnect()
+
+    def run_wiggle_test(
+        self,
+        motor_id: int,
+        pwm_min: int = 500,
+        pwm_max: int = 2500,
+        steps: int = 10,
+        dwell_time: float = 0.3,
+        cmd_func=None,
+    ) -> List[MotionSample]:
+        """
+        Run a "wiggle test" on a single motor.
+
+        Moves the motor from min to max and records marker positions.
+
+        Args:
+            motor_id: Servo/motor to test
+            pwm_min: Minimum PWM value
+            pwm_max: Maximum PWM value
+            steps: Number of positions to sample
+            dwell_time: Time to wait at each position
+            cmd_func: Function to send commands to robot
+
+        Returns:
+            List of MotionSamples
+        """
+        samples = []
+        pwm_values = np.linspace(pwm_min, pwm_max, steps, dtype=int)
+
+        print(f"\n  Wiggle test: Motor {motor_id}")
+        print(f"  PWM range: {pwm_min} - {pwm_max} ({steps} steps)")
+
+        for i, pwm in enumerate(pwm_values):
+            # Move motor
+            if cmd_func:
+                cmd_func(f"dog.set_servo({motor_id}, {pwm}, 300)")
+            elif self.robot:
+                self.robot.set_servo(motor_id, pwm, 300)
+
+            time.sleep(dwell_time)
+
+            # Capture markers
+            poses, annotated = self.tracker.detect_markers()
+
+            sample = MotionSample(
+                motor_id=motor_id,
+                pwm_value=int(pwm),
+                marker_poses=poses,
+            )
+            samples.append(sample)
+
+            # Display progress
+            marker_ids = list(poses.keys())
+            print(f"    Step {i+1}/{steps}: PWM={pwm}, Markers={marker_ids}")
+
+            # Show frame
+            cv2.imshow("Visual System ID", annotated)
+            cv2.waitKey(1)
+
+        return samples
+
+    def analyze_motor(self, samples: List[MotionSample]) -> Optional[JointInference]:
+        """
+        Analyze motion samples to infer joint properties.
+        """
+        if not samples:
+            return None
+
+        # Find markers that were visible in most frames
+        marker_counts = {}
+        for sample in samples:
+            for mid in sample.marker_poses.keys():
+                marker_counts[mid] = marker_counts.get(mid, 0) + 1
+
+        # Sort by visibility
+        sorted_markers = sorted(
+            marker_counts.items(),
+            key=lambda x: x[1],
+            reverse=True
+        )
+
+        if len(sorted_markers) < 2:
+            print("  Need at least 2 markers visible!")
+            return None
+
+        # Assume first marker (most visible) is reference, second is moving
+        # In practice, user should specify or we should detect which moves
+        reference = sorted_markers[0][0]
+
+        best_inference = None
+        best_confidence = 0.0
+
+        for marker_id, count in sorted_markers[1:]:
+            if count < len(samples) * 0.7:  # Need 70% visibility
+                continue
+
+            analyzer = MotionAnalyzer(samples)
+            inference = analyzer.analyze_motion(reference, marker_id)
+
+            if inference.confidence > best_confidence:
+                best_confidence = inference.confidence
+                best_inference = inference
+
+        return best_inference
+
+    def identify_robot(
+        self,
+        motor_ids: List[int],
+        cmd_func=None,
+    ) -> Dict[int, JointInference]:
+        """
+        Full robot identification.
+
+        Args:
+            motor_ids: List of motor IDs to test
+            cmd_func: Function to send robot commands
+
+        Returns:
+            Dict mapping motor_id -> JointInference
+        """
+        print("\n" + "=" * 60)
+        print("VISUAL SYSTEM IDENTIFICATION")
+        print("=" * 60)
+
+        # Check for markers
+        poses, annotated = self.tracker.detect_markers()
+        print(f"\nDetected {len(poses)} markers: {list(poses.keys())}")
+
+        if len(poses) < 2:
+            print("ERROR: Need at least 2 ArUco markers attached to robot!")
+            print("Print markers from: https://chev.me/arucogen/")
+            return {}
+
+        cv2.imshow("Visual System ID", annotated)
+        cv2.waitKey(1)
+
+        results = {}
+
+        for motor_id in motor_ids:
+            print(f"\n--- Testing Motor {motor_id} ---")
+
+            # Run wiggle test
+            samples = self.run_wiggle_test(
+                motor_id=motor_id,
+                cmd_func=cmd_func,
+            )
+
+            # Analyze
+            inference = self.analyze_motor(samples)
+
+            if inference:
+                results[motor_id] = inference
+                print(f"\n  Result: {inference.joint_type.value}")
+                if inference.axis is not None:
+                    print(f"  Axis: {inference.axis}")
+                if inference.joint_type == JointType.REVOLUTE:
+                    range_deg = math.degrees(inference.range_max - inference.range_min)
+                    print(f"  Range: {range_deg:.1f} degrees")
+                print(f"  Confidence: {inference.confidence:.2f}")
+            else:
+                print("  Could not infer joint properties")
+
+            # Return to center
+            if cmd_func:
+                cmd_func(f"dog.set_servo({motor_id}, 1500, 500)")
+            elif self.robot:
+                self.robot.set_servo(motor_id, 1500, 500)
+            time.sleep(0.5)
+
+        self.results = results
+        return results
+
+    def generate_urdf(self, robot_name: str = "robot") -> str:
+        """
+        Generate URDF from identified joints.
+
+        This is a simplified URDF generator. A full implementation
+        would need link geometries, masses, etc.
+        """
+        lines = [
+            '<?xml version="1.0"?>',
+            f'<robot name="{robot_name}">',
+            '  <!-- Auto-generated by Visual System Identification -->',
+            '',
+            '  <!-- Base Link (fixed to world) -->',
+            '  <link name="base_link">',
+            '    <visual>',
+            '      <geometry><box size="0.1 0.1 0.02"/></geometry>',
+            '    </visual>',
+            '  </link>',
+            '',
+        ]
+
+        parent_link = "base_link"
+
+        for i, (motor_id, inference) in enumerate(self.results.items()):
+            link_name = f"link_{motor_id}"
+            joint_name = f"joint_{motor_id}"
+
+            # Joint
+            joint_type = "revolute" if inference.joint_type == JointType.REVOLUTE else "fixed"
+
+            lines.append(f'  <joint name="{joint_name}" type="{joint_type}">')
+            lines.append(f'    <parent link="{parent_link}"/>')
+            lines.append(f'    <child link="{link_name}"/>')
+
+            if inference.origin is not None:
+                ox, oy, oz = inference.origin
+                lines.append(f'    <origin xyz="{ox:.4f} {oy:.4f} {oz:.4f}"/>')
+
+            if inference.axis is not None and inference.joint_type == JointType.REVOLUTE:
+                ax, ay, az = inference.axis
+                lines.append(f'    <axis xyz="{ax:.4f} {ay:.4f} {az:.4f}"/>')
+                lines.append(f'    <limit lower="{inference.range_min:.4f}" upper="{inference.range_max:.4f}" effort="10" velocity="1"/>')
+
+            lines.append('  </joint>')
+            lines.append('')
+
+            # Link
+            lines.append(f'  <link name="{link_name}">')
+            lines.append('    <visual>')
+            lines.append('      <geometry><cylinder radius="0.01" length="0.05"/></geometry>')
+            lines.append('    </visual>')
+            lines.append('  </link>')
+            lines.append('')
+
+            parent_link = link_name
+
+        lines.append('</robot>')
+
+        return '\n'.join(lines)
+
+    def save_results(self, path: Path):
+        """Save identification results to JSON."""
+        data = {
+            "timestamp": time.strftime("%Y-%m-%d %H:%M:%S"),
+            "joints": {
+                str(motor_id): inference.to_dict()
+                for motor_id, inference in self.results.items()
+            },
+        }
+
+        with open(path, 'w') as f:
+            json.dump(data, f, indent=2)
+
+
+def generate_aruco_markers(output_dir: Path, marker_ids: List[int] = [0, 1, 2, 3, 4]):
+    """
+    Generate printable ArUco markers.
+
+    Print these and attach to robot links.
+    """
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+
+    for marker_id in marker_ids:
+        marker_image = cv2.aruco.generateImageMarker(aruco_dict, marker_id, 200)
+
+        # Add border and ID label
+        bordered = cv2.copyMakeBorder(marker_image, 20, 40, 20, 20, cv2.BORDER_CONSTANT, value=255)
+        cv2.putText(bordered, f"ID: {marker_id}", (60, bordered.shape[0] - 10),
+                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, 0, 2)
+
+        path = output_dir / f"marker_{marker_id}.png"
+        cv2.imwrite(str(path), bordered)
+        print(f"Saved: {path}")
+
+    print(f"\nPrint markers at ~3cm size and attach to robot links")
+
+
+def demo_tracking():
+    """Demo: Just show marker tracking without robot."""
+    print("ArUco Marker Tracking Demo")
+    print("Press 'q' to quit, 's' to save screenshot")
+
+    tracker = ArUcoTracker(camera_index=0)
+    if not tracker.connect():
+        print("Failed to open camera!")
+        return
+
+    try:
+        while True:
+            poses, annotated = tracker.detect_markers()
+
+            # Show info
+            y = 30
+            for marker_id, pose in poses.items():
+                pos = pose.position
+                roll, pitch, yaw = pose.euler_angles
+                text = f"ID {marker_id}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})"
+                cv2.putText(annotated, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
+                y += 20
+                text = f"         rot=({roll:.1f}, {pitch:.1f}, {yaw:.1f}) deg"
+                cv2.putText(annotated, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
+                y += 25
+
+            cv2.imshow("ArUco Tracking", annotated)
+
+            key = cv2.waitKey(1) & 0xFF
+            if key == ord('q'):
+                break
+            elif key == ord('s'):
+                cv2.imwrite("/tmp/aruco_frame.jpg", annotated)
+                print("Saved to /tmp/aruco_frame.jpg")
+
+    finally:
+        tracker.disconnect()
+        cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser(description="Visual System Identification")
+    parser.add_argument("--generate-markers", action="store_true", help="Generate printable ArUco markers")
+    parser.add_argument("--demo", action="store_true", help="Demo marker tracking (no robot)")
+    parser.add_argument("--port", default="/dev/cu.usbserial-10", help="Robot serial port")
+    parser.add_argument("--motors", default="11,12,13", help="Motor IDs to test (comma-separated)")
+    parser.add_argument("--output", default="/tmp/visual_id_results.json", help="Output file")
+
+    args = parser.parse_args()
+
+    if args.generate_markers:
+        generate_aruco_markers(Path("/tmp/aruco_markers"))
+
+    elif args.demo:
+        demo_tracking()
+
+    else:
+        # Full identification
+        motor_ids = [int(x) for x in args.motors.split(",")]
+
+        # Connect to robot
+        import serial
+        ser = serial.Serial(args.port, 115200, timeout=2)
+        time.sleep(0.5)
+        ser.read(ser.in_waiting)
+
+        def cmd(command: str):
+            ser.write(f"{command}\r\n".encode())
+            time.sleep(0.3)
+            return ser.read(ser.in_waiting).decode('utf-8', errors='ignore')
+
+        cmd("from HW_MechDog import MechDog", 1.5)
+        time.sleep(1)
+        cmd("dog = MechDog()", 1.5)
+        time.sleep(1)
+
+        try:
+            identifier = VisualSystemIdentifier(camera_index=0)
+
+            if not identifier.connect():
+                print("Failed to open camera!")
+                exit(1)
+
+            results = identifier.identify_robot(motor_ids, cmd_func=cmd)
+
+            # Save results
+            identifier.save_results(Path(args.output))
+            print(f"\nResults saved to: {args.output}")
+
+            # Generate URDF
+            urdf = identifier.generate_urdf("mechdog_arm")
+            urdf_path = Path(args.output).with_suffix(".urdf")
+            with open(urdf_path, 'w') as f:
+                f.write(urdf)
+            print(f"URDF saved to: {urdf_path}")
+
+        finally:
+            identifier.disconnect()
+            ser.close()
+            cv2.destroyAllWindows()