Add self-collision checking with pinokin CollisionChecker

.github/workflows/tests.yml

@@ -36,29 +36,74 @@ jobs:
           fi
       - name: Run pre-commit
         uses: pre-commit/action@v3.0.1
+
   test:
     name: ${{ matrix.os }} / Python ${{ matrix.python-version }}
     runs-on: ${{ matrix.os }}
-    timeout-minutes: 30
+    timeout-minutes: 45
     strategy:
       fail-fast: false
       matrix:
         os: [ubuntu-latest, windows-latest, macos-latest]
         python-version: ['3.11', '3.12', '3.13', '3.14']
 
+    defaults:
+      run:
+        shell: bash -l {0}
+
     steps:
       - name: Checkout repository (with submodules)
         uses: actions/checkout@v4
 
-      - name: Setup Python
+      - name: Detect matching pinokin branch
+        id: pinokin
+        shell: bash
+        run: |
+          BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}"
+          if git ls-remote --heads https://github.com/Jepson2k/pinokin.git "$BRANCH" 2>/dev/null | grep -q .; then
+            echo "matching=true" >> "$GITHUB_OUTPUT"
+            echo "branch=$BRANCH" >> "$GITHUB_OUTPUT"
+          else
+            echo "matching=false" >> "$GITHUB_OUTPUT"
+          fi
+
+      # ----------------------------------------------------------------------
+      # Path A: matching pinokin branch -> conda env (provides libpinocchio +
+      # libcoal headers/libs) so pinokin can be built from source. We install
+      # PAROL6 first (which pulls in the pinned pinokin v0.1.6 wheel) and
+      # then override pinokin with the source-built wheel from the matching
+      # branch. This is the only way to exercise unreleased pinokin features
+      # before a release lands.
+      # ----------------------------------------------------------------------
+      - name: Setup Miniforge (matching pinokin branch)
+        if: steps.pinokin.outputs.matching == 'true'
+        uses: conda-incubator/setup-miniconda@v3
+        with:
+          miniforge-version: latest
+          python-version: ${{ matrix.python-version }}
+          conda-remove-defaults: true
+          activate-environment: parol6-test
+
+      - name: Clone matching pinokin branch
+        if: steps.pinokin.outputs.matching == 'true'
+        run: |
+          git clone --depth=1 --branch="${{ steps.pinokin.outputs.branch }}" https://github.com/Jepson2k/pinokin.git pinokin-src
+          conda env update -n parol6-test -f pinokin-src/environment.yml
+
+      # ----------------------------------------------------------------------
+      # Path B: no matching pinokin branch -> plain pip with the pinned
+      # release wheel. Anything in PAROL6 that requires a newer pinokin
+      # feature will fail loudly here (intended).
+      # ----------------------------------------------------------------------
+      - name: Setup Python (pinned pinokin)
+        if: steps.pinokin.outputs.matching != 'true'
         uses: actions/setup-python@v5
         with:
           python-version: ${{ matrix.python-version }}
           cache: pip
           cache-dependency-path: pyproject.toml
 
       - name: Install package
-        shell: bash
         run: |
           python -m pip install --upgrade pip
           pip install -e ".[dev]" pytest-timeout
@@ -72,6 +117,16 @@ jobs:
             pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}"
           fi
 
+      # Override the pinned pinokin v0.1.6 wheel with the matching-branch
+      # source build. --force-reinstall + --no-deps swaps just pinokin
+      # without disturbing other resolved dependencies.
+      - name: Override pinokin with matching-branch source build
+        if: steps.pinokin.outputs.matching == 'true'
+        run: |
+          cd pinokin-src
+          pip install . --no-build-isolation --force-reinstall --no-deps
+          python -c "from pinokin import CollisionChecker; print('CollisionChecker available')"
+
       - name: Show environment
         run: |
           python -V

parol6/PAROL6_ROBOT.py

@@ -2,13 +2,14 @@
 
 import atexit
 import logging
