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

ate/urdf/topology.py

@@ -0,0 +1,336 @@
+"""
+Kinematic topology discovery for URDF generation.
+
+This module handles discovering the parent-child relationships
+between robot links based on their motion patterns:
+- Links that move together are likely connected
+- The link with least motion is likely the base
+- Parent-child relationships form a tree structure
+"""
+
+import logging
+from typing import Dict, List, Optional, Tuple, Set
+from dataclasses import dataclass
+
+logger = logging.getLogger(__name__)
+
+try:
+    import numpy as np
+    NUMPY_AVAILABLE = True
+except ImportError:
+    NUMPY_AVAILABLE = False
+    np = None
+
+
+class TopologyError(Exception):
+    """Error during topology discovery."""
+    pass
+
+
+@dataclass
+class LinkNode:
+    """Node in the kinematic tree."""
+    name: str
+    parent: Optional[str]
+    children: List[str]
+    is_fixed: bool = False  # Base link
+
+
+@dataclass
+class KinematicTree:
+    """Kinematic tree structure."""
+    nodes: Dict[str, LinkNode]
+    root: str
+
+    def get_parent(self, link_name: str) -> Optional[str]:
+        """Get parent of a link."""
+        if link_name in self.nodes:
+            return self.nodes[link_name].parent
+        return None
+
+    def get_children(self, link_name: str) -> List[str]:
+        """Get children of a link."""
+        if link_name in self.nodes:
+            return self.nodes[link_name].children
+        return []
+
+    def get_chain_to_root(self, link_name: str) -> List[str]:
+        """Get chain of links from given link to root."""
+        chain = []
+        current = link_name
+        while current is not None:
+            chain.append(current)
+            current = self.get_parent(current)
+        return chain
+
+    def get_ordered_links(self) -> List[str]:
+        """Get links in depth-first order from root."""
+        ordered = []
+        visited = set()
+
+        def dfs(node_name: str):
+            if node_name in visited:
+                return
+            visited.add(node_name)
+            ordered.append(node_name)
+            for child in self.get_children(node_name):
+                dfs(child)
+
+        dfs(self.root)
+        return ordered
+
+    def validate(self) -> bool:
+        """Validate tree structure."""
+        # Check that all nodes are reachable from root
+        ordered = self.get_ordered_links()
+        return len(ordered) == len(self.nodes)
+
+
+def compute_motion_correlation(
+    trajectory1: "LinkTrajectory",
+    trajectory2: "LinkTrajectory",
+) -> float:
+    """
+    Compute motion correlation between two link trajectories.
+
+    High correlation indicates the links move together (likely connected).
+
+    Args:
+        trajectory1: First link trajectory
+        trajectory2: Second link trajectory
+
+    Returns:
+        Correlation coefficient [0, 1]
+    """
+    if not NUMPY_AVAILABLE:
+        raise TopologyError("NumPy not available")
+
+    from .motion_analysis import LinkTrajectory
+
+    # Find common frames
+    frames1 = set(trajectory1.frame_indices)
+    frames2 = set(trajectory2.frame_indices)
+    common = sorted(frames1 & frames2)
+
+    if len(common) < 3:
+        return 0.0
+
+    # Get velocities at common frames
+    vel1 = trajectory1.get_velocity()
+    vel2 = trajectory2.get_velocity()
+
+    # Align to common frames
+    idx1 = [trajectory1.frame_indices.index(f) for f in common[:-1]]
+    idx2 = [trajectory2.frame_indices.index(f) for f in common[:-1]]
+
+    v1 = vel1[idx1] if len(vel1) > 0 else np.zeros((len(common)-1, 3))
+    v2 = vel2[idx2] if len(vel2) > 0 else np.zeros((len(common)-1, 3))
+
+    # Compute correlation of velocity magnitudes
+    mag1 = np.linalg.norm(v1, axis=1)
+    mag2 = np.linalg.norm(v2, axis=1)
+
+    if np.std(mag1) < 1e-6 or np.std(mag2) < 1e-6:
+        return 0.5  # No motion variance
+
+    corr = np.corrcoef(mag1, mag2)[0, 1]
+    return float(np.clip(corr, 0, 1))
+
+
+def identify_base_link(
+    trajectories: Dict[str, "LinkTrajectory"],
+    fixed_hints: Optional[Set[str]] = None,
+) -> str:
+    """
+    Identify the base (fixed) link of the robot.
+
+    The base link typically has:
+    - Least total motion
+    - Or is explicitly marked as fixed by the user
+
+    Args:
+        trajectories: Dict of link trajectories
+        fixed_hints: Optional set of link names marked as fixed by user
+
+    Returns:
+        Name of the base link
+    """
+    if not NUMPY_AVAILABLE:
+        raise TopologyError("NumPy not available")
+
+    # If user marked a link as fixed, use that
+    if fixed_hints:
+        for name in fixed_hints:
+            if name in trajectories:
+                logger.info(f"Using user-marked base link: {name}")
+                return name
+
+    # Otherwise, find link with least motion
+    motion_scores = {}
+    for name, traj in trajectories.items():
+        motion_scores[name] = traj.get_total_displacement()
+
+    base = min(motion_scores, key=motion_scores.get)
+    logger.info(f"Identified base link: {base} (displacement: {motion_scores[base]:.4f}m)")
+
+    return base
+
+
+def build_kinematic_tree(
+    trajectories: Dict[str, "LinkTrajectory"],
+    fixed_hints: Optional[Set[str]] = None,
+    adjacency_hints: Optional[Dict[str, str]] = None,
+) -> KinematicTree:
+    """
+    Build kinematic tree from link trajectories.
+
+    Uses motion correlation to determine parent-child relationships:
+    - Base link has no parent
+    - Each other link's parent is the most correlated link that has less motion
+
+    Args:
+        trajectories: Dict of link trajectories
+        fixed_hints: Links marked as fixed/base by user
+        adjacency_hints: Optional dict of link -> parent hints
+
+    Returns:
+        KinematicTree structure
+    """
+    if not NUMPY_AVAILABLE:
+        raise TopologyError("NumPy not available")
+
+    link_names = list(trajectories.keys())
+    n_links = len(link_names)
+
+    if n_links == 0:
+        raise TopologyError("No links to build tree from")
+
+    # Identify base
+    base_name = identify_base_link(trajectories, fixed_hints)
+
+    # Compute pairwise correlations
+    correlations = np.zeros((n_links, n_links))
+    for i, name1 in enumerate(link_names):
+        for j, name2 in enumerate(link_names):
+            if i != j:
+                correlations[i, j] = compute_motion_correlation(
+                    trajectories[name1],
+                    trajectories[name2],
+                )
+
+    # Compute motion magnitudes
+    motion_magnitudes = {
+        name: traj.get_total_displacement()
+        for name, traj in trajectories.items()
+    }
+
+    # Build tree using Prim's algorithm variant
+    # Start from base, add links in order of decreasing correlation
+    nodes = {}
+    added = {base_name}
+    nodes[base_name] = LinkNode(
+        name=base_name,
+        parent=None,
+        children=[],
+        is_fixed=True,
+    )
+
+    while len(added) < n_links:
+        best_child = None
+        best_parent = None
+        best_score = -1
+
+        for parent_name in added:
+            parent_idx = link_names.index(parent_name)
+
+            for child_idx, child_name in enumerate(link_names):
+                if child_name in added:
+                    continue
+
+                # Use adjacency hints if available
+                if adjacency_hints and child_name in adjacency_hints:
+                    if adjacency_hints[child_name] == parent_name:
+                        best_child = child_name
+                        best_parent = parent_name
+                        best_score = 1.0
+                        break
+
+                # Score = correlation * preference for closer motion magnitude
+                corr = correlations[parent_idx, child_idx]
+                parent_motion = motion_magnitudes[parent_name]
+                child_motion = motion_magnitudes[child_name]
+
+                # Prefer parent with less motion than child
+                if parent_motion < child_motion:
+                    score = corr * 1.1
+                else:
+                    score = corr * 0.9
+
+                if score > best_score:
+                    best_score = score
+                    best_child = child_name
+                    best_parent = parent_name
+
+            if best_score >= 1.0:  # Hint matched
+                break
+
+        if best_child is None:
+            # Fallback: add remaining links to base
+            for name in link_names:
+                if name not in added:
+                    best_child = name
+                    best_parent = base_name
+                    break
+
+        # Add child node
+        nodes[best_child] = LinkNode(
+            name=best_child,
+            parent=best_parent,
+            children=[],
+            is_fixed=False,
+        )
+        nodes[best_parent].children.append(best_child)
+        added.add(best_child)
+
+        logger.debug(f"Added {best_child} as child of {best_parent} (score: {best_score:.3f})")
+
+    tree = KinematicTree(nodes=nodes, root=base_name)
+
+    if not tree.validate():
+        raise TopologyError("Built tree is invalid (disconnected nodes)")
+
+    logger.info(f"Built kinematic tree: {tree.get_ordered_links()}")
+    return tree
+
+
+def infer_joint_names(tree: KinematicTree) -> Dict[Tuple[str, str], str]:
+    """
+    Generate joint names from parent-child pairs.
+
+    Args:
+        tree: KinematicTree structure
+
+    Returns:
+        Dict mapping (parent, child) -> joint_name
+    """
+    joint_names = {}
+
+    for link_name, node in tree.nodes.items():
+        if node.parent is not None:
+            parent = node.parent
+            child = link_name
+            joint_name = f"{parent}_to_{child}_joint"
+            joint_names[(parent, child)] = joint_name
+
+    return joint_names
+
+
+__all__ = [
+    "TopologyError",
+    "LinkNode",
+    "KinematicTree",
+    "compute_motion_correlation",
+    "identify_base_link",
+    "build_kinematic_tree",
+    "infer_joint_names",
+]

