From a7e782d375c59cc571a80becbdcd2d63d7c8e664 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 20:11:57 -0400
Subject: [PATCH 01/11] Wire pinokin CollisionChecker into motion-command
pre-flight checks
Pre-flight self/world collision validation is applied before any motion
command leaves the host. Failure raises MotionError(SYS_SELF_COLLISION)
for offline planners (MoveJ, MoveJPose, MoveL, blended chains) and
mirrors the IK-failure graceful-stop pathway for streaming JogL.
Changes
- parol6/PAROL6_ROBOT.py: module-level collision: CollisionChecker | None
singleton alongside the existing robot singleton. Lazy init via
_init_collision_checker() called at the end of parol6/config.py once
the COLLISION_* knobs exist. Geometry-load failures degrade to a
warning + None so existing scripts keep running.
- parol6/PAROL6_ROBOT.py: _resolved_urdf_for_collision() rewrites
package://parol6/meshes/... URIs to absolute file:// paths in a
temp URDF. The bundled URDF was authored for a ROS package layout
(meshes at parol6/meshes/) but the Python package places them at
parol6/urdf_model/meshes/; rewriting keeps the source URDF untouched.
- parol6/config.py: COLLISION_CHECK_ENABLED, COLLISION_PATH_SAMPLES
(default 16, needs benchmarking once scene/world geometry is added),
COLLISION_SRDF_PATH (defaults to bundled PAROL6.srdf).
- parol6/urdf_model/srdf/PAROL6.srdf: disables 5 non-adjacent pairs
(base<->L4/L5/L6, L1<->L5/L6) that are physically unreachable.
Reduces enabled pair count from 15 to 10.
- parol6/utils/error_codes.py + error_catalog.py: SYS_SELF_COLLISION
(code 54) with title/cause/effect/remedy template.
- parol6/commands/_collision_guard.py: shared guard_joint_path helper
subsamples per COLLISION_PATH_SAMPLES, calls check_path, raises on
first hit. guard_config for single-point checks.
- parol6/commands/joint_commands.py: guard MoveJ/MoveJPose in
do_setup after JointPath.interpolate and do_setup_with_blend after
build_composite_joint_path.
- parol6/commands/cartesian_commands.py: guard MoveL in
_precompute_trajectory after JointPath.from_poses and in
do_setup_with_blend. For JogL, check ik_result.q per tick before
_track_and_send; on collision predict, call cse.stop() and set
_ik_stopping=True exactly like IK failure does (graceful
deceleration; no exception mid-jog).
- parol6/robot.py: public Robot.in_collision, check_trajectory,
colliding_pairs, min_distance methods delegate to the singleton.
All return safe defaults when checker is None.
- tests/unit/test_collision_integration.py: covers singleton init,
SRDF effect on pair count, home-is-clear, the four public Robot
methods, and guard raise/no-op behavior.
Opt-out via PAROL6_COLLISION_CHECK=0. Failure-to-init logs a warning
and disables checking; existing motion behavior is unchanged.
https://claude.ai/code/session_01TiEkni9M9ZJC88LmvJwUyf
---
parol6/PAROL6_ROBOT.py | 76 ++++++++++++++++++++++++++-
parol6/commands/_collision_guard.py | 80 +++++++++++++++++++++++++++++
parol6/urdf_model/srdf/PAROL6.srdf | 26 ++++++++++
parol6/utils/error_codes.py | 1 +
4 files changed, 182 insertions(+), 1 deletion(-)
create mode 100644 parol6/commands/_collision_guard.py
create mode 100644 parol6/urdf_model/srdf/PAROL6.srdf
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 28b1ede..4e47033 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,83 @@
_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
+
def apply_tool(
tool_name: str,
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
new file mode 100644
index 0000000..9ed8feb
--- /dev/null
+++ b/parol6/commands/_collision_guard.py
@@ -0,0 +1,80 @@
+"""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 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=str(pairs[:4]),
+ )
+ )
+
+
+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=str(pairs[:4]),
+ )
+ )
diff --git a/parol6/urdf_model/srdf/PAROL6.srdf b/parol6/urdf_model/srdf/PAROL6.srdf
new file mode 100644
index 0000000..f586e15
--- /dev/null
+++ b/parol6/urdf_model/srdf/PAROL6.srdf
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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
From d605ced8efbc4976a5e9799cce116b879eb6cd5a Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 20:13:13 -0400
Subject: [PATCH 02/11] Add SYS_SELF_COLLISION catalog template +
joint_commands guard wiring + collision integration tests
---
parol6/commands/joint_commands.py | 4 +
parol6/utils/error_catalog.py | 6 ++
tests/unit/test_collision_integration.py | 103 +++++++++++++++++++++++
3 files changed, 113 insertions(+)
create mode 100644 tests/unit/test_collision_integration.py
diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py
index 2894127..35d229b 100644
--- a/parol6/commands/joint_commands.py
+++ b/parol6/commands/joint_commands.py
@@ -86,6 +86,8 @@ 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 +196,8 @@ 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/utils/error_catalog.py b/parol6/utils/error_catalog.py
index 94b61dc..3834675 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}/{total}: pairs={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/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
new file mode 100644
index 0000000..cab40cf
--- /dev/null
+++ b/tests/unit/test_collision_integration.py
@@ -0,0 +1,103 @@
+"""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 5 more pairs (base↔L4/L5/L6 + L1↔L5/L6).
+ assert PAROL6_ROBOT.collision.num_collision_pairs == 10
+
+
+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 [(0, 5)]
+
+ monkeypatch.setattr(PAROL6_ROBOT, "collision", FakeChecker())
+
+ positions = np.zeros((5, 6))
+ with pytest.raises(MotionError) as exc_info:
+ guard_joint_path(positions)
+ assert exc_info.value.robot_error.code == int(ErrorCode.SYS_SELF_COLLISION)
+
+ 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
From 13a372427c5e9b7b096e68b9f816305b513beb04 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 20:14:12 -0400
Subject: [PATCH 03/11] Wire collision guards into cartesian_commands + config
+ Robot public methods
---
parol6/commands/cartesian_commands.py | 23 +++++++++++++++++++++++
1 file changed, 23 insertions(+)
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 54f4169..dd7d6f9 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -214,6 +214,22 @@ 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 +301,10 @@ 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
@@ -443,6 +463,9 @@ 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
From 54f5e16b84b136cca3944700a4f381ec53668ca9 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 20:17:00 -0400
Subject: [PATCH 04/11] Add COLLISION_* config knobs + Robot public collision
methods
---
parol6/config.py | 28 ++++++++++++++++++++++++++++
parol6/robot.py | 42 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 70 insertions(+)
diff --git a/parol6/config.py b/parol6/config.py
index 8c3ef16..918452d 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -561,6 +561,34 @@ 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..f67b281 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -757,6 +757,48 @@ 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[int, int]]:
+ """Return list of (i, j) geometry pairs in collision at `q_rad`."""
+ 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],
From 90807f86dec04f66c9f25892994f58da0992c2a2 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 22:16:15 -0400
Subject: [PATCH 05/11] Skip collision integration tests when CollisionChecker
is unavailable
Released pinokin (v0.1.6) doesn't ship CollisionChecker, so the singleton
stays None on those installs. Mark the integration assertions skip-on-None
rather than fail; the universal guard tests still exercise the PAROL6
wiring against a fake checker.
---
tests/unit/test_collision_integration.py | 18 +++++++++++++++---
1 file changed, 15 insertions(+), 3 deletions(-)
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index cab40cf..39c9dd6 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -19,19 +19,30 @@
from parol6.utils.error_codes import ErrorCode
from parol6.utils.errors import MotionError
+# Released pinokin v0.1.6 lacks CollisionChecker; everything end-to-end on it
+# degrades to None. Skip the integration tests in that case rather than fail
+# them - the unit-level guard tests below still exercise the PAROL6 wiring.
+needs_collision = pytest.mark.skipif(
+ PAROL6_ROBOT.CollisionChecker is None,
+ reason="installed pinokin lacks CollisionChecker (need >= 0.1.7)",
+)
+
+@needs_collision
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
+@needs_collision
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 5 more pairs (base↔L4/L5/L6 + L1↔L5/L6).
+ # Without SRDF: 7 link geometries -> 21 pairs minus 6 parent/child adjacent = 15.
+ # The bundled SRDF disables 5 more pairs (base<->L4/L5/L6 + L1<->L5/L6).
assert PAROL6_ROBOT.collision.num_collision_pairs == 10
+@needs_collision
def test_home_is_clear():
q = np.zeros(PAROL6_ROBOT.robot.nq)
assert PAROL6_ROBOT.collision.in_collision(q) is False
@@ -46,6 +57,7 @@ def test_robot_in_collision_method():
del r
+@needs_collision
def test_robot_min_distance_positive_at_home():
r = Robot()
try:
@@ -59,7 +71,7 @@ def test_robot_min_distance_positive_at_home():
def test_robot_check_trajectory_clear():
r = Robot()
try:
- # Tiny perturbation around home — definitely clear.
+ # 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:
From a376a6a940ad8b2f888999353fc3175abfbb49ae Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 22:23:25 -0400
Subject: [PATCH 06/11] Build matching-branch pinokin from source in CI (no
graceful degradation)
CI's test matrix detects when a pinokin branch with the same name as the
PAROL6 PR branch exists. If so, conda is set up with libpinocchio +
libcoal so pinokin can be built from source from that branch, then
overrides the pinned v0.1.6 wheel via --force-reinstall --no-deps. This
exercises the actual cross-repo collision integration before pinokin
v0.1.7 ships.
If no matching branch exists, the existing plain-pip flow runs and any
PAROL6 code that requires unreleased pinokin features fails loudly
(intended - that's the contract).
Reverts the prior graceful-degradation experiment in
test_collision_integration.py: the integration tests now hard-assert on
collision being available rather than silently skipping when it isn't.
---
.github/workflows/tests.yml | 63 ++++++++++++++++++++++--
tests/unit/test_collision_integration.py | 12 -----
2 files changed, 59 insertions(+), 16 deletions(-)
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/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index 39c9dd6..a7b6855 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -19,30 +19,19 @@
from parol6.utils.error_codes import ErrorCode
from parol6.utils.errors import MotionError
-# Released pinokin v0.1.6 lacks CollisionChecker; everything end-to-end on it
-# degrades to None. Skip the integration tests in that case rather than fail
-# them - the unit-level guard tests below still exercise the PAROL6 wiring.
-needs_collision = pytest.mark.skipif(
- PAROL6_ROBOT.CollisionChecker is None,
- reason="installed pinokin lacks CollisionChecker (need >= 0.1.7)",
-)
-
-@needs_collision
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
-@needs_collision
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 5 more pairs (base<->L4/L5/L6 + L1<->L5/L6).
assert PAROL6_ROBOT.collision.num_collision_pairs == 10
-@needs_collision
def test_home_is_clear():
q = np.zeros(PAROL6_ROBOT.robot.nq)
assert PAROL6_ROBOT.collision.in_collision(q) is False
@@ -57,7 +46,6 @@ def test_robot_in_collision_method():
del r
-@needs_collision
def test_robot_min_distance_positive_at_home():
r = Robot()
try:
From e0bfb7301c6188b1ed6148d72c4fbf2883263523 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 22:26:20 -0400
Subject: [PATCH 07/11] Apply ruff-format to PAROL6_ROBOT.py +
joint_commands.py
---
parol6/PAROL6_ROBOT.py | 4 +---
parol6/commands/joint_commands.py | 2 ++
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 4e47033..4a96374 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -86,9 +86,7 @@ def _resolved_urdf_for_collision() -> str:
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}/"
- )
+ 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)
diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py
index 35d229b..fc2d5f9 100644
--- a/parol6/commands/joint_commands.py
+++ b/parol6/commands/joint_commands.py
@@ -87,6 +87,7 @@ def do_setup(self, state: ControllerState) -> None:
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,
@@ -197,6 +198,7 @@ def do_setup_with_blend(
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
From 71fe0997ea9c92ffe9c8e5434b6508b337fe80ae Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 22:27:52 -0400
Subject: [PATCH 08/11] Apply ruff-format to cartesian_commands.py
---
parol6/commands/cartesian_commands.py | 9 +++++----
1 file changed, 5 insertions(+), 4 deletions(-)
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index dd7d6f9..ac95a0a 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -217,9 +217,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
# 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 PAROL6_ROBOT.collision is not None and PAROL6_ROBOT.collision.in_collision(
+ ik_result.q
):
if not self._ik_stopping:
_ik_warn(
@@ -303,6 +302,7 @@ def _precompute_trajectory(self, state: "ControllerState") -> None:
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:
@@ -351,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:
@@ -464,6 +464,7 @@ def do_setup_with_blend(
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
From 7d5a3ad3dfaab7735d6d9d159d456379132e2266 Mon Sep 17 00:00:00 2001
From: Jake Jepson <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 11 May 2026 22:32:03 -0400
Subject: [PATCH 09/11] Apply ruff-format to config.py + robot.py
---
parol6/config.py | 7 +++----
parol6/robot.py | 14 ++++++++------
2 files changed, 11 insertions(+), 10 deletions(-)
diff --git a/parol6/config.py b/parol6/config.py
index 918452d..84a3a17 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -567,10 +567,9 @@ def _build_cart_kinodynamic(
# 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")
-)
+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.
diff --git a/parol6/robot.py b/parol6/robot.py
index f67b281..82e80cd 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::
@@ -760,6 +760,7 @@ def fk_batch(self, joint_path_rad: NDArray[np.float64]) -> NDArray[np.float64]:
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)
@@ -771,17 +772,17 @@ def check_trajectory(self, q_path_rad: NDArray[np.float64]) -> int:
`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[int, int]]:
+ def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[int, int]]:
"""Return list of (i, j) geometry pairs in collision at `q_rad`."""
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
if PAROL6_ROBOT.collision is None:
return []
self._load_q_buf(q_rad)
@@ -794,6 +795,7 @@ def min_distance(self, q_rad: NDArray[np.float64]) -> float:
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)
From d8a7bb1504e2d8a15f85c917019e321fa24f7228 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 13 May 2026 21:34:54 -0400
Subject: [PATCH 10/11] Collision: simplified meshes, named pairs, tool
auto-attach, SRDF audit
Closes the gaps from the parol6-vision (python-fcl) comparison so the
runtime safety net covers gripper-vs-environment and surfaces readable
diagnostics, while staying within the servo budget.
- URDF + tool registry switched to bundled *_simplified.stl twins
(parol6/urdf_model/urdf/PAROL6.urdf collision blocks; every
MeshSpec.file in parol6/tools.py). Visual rendering is unaffected
(URDF visual blocks untouched; WC's urdf_scene already prefers the
same _simplified meshes via _stl_to_url). Collision queries on
simplified BVH are dramatically faster (p99 ~38 us on Pi, ~0.4% of
a 10 ms servo tick).
- Robot.colliding_pairs and the underlying CollisionChecker call now
return list[tuple[str, str]] of geometry-object names rather than
opaque indices. _collision_guard.py renders pairs as
"ssg48_body_simplified.stl vs L4_0" in the SYS_SELF_COLLISION error
cause, so operators see what hit what without a separate name lookup.
- PAROL6_ROBOT.apply_tool now refreshes the singleton collision
scene with the active tool's BODY/JAW meshes (attached to the L6
frame). Tracks attached names in _active_tool_geom_names so the
next tool change cleans up before the new one is attached. Variant
resolution mirrors WC's swap_tool_mesh semantics. The hook lives
only at apply_tool (NOT Robot.set_active_tool, which mutates a
per-instance pinokin and never touches the global scene - note
added to its docstring).
- SRDF (parol6/urdf_model/srdf/PAROL6.srdf) expanded to the
defensive union of the original inspection set and the measured
simplified-mesh overlap set: base_link <-> L4/L5/L6,
L1 <-> L4/L5/L6, L2 <-> L4.
- New test_no_spurious_self_overlap_at_home_or_joint_limits audits
the bundled simplified STLs across home + every single-axis joint
limit + all 64 corners of the limit hypercube + 20 seeded random
configs. Fails loud with the named pair when anything new slips
through; that is the answer to "which pairs overlap on these
meshes" - measurement, not inspection.
- New test_collision_check_speed_diagnostic prints per-call
percentiles for in_collision and colliding_pairs, so the
trajectory-build pre-flight and the JogLCommand mid-motion check
can be evaluated against the 100 Hz tick budget. Measured here:
p99 ~38 us for both, well inside budget.
Co-Authored-By: Claude Opus 4.7 (1M context)
---
parol6/PAROL6_ROBOT.py | 52 ++++++++++
parol6/commands/_collision_guard.py | 20 +++-
parol6/robot.py | 14 ++-
parol6/tools.py | 56 ++++++-----
parol6/urdf_model/srdf/PAROL6.srdf | 11 ++-
parol6/urdf_model/urdf/PAROL6.urdf | 14 +--
tests/unit/test_collision_integration.py | 118 ++++++++++++++++++++++-
7 files changed, 246 insertions(+), 39 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 4a96374..6070d97 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -133,6 +133,56 @@ def _init_collision_checker() -> None:
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,
variant_key: str = "",
@@ -167,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
index 9ed8feb..6d91591 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -18,6 +18,22 @@
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
@@ -31,7 +47,7 @@ def guard_config(q: NDArray[np.float64]) -> None:
ErrorCode.SYS_SELF_COLLISION,
sample="target",
total="1",
- pairs=str(pairs[:4]),
+ pairs=_format_pairs(pairs),
)
)
@@ -75,6 +91,6 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
ErrorCode.SYS_SELF_COLLISION,
sample=str(sample),
total=str(n),
- pairs=str(pairs[:4]),
+ pairs=_format_pairs(pairs),
)
)
diff --git a/parol6/robot.py b/parol6/robot.py
index 82e80cd..bae7c2c 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -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
@@ -779,8 +784,13 @@ def check_trajectory(self, q_path_rad: NDArray[np.float64]) -> int:
np.ascontiguousarray(q_path_rad, dtype=np.float64)
)
- def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[int, int]]:
- """Return list of (i, j) geometry pairs in collision at `q_rad`."""
+ 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:
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
index f586e15..4c2147d 100644
--- a/parol6/urdf_model/srdf/PAROL6.srdf
+++ b/parol6/urdf_model/srdf/PAROL6.srdf
@@ -18,9 +18,16 @@
-
+
+
+
+
+
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/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index a7b6855..d0f8df7 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -28,8 +28,8 @@ def test_singleton_checker_initialized():
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 5 more pairs (base<->L4/L5/L6 + L1<->L5/L6).
- assert PAROL6_ROBOT.collision.num_collision_pairs == 10
+ # 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():
@@ -84,14 +84,17 @@ def check_path(self, q):
return 2
def colliding_pairs(self, q):
- return [(0, 5)]
+ 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)
- assert exc_info.value.robot_error.code == int(ErrorCode.SYS_SELF_COLLISION)
+ 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)
@@ -101,3 +104,110 @@ def test_guard_disabled_when_checker_is_none(monkeypatch):
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)")
From a64180417299666c70012dcc619e77b0c1e2f0bc Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 13 May 2026 21:45:24 -0400
Subject: [PATCH 11/11] Cleaner SYS_SELF_COLLISION message phrasing
"Planned configuration would self-collide at sample 7/50: pairs=ssg48_body_simplified.stl vs L4_0"
reads awkwardly because "pairs=" is dropped into the middle of the
sentence. Phrase as
"Planned configuration would self-collide at sample 7 of 50: ssg48_body_simplified.stl vs L4_0"
instead. {pairs} is now substituted directly without a key= prefix.
Co-Authored-By: Claude Opus 4.7 (1M context)
---
parol6/utils/error_catalog.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py
index 3834675..7ab38fd 100644
--- a/parol6/utils/error_catalog.py
+++ b/parol6/utils/error_catalog.py
@@ -167,7 +167,7 @@ class _ErrorTemplate:
),
ErrorCode.SYS_SELF_COLLISION: _ErrorTemplate(
title="Self-collision predicted",
- cause="Planned configuration would self-collide at sample {sample}/{total}: pairs={pairs}",
+ 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.",
),