Skip to content

Official Codebase of 3DCodeVerse

Python
16.6 KB
Raw Blame
"""URDF data model + loader + forward kinematics (numpy, trimesh)."""

from __future__ import annotations

import math
import xml.etree.ElementTree as ET
from dataclasses import dataclass, field
from pathlib import Path

import numpy as np
import trimesh

from codeverse3d.contracts.common import MimicIssue, MimicSpec, mimic_issues

MOVABLE_TYPES = ("revolute", "prismatic", "continuous")
JOINT_TYPES = MOVABLE_TYPES + ("fixed",)
RESERVED_LINK_NAMES = frozenset({"world"})


class UrdfError(ValueError):
    """Raised when a URDF cannot be turned into a kinematic tree (use lint for details)."""


# ------------------------------------------------------------------ small math
def parse_floats(text: str | None, n: int, what: str) -> tuple[float, ...]:
    """Parse ``"a b c"`` → ``n`` finite floats."""
    parts = (text or "").split()
    if len(parts) != n:
        raise UrdfError(f"{what}: expected {n} number{'s' if n > 1 else ''}, got {text!r}")
    try:
        vals = tuple(float(p) for p in parts)
    except ValueError as e:
        raise UrdfError(f"{what}: not numeric: {text!r}") from e
    if not all(math.isfinite(v) for v in vals):
        raise UrdfError(f"{what}: not finite: {text!r}")
    return vals


def _scalar(el: ET.Element, attr: str, what: str, default: float | None = None) -> float | None:
    """One optional numeric attribute (``default`` when absent or empty)."""
    text = el.get(attr)
    return default if text is None or not text.strip() else parse_floats(text, 1, f"{what} {attr}")[0]


def rpy_to_matrix(rpy: tuple[float, float, float] | np.ndarray) -> np.ndarray:
    """URDF fixed-axis roll/pitch/yaw → 3x3 rotation (Rz·Ry·Rx)."""
    r, p, y = (float(v) for v in rpy)
    cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
    rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
    ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
    rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
    return rz @ ry @ rx


def matrix_to_rpy(R: np.ndarray) -> tuple[float, float, float]:
    """Inverse of :func:`rpy_to_matrix` (pitch in (-pi/2, pi/2) branch)."""
    sy = -R[2, 0]
    sy = max(-1.0, min(1.0, float(sy)))
    pitch = math.asin(sy)
    if abs(math.cos(pitch)) < 1e-9:
        roll = math.atan2(-R[1, 2], R[1, 1])
        yaw = 0.0
    else:
        roll = math.atan2(R[2, 1], R[2, 2])
        yaw = math.atan2(R[1, 0], R[0, 0])
    return (roll, pitch, yaw)


def make_transform(xyz=(0.0, 0.0, 0.0), rpy=(0.0, 0.0, 0.0)) -> np.ndarray:
    T = np.eye(4)
    T[:3, :3] = rpy_to_matrix(rpy)
    T[:3, 3] = np.asarray(xyz, dtype=float)
    return T


def invert_transform(T: np.ndarray) -> np.ndarray:
    R = T[:3, :3]
    out = np.eye(4)
    out[:3, :3] = R.T
    out[:3, 3] = -R.T @ T[:3, 3]
    return out


def rotation_about_axis(axis: np.ndarray, angle: float) -> np.ndarray:
    """Rodrigues rotation matrix (axis must be unit)."""
    x, y, z = (float(v) for v in axis)
    c, s, C = math.cos(angle), math.sin(angle), 1.0 - math.cos(angle)
    return np.array(
        [
            [c + x * x * C, x * y * C - z * s, x * z * C + y * s],
            [y * x * C + z * s, c + y * y * C, y * z * C - x * s],
            [z * x * C - y * s, z * y * C + x * s, c + z * z * C],
        ]
    )


def parse_origin(elem: ET.Element | None, what: str) -> np.ndarray:
    """``<origin xyz rpy>`` child of ``elem`` → 4x4 (identity when absent)."""
    if elem is None:
        return np.eye(4)
    o = elem.find("origin")
    if o is None:
        return np.eye(4)
    xyz = parse_floats(o.get("xyz"), 3, f"{what} origin xyz") if o.get("xyz") is not None else (0.0, 0.0, 0.0)
    rpy = parse_floats(o.get("rpy"), 3, f"{what} origin rpy") if o.get("rpy") is not None else (0.0, 0.0, 0.0)
    return make_transform(xyz, rpy)