+import os
 from dataclasses import dataclass
 from pathlib import Path
 from typing import Final
 
 import numpy as np
 from numpy.typing import NDArray
-from pinokin import Robot
+from pinokin import CollisionChecker, Robot
 
 from parol6.tools import get_tool_transform
 
@@ -56,10 +57,171 @@
 _urdf_path = str(
     Path(__file__).resolve().parent / "urdf_model" / "urdf" / "PAROL6.urdf"
 )
+_mesh_dir = str(Path(_urdf_path).resolve().parent.parent)
 
 # Current robot instance (tool transform applied in-place)
 robot: Robot = Robot(_urdf_path)
 
+# Self-collision checker bound to the same pinokin Robot. Built eagerly when
+# ``parol6.config`` is imported (config.py calls ``_init_collision_checker``),
+# i.e. on any ``import parol6``; stays None when collision checking is disabled
+# or geometry fails to load. Treat None as "checks disabled" everywhere.
+# TODO: defer construction to a server-side ``ensure_collision_checker()`` so
+# pure RobotClient script subprocesses don't pay the URDF-rewrite + BVH build.
+collision: CollisionChecker | None = None
+
+
+def _resolved_urdf_for_collision() -> str:
+    """Return a path to a URDF with `package://parol6/...` rewritten to
+    absolute `file://` paths so pinokin's mesh loader can resolve them.
+
+    The PAROL6 URDF was authored for a ROS package layout (meshes at
+    `parol6/meshes/`) but the Python package places them at
+    `parol6/urdf_model/meshes/`. Rewriting at runtime keeps the source
+    URDF unchanged and avoids fragile symlink farms.
+
+    Writes a fresh temp file each call and cleans it up at interpreter exit.
+    """
+    import tempfile
+
+    src = Path(_urdf_path)
+    text = src.read_text()
+    mesh_root = Path(_mesh_dir) / "meshes"
+    # `package://parol6/meshes/foo.STL` -> a plain absolute path coal/assimp can
+    # open. Use a POSIX-style path, NOT a `file://` URI: coal strips the scheme
+    # naively, which on Windows leaves an invalid `/D:/...` (leading slash before
+    # the drive letter). `as_posix()` gives `/abs/...` on POSIX and `D:/abs/...`
+    # on Windows — both openable directly.
+    rewritten = text.replace("package://parol6/meshes/", mesh_root.as_posix() + "/")
+    fd, tmp_path = tempfile.mkstemp(prefix="parol6_collision_", suffix=".urdf")
+    with os.fdopen(fd, "w") as f:
+        f.write(rewritten)
+
+    @atexit.register
+    def _cleanup_tmp_urdf() -> None:
+        try:
+            os.unlink(tmp_path)
+        except OSError:
+            pass
+
+    return tmp_path
+
+
+def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
+    """Build the singleton CollisionChecker when *enabled*.
+
+    Config values are passed in (by ``parol6.config`` after its knobs are
+    defined) rather than imported here, keeping the dependency one-directional
+    — ``config`` imports ``PAROL6_ROBOT``, not the other way around.
+    """
+    global collision
+    if not enabled:
+        collision = None
+        return
+
+    try:
+        # All package:// mesh URIs are rewritten to absolute file:// paths in
+        # the temp URDF, so no package_dirs resolution is needed.
+        urdf_for_collision = _resolved_urdf_for_collision()
+        c = CollisionChecker(robot, urdf_for_collision)
+        if srdf_path and os.path.exists(srdf_path):
+            c.load_srdf(srdf_path)
+        collision = c
+        logger.info(
+            "Collision checker loaded: %d pairs, %d geometry objects",
+            c.num_collision_pairs,
+            c.num_geometry_objects,
+        )
+    except Exception as e:  # noqa: BLE001
+        # Enabled but failed to build: fail loud. Silently running the arm with
+        # no collision checking is unsafe; require an explicit opt-out.
+        if os.getenv("PAROL6_ALLOW_NO_COLLISION"):
+            logger.warning(
+                "Collision checker init failed; continuing without it because "
+                "PAROL6_ALLOW_NO_COLLISION is set (UNSAFE): %s",
+                e,
+            )
+            collision = None
+            return
+        raise RuntimeError(
+            "Collision checker failed to initialize. Fix the cause, or set "
+            "PAROL6_ALLOW_NO_COLLISION=1 to run without collision checking "
+            f"(UNSAFE). Original error: {e}"
+        ) from e
+
+
+# Geometry-object names for meshes attached to the collision checker on
+# behalf of the currently-active tool, plus the (tool, variant) they were
+# attached for so an unchanged re-apply can skip the disk reload.
+_active_tool_geom_names: list[str] = []
+_active_tool_geom_key: tuple[str, str | None] | None = None
+
+
+def _refresh_collision_tool_geometry(
+    tool_key: str,
+    variant_key: str | None = None,
+) -> None:
+    """Sync the global collision checker's tool geometry with the active
+    tool. No-op if the checker isn't built yet (so this is safe to call
+    during early module init, before the checker is ensured).
+
+    Skips the work entirely when the (tool, variant) is unchanged: collision
+    mesh placement comes only from ``spec.origin``, never the TCP offset, so a
+    TCP-offset-only ``apply_tool`` would otherwise reload STLs and rebuild BVHs
+    on the control-loop thread for no change.
+    """
+    global _active_tool_geom_key
+    if collision is None:
+        return
+    key = (tool_key, variant_key)
+    if key == _active_tool_geom_key:
+        return
+    # Clear the previous tool's geometry. Mark the key inconsistent until the
+    # new attaches finish, so a mid-loop failure self-repairs on the next call
+    # (otherwise the early-return above would skip a partial attach forever).
+    for name in _active_tool_geom_names:
+        collision.remove_geometry_by_name(name)
+    _active_tool_geom_names.clear()
+    _active_tool_geom_key = None
+
+    from parol6.tools import get_registry
+
+    cfg = None if tool_key == "NONE" else get_registry().get(tool_key)
+    if cfg is not None:
+        # A variant with non-empty meshes wholesale replaces cfg.meshes; an
+        # empty variant falls back to cfg.meshes (deliberately — unlike WC's
+        # swap_tool_mesh, which renders nothing for a mesh-less variant).
+        meshes = cfg.meshes
+        if variant_key:
+            for v in cfg.variants:
+                if v.key == variant_key and v.meshes:
+                    meshes = v.meshes
+                    break
+        mesh_root = Path(_mesh_dir) / "meshes"
+        try:
+            for spec in meshes:
+                path = mesh_root / spec.file
+                # All current MeshSpecs use rpy=(0,0,0); rotation is baked into
+                # the STL geometry (see _MESH_RPY comment in tools.py). Add a
+                # rotation branch here when a non-identity rpy appears.
+                T = np.eye(4, dtype=np.float64)
+                T[:3, 3] = spec.origin
+                collision.attach_mesh_to_frame(
+                    spec.file,
+                    str(path),
+                    parent_frame="L6",
+                    placement=T,
+                )
+                _active_tool_geom_names.append(spec.file)
+        except Exception:
+            # Roll back a partial attach so the checker never holds half a tool.
+            for name in _active_tool_geom_names:
+                collision.remove_geometry_by_name(name)
+            _active_tool_geom_names.clear()
+            raise
+
+    _active_tool_geom_key = key
+
 
 def apply_tool(
     tool_name: str,
@@ -95,6 +257,8 @@ def apply_tool(
         robot.clear_tool_transform()
         logger.info(f"Applied tool {label} (identity)")
 
+    _refresh_collision_tool_geometry(tool_name, variant_key=variant_key or None)
+
 
 # Initialize with no tool
 apply_tool("NONE")