ate/urdf/validation.py

@@ -0,0 +1,371 @@
+"""
+URDF validation for generated robot descriptions.
+
+This module provides validation checks for URDF files:
+- XML syntax validation
+- Structural validation (links, joints, tree structure)
+- Physics validation (mass, inertia positivity)
+- Mesh file existence checks
+"""
+
+import logging
+from typing import Dict, List, Optional, Tuple
+from pathlib import Path
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+
+logger = logging.getLogger(__name__)
+
+
+class ValidationError(Exception):
+    """Error during URDF validation."""
+    pass
+
+
+@dataclass
+class ValidationResult:
+    """Result of URDF validation."""
+    valid: bool
+    errors: List[str]
+    warnings: List[str]
+    info: Dict
+
+    def __bool__(self) -> bool:
+        return self.valid
+
+    def summary(self) -> str:
+        lines = []
+        if self.valid:
+            lines.append("URDF is valid")
+        else:
+            lines.append("URDF has errors")
+
+        if self.errors:
+            lines.append(f"\nErrors ({len(self.errors)}):")
+            for err in self.errors:
+                lines.append(f"  - {err}")
+
+        if self.warnings:
+            lines.append(f"\nWarnings ({len(self.warnings)}):")
+            for warn in self.warnings:
+                lines.append(f"  - {warn}")
+
+        lines.append(f"\nInfo:")
+        for key, value in self.info.items():
+            lines.append(f"  {key}: {value}")
+
+        return "\n".join(lines)
+
+
+def validate_xml_syntax(urdf_content: str) -> Tuple[bool, Optional[str]]:
+    """
+    Validate XML syntax.
+
+    Args:
+        urdf_content: URDF XML string
+
+    Returns:
+        Tuple of (valid, error_message)
+    """
+    try:
+        ET.fromstring(urdf_content)
+        return True, None
+    except ET.ParseError as e:
+        return False, f"XML parse error: {e}"
+
+
+def validate_structure(root: ET.Element) -> Tuple[List[str], List[str]]:
+    """
+    Validate URDF structure (links, joints, tree).
+
+    Args:
+        root: Root XML element
+
+    Returns:
+        Tuple of (errors, warnings)
+    """
+    errors = []
+    warnings = []
+
+    # Check root element
+    if root.tag != "robot":
+        errors.append(f"Root element should be 'robot', got '{root.tag}'")
+        return errors, warnings
+
+    if "name" not in root.attrib:
+        warnings.append("Robot element has no 'name' attribute")
+
+    # Collect links and joints
+    links = {}
+    joints = []
+
+    for link in root.findall("link"):
+        name = link.get("name")
+        if not name:
+            errors.append("Link element missing 'name' attribute")
+            continue
+        if name in links:
+            errors.append(f"Duplicate link name: {name}")
+        links[name] = link
+
+    for joint in root.findall("joint"):
+        name = joint.get("name")
+        jtype = joint.get("type")
+        parent = joint.find("parent")
+        child = joint.find("child")
+
+        if not name:
+            errors.append("Joint element missing 'name' attribute")
+            continue
+        if not jtype:
+            errors.append(f"Joint '{name}' missing 'type' attribute")
+
+        if parent is None:
+            errors.append(f"Joint '{name}' missing 'parent' element")
+        elif parent.get("link") not in links:
+            errors.append(f"Joint '{name}' references unknown parent link: {parent.get('link')}")
+
+        if child is None:
+            errors.append(f"Joint '{name}' missing 'child' element")
+        elif child.get("link") not in links:
+            errors.append(f"Joint '{name}' references unknown child link: {child.get('link')}")
+
+        joints.append({
+            "name": name,
+            "type": jtype,
+            "parent": parent.get("link") if parent is not None else None,
+            "child": child.get("link") if child is not None else None,
+        })
+
+    # Check for valid joint types
+    VALID_JOINT_TYPES = ["revolute", "prismatic", "continuous", "fixed", "floating", "planar"]
+    for joint in joints:
+        if joint["type"] and joint["type"] not in VALID_JOINT_TYPES:
+            errors.append(f"Joint '{joint['name']}' has invalid type: {joint['type']}")
+
+    # Check for tree structure (no cycles, single root)
+    children = set()
+    for joint in joints:
+        if joint["child"]:
+            if joint["child"] in children:
+                errors.append(f"Link '{joint['child']}' has multiple parent joints")
+            children.add(joint["child"])
+
+    roots = set(links.keys()) - children
+    if len(roots) == 0:
+        errors.append("No root link found (cycle detected)")
+    elif len(roots) > 1:
+        warnings.append(f"Multiple root links found: {roots}")
+
+    # Check for required elements
+    for link_name, link in links.items():
+        inertial = link.find("inertial")
+        if inertial is None:
+            warnings.append(f"Link '{link_name}' has no inertial element")
+        else:
+            mass = inertial.find("mass")
+            if mass is None:
+                errors.append(f"Link '{link_name}' inertial missing mass")
+
+    return errors, warnings
+
+
+def validate_physics(root: ET.Element) -> Tuple[List[str], List[str]]:
+    """
+    Validate physics properties (mass, inertia).
+
+    Args:
+        root: Root XML element
+
+    Returns:
+        Tuple of (errors, warnings)
+    """
+    errors = []
+    warnings = []
+
+    for link in root.findall("link"):
+        name = link.get("name", "unnamed")
+        inertial = link.find("inertial")
+
+        if inertial is None:
+            continue
+
+        # Check mass
+        mass_elem = inertial.find("mass")
+        if mass_elem is not None:
+            try:
+                mass = float(mass_elem.get("value", "0"))
+                if mass <= 0:
+                    warnings.append(f"Link '{name}' has non-positive mass: {mass}")
+                elif mass < 0.001:
+                    warnings.append(f"Link '{name}' has very small mass: {mass}kg")
+            except ValueError:
+                errors.append(f"Link '{name}' has invalid mass value")
+
+        # Check inertia
+        inertia = inertial.find("inertia")
+        if inertia is not None:
+            try:
+                ixx = float(inertia.get("ixx", "0"))
+                iyy = float(inertia.get("iyy", "0"))
+                izz = float(inertia.get("izz", "0"))
+
+                # Diagonal elements must be positive
+                if ixx <= 0 or iyy <= 0 or izz <= 0:
+                    warnings.append(
+                        f"Link '{name}' has non-positive inertia diagonal: "
+                        f"ixx={ixx}, iyy={iyy}, izz={izz}"
+                    )
+
+                # Triangle inequality for principal moments
+                if ixx + iyy < izz or ixx + izz < iyy or iyy + izz < ixx:
+                    warnings.append(f"Link '{name}' inertia violates triangle inequality")
+
+            except ValueError:
+                errors.append(f"Link '{name}' has invalid inertia values")
+
+    return errors, warnings
+
+
+def validate_meshes(
+    root: ET.Element,
+    base_path: Path,
+) -> Tuple[List[str], List[str]]:
+    """
+    Validate mesh file existence.
+
+    Args:
+        root: Root XML element
+        base_path: Base path for resolving mesh paths
+
+    Returns:
+        Tuple of (errors, warnings)
+    """
+    errors = []
+    warnings = []
+
+    for link in root.findall("link"):
+        name = link.get("name", "unnamed")
+
+        for geom_type in ["visual", "collision"]:
+            geom = link.find(geom_type)
+            if geom is None:
+                continue
+
+            mesh = geom.find("geometry/mesh")
+            if mesh is not None:
+                filename = mesh.get("filename")
+                if filename:
+                    mesh_path = base_path / filename
+                    if not mesh_path.exists():
+                        warnings.append(
+                            f"Link '{name}' {geom_type} mesh not found: {filename}"
+                        )
+
+    return errors, warnings
+
+
+def validate_urdf(
+    urdf_path: Path,
+    check_meshes: bool = True,
+) -> ValidationResult:
+    """
+    Validate a URDF file.
+
+    Args:
+        urdf_path: Path to URDF file
+        check_meshes: Whether to check mesh file existence
+
+    Returns:
+        ValidationResult with errors/warnings
+    """
+    errors = []
+    warnings = []
+    info = {"path": str(urdf_path)}
+
+    # Read file
+    try:
+        with open(urdf_path, 'r') as f:
+            content = f.read()
+    except Exception as e:
+        return ValidationResult(
+            valid=False,
+            errors=[f"Failed to read file: {e}"],
+            warnings=[],
+            info=info,
+        )
+
+    # Validate XML
+    xml_valid, xml_error = validate_xml_syntax(content)
+    if not xml_valid:
+        return ValidationResult(
+            valid=False,
+            errors=[xml_error],
+            warnings=[],
+            info=info,
+        )
+
+    # Parse XML
+    root = ET.fromstring(content)
+    info["robot_name"] = root.get("name", "unnamed")
+
+    # Validate structure
+    struct_errors, struct_warnings = validate_structure(root)
+    errors.extend(struct_errors)
+    warnings.extend(struct_warnings)
+
+    # Validate physics
+    physics_errors, physics_warnings = validate_physics(root)
+    errors.extend(physics_errors)
+    warnings.extend(physics_warnings)
+
+    # Validate meshes
+    if check_meshes:
+        base_path = urdf_path.parent
+        mesh_errors, mesh_warnings = validate_meshes(root, base_path)
+        errors.extend(mesh_errors)
+        warnings.extend(mesh_warnings)
+
+    # Gather info
+    info["links"] = len(root.findall("link"))
+    info["joints"] = len(root.findall("joint"))
+
+    return ValidationResult(
+        valid=(len(errors) == 0),
+        errors=errors,
+        warnings=warnings,
+        info=info,
+    )
+
+
+def run_validation(session: "ScanSession") -> ValidationResult:
+    """
+    Run validation on session's generated URDF.
+
+    Args:
+        session: ScanSession with generated URDF
+
+    Returns:
+        ValidationResult
+    """
+    if not session.has_urdf():
+        return ValidationResult(
+            valid=False,
+            errors=["No URDF file generated yet"],
+            warnings=[],
+            info={},
+        )
+
+    return validate_urdf(session.urdf_path, check_meshes=True)
+
+
+__all__ = [
+    "ValidationError",
+    "ValidationResult",
+    "validate_xml_syntax",
+    "validate_structure",
+    "validate_physics",
+    "validate_meshes",
+    "validate_urdf",
+    "run_validation",
+]