From bacb44ff65b99a83372841c681aa63e707e3f400 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/16] 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 dd96c7bf8e1e9f0b949e80d939617358c2487493 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/16] 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 8479f8a3954a2e5663ca8052d20dcd93b39960ed 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/16] 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 5177634f27fbf46d8ba606c7fa8614f72ad503d6 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/16] 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 476b9c6..33f1681 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -564,6 +564,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 8ffa7b390173369f3d62a2519314bf25791298c4 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/16] 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 2644187a71c4c28bcc3f99713c6d87708f152b20 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/16] 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 4a4fc16a289302fb6554646d03f069b16323e1ed 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/16] 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 3b0f5f92e808859649c9f97e017fc251d883f5fc 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/16] 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 0d8e6accb15b0a18a5feaa935ea42a476f6036ac 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/16] 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 33f1681..439242e 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -570,10 +570,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 875824b078b41db72ef1d53189ab16c35a1642d3 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/16] 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 6d3a371eba1823b4ad8ff8b66e8f77a46b248508 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/16] 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.",
),
From f4a636f23772ead23d5ac3f67bdfb0715afd876d Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sat, 13 Jun 2026 19:30:47 -0400
Subject: [PATCH 12/16] Collision: address deep-review findings
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Safety coverage:
- JogJ stops before streaming a self-colliding configuration (only fires in the
collision case, so normal jogging is unaffected; an abrupt stop beats driving
the arm into itself — JogJ has no CSE smoother for a graceful stop).
- JogL release-deceleration skips streaming a colliding config and lets the CSE
finish stopping (was sending unchecked IK configs after timer expiry).
Efficiency / cleanup:
- _refresh_collision_tool_geometry skips the STL reload + BVH rebuild when the
(tool, variant) is unchanged — a TCP-offset-only apply_tool no longer reloads
geometry on the control-loop thread (placement depends only on spec.origin).
- guard_joint_path: drop the dead concatenate/endpoint subsample work and the
no-op inner ascontiguousarray (the entry conversion stays — load-bearing);
hoist the four function-level guard imports to module top (cycle-free).
- _init_collision_checker takes (enabled, srdf_path) instead of importing config
back (keeps the dependency one-directional); drop the dead package_dirs arg
(all mesh URIs are rewritten to file://); reuse _mesh_dir.
- guard raises TrajectoryPlanningError (server-side planning convention), not the
client-side MotionError. Delete dead guard_config.
Docs/comments: honest "Lazy" comment (+ TODO to actually defer the build off
import); SRDF final-state phrasing; cost estimate uses the measured ~38us/check;
neutral jog-recovery log; Robot collision-method docstrings note the
process-global checker; speed-diagnostic docstring reflects the shipped check.
Tests: TrajectoryPlanningError assertion; merged the three Robot() collision
tests into one shared-instance test; dropped the no-op try/finally del and the
redundant manual monkeypatch restore.
Co-Authored-By: Claude Opus 4.8 (1M context)
---
parol6/PAROL6_ROBOT.py | 63 +++++++++++++++---------
parol6/commands/_collision_guard.py | 50 ++++++-------------
parol6/commands/basic_commands.py | 15 ++++++
parol6/commands/cartesian_commands.py | 16 +++---
parol6/commands/joint_commands.py | 5 +-
parol6/config.py | 7 +--
parol6/robot.py | 12 ++++-
parol6/urdf_model/srdf/PAROL6.srdf | 7 ++-
tests/unit/test_collision_integration.py | 58 ++++++++--------------
9 files changed, 116 insertions(+), 117 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 6070d97..1bd3584 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -62,9 +62,12 @@
# 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.
+# 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
@@ -77,14 +80,13 @@ def _resolved_urdf_for_collision() -> str:
`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.
+ 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(_urdf_path).resolve().parent.parent / "meshes"
+ mesh_root = Path(_mesh_dir) / "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")
@@ -101,25 +103,25 @@ def _cleanup_tmp_urdf() -> None:
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,
- )
+def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
+ """Build the singleton CollisionChecker when *enabled*.
- if not COLLISION_CHECK_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, package_dirs=[_mesh_dir])
- if COLLISION_SRDF_PATH and os.path.exists(COLLISION_SRDF_PATH):
- c.load_srdf(COLLISION_SRDF_PATH)
+ 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",
@@ -134,9 +136,10 @@ def _init_collision_checker() -> 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.
+# 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(
@@ -145,9 +148,20 @@ def _refresh_collision_tool_geometry(
) -> 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)."""
+ 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
+ _active_tool_geom_key = key
for name in _active_tool_geom_names:
collision.remove_geometry_by_name(name)
_active_tool_geom_names.clear()
@@ -158,8 +172,9 @@ def _refresh_collision_tool_geometry(
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.
+ # 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:
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index 6d91591..ed692a0 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -1,8 +1,10 @@
"""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
+`guard_joint_path(positions)` raises `TrajectoryPlanningError(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).
+It runs server-side during trajectory build, so it raises the planning-time
+error type (like the other do_setup guards), not the client-side `MotionError`.
Disabled-by-config or unloaded-checker scenarios are no-ops.
"""
@@ -15,7 +17,7 @@
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
+from parol6.utils.errors import TrajectoryPlanningError
def _format_pairs(pairs: list[tuple[str, str]]) -> str:
@@ -34,24 +36,6 @@ def _format_pairs(pairs: list[tuple[str, str]]) -> str:
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.
@@ -62,31 +46,27 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
if checker is None:
return
+ # Load-bearing: normalize to a C-contiguous float64 array for the C++ bindings.
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.
+ # Subsample at COLLISION_PATH_SAMPLES interior points. np.linspace includes
+ # both endpoints, and n > target guarantees spacing > 1 so the rounded
+ # indices are strictly increasing; np.unique stays as cheap insurance.
target = max(2, COLLISION_PATH_SAMPLES + 2)
if n <= target:
- idx = np.arange(n)
+ idx = None
+ sub = pos # already contiguous float64 — no copy needed
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)
+ idx = np.unique(np.linspace(0, n - 1, target).round().astype(int))
+ sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array
hit = checker.check_path(sub)
if hit >= 0:
- sample = int(idx[hit])
+ sample = hit if idx is None else int(idx[hit])
pairs = checker.colliding_pairs(pos[sample])
- raise MotionError(
+ raise TrajectoryPlanningError(
make_error(
ErrorCode.SYS_SELF_COLLISION,
sample=str(sample),
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 59d0f52..768d6e9 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -29,6 +29,8 @@
from parol6.config import deg_to_steps
from parol6.server.transports.transport_factory import is_simulation_mode
+import parol6.PAROL6_ROBOT as PAROL6_ROBOT # noqa: N811
+
from .base import (
ExecutionStatusCode,
MotionCommand,
@@ -187,6 +189,19 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
se.set_jog_velocity(self._jog_vel_rad)
pos_rad, _vel, _finished = se.tick()
+
+ # Don't stream a self-colliding configuration. This only fires in the
+ # collision case (in_collision is False during normal jogging), so it
+ # never affects ordinary motion; an abrupt stop is acceptable when the
+ # alternative is driving the arm into itself. (The Cartesian jog uses a
+ # graceful CSE-based stop; JogJ has no equivalent smoother, so it halts.)
+ checker = PAROL6_ROBOT.collision
+ if checker is not None and checker.in_collision(pos_rad):
+ logger.warning("[JOGJ] self-collision predicted - stopping jog")
+ se.active = False
+ self.finish()
+ return ExecutionStatusCode.COMPLETED
+
self._q_rad_buf[:] = pos_rad
rad_to_steps(self._q_rad_buf, self._steps_buf)
self.set_move_position(state, self._steps_buf)
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index ac95a0a..41ab0bc 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -9,6 +9,7 @@
import numpy as np
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+from parol6.commands._collision_guard import guard_joint_path
from parol6.config import (
CART_ANG_JOG_MIN,
CART_LIN_JOG_MIN,
@@ -155,7 +156,11 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
if not finished and self._dot_buf > 1e-8:
ik_result = solve_ik(PAROL6_ROBOT.robot, smoothed_pose, self._q_ik_seed)
if ik_result.success and ik_result.q is not None:
- self._track_and_send(state, ik_result.q)
+ # Don't stream a self-colliding config during the release
+ # deceleration; skip the send and let the CSE finish stopping.
+ checker = PAROL6_ROBOT.collision
+ if checker is None or not checker.in_collision(ik_result.q):
+ self._track_and_send(state, ik_result.q)
return ExecutionStatusCode.EXECUTING
cse.active = False
@@ -229,9 +234,10 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
self._ik_stopping = True
return ExecutionStatusCode.EXECUTING
- # IK succeeded - if we were stopping, recover by resuming jogging
+ # Reachable + collision-free again — if we were stopping (IK failure or
+ # predicted self-collision), recover by resuming jogging.
if self._ik_stopping:
- logger.info("[CARTJOG] IK recovered - resuming jog")
+ logger.info("[CARTJOG] constraint cleared - resuming jog")
steps_to_rad(state.Position_in, self._q_rad_buf)
cse.sync_pose(get_fkine_se3(state))
self._q_commanded[:] = self._q_rad_buf
@@ -301,8 +307,6 @@ 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:
@@ -463,8 +467,6 @@ 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
diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py
index fc2d5f9..17f2cb6 100644
--- a/parol6/commands/joint_commands.py
+++ b/parol6/commands/joint_commands.py
@@ -16,6 +16,7 @@
import numpy as np
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+from parol6.commands._collision_guard import guard_joint_path
from parol6.commands.base import TrajectoryMoveCommandBase
from parol6.config import (
INTERVAL_S,
@@ -86,8 +87,6 @@ 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,
@@ -197,8 +196,6 @@ 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
diff --git a/parol6/config.py b/parol6/config.py
index 439242e..9d3c61f 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -576,8 +576,9 @@ def _build_cart_kinodynamic(
# 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.
+# At ~38 us p99 per check on the bundled simplified meshes (see the speed
+# diagnostic), 16 samples is ~0.7 ms per command; world geometry attached at
+# runtime may raise the per-check cost.
COLLISION_PATH_SAMPLES: int = int(os.getenv("PAROL6_COLLISION_PATH_SAMPLES", "16"))
# Optional SRDF file with disabled-pair info. Defaults to the bundled
@@ -589,7 +590,7 @@ def _build_cart_kinodynamic(
)
# Populate PAROL6_ROBOT.collision now that the config knobs are defined.
-PAROL6_ROBOT._init_collision_checker()
+PAROL6_ROBOT._init_collision_checker(COLLISION_CHECK_ENABLED, COLLISION_SRDF_PATH)
# -----------------------------------------------------------------------------
diff --git a/parol6/robot.py b/parol6/robot.py
index bae7c2c..3236630 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -763,7 +763,13 @@ def fk_batch(self, joint_path_rad: NDArray[np.float64]) -> NDArray[np.float64]:
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."""
+ """Return True iff `q_rad` is in self/world collision. False if disabled.
+
+ Queries the process-global ``PAROL6_ROBOT.collision`` checker (shared by
+ all Robot methods here); its tool geometry reflects ``PAROL6_ROBOT
+ .apply_tool`` calls in this process (server / dry-run), not
+ :meth:`set_active_tool`, which only mutates this instance's pinokin Robot.
+ """
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
if PAROL6_ROBOT.collision is None:
@@ -789,7 +795,9 @@ def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[str, str]]:
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).
+ ``"ssg48_body_simplified.stl"`` for the active tool's body mesh). Tool
+ geometry is present only when ``PAROL6_ROBOT.apply_tool`` attached it in
+ this process (see :meth:`in_collision`).
"""
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
diff --git a/parol6/urdf_model/srdf/PAROL6.srdf b/parol6/urdf_model/srdf/PAROL6.srdf
index 4c2147d..e8026e7 100644
--- a/parol6/urdf_model/srdf/PAROL6.srdf
+++ b/parol6/urdf_model/srdf/PAROL6.srdf
@@ -18,10 +18,9 @@
-
+
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index d0f8df7..106d5be 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -4,7 +4,7 @@
- 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
+- guard_joint_path raising TrajectoryPlanningError on a colliding sample
"""
from __future__ import annotations
@@ -17,7 +17,7 @@
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
+from parol6.utils.errors import TrajectoryPlanningError
def test_singleton_checker_initialized():
@@ -37,33 +37,18 @@ def test_home_is_clear():
assert PAROL6_ROBOT.collision.in_collision(q) is False
-def test_robot_in_collision_method():
+def test_robot_collision_methods_clear_at_home():
+ """Public Robot collision methods report clear at/near home on one instance.
+ (Robot defines no __del__; an unstarted instance holds no subprocess, so the
+ old per-test try/finally del was a no-op.)"""
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
+ q = np.zeros(6)
+ assert r.in_collision(q) is False
+ d = r.min_distance(q)
+ assert d > 0.0 and d != float("inf")
+ # 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
def test_guard_joint_path_clear_returns_none():
@@ -73,8 +58,8 @@ def test_guard_joint_path_clear_returns_none():
def test_guard_joint_path_raises_on_explicit_collision(monkeypatch):
- """Force a fake collision by patching the singleton checker temporarily."""
- real = PAROL6_ROBOT.collision
+ """Force a fake collision by patching the singleton checker temporarily.
+ monkeypatch auto-restores the real checker at teardown."""
class FakeChecker:
def in_collision(self, q):
@@ -89,15 +74,13 @@ def colliding_pairs(self, q):
monkeypatch.setattr(PAROL6_ROBOT, "collision", FakeChecker())
positions = np.zeros((5, 6))
- with pytest.raises(MotionError) as exc_info:
+ with pytest.raises(TrajectoryPlanningError) 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)
@@ -155,11 +138,10 @@ def test_no_spurious_self_overlap_at_home_or_joint_limits():
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.
+ Diagnostic only — does not assert a threshold. The per-tick mid-motion
+ collision check shipped (JogL release-decel gate, JogJ stop); this prints
+ percentiles to confirm its cost stays well within the 100 Hz tick budget
+ (10 ms).
Run via: pytest tests/unit/test_collision_integration.py::test_collision_check_speed_diagnostic -v -s
"""
From fe139e86ce33177316ef7ce0d5dfecad9a4b8844 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sun, 21 Jun 2026 12:36:22 -0400
Subject: [PATCH 13/16] Collision: curved-move guard, escape-from-collision,
fail-loud init, decel + attach fixes
- MoveC/S/P now call guard_joint_path (were unchecked) [S4-2]
- guard_joint_path permits an escaping move when the start is already in
collision (no new pair, no deeper) instead of trapping the arm [S4-1]
- collision-init failure raises unless PAROL6_ALLOW_NO_COLLISION set [S4-4]
- mesh URIs via Path.as_uri() so Windows file:// is valid [S4-6]
- tool geom-key set only after all attaches succeed, with rollback [S4-5]
- CartesianJog stops re-commanding jog velocity while _ik_stopping so
cse.stop() actually decelerates [S4-3]
Co-Authored-By: Claude Opus 4.8
---
parol6/PAROL6_ROBOT.py | 95 +++++++++++++++++----------
parol6/commands/_collision_guard.py | 44 +++++++++++--
parol6/commands/cartesian_commands.py | 17 +++--
parol6/commands/curved_commands.py | 3 +
4 files changed, 113 insertions(+), 46 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 1bd3584..90c4336 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -87,8 +87,10 @@ def _resolved_urdf_for_collision() -> str:
src = Path(_urdf_path)
text = src.read_text()
mesh_root = Path(_mesh_dir) / "meshes"
- # `package://parol6/meshes/foo.STL` -> `file:///abs/path/to/foo.STL`
- rewritten = text.replace("package://parol6/meshes/", f"file://{mesh_root}/")
+ # `package://parol6/meshes/foo.STL` -> `file:///abs/path/to/foo.STL`.
+ # Path.as_uri() yields a valid file URI on every platform (Windows drive
+ # letters / backslashes break a hand-built `file://` + str(path)).
+ rewritten = text.replace("package://parol6/meshes/", mesh_root.as_uri() + "/")
fd, tmp_path = tempfile.mkstemp(prefix="parol6_collision_", suffix=".urdf")
with os.fdopen(fd, "w") as f:
f.write(rewritten)
@@ -129,10 +131,21 @@ def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
c.num_geometry_objects,
)
except Exception as e: # noqa: BLE001
- logger.warning(
- "Failed to initialize collision checker (continuing without): %s", e
- )
- collision = None
+ # 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
@@ -161,41 +174,51 @@ def _refresh_collision_tool_geometry(
key = (tool_key, variant_key)
if key == _active_tool_geom_key:
return
- _active_tool_geom_key = key
+ # 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()
- if tool_key == "NONE":
- return
+ _active_tool_geom_key = None
+
from parol6.tools import get_registry
- cfg = get_registry().get(tool_key)
- if cfg is None:
- return
- # 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"
- 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)
+ 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(
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index ed692a0..08b01d0 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -19,6 +19,11 @@
from parol6.utils.error_codes import ErrorCode
from parol6.utils.errors import TrajectoryPlanningError
+# Penetration-depth tolerance (metres) for the start-in-collision escape check:
+# a move whose min-distance drops by no more than this counts as "not deeper",
+# absorbing numerical jitter in the signed-distance query.
+_ESCAPE_TOL = 1e-4
+
def _format_pairs(pairs: list[tuple[str, str]]) -> str:
"""Render colliding (name, name) pairs as a human-readable string.
@@ -37,10 +42,17 @@ def _format_pairs(pairs: list[tuple[str, str]]) -> str:
def guard_joint_path(positions: NDArray[np.float64]) -> None:
- """Raise MotionError if any sample in the path is in collision.
+ """Raise if the path drives into collision.
`positions` is (N, nq) joint positions in radians. Endpoints are always
included; up to COLLISION_PATH_SAMPLES interior samples are checked.
+
+ Normally any collision along the path is rejected. The exception is when the
+ path *starts* already in collision (checker enabled at a colliding pose, or a
+ tool attach created an overlap): rejecting outright would trap the arm, so an
+ *escaping* move is permitted — one that adds no new colliding pair and goes no
+ deeper than the start. Driving into a new collision, or deeper into the
+ current one, still raises.
"""
checker = PAROL6_ROBOT.collision
if checker is None:
@@ -62,10 +74,8 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
else:
idx = np.unique(np.linspace(0, n - 1, target).round().astype(int))
sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array
- hit = checker.check_path(sub)
- if hit >= 0:
- sample = hit if idx is None else int(idx[hit])
- pairs = checker.colliding_pairs(pos[sample])
+
+ def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
raise TrajectoryPlanningError(
make_error(
ErrorCode.SYS_SELF_COLLISION,
@@ -74,3 +84,27 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
pairs=_format_pairs(pairs),
)
)
+
+ hit = checker.check_path(sub)
+ if hit < 0:
+ return # entire path clear — the common case
+ if hit > 0:
+ # Start is clear but the path drives into a collision — reject it.
+ sample = hit if idx is None else int(idx[hit])
+ _raise(sample, checker.colliding_pairs(pos[sample]))
+
+ # hit == 0: already in collision at the start. Permit an escaping move — one
+ # that introduces no new colliding pair and goes no deeper than the start —
+ # so the arm isn't trapped. (A global min-distance trend alone can't tell an
+ # improving start-collision from a new shallower one, so we check pairs too.)
+ d0 = checker.min_distance(pos[0])
+ start_pairs = set(checker.colliding_pairs(pos[0]))
+ for j in range(1, sub.shape[0]):
+ new_pairs = set(checker.colliding_pairs(sub[j])) - start_pairs
+ deeper = checker.min_distance(sub[j]) < d0 - _ESCAPE_TOL
+ if new_pairs or deeper:
+ sample = j if idx is None else int(idx[j])
+ pairs = (
+ sorted(new_pairs) if new_pairs else checker.colliding_pairs(pos[sample])
+ )
+ _raise(sample, pairs)
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 41ab0bc..54fd3b8 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -187,11 +187,18 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
if self._vel_ratio > 1.0:
velocity /= self._vel_ratio
- # Set target velocity (WRF transforms to body frame, TRF uses body directly)
- if self.p.frame == "WRF":
- cse.set_jog_velocity_1dof_wrf(self._axis_index, velocity, self.is_rotation)
- else:
- cse.set_jog_velocity_1dof(self._axis_index, velocity, self.is_rotation)
+ # Set target velocity (WRF transforms to body frame, TRF uses body
+ # directly). While stopping (IK failure or predicted self-collision)
+ # leave the CSE target at zero so cse.stop()'s deceleration actually
+ # takes effect — re-commanding full velocity every tick would overwrite
+ # it and the arm would never decelerate (nor reach the vel<1e-6 exit).
+ if not self._ik_stopping:
+ if self.p.frame == "WRF":
+ cse.set_jog_velocity_1dof_wrf(
+ self._axis_index, velocity, self.is_rotation
+ )
+ else:
+ cse.set_jog_velocity_1dof(self._axis_index, velocity, self.is_rotation)
smoothed_pose, smoothed_vel, _finished = cse.tick()
diff --git a/parol6/commands/curved_commands.py b/parol6/commands/curved_commands.py
index fd2daf2..ad65814 100644
--- a/parol6/commands/curved_commands.py
+++ b/parol6/commands/curved_commands.py
@@ -11,6 +11,7 @@
import numpy as np
+from parol6.commands._collision_guard import guard_joint_path
from parol6.commands.base import TrajectoryMoveCommandBase
from parol6.config import INTERVAL_S, LIMITS, steps_to_rad
from parol6.motion import CircularMotion, JointPath, SplineMotion, TrajectoryBuilder
@@ -154,6 +155,8 @@ def do_setup(self, state: "ControllerState") -> None:
)
)
+ guard_joint_path(joint_path.positions)
+
builder = TrajectoryBuilder(
joint_path=joint_path,
profile=state.motion_profile,
From e262df2a0bca0a6dceab4b43141e87dbe06766ea Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 22 Jun 2026 21:08:18 -0400
Subject: [PATCH 14/16] deps: cap numpy<2.5 for numba compatibility
NumPy 2.5.0 (released 2026-06) violates numba 0.6x's numpy<2.5 pin, breaking
collection on Python 3.12+ (3.11 resolves an older numpy and passes). Bound
numpy to what numba supports.
Co-Authored-By: Claude Opus 4.8
---
pyproject.toml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/pyproject.toml b/pyproject.toml
index a0ffeff..e281677 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -41,7 +41,7 @@ dependencies = [
"ruckig>=0.12.2",
"toppra>=0.6.3",
"interpolatepy>=2.0.0",
- "numpy>=2.0",
+ "numpy>=2.0,<2.5", # numba (0.6x) requires numpy<2.5
"numba>=0.59",
"psutil>=5.9",
"msgspec>=0.18",
From 27bc80a34df54582f10ffbfb8b6abc149261b9cd Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 23 Jun 2026 12:40:20 -0400
Subject: [PATCH 15/16] collision: fix Windows mesh path + ty unresolved-import
on pinokin
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- Windows: rewrite package:// mesh refs to a plain POSIX path, not a file://
URI. coal strips the scheme naively, leaving an invalid '/D:/...' (leading
slash before the drive letter) so assimp couldn't open the meshes. as_posix()
yields '/abs/...' on POSIX and 'D:/abs/...' on Windows — both openable.
- Lint: scope a ty unresolved-import override to PAROL6_ROBOT.py; the lint env's
pinokin build can predate CollisionChecker (resolves fine at runtime).
Co-Authored-By: Claude Opus 4.8
---
parol6/PAROL6_ROBOT.py | 10 ++++++----
pyproject.toml | 8 ++++++++
2 files changed, 14 insertions(+), 4 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 90c4336..1434c6c 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -87,10 +87,12 @@ def _resolved_urdf_for_collision() -> str:
src = Path(_urdf_path)
text = src.read_text()
mesh_root = Path(_mesh_dir) / "meshes"
- # `package://parol6/meshes/foo.STL` -> `file:///abs/path/to/foo.STL`.
- # Path.as_uri() yields a valid file URI on every platform (Windows drive
- # letters / backslashes break a hand-built `file://` + str(path)).
- rewritten = text.replace("package://parol6/meshes/", mesh_root.as_uri() + "/")
+ # `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)
diff --git a/pyproject.toml b/pyproject.toml
index e281677..d7df522 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -112,6 +112,14 @@ include = [
[tool.ty.overrides.rules]
invalid-type-form = "ignore"
+# pinokin is a compiled extension; the lint env's build can predate
+# CollisionChecker, so ty can't resolve that symbol (it resolves fine at runtime
+# and against a matching pinokin build).
+[[tool.ty.overrides]]
+include = ["parol6/PAROL6_ROBOT.py"]
+[tool.ty.overrides.rules]
+unresolved-import = "ignore"
+
[tool.setuptools]
include-package-data = true
From b9a02c1994d972b982c7568a546bdbe67f61e397 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 24 Jun 2026 15:42:17 -0400
Subject: [PATCH 16/16] ci: drop shell:bash so Install runs in the conda env
The matching-pinokin path builds into a conda env; the job defaults to the
conda-aware login shell (bash -l). An explicit shell:bash on Install package
made pip install into base python while pytest ran in the conda env -> numba
ModuleNotFoundError. Use the job-default login shell instead.
Co-Authored-By: Claude Opus 4.8
---
.github/workflows/tests.yml | 1 -
1 file changed, 1 deletion(-)
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index e40d529..2e3b064 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -104,7 +104,6 @@ jobs:
cache-dependency-path: pyproject.toml
- name: Install package
- shell: bash
run: |
python -m pip install --upgrade pip
pip install -e ".[dev]" pytest-timeout