diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 828d6e7..5e70330 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -31,33 +31,78 @@ jobs:
pip install -e ".[dev]"
- 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
+ # ---- Common: waldoctl override + PAROL6 install ----
- name: Install cross-repo dependencies
- shell: bash
run: |
python -m pip install --upgrade pip
BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}"
- # waldoctl: try matching branch, fall back to tagged version
if git ls-remote --heads https://github.com/Jepson2k/waldoctl.git "$BRANCH" 2>/dev/null | grep -q .; then
pip install "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}"
fi
@@ -66,6 +111,16 @@ jobs:
run: |
pip install -e ".[dev]" pytest-timeout
+ # 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
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 28b1ede..6070d97 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/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,131 @@
_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. Lazy: only
+# constructed when collision checking is enabled and geometry loads
+# successfully. Treat None as "checks disabled" everywhere.
+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.
+
+ The rewritten file is created in the system temp dir on first call and
+ cleaned up at interpreter exit.
+ """
+ import tempfile
+
+ src = Path(_urdf_path)
+ text = src.read_text()
+ mesh_root = Path(_urdf_path).resolve().parent.parent / "meshes"
+ # `package://parol6/meshes/foo.STL` -> `file:///abs/path/to/foo.STL`
+ rewritten = text.replace("package://parol6/meshes/", f"file://{mesh_root}/")
+ 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() -> None:
+ """Build the singleton CollisionChecker if enabled in config."""
+ global collision
+ # Late import so importing this module never crashes on a circular import
+ # with parol6.config.
+ from parol6.config import (
+ COLLISION_CHECK_ENABLED,
+ COLLISION_SRDF_PATH,
+ )
+
+ if not COLLISION_CHECK_ENABLED:
+ collision = None
+ return
+
+ try:
+ urdf_for_collision = _resolved_urdf_for_collision()
+ c = CollisionChecker(robot, urdf_for_collision, package_dirs=[_mesh_dir])
+ if COLLISION_SRDF_PATH and os.path.exists(COLLISION_SRDF_PATH):
+ c.load_srdf(COLLISION_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
+ logger.warning(
+ "Failed to initialize collision checker (continuing without): %s", e
+ )
+ collision = None
+
+
+# Geometry-object names for meshes attached to the collision checker on
+# behalf of the currently-active tool. Mutated by
+# `_refresh_collision_tool_geometry` on every `apply_tool` call.
+_active_tool_geom_names: list[str] = []
+
+
+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 `_init_collision_checker` runs)."""
+ if collision is None:
+ return
+ for name in _active_tool_geom_names:
+ collision.remove_geometry_by_name(name)
+ _active_tool_geom_names.clear()
+ if tool_key == "NONE":
+ return
+ from parol6.tools import get_registry
+
+ cfg = get_registry().get(tool_key)
+ if cfg is None:
+ return
+ # ToolVariant.meshes (when non-empty) wholesale replaces cfg.meshes;
+ # mirrors WC's swap_tool_mesh semantics in urdf_scene.py.
+ 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"
+ 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)
+
def apply_tool(
tool_name: str,
@@ -95,6 +217,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")
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
new file mode 100644
index 0000000..6d91591
--- /dev/null
+++ b/parol6/commands/_collision_guard.py
@@ -0,0 +1,96 @@
+"""Shared self-collision pre-flight check used by motion commands.
+
+`guard_joint_path(positions)` raises `MotionError(SYS_SELF_COLLISION)` if any
+sampled configuration along the interpolated joint path would self-collide
+(or world-collide given runtime obstacles attached to the singleton checker).
+Disabled-by-config or unloaded-checker scenarios are no-ops.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from numpy.typing import NDArray
+
+import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+from parol6.config import COLLISION_PATH_SAMPLES
+from parol6.utils.error_catalog import make_error
+from parol6.utils.error_codes import ErrorCode
+from parol6.utils.errors import MotionError
+
+
+def _format_pairs(pairs: list[tuple[str, str]]) -> str:
+ """Render colliding (name, name) pairs as a human-readable string.
+
+ Caps at 4 to keep error messages tractable when many pairs collide
+ simultaneously (rare in practice; usually the first one is the
+ actionable one anyway).
+ """
+ if not pairs:
+ return "?"
+ head = pairs[:4]
+ rendered = ", ".join(f"{a} vs {b}" for a, b in head)
+ if len(pairs) > 4:
+ rendered += f" (+{len(pairs) - 4} more)"
+ return rendered
+
+
+def guard_config(q: NDArray[np.float64]) -> None:
+ """Raise MotionError if q is in collision. No-op if checker disabled."""
+ checker = PAROL6_ROBOT.collision
+ if checker is None:
+ return
+ q_arr = np.ascontiguousarray(q, dtype=np.float64)
+ if checker.in_collision(q_arr):
+ pairs = checker.colliding_pairs(q_arr)
+ raise MotionError(
+ make_error(
+ ErrorCode.SYS_SELF_COLLISION,
+ sample="target",
+ total="1",
+ pairs=_format_pairs(pairs),
+ )
+ )
+
+
+def guard_joint_path(positions: NDArray[np.float64]) -> None:
+ """Raise MotionError if any sample in the path is in collision.
+
+ `positions` is (N, nq) joint positions in radians. Endpoints are always
+ included; up to COLLISION_PATH_SAMPLES interior samples are checked.
+ """
+ checker = PAROL6_ROBOT.collision
+ if checker is None:
+ return
+
+ pos = np.ascontiguousarray(positions, dtype=np.float64)
+ n = pos.shape[0]
+ if n == 0:
+ return
+
+ # Subsample at COLLISION_PATH_SAMPLES interior points, always including
+ # first and last rows.
+ target = max(2, COLLISION_PATH_SAMPLES + 2)
+ if n <= target:
+ idx = np.arange(n)
+ else:
+ idx = np.unique(
+ np.concatenate(
+ [
+ np.linspace(0, n - 1, target).round().astype(int),
+ np.array([0, n - 1]),
+ ]
+ )
+ )
+ sub = np.ascontiguousarray(pos[idx], dtype=np.float64)
+ hit = checker.check_path(sub)
+ if hit >= 0:
+ sample = int(idx[hit])
+ pairs = checker.colliding_pairs(pos[sample])
+ raise MotionError(
+ make_error(
+ ErrorCode.SYS_SELF_COLLISION,
+ sample=str(sample),
+ total=str(n),
+ pairs=_format_pairs(pairs),
+ )
+ )
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 54f4169..ac95a0a 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -214,6 +214,21 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
return ExecutionStatusCode.COMPLETED
return ExecutionStatusCode.EXECUTING
+ # Self-collision predicted at next streamed config? Mirror the
+ # IK-failure graceful-stop pathway: decelerate via cse.stop() rather
+ # than raising mid-jog. Operator regains control after smoothing.
+ if PAROL6_ROBOT.collision is not None and PAROL6_ROBOT.collision.in_collision(
+ ik_result.q
+ ):
+ if not self._ik_stopping:
+ _ik_warn(
+ logger,
+ "[CARTJOG] self-collision predicted - initiating stop",
+ )
+ cse.stop()
+ self._ik_stopping = True
+ return ExecutionStatusCode.EXECUTING
+
# IK succeeded - if we were stopping, recover by resuming jogging
if self._ik_stopping:
logger.info("[CARTJOG] IK recovered - resuming jog")
@@ -285,6 +300,11 @@ def _precompute_trajectory(self, state: "ControllerState") -> None:
stop_on_failure=stop_on_failure,
)
+ if not joint_path.is_partial:
+ from parol6.commands._collision_guard import guard_joint_path
+
+ guard_joint_path(joint_path.positions)
+
if joint_path.is_partial:
ik_valid = joint_path.valid
assert ik_valid is not None
@@ -331,7 +351,7 @@ def _precompute_trajectory(self, state: "ControllerState") -> None:
)
def _compute_target_pose(self, state: "ControllerState") -> None:
- """Compute target pose — absolute or relative based on rel flag."""
+ """Compute target pose - absolute or relative based on rel flag."""
pose = self.p.pose
if self.p.rel:
@@ -443,6 +463,10 @@ def do_setup_with_blend(
self.do_setup(state)
return 0
+ from parol6.commands._collision_guard import guard_joint_path
+
+ guard_joint_path(joint_path.positions)
+
# Use minimum speed/accel across chain, sum durations when all duration-based
min_speed = self.p.resolved_speed
min_accel = self.p.accel
diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py
index 2894127..fc2d5f9 100644
--- a/parol6/commands/joint_commands.py
+++ b/parol6/commands/joint_commands.py
@@ -86,6 +86,9 @@ def do_setup(self, state: ControllerState) -> None:
current_rad = self._q_rad_buf
joint_path = JointPath.interpolate(current_rad, target_rad, n_samples=50)
+ from parol6.commands._collision_guard import guard_joint_path
+
+ guard_joint_path(joint_path.positions)
builder = TrajectoryBuilder(
joint_path=joint_path,
profile=state.motion_profile,
@@ -194,6 +197,9 @@ def do_setup_with_blend(
return 0
joint_path = JointPath(positions=positions)
+ from parol6.commands._collision_guard import guard_joint_path
+
+ guard_joint_path(joint_path.positions)
# Use minimum speed/accel across chain, sum durations when all duration-based
min_speed = self.p.resolved_speed
diff --git a/parol6/config.py b/parol6/config.py
index 8c3ef16..84a3a17 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -561,6 +561,33 @@ def _build_cart_kinodynamic(
dtype=np.float64,
)
+# -----------------------------------------------------------------------------
+# Self-collision checking (pinokin CollisionChecker)
+# -----------------------------------------------------------------------------
+# Pre-flight collision checks reject motion commands whose interpolated
+# joint path enters a self-colliding (or world-colliding) configuration.
+# Disable via env: PAROL6_COLLISION_CHECK=0
+COLLISION_CHECK_ENABLED: bool = os.getenv(
+ "PAROL6_COLLISION_CHECK", "1"
+).strip().lower() in ("1", "true", "yes", "on")
+
+# Number of interior joint-space samples checked along an interpolated path.
+# Endpoints are always checked. 0 => endpoints only.
+# Starting value 16 ≈ ~17 ms overhead per command at ~1 ms/check; tune after
+# benchmarking with world geometry attached.
+COLLISION_PATH_SAMPLES: int = int(os.getenv("PAROL6_COLLISION_PATH_SAMPLES", "16"))
+
+# Optional SRDF file with disabled-pair info. Defaults to the bundled
+# parol6/urdf_model/srdf/PAROL6.srdf when present.
+_default_srdf = Path(__file__).resolve().parent / "urdf_model" / "srdf" / "PAROL6.srdf"
+COLLISION_SRDF_PATH: str = os.getenv(
+ "PAROL6_COLLISION_SRDF",
+ str(_default_srdf) if _default_srdf.exists() else "",
+)
+
+# Populate PAROL6_ROBOT.collision now that the config knobs are defined.
+PAROL6_ROBOT._init_collision_checker()
+
# -----------------------------------------------------------------------------
# Utility Functions
diff --git a/parol6/robot.py b/parol6/robot.py
index ac444f5..bae7c2c 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -1,4 +1,4 @@
-"""Unified PAROL6 robot — lifecycle, configuration, kinematics, and factories.
+"""Unified PAROL6 robot - lifecycle, configuration, kinematics, and factories.
Inherits from ``waldoctl.Robot`` ABC.
All parol6-specific details (subprocess management, pinokin, IK solver, etc.)
@@ -509,7 +509,7 @@ def _resolve_mesh_dir() -> str:
@dataclass
class Parol6IKResult:
- """IK result — structurally compatible with the web commander's IKResult Protocol."""
+ """IK result - structurally compatible with the web commander's IKResult Protocol."""
q: NDArray[np.float64] # radians
success: bool
@@ -524,7 +524,7 @@ class Parol6IKResult:
class Robot(_RobotABC):
- """Unified PAROL6 robot — inherits from waldoctl.Robot ABC.
+ """Unified PAROL6 robot - inherits from waldoctl.Robot ABC.
Combines identity, configuration, FK/IK kinematics, controller lifecycle,
and client factories. Supports both sync and async context managers::
@@ -682,6 +682,11 @@ def set_active_tool(
*tcp_offset_m*: optional (x, y, z) user offset in meters, composed
on top of the tool's registered transform.
*variant_key*: optional variant whose TCP overrides the tool default.
+
+ Note: this mutates the per-instance pinokin Robot, not the global
+ `PAROL6_ROBOT.collision` scene. Server-side tool changes route
+ through `PAROL6_ROBOT.apply_tool`, which is where collision-mesh
+ attachment is wired.
"""
from parol6.tools import get_tool_transform
@@ -757,6 +762,55 @@ def fk_batch(self, joint_path_rad: NDArray[np.float64]) -> NDArray[np.float64]:
result[i, 5] = rpy[2]
return result
+ def in_collision(self, q_rad: NDArray[np.float64]) -> bool:
+ """Return True iff `q_rad` is in self/world collision. False if disabled."""
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ if PAROL6_ROBOT.collision is None:
+ return False
+ self._load_q_buf(q_rad)
+ return PAROL6_ROBOT.collision.in_collision(self._q_buf)
+
+ def check_trajectory(self, q_path_rad: NDArray[np.float64]) -> int:
+ """Returns first colliding row index in `q_path_rad`, or -1 if clear.
+
+ `q_path_rad` is (N, nq) joint positions in radians.
+ """
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ if PAROL6_ROBOT.collision is None:
+ return -1
+ return PAROL6_ROBOT.collision.check_path(
+ np.ascontiguousarray(q_path_rad, dtype=np.float64)
+ )
+
+ def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[str, str]]:
+ """Return list of (name, name) geometry pairs in collision at `q_rad`.
+
+ Names are URDF link names for arm geometry (e.g. ``"L4_0"``) and
+ the user-supplied name for runtime-attached geometry (e.g.
+ ``"ssg48_body_simplified.stl"`` for the active tool's body mesh).
+ """
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ if PAROL6_ROBOT.collision is None:
+ return []
+ self._load_q_buf(q_rad)
+ return PAROL6_ROBOT.collision.colliding_pairs(self._q_buf)
+
+ def min_distance(self, q_rad: NDArray[np.float64]) -> float:
+ """Return the minimum clearance over all active pairs at `q_rad`.
+
+ Positive => separation; negative => penetration depth.
+ Returns +inf when collision checking is disabled.
+ """
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ if PAROL6_ROBOT.collision is None:
+ return float("inf")
+ self._load_q_buf(q_rad)
+ return PAROL6_ROBOT.collision.min_distance(self._q_buf)
+
def ik_batch(
self,
poses: NDArray[np.float64],
diff --git a/parol6/tools.py b/parol6/tools.py
index a9f81de..af58625 100644
--- a/parol6/tools.py
+++ b/parol6/tools.py
@@ -444,9 +444,13 @@ def _make_tcp_transform(
# ---------------------------------------------------------------------------
_PNEUMATIC_VERTICAL_MESHES = (
- MeshSpec(file="pneumatic_gripper_vertical_body.stl", role=MeshRole.BODY),
- MeshSpec(file="pneumatic_gripper_vertical_right_jaw.stl", role=MeshRole.JAW),
- MeshSpec(file="pneumatic_gripper_vertical_left_jaw.stl", role=MeshRole.JAW),
+ MeshSpec(file="pneumatic_gripper_vertical_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(
+ file="pneumatic_gripper_vertical_right_jaw_simplified.stl", role=MeshRole.JAW
+ ),
+ MeshSpec(
+ file="pneumatic_gripper_vertical_left_jaw_simplified.stl", role=MeshRole.JAW
+ ),
)
_PNEUMATIC_VERTICAL_MOTION = (
LinearMotion(
@@ -460,9 +464,15 @@ def _make_tcp_transform(
)
_PNEUMATIC_HORIZONTAL_MESHES = (
- MeshSpec(file="pneumatic_gripper_horizontal_body.stl", role=MeshRole.BODY),
- MeshSpec(file="pneumatic_gripper_horizontal_right_jaw.stl", role=MeshRole.JAW),
- MeshSpec(file="pneumatic_gripper_horizontal_left_jaw.stl", role=MeshRole.JAW),
+ MeshSpec(
+ file="pneumatic_gripper_horizontal_body_simplified.stl", role=MeshRole.BODY
+ ),
+ MeshSpec(
+ file="pneumatic_gripper_horizontal_right_jaw_simplified.stl", role=MeshRole.JAW
+ ),
+ MeshSpec(
+ file="pneumatic_gripper_horizontal_left_jaw_simplified.stl", role=MeshRole.JAW
+ ),
)
_PNEUMATIC_HORIZONTAL_MOTION = (
LinearMotion(
@@ -517,15 +527,15 @@ def _make_tcp_transform(
)
_SSG48_FINGER_MESHES = (
- MeshSpec(file="ssg48_body.stl", role=MeshRole.BODY),
- MeshSpec(file="ssg48_finger_right.stl", role=MeshRole.JAW),
- MeshSpec(file="ssg48_finger_left.stl", role=MeshRole.JAW),
+ MeshSpec(file="ssg48_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(file="ssg48_finger_right_simplified.stl", role=MeshRole.JAW),
+ MeshSpec(file="ssg48_finger_left_simplified.stl", role=MeshRole.JAW),
)
_SSG48_PINCH_MESHES = (
- MeshSpec(file="ssg48_body.stl", role=MeshRole.BODY),
- MeshSpec(file="ssg48_pinch_right.stl", role=MeshRole.JAW),
- MeshSpec(file="ssg48_pinch_left.stl", role=MeshRole.JAW),
+ MeshSpec(file="ssg48_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(file="ssg48_pinch_right_simplified.stl", role=MeshRole.JAW),
+ MeshSpec(file="ssg48_pinch_left_simplified.stl", role=MeshRole.JAW),
)
register_tool(
@@ -582,21 +592,21 @@ def _make_tcp_transform(
)
_MSG_100_MESHES = (
- MeshSpec(file="msg_ai_100_body.stl", role=MeshRole.BODY),
- MeshSpec(file="msg_ai_100_right_jaw.stl", role=MeshRole.JAW),
- MeshSpec(file="msg_ai_100_left_jaw.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_100_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(file="msg_ai_100_right_jaw_simplified.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_100_left_jaw_simplified.stl", role=MeshRole.JAW),
)
_MSG_150_MESHES = (
- MeshSpec(file="msg_ai_150_body.stl", role=MeshRole.BODY),
- MeshSpec(file="msg_ai_150_right_jaw.stl", role=MeshRole.JAW),
- MeshSpec(file="msg_ai_150_left_jaw.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_150_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(file="msg_ai_150_right_jaw_simplified.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_150_left_jaw_simplified.stl", role=MeshRole.JAW),
)
_MSG_200_MESHES = (
- MeshSpec(file="msg_ai_200_body.stl", role=MeshRole.BODY),
- MeshSpec(file="msg_ai_200_right_jaw.stl", role=MeshRole.JAW),
- MeshSpec(file="msg_ai_200_left_jaw.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_200_body_simplified.stl", role=MeshRole.BODY),
+ MeshSpec(file="msg_ai_200_right_jaw_simplified.stl", role=MeshRole.JAW),
+ MeshSpec(file="msg_ai_200_left_jaw_simplified.stl", role=MeshRole.JAW),
)
register_tool(
@@ -652,7 +662,9 @@ def _make_tcp_transform(
name="Vacuum Gripper",
description="Vacuum gripper (pneumatic valve I/O)",
transform=_make_tcp_transform(z=-0.037),
- meshes=(MeshSpec(file="vacuum_gripper_body.stl", role=MeshRole.BODY),),
+ meshes=(
+ MeshSpec(file="vacuum_gripper_body_simplified.stl", role=MeshRole.BODY),
+ ),
motions=(),
io_port=1,
),
diff --git a/parol6/urdf_model/srdf/PAROL6.srdf b/parol6/urdf_model/srdf/PAROL6.srdf
new file mode 100644
index 0000000..4c2147d
--- /dev/null
+++ b/parol6/urdf_model/srdf/PAROL6.srdf
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/parol6/urdf_model/urdf/PAROL6.urdf b/parol6/urdf_model/urdf/PAROL6.urdf
index ac81a5f..dce672b 100644
--- a/parol6/urdf_model/urdf/PAROL6.urdf
+++ b/parol6/urdf_model/urdf/PAROL6.urdf
@@ -49,7 +49,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/base_link_simplified.stl" />
@@ -89,7 +89,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L1_simplified.stl" />
@@ -147,7 +147,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L2_simplified.stl" />
@@ -205,7 +205,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L3_simplified.stl" />
@@ -263,7 +263,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L4_simplified.stl" />
@@ -321,7 +321,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L5_simplified.stl" />
@@ -379,7 +379,7 @@
rpy="0 0 0" />
+ filename="package://parol6/meshes/L6_simplified.stl" />
diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py
index 94b61dc..7ab38fd 100644
--- a/parol6/utils/error_catalog.py
+++ b/parol6/utils/error_catalog.py
@@ -165,6 +165,12 @@ class _ErrorTemplate:
effect="Profile not changed.",
remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, TRAPEZOID, LINEAR.",
),
+ ErrorCode.SYS_SELF_COLLISION: _ErrorTemplate(
+ title="Self-collision predicted",
+ cause="Planned configuration would self-collide at sample {sample} of {total}: {pairs}",
+ effect="Motion command rejected before dispatch.",
+ remedy="Choose a different target, add intermediate waypoints, or disable via PAROL6_COLLISION_CHECK=0.",
+ ),
}
diff --git a/parol6/utils/error_codes.py b/parol6/utils/error_codes.py
index 5e32b39..6b6eaec 100644
--- a/parol6/utils/error_codes.py
+++ b/parol6/utils/error_codes.py
@@ -38,3 +38,4 @@ class ErrorCode(IntEnum):
SYS_ESTOP_ACTIVE = 51
SYS_PORT_SAVE_FAILED = 52
SYS_PROFILE_INVALID = 53
+ SYS_SELF_COLLISION = 54
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
new file mode 100644
index 0000000..d0f8df7
--- /dev/null
+++ b/tests/unit/test_collision_integration.py
@@ -0,0 +1,213 @@
+"""Unit tests for PAROL6's pinokin collision integration.
+
+Covers:
+- Singleton checker initialization in PAROL6_ROBOT
+- SRDF disabled-pair count
+- Public Robot.in_collision / colliding_pairs / min_distance / check_trajectory
+- guard_joint_path raising MotionError on a colliding sample
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+import parol6.config # noqa: F401 - imports trigger collision-checker init
+from parol6 import Robot
+from parol6.commands._collision_guard import guard_joint_path
+from parol6.utils.error_codes import ErrorCode
+from parol6.utils.errors import MotionError
+
+
+def test_singleton_checker_initialized():
+ assert PAROL6_ROBOT.collision is not None
+ assert PAROL6_ROBOT.collision.num_geometry_objects > 0
+ assert PAROL6_ROBOT.collision.num_collision_pairs > 0
+
+
+def test_srdf_disabled_pairs_reduce_pair_count():
+ # Without SRDF: 7 link geometries -> 21 pairs minus 6 parent/child adjacent = 15.
+ # The bundled SRDF disables 7 more pairs (base<->L4/L5/L6, L1<->L4/L5/L6, L2<->L4).
+ assert PAROL6_ROBOT.collision.num_collision_pairs == 8
+
+
+def test_home_is_clear():
+ q = np.zeros(PAROL6_ROBOT.robot.nq)
+ assert PAROL6_ROBOT.collision.in_collision(q) is False
+
+
+def test_robot_in_collision_method():
+ r = Robot()
+ try:
+ q = np.zeros(6)
+ assert r.in_collision(q) is False
+ finally:
+ del r
+
+
+def test_robot_min_distance_positive_at_home():
+ r = Robot()
+ try:
+ q = np.zeros(6)
+ d = r.min_distance(q)
+ assert d > 0.0 and d != float("inf")
+ finally:
+ del r
+
+
+def test_robot_check_trajectory_clear():
+ r = Robot()
+ try:
+ # Tiny perturbation around home - definitely clear.
+ q_path = np.linspace(np.zeros(6), 0.01 * np.ones(6), 5)
+ assert r.check_trajectory(q_path) == -1
+ finally:
+ del r
+
+
+def test_guard_joint_path_clear_returns_none():
+ positions = np.zeros((10, 6))
+ # No exception means no collision detected.
+ guard_joint_path(positions)
+
+
+def test_guard_joint_path_raises_on_explicit_collision(monkeypatch):
+ """Force a fake collision by patching the singleton checker temporarily."""
+ real = PAROL6_ROBOT.collision
+
+ class FakeChecker:
+ def in_collision(self, q):
+ return True
+
+ def check_path(self, q):
+ return 2
+
+ def colliding_pairs(self, q):
+ return [("ssg48_body_simplified.stl", "L4_0")]
+
+ monkeypatch.setattr(PAROL6_ROBOT, "collision", FakeChecker())
+
+ positions = np.zeros((5, 6))
+ with pytest.raises(MotionError) as exc_info:
+ guard_joint_path(positions)
+ err = exc_info.value.robot_error
+ assert err.code == int(ErrorCode.SYS_SELF_COLLISION)
+ # Cause string should embed the named pair, not raw int indices.
+ assert "ssg48_body_simplified.stl vs L4_0" in err.cause
+
+ monkeypatch.setattr(PAROL6_ROBOT, "collision", real)
+
+
+def test_guard_disabled_when_checker_is_none(monkeypatch):
+ monkeypatch.setattr(PAROL6_ROBOT, "collision", None)
+ positions = np.zeros((5, 6))
+ # No exception, returns None (no-op).
+ assert guard_joint_path(positions) is None
+
+
+def test_no_spurious_self_overlap_at_home_or_joint_limits():
+ """Audit the bundled simplified collision STLs against the SRDF.
+
+ Asserts that the checker reports no colliding pairs at home, at each
+ joint's lower/upper limit (single-axis), at every (low, high) corner
+ of the joint-limit hypercube, and across a handful of seeded random
+ configs. If this fails, the assertion message identifies the named
+ pair — add it to parol6/urdf_model/srdf/PAROL6.srdf and re-run.
+ """
+ import itertools
+
+ lo = PAROL6_ROBOT._joint_limits_radian[:, 0]
+ hi = PAROL6_ROBOT._joint_limits_radian[:, 1]
+ checker = PAROL6_ROBOT.collision
+
+ configs: list[tuple[str, np.ndarray]] = [("home", np.zeros(6))]
+ for j in range(6):
+ q_lo = np.zeros(6)
+ q_lo[j] = lo[j]
+ q_hi = np.zeros(6)
+ q_hi[j] = hi[j]
+ configs.append((f"J{j}=low", q_lo))
+ configs.append((f"J{j}=high", q_hi))
+
+ for bits in itertools.product((0, 1), repeat=6):
+ q = np.where(np.array(bits, dtype=bool), hi, lo)
+ configs.append((f"corner_{''.join(map(str, bits))}", q))
+
+ rng = np.random.default_rng(0xC011)
+ for k in range(20):
+ configs.append((f"rand_{k}", rng.uniform(lo, hi)))
+
+ failures: list[str] = []
+ for label, q in configs:
+ pairs = checker.colliding_pairs(np.ascontiguousarray(q, dtype=np.float64))
+ if pairs:
+ failures.append(f"{label}: {pairs}")
+
+ assert not failures, (
+ "Spurious self-collision in the bundled simplified STLs. Add these "
+ "pairs to parol6/urdf_model/srdf/PAROL6.srdf and re-run:\n"
+ + "\n".join(failures[:10])
+ + (f"\n... ({len(failures) - 10} more)" if len(failures) > 10 else "")
+ )
+
+
+def test_collision_check_speed_diagnostic(capsys):
+ """Time in_collision and colliding_pairs across a sampled workspace.
+
+ Diagnostic only — does not assert a threshold. Prints percentiles so
+ the JogLCommand mid-motion check can be evaluated against the 100 Hz
+ tick budget (10 ms). Decision criterion: if `in_collision` p99 is
+ well under 1000 us, the JogLCommand check is viable; otherwise drop
+ it and rely on the trajectory-build pre-flight guards.
+
+ Run via: pytest tests/unit/test_collision_integration.py::test_collision_check_speed_diagnostic -v -s
+ """
+ import time
+
+ rng = np.random.default_rng(0xC0FFEE)
+ lo = PAROL6_ROBOT._joint_limits_radian[:, 0]
+ hi = PAROL6_ROBOT._joint_limits_radian[:, 1]
+ qs = [np.zeros(6)] + [
+ np.ascontiguousarray(rng.uniform(lo, hi), dtype=np.float64) for _ in range(99)
+ ]
+ c = PAROL6_ROBOT.collision
+
+ # warm-up so first-call cache effects don't dominate
+ for q in qs[:10]:
+ c.in_collision(q)
+ c.colliding_pairs(q)
+
+ t_bool: list[int] = []
+ for q in qs:
+ t0 = time.perf_counter_ns()
+ c.in_collision(q)
+ t_bool.append(time.perf_counter_ns() - t0)
+
+ t_pairs: list[int] = []
+ for q in qs:
+ t0 = time.perf_counter_ns()
+ c.colliding_pairs(q)
+ t_pairs.append(time.perf_counter_ns() - t0)
+
+ def pct(a: list[int], p: float) -> float:
+ return float(np.percentile(a, p)) / 1000.0 # ns -> us
+
+ with capsys.disabled():
+ print(
+ f"\nin_collision us:"
+ f" min={pct(t_bool, 0):.1f}"
+ f" med={pct(t_bool, 50):.1f}"
+ f" p95={pct(t_bool, 95):.1f}"
+ f" p99={pct(t_bool, 99):.1f}"
+ f" max={pct(t_bool, 100):.1f}"
+ )
+ print(
+ f"colliding_pairs us:"
+ f" min={pct(t_pairs, 0):.1f}"
+ f" med={pct(t_pairs, 50):.1f}"
+ f" p95={pct(t_pairs, 95):.1f}"
+ f" p99={pct(t_pairs, 99):.1f}"
+ f" max={pct(t_pairs, 100):.1f}"
+ )
+ print("servo tick budget: 10000 us (100 Hz)")