# ------------------------------------------------------------------ model
@dataclass(frozen=True)
class Mimic:
    """A joint driven by another."""

    joint: str
    multiplier: float = 1.0
    offset: float = 0.0


@dataclass
class Joint:
    name: str
    type: str
    parent: str
    child: str
    origin: np.ndarray = field(default_factory=lambda: np.eye(4))  # joint frame in parent frame
    axis: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0, 0.0]))  # unit, joint frame
    lower: float | None = None
    upper: float | None = None
    effort: float = 0.0
    velocity: float = 0.0
    mimic: Mimic | None = None

    @property
    def movable(self) -> bool:
        return self.type in MOVABLE_TYPES

    def motion(self, q: float) -> np.ndarray:
        """Local motion transform for joint value ``q`` (identity for fixed)."""
        T = np.eye(4)
        if self.type in ("revolute", "continuous"):
            T[:3, :3] = rotation_about_axis(self.axis, q)
        elif self.type == "prismatic":
            T[:3, 3] = self.axis * q
        return T


@dataclass
class Link:
    name: str
    mesh: trimesh.Trimesh | None = None
    submeshes: list[trimesh.Trimesh] = field(default_factory=list)
    visual_origin: np.ndarray = field(default_factory=lambda: np.eye(4))
    mesh_file: str | None = None


@dataclass
class Robot:
    name: str
    links: dict[str, Link]
    joints: dict[str, Joint]
    root: str

    # -------------------------------------------------------------- topology
    def parent_joint(self, link: str) -> Joint | None:
        for j in self.joints.values():
            if j.child == link:
                return j
        return None

    def child_joints(self, link: str) -> list[Joint]:
        return [j for j in self.joints.values() if j.parent == link]

    def movable_joints(self) -> list[Joint]:
        return [j for j in self.joints.values() if j.movable]

    def independent_joints(self) -> list[Joint]:
        """The degrees of freedom."""
        return [j for j in self.joints.values() if j.movable and j.mimic is None]

    def link_order(self) -> list[str]:
        """Links in parent-before-child order (DFS from root)."""
        out: list[str] = []
        stack = [self.root]
        while stack:
            cur = stack.pop()
            out.append(cur)
            stack.extend(j.child for j in reversed(self.child_joints(cur)))
        return out

    def meshed_links(self) -> list[str]:
        return [n for n, link in self.links.items() if link.mesh is not None]


# ------------------------------------------------------------------ parsing
def _primitive_mesh(geom: ET.Element, what: str) -> trimesh.Trimesh:
    if (b := geom.find("box")) is not None:
        return trimesh.creation.box(parse_floats(b.get("size"), 3, f"{what} box size"))
    if (c := geom.find("cylinder")) is not None:
        return trimesh.creation.cylinder(radius=_scalar(c, "radius", what, 0.0), height=_scalar(c, "length", what, 0.0))
    if (s := geom.find("sphere")) is not None:
        return trimesh.creation.icosphere(subdivisions=3, radius=_scalar(s, "radius", what, 0.0))
    raise UrdfError(f"{what}: unsupported geometry {[e.tag for e in geom]}")


def _load_mesh_file(filename: str, urdf_dir: Path, meshes_dir: Path | None, scale: tuple[float, ...] | None) -> list[trimesh.Trimesh]:
    """Load a mesh file as a list of per-material sub-meshes (transforms baked, raw coords)."""
    candidates = []
    fn = filename.replace("package://", "").replace("file://", "")
    if meshes_dir is not None:
        candidates.append(meshes_dir / Path(fn).name)
        candidates.append(meshes_dir.parent / fn)
    candidates.append(urdf_dir / fn)
    candidates.append(Path(fn))
    for p in candidates:
        if p.is_file():
            loaded = trimesh.load(p)
            subs = loaded.dump() if isinstance(loaded, trimesh.Scene) else [loaded]
            subs = [m for m in subs if isinstance(m, trimesh.Trimesh) and len(m.faces) > 0]
            if not subs:
                raise UrdfError(f"mesh {p} has no triangles")
            for m in subs:
                if scale is not None and any(abs(s - 1.0) > 1e-12 for s in scale):
                    m.apply_scale(scale)
            return subs
    raise UrdfError(f"mesh file not found: {filename!r} (tried {[str(c) for c in candidates]})")


def _link_from_xml(el: ET.Element, urdf_dir: Path, meshes_dir: Path | None, *, load_meshes: bool) -> Link:
    name = el.get("name", "")
    link = Link(name=name)
    parts: list[trimesh.Trimesh] = []
    first_origin: np.ndarray | None = None
    for vis in el.findall("visual"):
        T_vis = parse_origin(vis, f"link {name} visual")
        geom = vis.find("geometry")
        if geom is None:
            raise UrdfError(f"link {name}: visual without <geometry>")
        if first_origin is None:
            first_origin = T_vis
        mesh_el = geom.find("mesh")
        if mesh_el is not None:
            link.mesh_file = mesh_el.get("filename", "")
            if not load_meshes:
                continue
            scale = parse_floats(mesh_el.get("scale"), 3, f"link {name} mesh scale") if mesh_el.get("scale") else None
            pieces = _load_mesh_file(link.mesh_file, urdf_dir, meshes_dir, scale)
        else:
            pieces = [_primitive_mesh(geom, f"link {name}")]
        for m in pieces:
            m = m.copy()
            m.apply_transform(T_vis)
            parts.append(m)
    if first_origin is not None:
        link.visual_origin = first_origin
    if parts:
        link.submeshes = parts
        merged = parts[0].copy() if len(parts) == 1 else trimesh.util.concatenate(parts)
        merged.merge_vertices(merge_tex=True, merge_norm=True)
        link.mesh = merged
    return link


def parse_joint(el: ET.Element) -> Joint:
    """Parse one ``<joint>`` element into a :class:`Joint`, raising :class:`UrdfError` on anything unsupported."""
    name = el.get("name", "")
    jtype = el.get("type", "")
    if jtype not in JOINT_TYPES:
        raise UrdfError(f"joint {name}: unsupported type {jtype!r}")
    parent = el.find("parent")
    child = el.find("child")
    if parent is None or child is None or not parent.get("link") or not child.get("link"):
        raise UrdfError(f"joint {name}: needs <parent link> and <child link>")
    j = Joint(name=name, type=jtype, parent=parent.get("link", ""), child=child.get("link", ""))
    j.origin = parse_origin(el, f"joint {name}")
    ax = el.find("axis")
    if ax is not None and ax.get("xyz") is not None:
        a = np.asarray(parse_floats(ax.get("xyz"), 3, f"joint {name} axis"), dtype=float)
        n = float(np.linalg.norm(a))
        if n < 1e-9:
            raise UrdfError(f"joint {name}: zero axis")
        j.axis = a / n
    lim = el.find("limit")
    if lim is not None:
        what = f"joint {name} <limit>"
        j.effort = _scalar(lim, "effort", what, 0.0)
        j.velocity = _scalar(lim, "velocity", what, 0.0)
        j.lower = _scalar(lim, "lower", what)
        j.upper = _scalar(lim, "upper", what)
    if jtype in ("revolute", "prismatic"):
        if j.lower is None or j.upper is None:
            unit = "rad" if jtype == "revolute" else "m"
            raise UrdfError(f"joint {name}: {jtype} joints need <limit lower upper effort velocity> ({unit}), "
                            'e.g. <limit lower="0" upper="1.57" effort="10" velocity="1"/>')
        if j.upper < j.lower:
            raise UrdfError(f"joint {name}: upper {j.upper} < lower {j.lower}; swap them "
                            "(to move the other way, negate the <axis> instead)")
    mim = el.find("mimic")
    if mim is not None:
        src = mim.get("joint")
        if not src:
            raise UrdfError(f"joint {name}: <mimic> needs joint=\"<other joint>\"")
        mult = _scalar(mim, "multiplier", f"joint {name} <mimic>", 1.0)
        off = _scalar(mim, "offset", f"joint {name} <mimic>", 0.0)
        j.mimic = Mimic(joint=src, multiplier=mult, offset=off)
    return j


def _mimic_message(issue: MimicIssue, joints: dict[str, Joint]) -> str:
    """Describe a broken ``<mimic>`` coupling rule in URDF words."""
    j, t, kind = issue.joint, issue.target, issue.kind
    if kind == "immobile":
        return f"joint {j}: <mimic> on a {joints[j].type} joint has nothing to follow"
    if kind == "zero_multiplier":
        return f'joint {j}: <mimic multiplier="0"> — the joint cannot move; use type=fixed'
    if kind == "unknown_target":
        return f'joint {j}: <mimic joint="{t}"> names no joint; known: {sorted(joints)}'
    if kind == "immobile_target":
        return f"joint {j}: mimics {t!r}, which is {joints[t].type} and never moves"
    if kind == "self":
        return f'joint {j}: <mimic joint="{j}"> names itself'
    return f"joint {j}: <mimic> chain is a cycle through {(issue.detail or j)!r}"


def load_urdf(urdf_path: Path | str, meshes_dir: Path | str | None = None, *, load_meshes: bool = True) -> Robot:
    """Parse a URDF file into a :class:`Robot`, raising :class:`UrdfError` on malformed input."""
    urdf_path = Path(urdf_path)
    try:
        root_el = ET.parse(urdf_path).getroot()
    except ET.ParseError as e:
        raise UrdfError(f"{urdf_path.name}: XML parse error: {e}") from e
    if root_el.tag != "robot":
        raise UrdfError(f"{urdf_path.name}: root element must be <robot>, got <{root_el.tag}>")
    meshes = Path(meshes_dir) if meshes_dir is not None else None
    links = {}
    for el in root_el.findall("link"):
        link = _link_from_xml(el, urdf_path.parent, meshes, load_meshes=load_meshes)
        if link.name in links:
            raise UrdfError(f"duplicate link name {link.name!r}")
        if link.name in RESERVED_LINK_NAMES:
            raise UrdfError(f"link name {link.name!r} is reserved by the GLB scene graph; rename it (e.g. 'base')")
        links[link.name] = link
    joints = {}
    for el in root_el.findall("joint"):
        j = parse_joint(el)
        if j.name in joints:
            raise UrdfError(f"duplicate joint name {j.name!r}")
        if j.parent not in links or j.child not in links:
            raise UrdfError(f"joint {j.name}: unknown parent/child link ({j.parent!r}/{j.child!r})")
        joints[j.name] = j
    if not links:
        raise UrdfError("URDF has no links")
    children = [j.child for j in joints.values()]
    if len(set(children)) != len(children):
        dup = sorted({c for c in children if children.count(c) > 1})
        raise UrdfError(f"links with more than one parent joint: {dup}")
    for issue in mimic_issues([
            MimicSpec(key=j.name, name=j.name, movable=j.movable,
                      target=j.mimic.joint if j.mimic else None,
                      multiplier=j.mimic.multiplier if j.mimic else 1.0)
            for j in joints.values()]):
        raise UrdfError(_mimic_message(issue, joints))
    roots = [n for n in links if n not in children]
    if len(roots) != 1:
        raise UrdfError(f"expected exactly one root link, found {roots}")
    robot = Robot(name=root_el.get("name", "robot"), links=links, joints=joints, root=roots[0])
    if len(robot.link_order()) != len(links):
        raise UrdfError("joint graph contains a cycle or unreachable links")
    return robot


# ------------------------------------------------------------------ FK
def resolve_q(robot: Robot, q: dict[str, float] | None = None) -> dict[str, float]:
    """``q`` with every mimicking joint's value computed from the joint it follows."""
    out = {k: float(v) for k, v in (q or {}).items()}
    for j in robot.joints.values():
        if j.mimic is None:
            continue
        cur, chain = j, []
        while cur.mimic is not None:
            chain.append(cur)
            cur = robot.joints[cur.mimic.joint]
        v = float(out.get(cur.name, 0.0))
        for link_j in reversed(chain):
            assert link_j.mimic is not None
            v = link_j.mimic.multiplier * v + link_j.mimic.offset
        out[j.name] = v
    return out


def fk(robot: Robot, q: dict[str, float] | None = None) -> dict[str, np.ndarray]:
    """World transform of every link frame for joint values ``q`` (missing → 0)."""
    q = q or {}
    unknown = set(q) - set(robot.joints)
    if unknown:
        raise UrdfError(f"fk: unknown joints {sorted(unknown)}; known: {sorted(robot.joints)}")
    q = resolve_q(robot, q)
    out: dict[str, np.ndarray] = {robot.root: np.eye(4)}
    for name in robot.link_order():
        if name == robot.root:
            continue
        j = robot.parent_joint(name)
        assert j is not None
        out[name] = out[j.parent] @ j.origin @ j.motion(float(q.get(j.name, 0.0)))
    return out


def link_world_meshes(robot: Robot, q: dict[str, float] | None = None) -> dict[str, trimesh.Trimesh]:
    """Posed copies of every link mesh in world coordinates."""
    T = fk(robot, q)
    out = {}
    for name, link in robot.links.items():
        if link.mesh is None:
            continue
        m = link.mesh.copy()
        m.apply_transform(T[name])
        out[name] = m
    return out