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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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/36] 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
From 968cd5e55c76ea5cc8b3004ac08c706bbd22e71f Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 29 Jun 2026 08:29:21 -0400
Subject: [PATCH 17/36] WC: report self-collision pairs to the frontend via
status
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
When a move is blocked or a continuous jog is stopped by a predicted
self-collision, capture the colliding geometry/link pairs at the predicted
colliding config and surface them on the status broadcast so the frontend
can highlight the offending parts.
- ControllerState gains collision_active/collision_pairs + clear_collision();
reset() clears them.
- Move path: the guard attaches structured pairs to TrajectoryPlanningError;
they ride the pickled ErrorSegment across the planner subprocess and land on
state in the segment player.
- Jog path: JogJ/JogL capture pairs on the (rare) stop branch only — never on
the clean per-tick path, preserving the 100 Hz no-alloc rule. Cleared on a
fresh jog, on JogL recovery, and at command intake.
- Wire: collision_active/collision_pairs appended to the STATUS tuple
(len-guarded decode → backward compatible); cached + dirty-checked.
- Robot gains has_collision_checking to satisfy the new waldoctl ABC flag.
Co-Authored-By: Claude Opus 4.8
---
parol6/commands/_collision_guard.py | 5 ++++-
parol6/commands/basic_commands.py | 4 ++++
parol6/commands/cartesian_commands.py | 7 +++++++
parol6/protocol/wire.py | 25 +++++++++++++++++++++++--
parol6/robot.py | 6 ++++++
parol6/server/controller.py | 4 ++++
parol6/server/motion_planner.py | 2 ++
parol6/server/segment_player.py | 3 +++
parol6/server/state.py | 12 ++++++++++++
parol6/server/status_cache.py | 15 +++++++++++++++
parol6/utils/errors.py | 2 ++
11 files changed, 82 insertions(+), 3 deletions(-)
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index 08b01d0..13b7c7e 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -76,7 +76,7 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array
def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
- raise TrajectoryPlanningError(
+ exc = TrajectoryPlanningError(
make_error(
ErrorCode.SYS_SELF_COLLISION,
sample=str(sample),
@@ -84,6 +84,9 @@ def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
pairs=_format_pairs(pairs),
)
)
+ # Structured channel for the viz, alongside the formatted error string.
+ exc.colliding_pairs = list(pairs)
+ raise exc
hit = checker.check_path(sub)
if hit < 0:
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 768d6e9..2f68daa 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -156,6 +156,7 @@ def do_setup(self, state: "ControllerState") -> None:
)
self.start_timer(self.p.duration)
self._jog_initialized = False
+ state.clear_collision()
def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
"""Execute one tick of joint jogging via StreamingExecutor."""
@@ -198,6 +199,9 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
checker = PAROL6_ROBOT.collision
if checker is not None and checker.in_collision(pos_rad):
logger.warning("[JOGJ] self-collision predicted - stopping jog")
+ # Allocate only here (the rare stop), never on the clean per-tick path.
+ state.collision_pairs = tuple(checker.colliding_pairs(pos_rad))
+ state.collision_active = True
se.active = False
self.finish()
return ExecutionStatusCode.COMPLETED
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 54fd3b8..85616a4 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -115,6 +115,7 @@ def do_setup(self, state: "ControllerState") -> None:
self._axis_sign = 1.0 if vels[max_idx] >= 0 else -1.0
self.start_timer(self.p.duration)
self._ik_stopping = False
+ state.clear_collision()
def _track_and_send(self, state: "ControllerState", ik_q: np.ndarray) -> None:
"""Velocity-clamp IK result, update tracked position, send MOVE."""
@@ -237,6 +238,11 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
logger,
"[CARTJOG] self-collision predicted - initiating stop",
)
+ # Capture once on the stop transition (not every decel tick).
+ state.collision_pairs = tuple(
+ PAROL6_ROBOT.collision.colliding_pairs(ik_result.q)
+ )
+ state.collision_active = True
cse.stop()
self._ik_stopping = True
return ExecutionStatusCode.EXECUTING
@@ -245,6 +251,7 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
# predicted self-collision), recover by resuming jogging.
if self._ik_stopping:
logger.info("[CARTJOG] constraint cleared - resuming jog")
+ state.clear_collision()
steps_to_rad(state.Position_in, self._q_rad_buf)
cse.sync_pose(get_fkine_se3(state))
self._q_commanded[:] = self._q_rad_buf
diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py
index fba49c8..a836224 100644
--- a/parol6/protocol/wire.py
+++ b/parol6/protocol/wire.py
@@ -9,7 +9,7 @@
Wire format uses msgpack arrays with integer type codes:
- OK: MsgType.OK (just the integer)
- ERROR: [MsgType.ERROR, message]
-- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active]
+- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs]
- RESPONSE: [MsgType.RESPONSE, query_type, value]
- COMMAND: [CmdType.XXX, ...params]
"""
@@ -1279,6 +1279,8 @@ def pack_status(
tool_status: ToolStatus | None = None,
tcp_speed: float = 0.0,
simulator_active: bool = False,
+ collision_active: bool = False,
+ collision_pairs: tuple[tuple[str, str], ...] = (),
) -> bytes:
"""Pack a status broadcast message.
@@ -1318,6 +1320,8 @@ def pack_status(
else None,
tcp_speed,
simulator_active,
+ collision_active,
+ collision_pairs,
),
option=ormsgpack.OPT_SERIALIZE_NUMPY,
)
@@ -1355,6 +1359,8 @@ class StatusBuffer:
queued_duration: float = 0.0
tcp_speed: float = 0.0
simulator_active: bool = False
+ collision_active: bool = False
+ collision_pairs: list[tuple[str, str]] = field(default_factory=list)
def __post_init__(self) -> None:
self._cart_en_dict: dict[str, np.ndarray] = {
@@ -1398,6 +1404,8 @@ def copy(self) -> "StatusBuffer":
queued_duration=self.queued_duration,
tcp_speed=self.tcp_speed,
simulator_active=self.simulator_active,
+ collision_active=self.collision_active,
+ collision_pairs=list(self.collision_pairs),
)
@@ -1408,7 +1416,8 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
action_current, action_state, joint_en, cart_en_wrf, cart_en_trf,
executing_index, completed_index, last_checkpoint,
error, queued_segments, queued_duration, action_params,
- tool_status_tuple, tcp_speed, simulator_active]
+ tool_status_tuple, tcp_speed, simulator_active,
+ collision_active, collision_pairs]
Args:
data: Raw msgpack bytes
@@ -1465,6 +1474,18 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
if len(msg) > 19:
buf.simulator_active = bool(msg[19])
+ # Collision viz (appended after simulator_active; len-guarded for
+ # backward-compat with pre-collision status producers).
+ if len(msg) > 20:
+ buf.collision_active = bool(msg[20])
+ if len(msg) > 21:
+ raw_pairs = msg[21]
+ cp = buf.collision_pairs
+ cp.clear()
+ if raw_pairs:
+ for p in raw_pairs:
+ cp.append((p[0], p[1]))
+
return True
except Exception as e:
logger.debug("decode_status_bin_into: %s", e)
diff --git a/parol6/robot.py b/parol6/robot.py
index f30e827..cda3756 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -600,6 +600,12 @@ def has_force_torque(self) -> bool:
def has_freedrive(self) -> bool:
return False
+ @property
+ def has_collision_checking(self) -> bool:
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ return PAROL6_ROBOT.collision is not None
+
@property
def digital_outputs(self) -> int:
return 2
diff --git a/parol6/server/controller.py b/parol6/server/controller.py
index 2d42365..48d9ded 100644
--- a/parol6/server/controller.py
+++ b/parol6/server/controller.py
@@ -713,6 +713,8 @@ def _handle_motion_command(
if state.error is not None:
state.error = None
state.action_state = ActionState.IDLE
+ # Unconditional: a jog self-collision sets the viz but no state.error.
+ state.clear_collision()
cmd_obj, _, error_msg = create_command_from_struct(command.p)
if cmd_obj is None:
@@ -743,6 +745,8 @@ def _handle_motion_command(
if state.error is not None:
state.error = None
state.action_state = ActionState.IDLE
+ # Unconditional: a jog self-collision sets the viz but no state.error.
+ state.clear_collision()
cmd_index = self._assign_command_index(state)
# Only sync Position_in when segment player is idle — if segments are
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index 6dd921f..2cdbbfb 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -69,6 +69,7 @@ class ErrorSegment:
error: RobotError
cartesian_path: np.ndarray | None = None # (N, 6) full TCP path
ik_valid: np.ndarray | None = None # (N,) per-pose bool
+ colliding_pairs: list[tuple[str, str]] | None = None # self-collision viz
Segment = Union[TrajectorySegment, InlineSegment, ErrorSegment]
@@ -386,6 +387,7 @@ def _emit_error(
error=robot_error,
cartesian_path=cartesian_path,
ik_valid=ik_valid,
+ colliding_pairs=getattr(exc, "colliding_pairs", None),
)
)
diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py
index e01b131..f7e33ea 100644
--- a/parol6/server/segment_player.py
+++ b/parol6/server/segment_player.py
@@ -140,6 +140,9 @@ def tick(self, state: ControllerState) -> bool:
"Command %d failed: %s", active.command_index, active.error
)
state.error = active.error
+ pairs = active.colliding_pairs
+ state.collision_active = bool(pairs)
+ state.collision_pairs = tuple(pairs) if pairs else ()
state.action_state = ActionState.ERROR
state.action_current = ""
state.action_params = ""
diff --git a/parol6/server/state.py b/parol6/server/state.py
index a45ca5f..cae636e 100644
--- a/parol6/server/state.py
+++ b/parol6/server/state.py
@@ -251,6 +251,12 @@ class ControllerState:
queued_segments: int = 0
queued_duration: float = 0.0
+ # Self-collision viz: colliding pairs captured at the predicted colliding
+ # config when a move is blocked or a jog is stopped; cleared when a
+ # subsequent motion proceeds without collision.
+ collision_active: bool = False
+ collision_pairs: tuple[tuple[str, str], ...] = ()
+
# Network setup and uptime
ip: str = "127.0.0.1"
port: int = 5001
@@ -300,6 +306,11 @@ def __post_init__(self) -> None:
self.InOut_in[4] = 1 # E-STOP released (0=pressed, 1=released)
self.gripper_hw = GripperHWState(self.Gripper_data_out, self.Gripper_data_in)
+ def clear_collision(self) -> None:
+ """Drop any captured self-collision pairs (zero-alloc — interned ())."""
+ self.collision_active = False
+ self.collision_pairs = ()
+
def reset(self) -> None:
"""
Reset robot state to initial values without losing connection state.
@@ -354,6 +365,7 @@ def reset(self) -> None:
# Error and pipeline depth
self.error = None
+ self.clear_collision()
self.queued_segments = 0
self.queued_duration = 0.0
diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py
index 0264f0a..e0f2aa2 100644
--- a/parol6/server/status_cache.py
+++ b/parol6/server/status_cache.py
@@ -159,6 +159,10 @@ def __init__(self) -> None:
# Error tracking field
self._error: RobotError | None = None
+ # Self-collision viz tracking
+ self._collision_active: bool = False
+ self._collision_pairs: tuple[tuple[str, str], ...] = ()
+
# Pipeline depth fields
self._queued_segments: int = 0
self._queued_duration: float = 0.0
@@ -460,6 +464,14 @@ def update_from_state(self, state: ControllerState) -> None:
if error_changed:
self._error = state.error
+ collision_changed = (
+ self._collision_active != state.collision_active
+ or self._collision_pairs != state.collision_pairs
+ )
+ if collision_changed:
+ self._collision_active = state.collision_active
+ self._collision_pairs = state.collision_pairs
+
depth_changed = (
self._queued_segments != state.queued_segments
or self._queued_duration != state.queued_duration
@@ -479,6 +491,7 @@ def update_from_state(self, state: ControllerState) -> None:
or action_changed
or queue_changed
or error_changed
+ or collision_changed
or depth_changed
):
self._binary_dirty = True
@@ -508,6 +521,8 @@ def to_binary(self) -> bytes:
self.tool_status,
self.tcp_speed,
simulator_active=is_simulation_mode(),
+ collision_active=self._collision_active,
+ collision_pairs=self._collision_pairs,
)
self._binary_dirty = False
return self._binary_cache
diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py
index 506670b..a47d793 100644
--- a/parol6/utils/errors.py
+++ b/parol6/utils/errors.py
@@ -24,6 +24,8 @@ class TrajectoryPlanningError(RuntimeError):
def __init__(self, robot_error: RobotError):
self.robot_error = robot_error
+ # Structured self-collision pairs for the viz, set by the collision guard.
+ self.colliding_pairs: list[tuple[str, str]] | None = None
super().__init__(str(robot_error))
From a0cb14f25e3c7758ef79a52c264992f31086b6c6 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Mon, 29 Jun 2026 17:20:40 -0400
Subject: [PATCH 18/36] =?UTF-8?q?WC:=20add=20SET=5FSHAPES=20=E2=80=94=20wo?=
=?UTF-8?q?rkspace=20keep-out=20shapes=20synced=20to=20the=20checkers?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
A SetShapesCmd carries the generic shape wire form (kind/params/pose/collision/
margin/name, matching waldoctl Shape.to_wire). Like SELECT_TOOL it rides the
planner→inline→segment-player path, so PAROL6_ROBOT.apply_shapes runs in both
the controller and planner processes against each one's collision checker.
Collision-enabled shapes become world obstacles via the generic add_obstacle;
visual-only shapes are skipped. async client gains set_shapes.
Co-Authored-By: Claude Opus 4.8
---
parol6/PAROL6_ROBOT.py | 48 ++++++++++++++++++++++++++++++-
parol6/client/async_client.py | 20 ++++++++++++-
parol6/commands/shape_commands.py | 31 ++++++++++++++++++++
parol6/protocol/wire.py | 25 ++++++++++++++++
parol6/server/motion_planner.py | 10 ++++++-
parol6/server/state.py | 4 +++
6 files changed, 135 insertions(+), 3 deletions(-)
create mode 100644 parol6/commands/shape_commands.py
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 1434c6c..d98607b 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -3,9 +3,10 @@
import atexit
import logging
import os
+from collections.abc import Iterable, Sequence
from dataclasses import dataclass
from pathlib import Path
-from typing import Final
+from typing import Any, Final
import numpy as np
from numpy.typing import NDArray
@@ -156,6 +157,10 @@ def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
_active_tool_geom_names: list[str] = []
_active_tool_geom_key: tuple[str, str | None] | None = None
+# Geometry-object names for user-placed workspace shapes (keep-out barriers)
+# currently on this process's collision checker.
+_active_shape_names: list[str] = []
+
def _refresh_collision_tool_geometry(
tool_key: str,
@@ -260,6 +265,47 @@ def apply_tool(
_refresh_collision_tool_geometry(tool_name, variant_key=variant_key or None)
+def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
+ """[x, y, z, rx, ry, rz] (m, rad RPY) -> 4x4 world transform (R = Rz·Ry·Rx)."""
+ x, y, z, rx, ry, rz = pose
+ cx, sx = np.cos(rx), np.sin(rx)
+ cy, sy = np.cos(ry), np.sin(ry)
+ cz, sz = np.cos(rz), np.sin(rz)
+ rot = np.array(
+ [
+ [cz * cy, cz * sy * sx - sz * cx, cz * sy * cx + sz * sx],
+ [sz * cy, sz * sy * sx + cz * cx, sz * sy * cx - cz * sx],
+ [-sy, cy * sx, cy * cx],
+ ]
+ )
+ T = np.eye(4)
+ T[:3, :3] = rot
+ T[:3, 3] = (x, y, z)
+ return T
+
+
+def apply_shapes(shapes: "Iterable[Any]") -> None:
+ """Replace the workspace collision-world shapes on this process's checker.
+
+ Each shape exposes ``kind``/``params``/``pose``/``collision``/``name`` (a
+ parol6 ``ShapeWire`` or any duck-typed equivalent). Only collision-enabled
+ shapes are added — visual-only ones are skipped. Runs per-process; the
+ controller and planner each call it against their own checker.
+ """
+ global _active_shape_names
+ if collision is None:
+ return
+ for name in _active_shape_names:
+ collision.remove_geometry_by_name(name)
+ _active_shape_names = []
+ for s in shapes:
+ if not s.collision:
+ continue
+ name = f"shape:{s.name}"
+ collision.add_obstacle(name, s.kind, list(s.params), _pose_to_matrix(s.pose))
+ _active_shape_names.append(name)
+
+
# Initialize with no tool
apply_tool("NONE")
diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py
index ec66551..e6cd96b 100644
--- a/parol6/client/async_client.py
+++ b/parol6/client/async_client.py
@@ -14,7 +14,7 @@
import msgspec
import numpy as np
-from waldoctl import RobotClient as _RobotClientABC, ToolStatus
+from waldoctl import RobotClient as _RobotClientABC, Shape, ToolStatus
from waldoctl.status import ActionState, ActivityResult, ToolResult
from waldoctl.tools import ToolSpec
@@ -68,7 +68,9 @@
ResponseMsg,
SelectProfileCmd,
SelectToolCmd,
+ SetShapesCmd,
SetTcpOffsetCmd,
+ ShapeWire,
ServoJCmd,
ServoJPoseCmd,
ServoLCmd,
@@ -931,6 +933,22 @@ async def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int:
"""
return await self._send(SetTcpOffsetCmd(x=x, y=y, z=z))
+ async def set_shapes(self, shapes: list[Shape]) -> int:
+ """Replace the workspace collision-world shapes (keep-out barriers).
+
+ Collision-enabled shapes are added to the controller's checkers so motion
+ is blocked against them; an empty list clears all shapes.
+
+ Category: Configuration
+
+ Example:
+ rbt.set_shapes([Box(name="table", x=0.6, y=0.4, z=0.02,
+ pose=(0.3, 0, -0.01, 0, 0, 0))])
+ """
+ return await self._send(
+ SetShapesCmd(shapes=[ShapeWire(*s.to_wire()) for s in shapes])
+ )
+
async def tcp_offset(self) -> list[float]:
"""Query current TCP offset in mm [x, y, z].
diff --git a/parol6/commands/shape_commands.py b/parol6/commands/shape_commands.py
new file mode 100644
index 0000000..58f3a97
--- /dev/null
+++ b/parol6/commands/shape_commands.py
@@ -0,0 +1,31 @@
+"""Workspace collision-world shapes — the SET_SHAPES command."""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING
+
+from parol6.commands.base import ExecutionStatusCode, MotionCommand
+from parol6.protocol.wire import CmdType, SetShapesCmd
+from parol6.server.command_registry import register_command
+
+if TYPE_CHECKING:
+ from parol6.server.state import ControllerState
+
+
+@register_command(CmdType.SET_SHAPES)
+class SetShapesCommand(MotionCommand[SetShapesCmd]):
+ """Replace the workspace keep-out shapes on the collision checkers.
+
+ Routed through the planner (like SELECT_TOOL) so the planner subprocess
+ updates its own checker; this controller-side step updates the live
+ control-loop checker.
+ """
+
+ PARAMS_TYPE = SetShapesCmd
+
+ __slots__ = ()
+
+ def execute_step(self, state: ControllerState) -> ExecutionStatusCode:
+ state.set_shapes(self.p.shapes)
+ self.finish()
+ return ExecutionStatusCode.COMPLETED
diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py
index a836224..da8f555 100644
--- a/parol6/protocol/wire.py
+++ b/parol6/protocol/wire.py
@@ -152,6 +152,8 @@ class CmdType(IntEnum):
# Simulator state query
IS_SIMULATOR = auto()
+ # Workspace collision-world shapes (keep-out geometry)
+ SET_SHAPES = auto()
# =============================================================================
@@ -616,6 +618,29 @@ class SetTcpOffsetCmd(
z: float = 0.0
+class ShapeWire(msgspec.Struct, array_like=True, frozen=True, gc=False):
+ """One workspace shape — mirrors waldoctl ``Shape.to_wire()``."""
+
+ kind: str
+ params: list[float]
+ pose: list[float]
+ collision: bool
+ margin: float | None
+ name: str
+
+
+class SetShapesCmd(
+ msgspec.Struct,
+ tag=int(CmdType.SET_SHAPES),
+ array_like=True,
+ frozen=True,
+ gc=False,
+):
+ """SET_SHAPES: replace the workspace collision-world shapes."""
+
+ shapes: list[ShapeWire]
+
+
class SelectProfileCmd(
msgspec.Struct,
tag=int(CmdType.SELECT_PROFILE),
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index 2cdbbfb..f480906 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -23,7 +23,13 @@
import numpy as np
-from parol6.protocol.wire import HomeCmd, SelectToolCmd, SetTcpOffsetCmd, ToolActionCmd
+from parol6.protocol.wire import (
+ HomeCmd,
+ SelectToolCmd,
+ SetShapesCmd,
+ SetTcpOffsetCmd,
+ ToolActionCmd,
+)
from parol6.server.command_executor import _format_cmd_params
from parol6.utils.error_catalog import RobotError, extract_robot_error
from parol6.utils.error_codes import ErrorCode
@@ -463,6 +469,8 @@ def _handle_inline(self, command_index: int, params: object) -> None:
)
elif isinstance(params, HomeCmd):
self.state.Position_in[:] = self._home_steps
+ elif isinstance(params, SetShapesCmd):
+ self._robot_module.apply_shapes(params.shapes)
# ---------------------------------------------------------------------------
diff --git a/parol6/server/state.py b/parol6/server/state.py
index cae636e..50d6715 100644
--- a/parol6/server/state.py
+++ b/parol6/server/state.py
@@ -406,6 +406,10 @@ def set_tool(self, tool_name: str, variant_key: str = "") -> None:
label = f"{tool_name}:{variant_key}" if variant_key else tool_name
logger.info(f"Tool changed to {label}")
+ def set_shapes(self, shapes: list) -> None:
+ """Apply the workspace collision-world shapes to the control-loop checker."""
+ PAROL6_ROBOT.apply_shapes(shapes)
+
@property
def tcp_offset_m(self) -> tuple[float, float, float]:
"""Current TCP offset in meters (tool-local frame)."""
From 130f7fb7acdec215693a6db838ad958735258593 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 30 Jun 2026 08:50:23 -0400
Subject: [PATCH 19/36] WC: collision clearance + jog speed-buffer + joint
self-collision greying
- COLLISION_CLEARANCE_M (default 5mm) sets the checker's fixed clearance at
init, so all three process checkers keep a uniform near-miss buffer.
- JogJ now checks a velocity-scaled lookahead (COLLISION_JOG_LOOKAHEAD_S), so
faster jogs stop further from contact; composes with the clearance. In-place
to keep the 100Hz path allocation-free.
- The IK worker greys a joint direction whose small step self-collides
(_gate_joint_enable_collision, proximity-gated), feeding the existing
can_jog_* path so the jog buttons disable toward a self-collision. Cartesian
and tool/shape greying need the ik-worker geometry sync (a follow-up).
Co-Authored-By: Claude Opus 4.8
---
parol6/PAROL6_ROBOT.py | 8 ++++--
parol6/commands/basic_commands.py | 27 ++++++++++++------
parol6/config.py | 16 ++++++++++-
parol6/server/ik_worker.py | 32 ++++++++++++++++++++-
tests/unit/test_collision_enablement.py | 37 +++++++++++++++++++++++++
5 files changed, 108 insertions(+), 12 deletions(-)
create mode 100644 tests/unit/test_collision_enablement.py
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index d98607b..0845d56 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -108,7 +108,9 @@ def _cleanup_tmp_urdf() -> None:
return tmp_path
-def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
+def _init_collision_checker(
+ enabled: bool, srdf_path: str, clearance_margin: float = 0.0
+) -> None:
"""Build the singleton CollisionChecker when *enabled*.
Config values are passed in (by ``parol6.config`` after its knobs are
@@ -124,7 +126,9 @@ def _init_collision_checker(enabled: bool, srdf_path: str) -> None:
# 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)
+ c = CollisionChecker(
+ robot, urdf_for_collision, clearance_margin=clearance_margin
+ )
if srdf_path and os.path.exists(srdf_path):
c.load_srdf(srdf_path)
collision = c
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 2f68daa..9a9bc22 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -8,6 +8,7 @@
import numpy as np
from parol6.config import (
+ COLLISION_JOG_LOOKAHEAD_S,
JOG_MIN_STEPS,
LIMITS,
rad_to_steps,
@@ -129,6 +130,7 @@ class JogJCommand(MotionCommand[JogJCmd]):
"speeds_out",
"_jog_initialized",
"_jog_vel_rad",
+ "_lookahead_buf",
)
def __init__(self, p: JogJCmd):
@@ -136,6 +138,7 @@ def __init__(self, p: JogJCmd):
self.speeds_out = np.zeros(6, dtype=np.int32)
self._jog_initialized = False
self._jog_vel_rad = np.zeros(6, dtype=np.float64)
+ self._lookahead_buf = np.zeros(6, dtype=np.float64)
def do_setup(self, state: "ControllerState") -> None:
"""Pre-compute step speeds and rad/s velocities for all 6 joints."""
@@ -197,14 +200,22 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
# 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")
- # Allocate only here (the rare stop), never on the clean per-tick path.
- state.collision_pairs = tuple(checker.colliding_pairs(pos_rad))
- state.collision_active = True
- se.active = False
- self.finish()
- return ExecutionStatusCode.COMPLETED
+ if checker is not None:
+ # Look a velocity-scaled horizon ahead so faster jogs stop further
+ # from contact; composes with the checker's fixed clearance. In-place
+ # to keep the hot path allocation-free.
+ la = self._lookahead_buf
+ la[:] = self._jog_vel_rad
+ la *= COLLISION_JOG_LOOKAHEAD_S
+ la += pos_rad
+ if checker.in_collision(la):
+ logger.warning("[JOGJ] self-collision predicted - stopping jog")
+ # Allocate only here (the rare stop), never on the clean tick.
+ state.collision_pairs = tuple(checker.colliding_pairs(la))
+ state.collision_active = True
+ se.active = False
+ self.finish()
+ return ExecutionStatusCode.COMPLETED
self._q_rad_buf[:] = pos_rad
rad_to_steps(self._q_rad_buf, self._steps_buf)
diff --git a/parol6/config.py b/parol6/config.py
index 9d3c61f..1ddb8e2 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -589,8 +589,22 @@ def _build_cart_kinodynamic(
str(_default_srdf) if _default_srdf.exists() else "",
)
+# Fixed clearance (m): geometry within this distance counts as colliding, so the
+# arm keeps a near-miss buffer from itself and from keep-out shapes (absorbs
+# calibration/model error). Applied uniformly to every collision query.
+COLLISION_CLEARANCE_M: float = float(os.getenv("PAROL6_COLLISION_CLEARANCE_M", "0.005"))
+
+# Velocity-scaled jog stopping buffer (s): a jog is stopped if the config this
+# many seconds ahead (at the current jog velocity) would collide — so faster
+# jogs stop further from contact. Composes with COLLISION_CLEARANCE_M.
+COLLISION_JOG_LOOKAHEAD_S: float = float(
+ os.getenv("PAROL6_COLLISION_JOG_LOOKAHEAD_S", "0.15")
+)
+
# Populate PAROL6_ROBOT.collision now that the config knobs are defined.
-PAROL6_ROBOT._init_collision_checker(COLLISION_CHECK_ENABLED, COLLISION_SRDF_PATH)
+PAROL6_ROBOT._init_collision_checker(
+ COLLISION_CHECK_ENABLED, COLLISION_SRDF_PATH, COLLISION_CLEARANCE_M
+)
# -----------------------------------------------------------------------------
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index d8d91a9..bbc05a1 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -29,6 +29,11 @@
logger = logging.getLogger(__name__)
+# Directional self-collision enablement: when the arm is within _ENABLE_NEAR_M of
+# self-collision, grey a joint direction whose _ENABLE_STEP_RAD step would collide.
+_ENABLE_STEP_RAD = np.radians(2.0)
+_ENABLE_NEAR_M = 0.05
+
def ik_enablement_worker_main(
input_shm_name: str,
@@ -109,8 +114,9 @@ def ik_enablement_worker_main(
response_version = 0
- # Pre-allocate work array for cartesian targets
+ # Pre-allocate work arrays for cartesian targets + the enablement step.
cart_targets = np.zeros((12, 4, 4), dtype=np.float64)
+ q_step = np.zeros(6, dtype=np.float64)
logger.debug("IK worker subprocess started")
@@ -138,6 +144,13 @@ def ik_enablement_worker_main(
_compute_joint_enable(q_rad, qlim, joint_en)
# else: joint_en stays all ones (pre-allocated default)
+ # Self-collision gate: grey a joint direction whose small step would
+ # collide. The ik-worker checker has the arm links (tool/shape greying
+ # needs geometry sync — a follow-up).
+ _gate_joint_enable_collision(
+ PAROL6_ROBOT.collision, q_rad, joint_en, q_step
+ )
+
# Compute cartesian enablement for both frames
_compute_cart_enable(
T_matrix,
@@ -207,6 +220,23 @@ def _compute_joint_enable(
out[i * 2 + 1] = 1 if (q_rad[i] - delta_rad) >= qlim[0, i] else 0
+def _gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
+ """Clear a joint direction in ``joint_en`` whose ``_ENABLE_STEP_RAD`` step
+ self-collides. Proximity-gated (skip the per-direction checks when the arm is
+ farther than ``_ENABLE_NEAR_M`` from collision) so it stays cheap. Not njit —
+ it calls the C++ checker.
+ """
+ if checker is None or checker.min_distance(q_rad) >= _ENABLE_NEAR_M:
+ return
+ for j in range(6):
+ for slot, sign in ((2 * j, 1.0), (2 * j + 1, -1.0)):
+ if joint_en[slot]:
+ q_step[:] = q_rad
+ q_step[j] += sign * _ENABLE_STEP_RAD
+ if checker.in_collision(q_step):
+ joint_en[slot] = 0
+
+
# Axis directions: [dx, dy, dz, rx, ry, rz] for each of 12 axes
# Order: X+, X-, Y+, Y-, Z+, Z-, RX+, RX-, RY+, RY-, RZ+, RZ-
_AXIS_DIRS = np.array(
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
new file mode 100644
index 0000000..c7c96d7
--- /dev/null
+++ b/tests/unit/test_collision_enablement.py
@@ -0,0 +1,37 @@
+"""The directional self-collision enablement gate used by the IK worker."""
+
+import numpy as np
+
+from parol6.server.ik_worker import _ENABLE_NEAR_M, _gate_joint_enable_collision
+
+
+class _FakeChecker:
+ def __init__(self, distance, collides):
+ self._distance = distance
+ self._collides = collides
+ self.queries = 0
+
+ def min_distance(self, q):
+ return self._distance
+
+ def in_collision(self, q):
+ self.queries += 1
+ return self._collides(q)
+
+
+def test_gate_skips_per_direction_checks_when_far():
+ joint_en = np.ones(12, dtype=np.uint8)
+ checker = _FakeChecker(_ENABLE_NEAR_M + 0.1, lambda q: True)
+ _gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ assert joint_en.tolist() == [1] * 12
+ assert checker.queries == 0 # proximity-gated: no in_collision when far
+
+
+def test_gate_greys_only_the_colliding_direction():
+ joint_en = np.ones(12, dtype=np.uint8)
+ # Near collision; stepping joint index 1 (J2) positive trips it.
+ checker = _FakeChecker(_ENABLE_NEAR_M - 0.01, lambda q: q[1] > 0.01)
+ _gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ assert joint_en[2] == 0 # J2+ greyed
+ assert joint_en[3] == 1 # J2- still allowed
+ assert joint_en[0] == 1 and joint_en[1] == 1 # J1 untouched
From 69c56254d70e323973876ef3bd8ad316983d2fdd Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 30 Jun 2026 09:36:22 -0400
Subject: [PATCH 20/36] WC: gate joint greying on the full checker (covers tool
+ shapes)
Move the directional self-collision gate from the ik-worker (links-only) to the
controller's status_cache _poll_ik_results, which runs against the process-global
checker carrying the tool (apply_tool) and keep-out shapes (set_shapes). So the
jog/move joint buttons now grey for tool and shape collisions, not just
link-vs-link. gate_joint_enable_collision is promoted to a shared helper.
Cartesian-direction greying needs the per-direction IK targets and is a follow-up.
Co-Authored-By: Claude Opus 4.8
---
parol6/server/ik_worker.py | 14 ++++----------
parol6/server/status_cache.py | 16 +++++++++++++++-
tests/unit/test_collision_enablement.py | 6 +++---
3 files changed, 22 insertions(+), 14 deletions(-)
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index bbc05a1..6134073 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -114,9 +114,8 @@ def ik_enablement_worker_main(
response_version = 0
- # Pre-allocate work arrays for cartesian targets + the enablement step.
+ # Pre-allocate work array for cartesian targets.
cart_targets = np.zeros((12, 4, 4), dtype=np.float64)
- q_step = np.zeros(6, dtype=np.float64)
logger.debug("IK worker subprocess started")
@@ -143,13 +142,8 @@ def ik_enablement_worker_main(
if qlim is not None:
_compute_joint_enable(q_rad, qlim, joint_en)
# else: joint_en stays all ones (pre-allocated default)
-
- # Self-collision gate: grey a joint direction whose small step would
- # collide. The ik-worker checker has the arm links (tool/shape greying
- # needs geometry sync — a follow-up).
- _gate_joint_enable_collision(
- PAROL6_ROBOT.collision, q_rad, joint_en, q_step
- )
+ # The collision gate runs in the controller (status_cache), whose
+ # checker has the tool + shapes; the ik-worker only has the links.
# Compute cartesian enablement for both frames
_compute_cart_enable(
@@ -220,7 +214,7 @@ def _compute_joint_enable(
out[i * 2 + 1] = 1 if (q_rad[i] - delta_rad) >= qlim[0, i] else 0
-def _gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
+def gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
"""Clear a joint direction in ``joint_en`` whose ``_ENABLE_STEP_RAD`` step
self-collides. Proximity-gated (skip the per-direction checks when the arm is
farther than ``_ENABLE_NEAR_M`` from collision) so it stays cheap. Not njit —
diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py
index e0f2aa2..5c0b345 100644
--- a/parol6/server/status_cache.py
+++ b/parol6/server/status_cache.py
@@ -30,7 +30,10 @@
SHM_EXTRA_KWARGS,
unregister_shm,
)
-from parol6.server.ik_worker import ik_enablement_worker_main
+from parol6.server.ik_worker import (
+ gate_joint_enable_collision,
+ ik_enablement_worker_main,
+)
from parol6.server.state import ControllerState, get_fkine_flat_mm, get_fkine_se3
from parol6.tools import get_tool_transform
from parol6 import config as _cfg
@@ -189,6 +192,7 @@ def __init__(self) -> None:
# IK enablement results (pre-allocated for zero-alloc reads)
self._joint_en = np.ones(12, dtype=np.uint8)
+ self._ik_gate_q_step = np.zeros(6, dtype=np.float64)
self._cart_en_wrf = np.ones(12, dtype=np.uint8)
self._cart_en_trf = np.ones(12, dtype=np.uint8)
@@ -336,6 +340,16 @@ def _poll_ik_results(self) -> bool:
)
if new_version > 0:
self._ik_last_version = new_version
+ # Gate joint enablement on the controller's FULL checker (links +
+ # tool + shapes) — the ik-worker only has the arm links.
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ gate_joint_enable_collision(
+ PAROL6_ROBOT.collision,
+ self._ik_last_q_rad,
+ self._joint_en,
+ self._ik_gate_q_step,
+ )
return True
return False
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
index c7c96d7..e66ed47 100644
--- a/tests/unit/test_collision_enablement.py
+++ b/tests/unit/test_collision_enablement.py
@@ -2,7 +2,7 @@
import numpy as np
-from parol6.server.ik_worker import _ENABLE_NEAR_M, _gate_joint_enable_collision
+from parol6.server.ik_worker import _ENABLE_NEAR_M, gate_joint_enable_collision
class _FakeChecker:
@@ -22,7 +22,7 @@ def in_collision(self, q):
def test_gate_skips_per_direction_checks_when_far():
joint_en = np.ones(12, dtype=np.uint8)
checker = _FakeChecker(_ENABLE_NEAR_M + 0.1, lambda q: True)
- _gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
assert joint_en.tolist() == [1] * 12
assert checker.queries == 0 # proximity-gated: no in_collision when far
@@ -31,7 +31,7 @@ def test_gate_greys_only_the_colliding_direction():
joint_en = np.ones(12, dtype=np.uint8)
# Near collision; stepping joint index 1 (J2) positive trips it.
checker = _FakeChecker(_ENABLE_NEAR_M - 0.01, lambda q: q[1] > 0.01)
- _gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
assert joint_en[2] == 0 # J2+ greyed
assert joint_en[3] == 1 # J2- still allowed
assert joint_en[0] == 1 and joint_en[1] == 1 # J1 untouched
From ac1c5a08268dfa74513e6e141f2c6ae89444d4ae Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 30 Jun 2026 09:42:33 -0400
Subject: [PATCH 21/36] Dedup the disabled-checker guard across Robot collision
methods
A _collision_checker property folds the repeated lazy import + None check used
by in_collision / check_trajectory / colliding_pairs / min_distance. Zero
behavior change.
Co-Authored-By: Claude Opus 4.8
---
parol6/robot.py | 44 ++++++++++++++++++++++----------------------
1 file changed, 22 insertions(+), 22 deletions(-)
diff --git a/parol6/robot.py b/parol6/robot.py
index cda3756..23eabd7 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -780,33 +780,35 @@ 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.
+ @property
+ def _collision_checker(self):
+ """The process-global checker, or None when collision checking is off.
- 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.
+ Shared by every collision method 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 Robot.
"""
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
- if PAROL6_ROBOT.collision is None:
+ return PAROL6_ROBOT.collision
+
+ def in_collision(self, q_rad: NDArray[np.float64]) -> bool:
+ """Return True iff `q_rad` is in self/world collision. False if disabled."""
+ c = self._collision_checker
+ if c is None:
return False
self._load_q_buf(q_rad)
- return PAROL6_ROBOT.collision.in_collision(self._q_buf)
+ return c.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:
+ c = self._collision_checker
+ if c is None:
return -1
- return PAROL6_ROBOT.collision.check_path(
- np.ascontiguousarray(q_path_rad, dtype=np.float64)
- )
+ return c.check_path(np.ascontiguousarray(q_path_rad, dtype=np.float64))
def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[str, str]]:
"""Return list of (name, name) geometry pairs in collision at `q_rad`.
@@ -817,12 +819,11 @@ def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[str, str]]:
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
-
- if PAROL6_ROBOT.collision is None:
+ c = self._collision_checker
+ if c is None:
return []
self._load_q_buf(q_rad)
- return PAROL6_ROBOT.collision.colliding_pairs(self._q_buf)
+ return c.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`.
@@ -830,12 +831,11 @@ def min_distance(self, q_rad: NDArray[np.float64]) -> float:
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:
+ c = self._collision_checker
+ if c is None:
return float("inf")
self._load_q_buf(q_rad)
- return PAROL6_ROBOT.collision.min_distance(self._q_buf)
+ return c.min_distance(self._q_buf)
def ik_batch(
self,
From 16195b87936955171befcd1c177de1635cf0824b Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Thu, 2 Jul 2026 09:21:16 -0400
Subject: [PATCH 22/36] Review fixes: uniform intake collision-clear +
_pose_to_matrix why-comment
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Streamable intake (servo/jog/teleport) now clears the collision viz like the
tool-action and planner intakes — a jog-collision red no longer outlives a
subsequent clean servo. The per-jog do_setup clears are dropped (intake covers
all streamables). _pose_to_matrix documents why Rz·Ry·Rx is deliberate (matches
the frontend's reversed-Euler render order; se3_from_rpy would mis-orient
shapes).
Co-Authored-By: Claude Fable 5
---
parol6/PAROL6_ROBOT.py | 7 ++++++-
parol6/commands/basic_commands.py | 1 -
parol6/commands/cartesian_commands.py | 1 -
parol6/server/controller.py | 2 ++
4 files changed, 8 insertions(+), 3 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 0845d56..8a95ffa 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -270,7 +270,12 @@ def apply_tool(
def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
- """[x, y, z, rx, ry, rz] (m, rad RPY) -> 4x4 world transform (R = Rz·Ry·Rx)."""
+ """[x, y, z, rx, ry, rz] (m, rad RPY) -> 4x4 world transform (R = Rz·Ry·Rx).
+
+ Rz·Ry·Rx matches the frontend's shape render rotation (NiceGUI ``rotate``,
+ XYZ order). Deliberately NOT ``pinokin.se3_from_rpy`` (Rx·Ry·Rz) — swapping
+ it in would mis-orient any multi-axis-tilted shape vs its rendered pose.
+ """
x, y, z, rx, ry, rz = pose
cx, sx = np.cos(rx), np.sin(rx)
cy, sy = np.cos(ry), np.sin(ry)
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 9a9bc22..ce03caa 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -159,7 +159,6 @@ def do_setup(self, state: "ControllerState") -> None:
)
self.start_timer(self.p.duration)
self._jog_initialized = False
- state.clear_collision()
def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
"""Execute one tick of joint jogging via StreamingExecutor."""
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 85616a4..66ea2e1 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -115,7 +115,6 @@ def do_setup(self, state: "ControllerState") -> None:
self._axis_sign = 1.0 if vels[max_idx] >= 0 else -1.0
self.start_timer(self.p.duration)
self._ik_stopping = False
- state.clear_collision()
def _track_and_send(self, state: "ControllerState", ik_q: np.ndarray) -> None:
"""Velocity-clamp IK result, update tracked position, send MOVE."""
diff --git a/parol6/server/controller.py b/parol6/server/controller.py
index 48d9ded..40fb42d 100644
--- a/parol6/server/controller.py
+++ b/parol6/server/controller.py
@@ -686,6 +686,8 @@ def _handle_motion_command(
# Streaming commands: cancel segment playback + existing streamable handling
if getattr(command, "streamable", False):
self._segment_player.cancel(state)
+ # Unconditional: a jog self-collision sets the viz but no state.error.
+ state.clear_collision()
if self.udp_transport:
drained = self.udp_transport.drain_buffer()
if drained > 0:
From 949422ce84604172a3fbc0b4f1e625a5a6fe0226 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Thu, 2 Jul 2026 09:21:18 -0400
Subject: [PATCH 23/36] IK worker owns the enablement collision gate; add
cartesian gating
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Per the collision plan: the worker's checker is kept in sync with the
controller's geometry (SyncTool/SyncShapes over a new command queue, driven by
tool-change detection + a shapes_version on state), the joint gate moves from
status_cache into the worker loop, and _compute_cart_enable now also requires
the solved config to be collision-free (proximity-gated) — cartesian jog
buttons grey toward blocked directions. A geometry sync recomputes at the last
requested pose, so greying updates even while the arm is stationary.
Co-Authored-By: Claude Fable 5
---
parol6/server/ik_worker.py | 99 ++++++++++++++++++++-----
parol6/server/state.py | 13 +++-
parol6/server/status_cache.py | 50 +++++++++----
tests/unit/test_collision_enablement.py | 89 +++++++++++++++++++++-
4 files changed, 215 insertions(+), 36 deletions(-)
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index 6134073..cdd4bad 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -7,8 +7,11 @@
import logging
import signal
+from dataclasses import dataclass
from multiprocessing.shared_memory import SharedMemory
from multiprocessing.synchronize import Event
+from queue import Empty
+from typing import TYPE_CHECKING
import numpy as np
from numba import njit
@@ -27,31 +30,70 @@
unregister_shm,
)
+if TYPE_CHECKING:
+ import multiprocessing
+
logger = logging.getLogger(__name__)
-# Directional self-collision enablement: when the arm is within _ENABLE_NEAR_M of
-# self-collision, grey a joint direction whose _ENABLE_STEP_RAD step would collide.
+# Directional collision enablement: when the arm is within _ENABLE_NEAR_M of
+# collision, grey a direction whose _ENABLE_STEP_RAD step would collide.
_ENABLE_STEP_RAD = np.radians(2.0)
_ENABLE_NEAR_M = 0.05
+@dataclass(frozen=True)
+class SyncTool:
+ """Sync the worker checker's tool geometry (mirrors the planner's SyncTool)."""
+
+ tool_name: str
+ variant_key: str = ""
+
+
+@dataclass(frozen=True)
+class SyncShapes:
+ """Sync the worker checker's workspace keep-out shapes (ShapeWire tuple)."""
+
+ shapes: tuple
+
+
+def _drain_sync(cmd_queue: "multiprocessing.Queue", robot_module) -> bool:
+ """Apply queued geometry syncs to this process's checker; True if any applied."""
+ applied = False
+ while True:
+ try:
+ msg = cmd_queue.get_nowait()
+ except Empty:
+ return applied
+ if isinstance(msg, SyncTool):
+ robot_module.apply_tool(msg.tool_name, variant_key=msg.variant_key)
+ applied = True
+ elif isinstance(msg, SyncShapes):
+ robot_module.apply_shapes(msg.shapes)
+ applied = True
+
+
def ik_enablement_worker_main(
input_shm_name: str,
output_shm_name: str,
shutdown_event: Event,
request_event: Event,
+ command_queue: "multiprocessing.Queue",
) -> None:
"""
Main entry point for IK enablement worker subprocess.
This worker waits for request signals, computes joint and cartesian
- enablement, and writes results to the output shared memory.
+ enablement, and writes results to the output shared memory. Geometry
+ syncs (tool / keep-out shapes) arrive via ``command_queue`` and trigger
+ a recompute at the last requested pose so enablement never goes stale
+ while the arm is stationary.
Args:
input_shm_name: Name of input shared memory segment
output_shm_name: Name of output shared memory segment
shutdown_event: Event to signal shutdown
request_event: Event signaled when new request is available
+ command_queue: SyncTool / SyncShapes geometry sync messages
"""
# Ignore SIGINT in worker - main process handles shutdown via shutdown_event
signal.signal(signal.SIGINT, signal.SIG_IGN)
@@ -107,25 +149,36 @@ def ik_enablement_worker_main(
# Initialize robot model in this process
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+ from parol6.tools import register_plugin_tools
from parol6.utils.ik import solve_ik
+ # Spawn-mode subprocess: the registry is freshly imported with only native
+ # tools; SyncTool of a plugin tool needs them registered here too.
+ register_plugin_tools()
+
robot = PAROL6_ROBOT.robot
qlim = robot.qlim
response_version = 0
+ have_request = False
- # Pre-allocate work array for cartesian targets.
+ # Pre-allocate work arrays for cartesian targets + the joint-gate step.
cart_targets = np.zeros((12, 4, 4), dtype=np.float64)
+ q_step = np.zeros(6, dtype=np.float64)
logger.debug("IK worker subprocess started")
try:
while not shutdown_event.is_set():
- # Wait for request signal or timeout for shutdown check
- if not request_event.wait(timeout=0.1):
- continue # Timeout - loop back to check shutdown
-
- request_event.clear()
+ signaled = request_event.wait(timeout=0.1)
+ # A geometry sync recomputes at the last requested pose so greying
+ # reflects a shape/tool change even while the arm is stationary.
+ synced = _drain_sync(command_queue, PAROL6_ROBOT)
+ if not signaled and not (synced and have_request):
+ continue
+ if signaled:
+ request_event.clear()
+ have_request = True
# Apply tool transform if it changed since last request
if not np.array_equal(tool_T, last_tool_T):
@@ -138,12 +191,13 @@ def ik_enablement_worker_main(
# Input data is already available via views (q_rad, T_matrix)
- # Compute joint enablement
+ # Compute joint enablement, then gate directions whose small step
+ # would collide (self + tool + shapes; geometry kept in sync above).
if qlim is not None:
_compute_joint_enable(q_rad, qlim, joint_en)
# else: joint_en stays all ones (pre-allocated default)
- # The collision gate runs in the controller (status_cache), whose
- # checker has the tool + shapes; the ik-worker only has the links.
+ checker = PAROL6_ROBOT.collision
+ gate_joint_enable_collision(checker, q_rad, joint_en, q_step)
# Compute cartesian enablement for both frames
_compute_cart_enable(
@@ -155,6 +209,7 @@ def ik_enablement_worker_main(
_AXIS_DIRS,
cart_targets,
cart_en_wrf,
+ checker=checker,
)
_compute_cart_enable(
T_matrix,
@@ -165,6 +220,7 @@ def ik_enablement_worker_main(
_AXIS_DIRS,
cart_targets,
cart_en_trf,
+ checker=checker,
)
# Output data written directly via views, just update version
@@ -216,9 +272,9 @@ def _compute_joint_enable(
def gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
"""Clear a joint direction in ``joint_en`` whose ``_ENABLE_STEP_RAD`` step
- self-collides. Proximity-gated (skip the per-direction checks when the arm is
- farther than ``_ENABLE_NEAR_M`` from collision) so it stays cheap. Not njit —
- it calls the C++ checker.
+ collides — self, tool, or keep-out shape. Proximity-gated (skip the
+ per-direction checks when the arm is farther than ``_ENABLE_NEAR_M`` from
+ collision) so it stays cheap. Not njit — it calls the C++ checker.
"""
if checker is None or checker.min_distance(q_rad) >= _ENABLE_NEAR_M:
return
@@ -307,10 +363,14 @@ def _compute_cart_enable(
out: np.ndarray,
delta_mm: float = 0.5,
delta_deg: float = 0.5,
+ checker=None,
) -> None:
"""
Compute per-axis +/- enable bits for the given frame via small-step IK.
+ A direction is disabled when IK fails or (near collision, like the joint
+ gate) its solved config would collide — self, tool, or keep-out shape.
+
Writes to out array (12 elements) in axis order:
X+, X-, Y+, Y-, Z+, Z-, RX+, RX-, RY+, RY-, RZ+, RZ-
"""
@@ -320,10 +380,15 @@ def _compute_cart_enable(
# Compute all 12 target poses in one numba call
_compute_target_poses(T, t_step, r_step, is_wrf, axis_dirs, targets)
- # Check IK for each target
+ near = checker is not None and checker.min_distance(q_rad) < _ENABLE_NEAR_M
+
+ # Check IK (and, when near collision, the solved config) for each target
for i in range(12):
try:
ik = solve_ik(robot, targets[i], q_rad, quiet_logging=True)
- out[i] = 1 if ik.success else 0
+ ok = bool(ik.success)
+ if ok and near and checker.in_collision(ik.q):
+ ok = False
+ out[i] = 1 if ok else 0
except Exception:
out[i] = 0
diff --git a/parol6/server/state.py b/parol6/server/state.py
index 50d6715..3cb16a1 100644
--- a/parol6/server/state.py
+++ b/parol6/server/state.py
@@ -257,6 +257,11 @@ class ControllerState:
collision_active: bool = False
collision_pairs: tuple[tuple[str, str], ...] = ()
+ # Workspace keep-out shapes (last applied), versioned so the status cache
+ # can mirror them to the IK worker's checker.
+ shapes: list = field(default_factory=list)
+ shapes_version: int = 0
+
# Network setup and uptime
ip: str = "127.0.0.1"
port: int = 5001
@@ -407,8 +412,14 @@ def set_tool(self, tool_name: str, variant_key: str = "") -> None:
logger.info(f"Tool changed to {label}")
def set_shapes(self, shapes: list) -> None:
- """Apply the workspace collision-world shapes to the control-loop checker."""
+ """Apply the workspace collision-world shapes to the control-loop checker.
+
+ Also retained (with a version bump) so the status cache can mirror them
+ to the IK worker's checker for enablement greying.
+ """
PAROL6_ROBOT.apply_shapes(shapes)
+ self.shapes = list(shapes)
+ self.shapes_version += 1
@property
def tcp_offset_m(self) -> tuple[float, float, float]:
diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py
index 5c0b345..9a63072 100644
--- a/parol6/server/status_cache.py
+++ b/parol6/server/status_cache.py
@@ -31,7 +31,8 @@
unregister_shm,
)
from parol6.server.ik_worker import (
- gate_joint_enable_collision,
+ SyncShapes,
+ SyncTool,
ik_enablement_worker_main,
)
from parol6.server.state import ControllerState, get_fkine_flat_mm, get_fkine_se3
@@ -192,10 +193,12 @@ def __init__(self) -> None:
# IK enablement results (pre-allocated for zero-alloc reads)
self._joint_en = np.ones(12, dtype=np.uint8)
- self._ik_gate_q_step = np.zeros(6, dtype=np.float64)
self._cart_en_wrf = np.ones(12, dtype=np.uint8)
self._cart_en_trf = np.ones(12, dtype=np.uint8)
+ # Last shapes_version synced to the IK worker's checker
+ self._last_shapes_version: int = 0
+
# IK worker subprocess state
self._ik_stopped = False
self._ik_last_version = 0
@@ -256,6 +259,7 @@ def __init__(self) -> None:
# Spawn subprocess
self._ik_shutdown_event: Event = multiprocessing.Event()
self._ik_request_event: Event = multiprocessing.Event()
+ self._ik_cmd_queue: multiprocessing.Queue = multiprocessing.Queue()
self._ik_process: Process = Process(
target=ik_enablement_worker_main,
args=(
@@ -263,6 +267,7 @@ def __init__(self) -> None:
output_name,
self._ik_shutdown_event,
self._ik_request_event,
+ self._ik_cmd_queue,
),
daemon=True,
name="IKWorkerProcess",
@@ -278,6 +283,9 @@ def _stop_ik_worker(self) -> None:
# Signal shutdown
self._ik_shutdown_event.set()
+ # Don't block interpreter exit on the queue's feeder thread
+ self._ik_cmd_queue.close()
+ self._ik_cmd_queue.cancel_join_thread()
# Wait for process to exit
if self._ik_process.is_alive():
@@ -308,10 +316,19 @@ def _stop_ik_worker(self) -> None:
except BufferError:
pass
- # Clean up shared memory
- _cleanup_shm(self._ik_input_shm)
- _cleanup_shm(self._ik_output_shm)
- logger.debug("IK worker stopped")
+ # Clean up shared memory. When the __del__ safety net runs this during
+ # interpreter shutdown, module globals may already be nulled — the OS
+ # reclaims the segments anyway at that point.
+ if _cleanup_shm is not None:
+ _cleanup_shm(self._ik_input_shm)
+ _cleanup_shm(self._ik_output_shm)
+ logger.debug("IK worker stopped")
+
+ def _sync_ik_geometry(self, msg: SyncTool | SyncShapes) -> None:
+ """Push a geometry sync to the IK worker's checker (non-blocking)."""
+ if self._ik_stopped:
+ return
+ self._ik_cmd_queue.put_nowait(msg)
def _submit_ik_request(self, q_rad: np.ndarray, T_matrix: np.ndarray) -> None:
"""Submit an IK enablement request (non-blocking, zero-alloc).
@@ -340,16 +357,6 @@ def _poll_ik_results(self) -> bool:
)
if new_version > 0:
self._ik_last_version = new_version
- # Gate joint enablement on the controller's FULL checker (links +
- # tool + shapes) — the ik-worker only has the arm links.
- import parol6.PAROL6_ROBOT as PAROL6_ROBOT
-
- gate_joint_enable_collision(
- PAROL6_ROBOT.collision,
- self._ik_last_q_rad,
- self._joint_en,
- self._ik_gate_q_step,
- )
return True
return False
@@ -401,6 +408,17 @@ def update_from_state(self, state: ControllerState) -> None:
state.current_tool, variant_key=state.current_tool_variant
)
self._ik_input_tool_view.reshape(4, 4)[:] = T_tool
+ # Sync the tool's collision geometry to the IK worker's checker
+ self._sync_ik_geometry(
+ SyncTool(
+ tool_name=state.current_tool,
+ variant_key=state.current_tool_variant,
+ )
+ )
+
+ if state.shapes_version != self._last_shapes_version:
+ self._last_shapes_version = state.shapes_version
+ self._sync_ik_geometry(SyncShapes(shapes=tuple(state.shapes)))
if pos_changed or tool_changed:
self.pose[:] = get_fkine_flat_mm(state)
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
index e66ed47..0f342a1 100644
--- a/tests/unit/test_collision_enablement.py
+++ b/tests/unit/test_collision_enablement.py
@@ -1,8 +1,20 @@
-"""The directional self-collision enablement gate used by the IK worker."""
+"""Directional collision enablement gates + geometry sync in the IK worker."""
+
+import multiprocessing
+import time
+from dataclasses import dataclass
import numpy as np
-from parol6.server.ik_worker import _ENABLE_NEAR_M, gate_joint_enable_collision
+from parol6.server.ik_worker import (
+ _AXIS_DIRS,
+ _ENABLE_NEAR_M,
+ SyncShapes,
+ SyncTool,
+ _compute_cart_enable,
+ _drain_sync,
+ gate_joint_enable_collision,
+)
class _FakeChecker:
@@ -35,3 +47,76 @@ def test_gate_greys_only_the_colliding_direction():
assert joint_en[2] == 0 # J2+ greyed
assert joint_en[3] == 1 # J2- still allowed
assert joint_en[0] == 1 and joint_en[1] == 1 # J1 untouched
+
+
+@dataclass
+class _FakeIK:
+ success: bool
+ q: np.ndarray
+
+
+def _cart_enable(checker, solve_ik):
+ out = np.ones(12, dtype=np.uint8)
+ _compute_cart_enable(
+ np.eye(4),
+ True,
+ np.zeros(6),
+ None,
+ solve_ik,
+ _AXIS_DIRS,
+ np.zeros((12, 4, 4), dtype=np.float64),
+ out,
+ checker=checker,
+ )
+ return out
+
+
+def test_cart_gate_greys_colliding_directions_when_near():
+ # IK succeeds everywhere; the solved config for X+ (axis 0) collides.
+ calls = []
+
+ def solve_ik(robot, target, q_seed, quiet_logging=True):
+ calls.append(target.copy())
+ return _FakeIK(True, target[:3, 3].repeat(2))
+
+ checker = _FakeChecker(_ENABLE_NEAR_M - 0.01, lambda q: q[0] > 0)
+ out = _cart_enable(checker, solve_ik)
+ assert out[0] == 0 # X+ greyed (solved config collides)
+ assert out[1] == 1 # X- fine
+ assert len(calls) == 12
+
+
+def test_cart_gate_skips_collision_checks_when_far():
+ def solve_ik(robot, target, q_seed, quiet_logging=True):
+ return _FakeIK(True, np.zeros(6))
+
+ checker = _FakeChecker(_ENABLE_NEAR_M + 0.1, lambda q: True)
+ out = _cart_enable(checker, solve_ik)
+ assert out.tolist() == [1] * 12
+ assert checker.queries == 0
+
+
+def test_drain_sync_applies_tool_and_shapes():
+ applied = []
+
+ class _FakeRobotModule:
+ @staticmethod
+ def apply_tool(tool_name, variant_key=""):
+ applied.append(("tool", tool_name, variant_key))
+
+ @staticmethod
+ def apply_shapes(shapes):
+ applied.append(("shapes", tuple(shapes)))
+
+ q = multiprocessing.Queue()
+ q.put(SyncTool(tool_name="ssg48", variant_key="v1"))
+ q.put(SyncShapes(shapes=("wire1", "wire2")))
+ deadline = time.monotonic() + 5.0
+ while len(applied) < 2 and time.monotonic() < deadline:
+ _drain_sync(q, _FakeRobotModule) # feeder-thread latency: poll until seen
+ time.sleep(0.005)
+ assert ("tool", "ssg48", "v1") in applied
+ assert ("shapes", ("wire1", "wire2")) in applied
+ assert _drain_sync(q, _FakeRobotModule) is False # empty → no-op
+ q.close()
+ q.cancel_join_thread()
From 7ad5814cc37544392267320ef5571ad1b73bb829 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Thu, 2 Jul 2026 09:21:19 -0400
Subject: [PATCH 24/36] Client Robot: sync tool geometry + shapes onto the
process checker
set_active_tool now refreshes the global checker's tool meshes and apply_shapes
mirrors the client's set_shapes locally, so host-side collision queries
(editing-pose highlight, dry-run preview) see the same collision world as the
server.
Co-Authored-By: Claude Fable 5
---
parol6/robot.py | 34 ++++++++++++++++++++++++----------
1 file changed, 24 insertions(+), 10 deletions(-)
diff --git a/parol6/robot.py b/parol6/robot.py
index 23eabd7..6eb9b96 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -674,10 +674,9 @@ def set_active_tool(
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.
+ Also syncs the tool's collision meshes onto this process's global
+ checker so client-side collision queries (preview / editing pose)
+ see the attached tool.
"""
from parol6.tools import get_tool_transform
@@ -700,6 +699,11 @@ def set_active_tool(
else:
self._pinokin.clear_tool_transform()
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ # Registry-unknown (plugin) tools just clear the old geometry.
+ PAROL6_ROBOT._refresh_collision_tool_geometry(tool_key, variant_key=variant_key)
+
def _plugin_tool_transform(
self, tool_key: str, variant_key: str | None
) -> NDArray[np.float64]:
@@ -784,9 +788,9 @@ def fk_batch(self, joint_path_rad: NDArray[np.float64]) -> NDArray[np.float64]:
def _collision_checker(self):
"""The process-global checker, or None when collision checking is off.
- Shared by every collision method 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 Robot.
+ Shared by every collision method here; its tool geometry follows
+ ``PAROL6_ROBOT.apply_tool`` (server / dry-run) and
+ :meth:`set_active_tool` (client) calls in this process.
"""
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
@@ -815,9 +819,8 @@ 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). Tool
- geometry is present only when ``PAROL6_ROBOT.apply_tool`` attached it in
- this process (see :meth:`in_collision`).
+ ``"ssg48_body_simplified.stl"`` for the active tool's body mesh, or
+ ``"shape:"`` for a workspace keep-out shape).
"""
c = self._collision_checker
if c is None:
@@ -837,6 +840,17 @@ def min_distance(self, q_rad: NDArray[np.float64]) -> float:
self._load_q_buf(q_rad)
return c.min_distance(self._q_buf)
+ def apply_shapes(self, shapes) -> None:
+ """Apply keep-out shapes to this process's checker (preview/editing viz).
+
+ Local-only twin of the client's ``set_shapes`` (which updates the
+ server's checkers). Accepts waldoctl ``Shape`` objects.
+ """
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+ from parol6.protocol.wire import ShapeWire
+
+ PAROL6_ROBOT.apply_shapes([ShapeWire(*s.to_wire()) for s in shapes])
+
def ik_batch(
self,
poses: NDArray[np.float64],
From d362a0f49d210f535be894fbc2a9cbea421fff93 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Fri, 3 Jul 2026 20:56:53 -0400
Subject: [PATCH 25/36] Jog collision semantics: check the streamed config,
clamp lookahead, allow escape
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
JogJ only sampled the velocity-scaled lookahead, so anything inside the first
lookahead window at jog start was tunneled through unchecked; the lookahead
also evaluated poses past the joint limits, phantom-tripping near mechanical
stops. Both jogs now use a shared collision_blocked() in _collision_guard:
blocked when the next config collides, except when the arm is ALREADY inside
(a keep-out placed over it) — then only deeper motion is blocked, mirroring
guard_joint_path's start-in-collision escape rule. JogJ clamps the lookahead
to qlim in place.
Co-Authored-By: Claude Fable 5
---
parol6/commands/_collision_guard.py | 16 +++++++++++++++
parol6/commands/basic_commands.py | 29 ++++++++++++++++++---------
parol6/commands/cartesian_commands.py | 21 ++++++++++---------
3 files changed, 46 insertions(+), 20 deletions(-)
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index 13b7c7e..eb81a0c 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -25,6 +25,22 @@
_ESCAPE_TOL = 1e-4
+def collision_blocked(checker, current_q, target_q) -> bool:
+ """Whether streaming toward ``target_q`` must stop.
+
+ Approaching: blocked when ``target_q`` collides. Already colliding (a
+ keep-out placed over the arm, or a tool-attach overlap): blocked only when
+ the motion goes *deeper* — escaping is allowed, mirroring
+ :func:`guard_joint_path`'s start-in-collision rule.
+ """
+ if checker.in_collision(current_q):
+ return bool(
+ checker.min_distance(target_q)
+ < checker.min_distance(current_q) - _ESCAPE_TOL
+ )
+ return bool(checker.in_collision(target_q))
+
+
def _format_pairs(pairs: list[tuple[str, str]]) -> str:
"""Render colliding (name, name) pairs as a human-readable string.
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index ce03caa..798a21f 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -25,6 +25,7 @@
from parol6.protocol.wire import CommandCode
from parol6.server.command_registry import register_command
from parol6.server.state import ControllerState
+from parol6.commands._collision_guard import collision_blocked
from parol6.utils.error_catalog import make_error
from parol6.utils.error_codes import ErrorCode
from parol6.config import deg_to_steps
@@ -131,6 +132,7 @@ class JogJCommand(MotionCommand[JogJCmd]):
"_jog_initialized",
"_jog_vel_rad",
"_lookahead_buf",
+ "_qlim",
)
def __init__(self, p: JogJCmd):
@@ -139,6 +141,7 @@ def __init__(self, p: JogJCmd):
self._jog_initialized = False
self._jog_vel_rad = np.zeros(6, dtype=np.float64)
self._lookahead_buf = np.zeros(6, dtype=np.float64)
+ self._qlim: np.ndarray | None = None
def do_setup(self, state: "ControllerState") -> None:
"""Pre-compute step speeds and rad/s velocities for all 6 joints."""
@@ -159,6 +162,7 @@ def do_setup(self, state: "ControllerState") -> None:
)
self.start_timer(self.p.duration)
self._jog_initialized = False
+ self._qlim = PAROL6_ROBOT.robot.qlim
def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
"""Execute one tick of joint jogging via StreamingExecutor."""
@@ -193,22 +197,27 @@ 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.)
+ # Never stream a config that collides or approaches collision: the
+ # streamed config itself is checked (catches anything inside the
+ # lookahead window at jog start) plus a velocity-scaled horizon so
+ # faster jogs stop further from contact — both compose with the
+ # checker's fixed clearance. Exception: when the arm is ALREADY inside
+ # (a keep-out placed over it), escaping motion is allowed, mirroring
+ # the planner guard's start-in-collision semantics. An abrupt stop is
+ # acceptable when the alternative is driving deeper. (The Cartesian jog
+ # uses a graceful CSE-based stop; JogJ has no smoother, so it halts.)
checker = PAROL6_ROBOT.collision
if checker is not None:
- # Look a velocity-scaled horizon ahead so faster jogs stop further
- # from contact; composes with the checker's fixed clearance. In-place
- # to keep the hot path allocation-free.
+ # In-place to keep the hot path allocation-free; clamped to joint
+ # limits so a pose past the mechanical stop can't phantom-trip.
la = self._lookahead_buf
la[:] = self._jog_vel_rad
la *= COLLISION_JOG_LOOKAHEAD_S
la += pos_rad
- if checker.in_collision(la):
- logger.warning("[JOGJ] self-collision predicted - stopping jog")
+ if self._qlim is not None:
+ np.clip(la, self._qlim[0], self._qlim[1], out=la)
+ if collision_blocked(checker, pos_rad, la):
+ logger.warning("[JOGJ] collision predicted - stopping jog")
# Allocate only here (the rare stop), never on the clean tick.
state.collision_pairs = tuple(checker.colliding_pairs(la))
state.collision_active = True
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 66ea2e1..0dc3e3b 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -9,7 +9,7 @@
import numpy as np
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
-from parol6.commands._collision_guard import guard_joint_path
+from parol6.commands._collision_guard import collision_blocked, guard_joint_path
from parol6.config import (
CART_ANG_JOG_MIN,
CART_LIN_JOG_MIN,
@@ -226,21 +226,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
+ # 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. When the arm is
+ # ALREADY inside (a keep-out placed over it), escaping motion is
+ # allowed, mirroring the planner guard's start-in-collision semantics.
+ checker = PAROL6_ROBOT.collision
+ if checker is not None and collision_blocked(
+ checker, self._q_commanded, ik_result.q
):
if not self._ik_stopping:
_ik_warn(
logger,
- "[CARTJOG] self-collision predicted - initiating stop",
+ "[CARTJOG] collision predicted - initiating stop",
)
# Capture once on the stop transition (not every decel tick).
- state.collision_pairs = tuple(
- PAROL6_ROBOT.collision.colliding_pairs(ik_result.q)
- )
+ state.collision_pairs = tuple(checker.colliding_pairs(ik_result.q))
state.collision_active = True
cse.stop()
self._ik_stopping = True
From eed2702f7ef5fc2b902a9617f52adff65f7192a9 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Fri, 3 Jul 2026 20:56:54 -0400
Subject: [PATCH 26/36] Enablement gates: escape parity when inside a keep-out;
syncs can't kill the worker
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Inside a colliding config, the joint/cartesian gates greyed all 24 directions,
locking the UI with no way out — they now grey only directions that go deeper.
_drain_sync applies each geometry sync under its own exception guard: one bad
tool mesh or plugin import must not permanently kill the enablement worker.
Co-Authored-By: Claude Fable 5
---
parol6/server/ik_worker.py | 51 ++++++++++++++-----
tests/unit/test_collision_enablement.py | 66 ++++++++++++++++++++++++-
2 files changed, 103 insertions(+), 14 deletions(-)
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index cdd4bad..6b2f3e5 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -18,6 +18,7 @@
from pinokin import se3_from_trans, se3_mul, se3_rx, se3_ry, se3_rz
+from parol6.commands._collision_guard import _ESCAPE_TOL
from parol6.server.ik_layout import (
IK_INPUT_Q_OFFSET,
IK_INPUT_T_OFFSET,
@@ -57,19 +58,27 @@ class SyncShapes:
def _drain_sync(cmd_queue: "multiprocessing.Queue", robot_module) -> bool:
- """Apply queued geometry syncs to this process's checker; True if any applied."""
+ """Apply queued geometry syncs to this process's checker; True if any applied.
+
+ A failing sync (missing tool mesh, plugin import quirk in this spawned
+ process) is logged and skipped — it must never kill the worker, which
+ would freeze enablement for the rest of the session.
+ """
applied = False
while True:
try:
msg = cmd_queue.get_nowait()
except Empty:
return applied
- if isinstance(msg, SyncTool):
- robot_module.apply_tool(msg.tool_name, variant_key=msg.variant_key)
- applied = True
- elif isinstance(msg, SyncShapes):
- robot_module.apply_shapes(msg.shapes)
- applied = True
+ try:
+ if isinstance(msg, SyncTool):
+ robot_module.apply_tool(msg.tool_name, variant_key=msg.variant_key)
+ applied = True
+ elif isinstance(msg, SyncShapes):
+ robot_module.apply_shapes(msg.shapes)
+ applied = True
+ except Exception:
+ logger.exception("IK worker geometry sync failed for %r", msg)
def ik_enablement_worker_main(
@@ -274,16 +283,26 @@ def gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
"""Clear a joint direction in ``joint_en`` whose ``_ENABLE_STEP_RAD`` step
collides — self, tool, or keep-out shape. Proximity-gated (skip the
per-direction checks when the arm is farther than ``_ENABLE_NEAR_M`` from
- collision) so it stays cheap. Not njit — it calls the C++ checker.
+ collision) so it stays cheap. When the arm is ALREADY colliding (a keep-out
+ placed over it), escaping directions stay enabled — grey only those that go
+ deeper — mirroring the jog/planner escape semantics; otherwise every button
+ would grey with no way out. Not njit — it calls the C++ checker.
"""
- if checker is None or checker.min_distance(q_rad) >= _ENABLE_NEAR_M:
+ if checker is None:
+ return
+ md_now = checker.min_distance(q_rad)
+ if md_now >= _ENABLE_NEAR_M:
return
+ inside = checker.in_collision(q_rad)
for j in range(6):
for slot, sign in ((2 * j, 1.0), (2 * j + 1, -1.0)):
if joint_en[slot]:
q_step[:] = q_rad
q_step[j] += sign * _ENABLE_STEP_RAD
- if checker.in_collision(q_step):
+ if inside:
+ if checker.min_distance(q_step) < md_now - _ESCAPE_TOL:
+ joint_en[slot] = 0
+ elif checker.in_collision(q_step):
joint_en[slot] = 0
@@ -380,15 +399,21 @@ def _compute_cart_enable(
# Compute all 12 target poses in one numba call
_compute_target_poses(T, t_step, r_step, is_wrf, axis_dirs, targets)
- near = checker is not None and checker.min_distance(q_rad) < _ENABLE_NEAR_M
+ md_now = checker.min_distance(q_rad) if checker is not None else np.inf
+ near = md_now < _ENABLE_NEAR_M
+ # Already colliding: keep escaping directions enabled (see the joint gate).
+ inside = near and checker is not None and checker.in_collision(q_rad)
# Check IK (and, when near collision, the solved config) for each target
for i in range(12):
try:
ik = solve_ik(robot, targets[i], q_rad, quiet_logging=True)
ok = bool(ik.success)
- if ok and near and checker.in_collision(ik.q):
- ok = False
+ if ok and near and checker is not None:
+ if inside:
+ ok = checker.min_distance(ik.q) >= md_now - _ESCAPE_TOL
+ elif checker.in_collision(ik.q):
+ ok = False
out[i] = 1 if ok else 0
except Exception:
out[i] = 0
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
index 0f342a1..d25cabd 100644
--- a/tests/unit/test_collision_enablement.py
+++ b/tests/unit/test_collision_enablement.py
@@ -6,6 +6,7 @@
import numpy as np
+from parol6.commands._collision_guard import collision_blocked
from parol6.server.ik_worker import (
_AXIS_DIRS,
_ENABLE_NEAR_M,
@@ -18,13 +19,15 @@
class _FakeChecker:
+ """``distance`` may be a float or a callable(q) for escape-semantics tests."""
+
def __init__(self, distance, collides):
self._distance = distance
self._collides = collides
self.queries = 0
def min_distance(self, q):
- return self._distance
+ return self._distance(q) if callable(self._distance) else self._distance
def in_collision(self, q):
self.queries += 1
@@ -49,6 +52,30 @@ def test_gate_greys_only_the_colliding_direction():
assert joint_en[0] == 1 and joint_en[1] == 1 # J1 untouched
+def test_gate_keeps_escaping_directions_when_already_inside():
+ """Arm inside a keep-out: only deeper directions grey — never all of them."""
+ joint_en = np.ones(12, dtype=np.uint8)
+ # Penetrating (in_collision True at q); J1+ escapes (distance grows with
+ # q[0]), J1- digs deeper. Other joints don't change the distance.
+ checker = _FakeChecker(lambda q: -0.01 + q[0], lambda q: True)
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ assert joint_en[0] == 1 # J1+ escapes -> enabled
+ assert joint_en[1] == 0 # J1- deeper -> greyed
+ assert joint_en[2] == 1 and joint_en[3] == 1 # no-change dirs stay enabled
+
+
+def test_collision_blocked_escape_semantics():
+ """Shared jog/guard decision: approach blocks, escape is allowed."""
+ # Approaching: current clear, target colliding -> blocked.
+ approaching = _FakeChecker(0.02, lambda q: bool(q[0] > 0.5))
+ assert collision_blocked(approaching, np.zeros(6), np.full(6, 1.0)) is True
+ assert collision_blocked(approaching, np.zeros(6), np.full(6, 0.1)) is False
+ # Already inside: escape (distance grows) allowed, deeper blocked.
+ inside = _FakeChecker(lambda q: -0.01 + q[0], lambda q: True)
+ assert collision_blocked(inside, np.zeros(6), np.full(6, 0.1)) is False
+ assert collision_blocked(inside, np.zeros(6), np.full(6, -0.1)) is True
+
+
@dataclass
class _FakeIK:
success: bool
@@ -96,6 +123,18 @@ def solve_ik(robot, target, q_seed, quiet_logging=True):
assert checker.queries == 0
+def test_cart_gate_keeps_escaping_directions_when_already_inside():
+ def solve_ik(robot, target, q_seed, quiet_logging=True):
+ return _FakeIK(True, target[:3, 3].repeat(2))
+
+ # Penetrating everywhere; solved configs with +x motion escape (distance
+ # grows with q[0]), -x digs deeper.
+ checker = _FakeChecker(lambda q: -0.01 + q[0], lambda q: True)
+ out = _cart_enable(checker, solve_ik)
+ assert out[0] == 1 # X+ escapes -> enabled
+ assert out[1] == 0 # X- deeper -> greyed
+
+
def test_drain_sync_applies_tool_and_shapes():
applied = []
@@ -120,3 +159,28 @@ def apply_shapes(shapes):
assert _drain_sync(q, _FakeRobotModule) is False # empty → no-op
q.close()
q.cancel_join_thread()
+
+
+def test_drain_sync_survives_a_failing_apply():
+ """A raising sync is logged + skipped — the worker must not die."""
+ applied = []
+
+ class _RaisingModule:
+ @staticmethod
+ def apply_tool(tool_name, variant_key=""):
+ raise RuntimeError("mesh missing")
+
+ @staticmethod
+ def apply_shapes(shapes):
+ applied.append(tuple(shapes))
+
+ q = multiprocessing.Queue()
+ q.put(SyncTool(tool_name="broken"))
+ q.put(SyncShapes(shapes=("w",)))
+ deadline = time.monotonic() + 5.0
+ while not applied and time.monotonic() < deadline:
+ _drain_sync(q, _RaisingModule) # must not raise
+ time.sleep(0.005)
+ assert applied == [("w",)]
+ q.close()
+ q.cancel_join_thread()
From 203fcdf194ad0bdbbdffb4dd15a7511216bc36ea Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Fri, 3 Jul 2026 20:56:55 -0400
Subject: [PATCH 27/36] SET_SHAPES is a SystemCommand with explicit planner
sync; sync wrapper; dup-name validation
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Shapes are safety configuration, not motion: as a MotionCommand they were
rejected while disabled (e-stop) and applied controller-side via an
InlineSegment that any segment-player cancel — including every streamed jog
packet's intake — silently drained, splitting the planner and control-loop
checkers with no error. Now modeled on SELECT_PROFILE: immediate at intake,
mirrored to the planner via a SyncShapes message, re-synced at e-stop/reset
alongside sync_tool. The sync client gains the missing set_shapes wrapper, and
apply_shapes validates duplicate names before mutating (pinokin raises
mid-add, which would leave a half-applied collision world).
Co-Authored-By: Claude Fable 5
---
parol6/PAROL6_ROBOT.py | 11 ++++++---
parol6/client/sync_client.py | 4 ++++
parol6/commands/shape_commands.py | 11 +++++----
parol6/server/controller.py | 7 ++++++
parol6/server/motion_planner.py | 30 ++++++++++++++++++++----
tests/unit/test_collision_integration.py | 24 +++++++++++++++++++
6 files changed, 75 insertions(+), 12 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 8a95ffa..ce18c8c 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -304,12 +304,17 @@ def apply_shapes(shapes: "Iterable[Any]") -> None:
global _active_shape_names
if collision is None:
return
+ # Validate before touching the checker: a duplicate name would raise from
+ # pinokin mid-add, leaving a half-applied collision world.
+ active = [s for s in shapes if s.collision]
+ names = [s.name for s in active]
+ if len(set(names)) != len(names):
+ dups = sorted({n for n in names if names.count(n) > 1})
+ raise ValueError(f"Duplicate shape name(s): {', '.join(dups)}")
for name in _active_shape_names:
collision.remove_geometry_by_name(name)
_active_shape_names = []
- for s in shapes:
- if not s.collision:
- continue
+ for s in active:
name = f"shape:{s.name}"
collision.add_obstacle(name, s.kind, list(s.params), _pose_to_matrix(s.pose))
_active_shape_names.append(name)
diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py
index 0febb7e..8ce489f 100644
--- a/parol6/client/sync_client.py
+++ b/parol6/client/sync_client.py
@@ -323,6 +323,10 @@ def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int:
"""Set TCP offset in mm, composed on top of the current tool transform."""
return _run(self._inner.set_tcp_offset(x=x, y=y, z=z))
+ def set_shapes(self, shapes: list) -> int:
+ """Replace the workspace collision-world shapes (keep-out barriers)."""
+ return _run(self._inner.set_shapes(shapes))
+
def tcp_offset(self) -> list[float]:
"""Query current TCP offset in mm [x, y, z]."""
return _run(self._inner.tcp_offset())
diff --git a/parol6/commands/shape_commands.py b/parol6/commands/shape_commands.py
index 58f3a97..989e1df 100644
--- a/parol6/commands/shape_commands.py
+++ b/parol6/commands/shape_commands.py
@@ -4,7 +4,7 @@
from typing import TYPE_CHECKING
-from parol6.commands.base import ExecutionStatusCode, MotionCommand
+from parol6.commands.base import ExecutionStatusCode, SystemCommand
from parol6.protocol.wire import CmdType, SetShapesCmd
from parol6.server.command_registry import register_command
@@ -13,12 +13,13 @@
@register_command(CmdType.SET_SHAPES)
-class SetShapesCommand(MotionCommand[SetShapesCmd]):
+class SetShapesCommand(SystemCommand[SetShapesCmd]):
"""Replace the workspace keep-out shapes on the collision checkers.
- Routed through the planner (like SELECT_TOOL) so the planner subprocess
- updates its own checker; this controller-side step updates the live
- control-loop checker.
+ A SystemCommand (like SELECT_PROFILE): safety configuration applies
+ immediately at intake — never enabled-gated, never queued behind (or
+ dropped with) motion. The controller mirrors the applied shapes to the
+ planner subprocess via ``sync_shapes`` after this executes.
"""
PARAMS_TYPE = SetShapesCmd
diff --git a/parol6/server/controller.py b/parol6/server/controller.py
index 40fb42d..c0341cf 100644
--- a/parol6/server/controller.py
+++ b/parol6/server/controller.py
@@ -22,6 +22,7 @@
QueryCommand,
SystemCommand,
)
+from parol6.commands.shape_commands import SetShapesCommand
from parol6.commands.system_commands import SelectProfileCommand
from parol6.commands.utility_commands import ResetCommand
from parol6.server.command_executor import CommandExecutor, QueueFullError
@@ -360,6 +361,7 @@ def _handle_estop(self, state: ControllerState) -> None:
variant_key=state.current_tool_variant,
tcp_offset_m=state.tcp_offset_m,
)
+ self._planner.sync_shapes(state.shapes)
if self._executor.active_command:
self._executor.cancel_active_command("E-Stop activated")
self._executor.clear_queue("E-Stop activated")
@@ -807,6 +809,7 @@ def _handle_system_command(
variant_key=state.current_tool_variant,
tcp_offset_m=state.tcp_offset_m,
)
+ self._planner.sync_shapes(state.shapes)
# Infrastructure side effects (only 2-3 commands trigger these)
if command._switch_simulator is not None:
@@ -829,6 +832,10 @@ def _handle_system_command(
if isinstance(command, SelectProfileCommand):
self._planner.sync_profile(state.motion_profile)
+ # Mirror applied keep-out shapes to the planner's checker
+ if isinstance(command, SetShapesCommand):
+ self._planner.sync_shapes(state.shapes)
+
if code == ExecutionStatusCode.COMPLETED:
self._reply_ok(addr)
else:
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index f480906..9b165dc 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -26,7 +26,6 @@
from parol6.protocol.wire import (
HomeCmd,
SelectToolCmd,
- SetShapesCmd,
SetTcpOffsetCmd,
ToolActionCmd,
)
@@ -119,12 +118,21 @@ class SyncTool:
tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0)
+@dataclass
+class SyncShapes:
+ """Replace the planner checker's workspace keep-out shapes (ShapeWire list)."""
+
+ shapes: list
+
+
@dataclass
class CancelAll:
"""Clear the planner's internal state and discard pending work."""
-PlannerMessage = Union[PlanCommand, SyncPosition, SyncProfile, SyncTool, CancelAll]
+PlannerMessage = Union[
+ PlanCommand, SyncPosition, SyncProfile, SyncTool, SyncShapes, CancelAll
+]
# ---------------------------------------------------------------------------
@@ -247,6 +255,10 @@ def sync_tool(
tool_name, variant_key=variant_key, tcp_offset_m=tcp_offset_m
)
+ def sync_shapes(self, shapes: list) -> None:
+ """Replace this process checker's workspace keep-out shapes."""
+ self._robot_module.apply_shapes(shapes)
+
# -- trajectory handling --
def _handle_trajectory(
@@ -469,8 +481,6 @@ def _handle_inline(self, command_index: int, params: object) -> None:
)
elif isinstance(params, HomeCmd):
self.state.Position_in[:] = self._home_steps
- elif isinstance(params, SetShapesCmd):
- self._robot_module.apply_shapes(params.shapes)
# ---------------------------------------------------------------------------
@@ -523,6 +533,10 @@ def apply_tool(
tool_name, variant_key=variant_key, tcp_offset_m=tcp_offset_m
)
+ def apply_shapes(self, shapes: list) -> None:
+ """Sync workspace keep-out shapes onto this process's checker."""
+ self._planner.sync_shapes(shapes)
+
# ---------------------------------------------------------------------------
# Worker process entry point
@@ -607,6 +621,10 @@ def motion_planner_main(
)
continue
+ if isinstance(msg, SyncShapes):
+ worker.apply_shapes(msg.shapes)
+ continue
+
if isinstance(msg, PlanCommand):
try:
worker.process_command(msg)
@@ -756,6 +774,10 @@ def sync_tool(
)
)
+ def sync_shapes(self, shapes: list) -> None:
+ """Replace the planner checker's workspace keep-out shapes."""
+ self.submit(SyncShapes(shapes=list(shapes)))
+
def cancel(self) -> None:
"""Cancel all pending work in the planner."""
self.submit(CancelAll())
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index 106d5be..9b056bd 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -193,3 +193,27 @@ def pct(a: list[int], p: float) -> float:
f" max={pct(t_pairs, 100):.1f}"
)
print("servo tick budget: 10000 us (100 Hz)")
+
+
+def test_apply_shapes_rejects_duplicate_names_without_mutation():
+ """Duplicate names fail fast — never a half-applied collision world."""
+ from parol6.protocol.wire import ShapeWire
+
+ def wire(name: str) -> ShapeWire:
+ return ShapeWire(
+ kind="box",
+ params=[0.1, 0.1, 0.1],
+ pose=[1.0, 1.0, 1.0, 0, 0, 0],
+ collision=True,
+ margin=None,
+ name=name,
+ )
+
+ try:
+ PAROL6_ROBOT.apply_shapes([wire("a")])
+ with pytest.raises(ValueError, match="Duplicate shape name"):
+ PAROL6_ROBOT.apply_shapes([wire("b"), wire("b")])
+ # The prior world is untouched by the rejected call.
+ assert PAROL6_ROBOT._active_shape_names == ["shape:a"]
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
From fbde3043df1ca4fcca6125bc0905f06b79e4bdbc Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sat, 4 Jul 2026 15:45:58 -0400
Subject: [PATCH 28/36] Round-3 review fixes: shape validation, dry-run parity,
hot-path allocs, gate clamp
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- apply_shapes fully pre-validates (kind, param count, finite 6-pose, name
uniqueness across ALL shapes incl. visual-only) before the checker-None
return and before removing the old world — a malformed SET_SHAPES could
previously reply error while silently disarming every enforced barrier.
- The dry-run planner re-applies SetShapesCmd inline (only reachable there;
the live path routes SET_SHAPES as a SystemCommand + SyncShapes), so a
script's set_shapes() shapes its preview world instead of being swallowed.
- set_active_tool guards the collision-geometry refresh: a plugin tool whose
meshes live outside parol6's mesh root must not break tool selection after
the kinematic transform applied.
- JogJ joint limits come from a once-per-process module cache — robot.qlim
allocates a fresh matrix per access and do_setup re-runs per streamed
packet, and the per-tick row indexing allocated view objects.
- The enablement gate's ±2° probe is clamped to qlim (like JogJ's lookahead),
so geometry just past a mechanical stop can't grey a direction the jog
itself would permit.
Co-Authored-By: Claude Fable 5
---
parol6/PAROL6_ROBOT.py | 56 ++++++++++++++++++++----
parol6/commands/basic_commands.py | 25 ++++++++---
parol6/robot.py | 14 +++++-
parol6/server/ik_worker.py | 23 ++++++++--
parol6/server/motion_planner.py | 6 +++
tests/unit/test_collision_enablement.py | 17 +++++++
tests/unit/test_collision_integration.py | 50 ++++++++++++++-------
7 files changed, 156 insertions(+), 35 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index ce18c8c..c291d05 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -2,6 +2,7 @@
import atexit
import logging
+import math
import os
from collections.abc import Iterable, Sequence
from dataclasses import dataclass
@@ -293,28 +294,65 @@ def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
return T
+# Mirrors pinokin add_obstacle's kind switch — validated here so a bad shape
+# raises BEFORE the old collision world is removed.
+_SHAPE_PARAM_COUNTS = {
+ "box": 3,
+ "sphere": 1,
+ "cylinder": 2,
+ "capsule": 2,
+ "cone": 2,
+ "ellipsoid": 3,
+ "plane": 4,
+}
+
+
+def _validate_shapes(shapes: "Iterable[Any]") -> "list[Any]":
+ """Raise ValueError on any invalid shape; return the materialized list.
+
+ Covers ALL shapes (visual-only included): a marker sharing a keep-out's
+ name would shadow it in the frontend's highlight mapping, and a malformed
+ wire must never reach the checker mid-apply.
+ """
+ shapes = list(shapes)
+ names = [s.name for s in shapes]
+ if len(set(names)) != len(names):
+ dups = sorted({n for n in names if names.count(n) > 1})
+ raise ValueError(f"Duplicate shape name(s): {', '.join(dups)}")
+ for s in shapes:
+ expected = _SHAPE_PARAM_COUNTS.get(s.kind)
+ if expected is None:
+ raise ValueError(f"Shape {s.name!r}: unknown kind {s.kind!r}")
+ if len(s.params) != expected:
+ raise ValueError(
+ f"Shape {s.name!r}: kind {s.kind!r} takes {expected} param(s), "
+ f"got {len(s.params)}"
+ )
+ if len(s.pose) != 6 or not all(math.isfinite(v) for v in s.pose):
+ raise ValueError(f"Shape {s.name!r}: pose must be 6 finite numbers")
+ return shapes
+
+
def apply_shapes(shapes: "Iterable[Any]") -> None:
"""Replace the workspace collision-world shapes on this process's checker.
Each shape exposes ``kind``/``params``/``pose``/``collision``/``name`` (a
parol6 ``ShapeWire`` or any duck-typed equivalent). Only collision-enabled
shapes are added — visual-only ones are skipped. Runs per-process; the
- controller and planner each call it against their own checker.
+ controller and planner each call it against their own checker. Validation
+ runs even without a checker: checker-off frontends rely on it to reject a
+ shape set the backend would refuse.
"""
global _active_shape_names
+ shapes = _validate_shapes(shapes)
if collision is None:
return
- # Validate before touching the checker: a duplicate name would raise from
- # pinokin mid-add, leaving a half-applied collision world.
- active = [s for s in shapes if s.collision]
- names = [s.name for s in active]
- if len(set(names)) != len(names):
- dups = sorted({n for n in names if names.count(n) > 1})
- raise ValueError(f"Duplicate shape name(s): {', '.join(dups)}")
for name in _active_shape_names:
collision.remove_geometry_by_name(name)
_active_shape_names = []
- for s in active:
+ for s in shapes:
+ if not s.collision:
+ continue
name = f"shape:{s.name}"
collision.add_obstacle(name, s.kind, list(s.params), _pose_to_matrix(s.pose))
_active_shape_names.append(name)
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 798a21f..0209b49 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -40,6 +40,24 @@
logger = logging.getLogger(__name__)
+_QLIM_ROWS: tuple[np.ndarray, np.ndarray] | None = None
+
+
+def _qlim_rows() -> tuple[np.ndarray, np.ndarray]:
+ """Joint-limit rows, fetched once per process — ``robot.qlim`` allocates a
+ fresh matrix per access and this is consumed on the 100 Hz jog path."""
+ global _QLIM_ROWS
+ if _QLIM_ROWS is None:
+ qlim = PAROL6_ROBOT.robot.qlim
+ if qlim is None:
+ _QLIM_ROWS = (np.full(6, -np.inf), np.full(6, np.inf))
+ else:
+ _QLIM_ROWS = (
+ np.ascontiguousarray(qlim[0], dtype=np.float64),
+ np.ascontiguousarray(qlim[1], dtype=np.float64),
+ )
+ return _QLIM_ROWS
+
def _limit_hit_mask(pos_steps: np.ndarray, speeds: np.ndarray) -> np.ndarray:
return ((speeds > 0) & (pos_steps >= LIMITS.joint.position.steps[:, 1])) | (
@@ -132,7 +150,6 @@ class JogJCommand(MotionCommand[JogJCmd]):
"_jog_initialized",
"_jog_vel_rad",
"_lookahead_buf",
- "_qlim",
)
def __init__(self, p: JogJCmd):
@@ -141,7 +158,6 @@ def __init__(self, p: JogJCmd):
self._jog_initialized = False
self._jog_vel_rad = np.zeros(6, dtype=np.float64)
self._lookahead_buf = np.zeros(6, dtype=np.float64)
- self._qlim: np.ndarray | None = None
def do_setup(self, state: "ControllerState") -> None:
"""Pre-compute step speeds and rad/s velocities for all 6 joints."""
@@ -162,7 +178,6 @@ def do_setup(self, state: "ControllerState") -> None:
)
self.start_timer(self.p.duration)
self._jog_initialized = False
- self._qlim = PAROL6_ROBOT.robot.qlim
def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
"""Execute one tick of joint jogging via StreamingExecutor."""
@@ -214,8 +229,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
la[:] = self._jog_vel_rad
la *= COLLISION_JOG_LOOKAHEAD_S
la += pos_rad
- if self._qlim is not None:
- np.clip(la, self._qlim[0], self._qlim[1], out=la)
+ lo, hi = _qlim_rows()
+ np.clip(la, lo, hi, out=la)
if collision_blocked(checker, pos_rad, la):
logger.warning("[JOGJ] collision predicted - stopping jog")
# Allocate only here (the rare stop), never on the clean tick.
diff --git a/parol6/robot.py b/parol6/robot.py
index 6eb9b96..01a279b 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -701,8 +701,18 @@ def set_active_tool(
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
- # Registry-unknown (plugin) tools just clear the old geometry.
- PAROL6_ROBOT._refresh_collision_tool_geometry(tool_key, variant_key=variant_key)
+ # Best-effort viz/preview parity: registry-unknown (plugin) tools just
+ # clear the old geometry, and a mesh-attach failure (e.g. a plugin tool
+ # whose mesh files live outside parol6's mesh root) must never break
+ # tool selection — the kinematic transform above is already applied.
+ try:
+ PAROL6_ROBOT._refresh_collision_tool_geometry(
+ tool_key, variant_key=variant_key
+ )
+ except Exception as e:
+ logger.warning(
+ "Tool collision geometry not attached for %r: %s", tool_key, e
+ )
def _plugin_tool_transform(
self, tool_key: str, variant_key: str | None
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index 6b2f3e5..e0eb763 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -167,6 +167,14 @@ def ik_enablement_worker_main(
robot = PAROL6_ROBOT.robot
qlim = robot.qlim
+ qlim_rows = (
+ (
+ np.ascontiguousarray(qlim[0], dtype=np.float64),
+ np.ascontiguousarray(qlim[1], dtype=np.float64),
+ )
+ if qlim is not None
+ else None
+ )
response_version = 0
have_request = False
@@ -206,7 +214,9 @@ def ik_enablement_worker_main(
_compute_joint_enable(q_rad, qlim, joint_en)
# else: joint_en stays all ones (pre-allocated default)
checker = PAROL6_ROBOT.collision
- gate_joint_enable_collision(checker, q_rad, joint_en, q_step)
+ gate_joint_enable_collision(
+ checker, q_rad, joint_en, q_step, qlim=qlim_rows
+ )
# Compute cartesian enablement for both frames
_compute_cart_enable(
@@ -279,14 +289,16 @@ def _compute_joint_enable(
out[i * 2 + 1] = 1 if (q_rad[i] - delta_rad) >= qlim[0, i] else 0
-def gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
+def gate_joint_enable_collision(checker, q_rad, joint_en, q_step, qlim=None) -> None:
"""Clear a joint direction in ``joint_en`` whose ``_ENABLE_STEP_RAD`` step
collides — self, tool, or keep-out shape. Proximity-gated (skip the
per-direction checks when the arm is farther than ``_ENABLE_NEAR_M`` from
collision) so it stays cheap. When the arm is ALREADY colliding (a keep-out
placed over it), escaping directions stay enabled — grey only those that go
deeper — mirroring the jog/planner escape semantics; otherwise every button
- would grey with no way out. Not njit — it calls the C++ checker.
+ would grey with no way out. ``qlim`` is optional ``(low, high)`` rows: the
+ probe is clamped so a pose past the mechanical stop (which the jog itself
+ can never reach) can't grey the button. Not njit — it calls the C++ checker.
"""
if checker is None:
return
@@ -299,6 +311,11 @@ def gate_joint_enable_collision(checker, q_rad, joint_en, q_step) -> None:
if joint_en[slot]:
q_step[:] = q_rad
q_step[j] += sign * _ENABLE_STEP_RAD
+ if qlim is not None:
+ if q_step[j] < qlim[0][j]:
+ q_step[j] = qlim[0][j]
+ elif q_step[j] > qlim[1][j]:
+ q_step[j] = qlim[1][j]
if inside:
if checker.min_distance(q_step) < md_now - _ESCAPE_TOL:
joint_en[slot] = 0
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index 9b165dc..b809336 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -26,6 +26,7 @@
from parol6.protocol.wire import (
HomeCmd,
SelectToolCmd,
+ SetShapesCmd,
SetTcpOffsetCmd,
ToolActionCmd,
)
@@ -481,6 +482,11 @@ def _handle_inline(self, command_index: int, params: object) -> None:
)
elif isinstance(params, HomeCmd):
self.state.Position_in[:] = self._home_steps
+ elif isinstance(params, SetShapesCmd):
+ # Only reachable via the DRY-RUN planner: a script's set_shapes()
+ # must shape its preview world. The live path routes SET_SHAPES as
+ # a SystemCommand + SyncShapes, never through process().
+ self._robot_module.apply_shapes(params.shapes)
# ---------------------------------------------------------------------------
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
index d25cabd..3581bcd 100644
--- a/tests/unit/test_collision_enablement.py
+++ b/tests/unit/test_collision_enablement.py
@@ -52,6 +52,23 @@ def test_gate_greys_only_the_colliding_direction():
assert joint_en[0] == 1 and joint_en[1] == 1 # J1 untouched
+def test_gate_clamps_probe_to_joint_limits():
+ """Geometry just past a mechanical stop must not grey a direction the jog
+ (whose own lookahead clamps to qlim) would actually permit."""
+ joint_en = np.ones(12, dtype=np.uint8)
+ # Near collision; anything beyond J1's upper limit collides.
+ limit = np.radians(1.0)
+ checker = _FakeChecker(_ENABLE_NEAR_M - 0.01, lambda q: q[0] > limit)
+ qlim = (np.full(6, -np.pi), np.full(6, np.pi).copy())
+ qlim[1][0] = limit # J1 upper limit right at the collision boundary
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6), qlim=qlim)
+ assert joint_en[0] == 1 # J1+ clamped to the limit -> no phantom grey
+ # Without the clamp the unreachable probe greys the button.
+ joint_en[:] = 1
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ assert joint_en[0] == 0
+
+
def test_gate_keeps_escaping_directions_when_already_inside():
"""Arm inside a keep-out: only deeper directions grey — never all of them."""
joint_en = np.ones(12, dtype=np.uint8)
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index 9b056bd..7f902aa 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -195,25 +195,43 @@ def pct(a: list[int], p: float) -> float:
print("servo tick budget: 10000 us (100 Hz)")
-def test_apply_shapes_rejects_duplicate_names_without_mutation():
- """Duplicate names fail fast — never a half-applied collision world."""
+def _shape_wire(name: str, **overrides):
from parol6.protocol.wire import ShapeWire
- def wire(name: str) -> ShapeWire:
- return ShapeWire(
- kind="box",
- params=[0.1, 0.1, 0.1],
- pose=[1.0, 1.0, 1.0, 0, 0, 0],
- collision=True,
- margin=None,
- name=name,
- )
+ fields = dict(
+ kind="box",
+ params=[0.1, 0.1, 0.1],
+ pose=[1.0, 1.0, 1.0, 0, 0, 0],
+ collision=True,
+ margin=None,
+ name=name,
+ )
+ fields.update(overrides)
+ return ShapeWire(**fields)
+
+def test_apply_shapes_rejects_invalid_input_without_mutation():
+ """Any invalid shape fails fast — never a half-applied collision world.
+
+ Critically, the OLD world must survive a rejected call: the remove-then-add
+ loop only runs after validation, so an error reply can't silently disarm
+ every barrier.
+ """
+ bad_sets = [
+ ("Duplicate shape name", [_shape_wire("b"), _shape_wire("b")]),
+ # A visual-only marker sharing a keep-out's name shadows it in the
+ # frontend's highlight mapping — rejected too.
+ ("Duplicate shape name", [_shape_wire("b"), _shape_wire("b", collision=False)]),
+ ("unknown kind", [_shape_wire("b", kind="torus")]),
+ ("takes 3 param", [_shape_wire("b", params=[0.1])]),
+ ("6 finite numbers", [_shape_wire("b", pose=[0, 0, 0, 0, 0])]),
+ ("6 finite numbers", [_shape_wire("b", pose=[0, 0, float("nan"), 0, 0, 0])]),
+ ]
try:
- PAROL6_ROBOT.apply_shapes([wire("a")])
- with pytest.raises(ValueError, match="Duplicate shape name"):
- PAROL6_ROBOT.apply_shapes([wire("b"), wire("b")])
- # The prior world is untouched by the rejected call.
- assert PAROL6_ROBOT._active_shape_names == ["shape:a"]
+ PAROL6_ROBOT.apply_shapes([_shape_wire("a")])
+ for match, shapes in bad_sets:
+ with pytest.raises(ValueError, match=match):
+ PAROL6_ROBOT.apply_shapes(shapes)
+ assert PAROL6_ROBOT._active_shape_names == ["shape:a"]
finally:
PAROL6_ROBOT.apply_shapes([])
From 7560ba86661dcbf87b8bf4d0cfc6292596840004 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sun, 5 Jul 2026 17:37:57 -0400
Subject: [PATCH 29/36] Shapes: canonical Shape in-process, acked SET_SHAPES,
readback, installation layer, margin, display vocabulary
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- SET_SHAPES decodes to waldoctl Shapes at intake (the codec boundary);
apply_shapes / SyncShapes / the planner all speak Shape — a preview
script's set_shapes works end-to-end (previously TypeError)
- SET_SHAPES joins SYSTEM_CMD_TYPES: 1 confirmed / 0 timeout, raises on
rejection; new SHAPES readback query; scene_epoch in the STATUS wire
- config INSTALLATION_SHAPES: an install: layer every program inherits,
untouched by set_shapes, loaded per-process at import
- apply_shapes plumbs the per-shape margin into the checker; set-level
validation only (values enforced at Shape construction)
- colliding pairs report URDF link names + shape:/install:/tool: names,
never checker-internal identifiers; tool geometry attached as
tool::
- JogL release deceleration uses the escape-aware gate — the target no
longer freezes when releasing while escaping a keep-out
- _pose_to_matrix documented against the waldoctl RPY contract
- CLAUDE.md: no-testing-theatre rules, no declared-but-unimplemented API
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
CLAUDE.md | 11 +
parol6/PAROL6_ROBOT.py | 131 ++++++++----
parol6/ack_policy.py | 2 +
parol6/client/async_client.py | 39 +++-
parol6/client/dry_run_client.py | 13 ++
parol6/client/sync_client.py | 11 +-
parol6/commands/_collision_guard.py | 3 +
parol6/commands/basic_commands.py | 4 +-
parol6/commands/cartesian_commands.py | 15 +-
parol6/commands/query_commands.py | 30 +++
parol6/commands/shape_commands.py | 15 +-
parol6/config.py | 15 ++
parol6/protocol/wire.py | 42 +++-
parol6/robot.py | 20 +-
parol6/server/ik_worker.py | 2 +-
parol6/server/motion_planner.py | 6 +-
parol6/server/state.py | 6 +-
parol6/server/status_cache.py | 3 +
tests/integration/test_shapes_e2e.py | 46 ++++
tests/unit/test_collision_integration.py | 261 +++++++++++++++++++----
20 files changed, 562 insertions(+), 113 deletions(-)
create mode 100644 tests/integration/test_shapes_e2e.py
diff --git a/CLAUDE.md b/CLAUDE.md
index 6eba9e4..8dd1a55 100644
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -132,6 +132,7 @@ For streamable commands (`streamable = True`), `do_setup()` also runs at high fr
## Code Style
- **Comments**: Describe the final code state, not what changed. Avoid "changed X to Y" or "added this because..." comments.
+- **Never ship declared-but-unimplemented API surface.** No "reserved" fields or params, no docstrings saying "not yet applied" — implement it or don't declare it.
- **Type annotations**: Fix type errors properly instead of using `# type: ignore`. Prefer:
- `@overload` decorators for functions with different return types based on input
- `assert` statements to narrow types after None checks
@@ -148,3 +149,13 @@ Prefer fewer, comprehensive integration tests that mimic manual testing over a l
- **Test results are in `test-results.xml`.** Pytest writes JUnit XML to `test-results.xml` automatically. When diagnosing failures, read this file — it contains test names, durations, failure messages, and captured output. This is more reliable than parsing console output.
- **NEVER run parol6 and web commander test suites in parallel** — no proper isolation, they share resources and have timing issues when resource-constrained. Always run sequentially.
- **NEVER allow subagents to run tests.** Many tests are timing-sensitive and the system doesn't have enough resources for agents and tests to run simultaneously. Only the main conversation should run tests, and only after all agents have completed.
+
+### No testing theatre
+
+- Default to real components (fake-serial controller, simulator). A hand-rolled fake is a last resort.
+- Never fake a contract you haven't read — match the real code's raise-vs-return behavior, return types/codes, and signatures.
+- If a fake must mimic protocol behavior (acks, return codes, ordering, lifecycle), the test is at the wrong layer — use the real path. Fakes are for one-line oracles only (e.g. a distance function feeding gate-logic tests).
+- Enter through the real path (command dispatch / client call), not internal helpers fed hand-built inputs.
+- Derive cases from the requirement, not the code under test — "rejects invalid input" means NaN/inf/negative/zero, not the cases the code already handles.
+- Assert outcomes, not interactions — "the fake was called" proves nothing.
+- A regression test must fail against the bug before the fix. Born-green regression tests are theatre.
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index c291d05..a1e2f51 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -2,7 +2,6 @@
import atexit
import logging
-import math
import os
from collections.abc import Iterable, Sequence
from dataclasses import dataclass
@@ -163,8 +162,15 @@ def _init_collision_checker(
_active_tool_geom_key: tuple[str, str | None] | None = None
# Geometry-object names for user-placed workspace shapes (keep-out barriers)
-# currently on this process's collision checker.
+# currently on this process's collision checker, plus the applied Shape list
+# itself for readback.
_active_shape_names: list[str] = []
+_program_shapes: list = []
+
+# Installation-layer shapes (from robot config) and their geometry names.
+# Every program inherits these; set_shapes cannot change them.
+_installation_shapes: list = []
+_installation_geom_names: list[str] = []
def _refresh_collision_tool_geometry(
@@ -216,13 +222,17 @@ def _refresh_collision_tool_geometry(
# rotation branch here when a non-identity rpy appears.
T = np.eye(4, dtype=np.float64)
T[:3, 3] = spec.origin
+ # tool: namespace so colliding-pair reports speak the defined
+ # vocabulary (link names / shape: / install: / tool:) instead
+ # of raw mesh filenames.
+ geom_name = f"tool:{tool_key}:{spec.file}"
collision.attach_mesh_to_frame(
- spec.file,
+ geom_name,
str(path),
parent_frame="L6",
placement=T,
)
- _active_tool_geom_names.append(spec.file)
+ _active_tool_geom_names.append(geom_name)
except Exception:
# Roll back a partial attach so the checker never holds half a tool.
for name in _active_tool_geom_names:
@@ -273,9 +283,10 @@ def apply_tool(
def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
"""[x, y, z, rx, ry, rz] (m, rad RPY) -> 4x4 world transform (R = Rz·Ry·Rx).
- Rz·Ry·Rx matches the frontend's shape render rotation (NiceGUI ``rotate``,
- XYZ order). Deliberately NOT ``pinokin.se3_from_rpy`` (Rx·Ry·Rz) — swapping
- it in would mis-orient any multi-axis-tilted shape vs its rendered pose.
+ Implements the waldoctl ``Shape.pose`` contract: extrinsic-XYZ RPY, i.e.
+ R = Rz·Ry·Rx. Deliberately NOT ``pinokin.se3_from_rpy`` (Rx·Ry·Rz) —
+ swapping it in would mis-orient any multi-axis-tilted shape versus every
+ other implementation of the contract (including the frontend's renderer).
"""
x, y, z, rx, ry, rz = pose
cx, sx = np.cos(rx), np.sin(rx)
@@ -294,25 +305,20 @@ def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
return T
-# Mirrors pinokin add_obstacle's kind switch — validated here so a bad shape
-# raises BEFORE the old collision world is removed.
-_SHAPE_PARAM_COUNTS = {
- "box": 3,
- "sphere": 1,
- "cylinder": 2,
- "capsule": 2,
- "cone": 2,
- "ellipsoid": 3,
- "plane": 4,
-}
+# Kinds pinokin's add_obstacle supports. Guards atomicity across a
+# waldoctl/pinokin version skew: a kind the checker would reject raises here,
+# BEFORE the old collision world is removed.
+_SHAPE_KINDS = frozenset(
+ {"box", "sphere", "cylinder", "capsule", "cone", "ellipsoid", "plane"}
+)
def _validate_shapes(shapes: "Iterable[Any]") -> "list[Any]":
- """Raise ValueError on any invalid shape; return the materialized list.
+ """Set-level validation; per-shape values are enforced at Shape construction.
Covers ALL shapes (visual-only included): a marker sharing a keep-out's
- name would shadow it in the frontend's highlight mapping, and a malformed
- wire must never reach the checker mid-apply.
+ name would shadow it in the frontend's highlight mapping. Raises before
+ any mutation so an error can never leave a half-applied world.
"""
shapes = list(shapes)
names = [s.name for s in shapes]
@@ -320,31 +326,25 @@ def _validate_shapes(shapes: "Iterable[Any]") -> "list[Any]":
dups = sorted({n for n in names if names.count(n) > 1})
raise ValueError(f"Duplicate shape name(s): {', '.join(dups)}")
for s in shapes:
- expected = _SHAPE_PARAM_COUNTS.get(s.kind)
- if expected is None:
+ if s.kind not in _SHAPE_KINDS:
raise ValueError(f"Shape {s.name!r}: unknown kind {s.kind!r}")
- if len(s.params) != expected:
- raise ValueError(
- f"Shape {s.name!r}: kind {s.kind!r} takes {expected} param(s), "
- f"got {len(s.params)}"
- )
- if len(s.pose) != 6 or not all(math.isfinite(v) for v in s.pose):
- raise ValueError(f"Shape {s.name!r}: pose must be 6 finite numbers")
return shapes
def apply_shapes(shapes: "Iterable[Any]") -> None:
- """Replace the workspace collision-world shapes on this process's checker.
-
- Each shape exposes ``kind``/``params``/``pose``/``collision``/``name`` (a
- parol6 ``ShapeWire`` or any duck-typed equivalent). Only collision-enabled
- shapes are added — visual-only ones are skipped. Runs per-process; the
- controller and planner each call it against their own checker. Validation
- runs even without a checker: checker-off frontends rely on it to reject a
- shape set the backend would refuse.
+ """Replace the program-layer collision-world shapes on this process's checker.
+
+ Takes waldoctl ``Shape`` objects (the canonical in-process type — wire
+ conversion happens only at the protocol codec). Only collision-enabled
+ shapes are added — visual-only ones are skipped. Installation-layer shapes
+ (``install:`` names, from robot config) are never touched. Runs
+ per-process; the controller and planner each call it against their own
+ checker. Validation runs even without a checker: checker-off frontends
+ rely on it to reject a shape set the backend would refuse.
"""
- global _active_shape_names
+ global _active_shape_names, _program_shapes
shapes = _validate_shapes(shapes)
+ _program_shapes = shapes
if collision is None:
return
for name in _active_shape_names:
@@ -354,10 +354,61 @@ def apply_shapes(shapes: "Iterable[Any]") -> None:
if not s.collision:
continue
name = f"shape:{s.name}"
- collision.add_obstacle(name, s.kind, list(s.params), _pose_to_matrix(s.pose))
+ collision.add_obstacle(
+ name, s.kind, s.params(), _pose_to_matrix(s.pose), margin=s.margin
+ )
_active_shape_names.append(name)
+def apply_installation_shapes(shapes: "Iterable[Any]") -> None:
+ """Replace the installation-layer shapes (from robot config) on this
+ process's checker.
+
+ Applied at import via ``parol6.config.INSTALLATION_SHAPES``, so every
+ process (controller, planner, ik-worker, dry-run) inherits the same
+ installation world without any sync. ``set_shapes`` cannot touch these.
+ """
+ global _installation_shapes
+ shapes = _validate_shapes(shapes)
+ _installation_shapes = shapes
+ if collision is None:
+ return
+ for name in _installation_geom_names:
+ collision.remove_geometry_by_name(name)
+ _installation_geom_names.clear()
+ for s in shapes:
+ if not s.collision:
+ continue
+ name = f"install:{s.name}"
+ collision.add_obstacle(
+ name, s.kind, s.params(), _pose_to_matrix(s.pose), margin=s.margin
+ )
+ _installation_geom_names.append(name)
+
+
+def installation_shapes() -> "list[Any]":
+ """The installation-layer shapes (waldoctl ``Shape`` list) for readback."""
+ return list(_installation_shapes)
+
+
+def program_shapes() -> "list[Any]":
+ """The program-layer shapes last applied to this process (for readback)."""
+ return list(_program_shapes)
+
+
+def display_pairs(
+ pairs: "Iterable[tuple[str, str]]",
+) -> "list[tuple[str, str]]":
+ """Translate checker geometry names into the reporting vocabulary:
+ URDF link names for arm geometry; ``shape:``/``install:``/``tool:``
+ names keep their user-supplied form. Never leaks backend-internal
+ identifiers (e.g. Pinocchio's ``L4_0``) to clients."""
+ if collision is None:
+ return [tuple(p) for p in pairs]
+ m = dict(collision.geometry_link_names)
+ return [(m.get(a, a), m.get(b, b)) for a, b in pairs]
+
+
# Initialize with no tool
apply_tool("NONE")
diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py
index e8512a7..af4e2fc 100644
--- a/parol6/ack_policy.py
+++ b/parol6/ack_policy.py
@@ -12,6 +12,7 @@
CmdType.RESET,
CmdType.WRITE_IO,
CmdType.SET_TCP_OFFSET,
+ CmdType.SET_SHAPES,
}
# Query command types (use request/response, not ACK)
@@ -33,6 +34,7 @@
CmdType.PING,
CmdType.IS_SIMULATOR,
CmdType.TCP_OFFSET,
+ CmdType.SHAPES,
}
# Streaming commands are fire-and-forget (no ACK needed)
diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py
index e6cd96b..3dc29b7 100644
--- a/parol6/client/async_client.py
+++ b/parol6/client/async_client.py
@@ -14,7 +14,8 @@
import msgspec
import numpy as np
-from waldoctl import RobotClient as _RobotClientABC, Shape, ToolStatus
+from waldoctl import RobotClient as _RobotClientABC, Shape, ShapeWorld, ToolStatus
+from waldoctl.shapes import shape_from_wire
from waldoctl.status import ActionState, ActivityResult, ToolResult
from waldoctl.tools import ToolSpec
@@ -69,6 +70,8 @@
SelectProfileCmd,
SelectToolCmd,
SetShapesCmd,
+ ShapesCmd,
+ ShapesResultStruct,
SetTcpOffsetCmd,
ShapeWire,
ServoJCmd,
@@ -934,10 +937,15 @@ async def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int:
return await self._send(SetTcpOffsetCmd(x=x, y=y, z=z))
async def set_shapes(self, shapes: list[Shape]) -> int:
- """Replace the workspace collision-world shapes (keep-out barriers).
+ """Replace the program-layer collision-world shapes (keep-out barriers).
Collision-enabled shapes are added to the controller's checkers so motion
- is blocked against them; an empty list clears all shapes.
+ is blocked against them; an empty list clears the program layer.
+ Installation-layer shapes (robot config) are unaffected.
+
+ Acknowledged: returns 1 only after the controller confirms the world
+ was applied; 0 on timeout. Raises MotionError if the controller
+ rejects the shapes.
Category: Configuration
@@ -949,6 +957,31 @@ async def set_shapes(self, shapes: list[Shape]) -> int:
SetShapesCmd(shapes=[ShapeWire(*s.to_wire()) for s in shapes])
)
+ async def shapes(self) -> ShapeWorld | None:
+ """The collision world the controller is currently enforcing, by layer.
+
+ Readback truth for displays; re-query when ``StatusBuffer.scene_epoch``
+ changes. Returns None if the controller is unreachable.
+
+ Category: Query
+
+ Example:
+ world = rbt.shapes()
+ """
+ resp = await self._request(ShapesCmd())
+ if not isinstance(resp, ShapesResultStruct):
+ return None
+ return ShapeWorld(
+ installation=tuple(
+ shape_from_wire(w.kind, w.params, w.pose, w.collision, w.margin, w.name)
+ for w in resp.installation
+ ),
+ program=tuple(
+ shape_from_wire(w.kind, w.params, w.pose, w.collision, w.margin, w.name)
+ for w in resp.program
+ ),
+ )
+
async def tcp_offset(self) -> list[float]:
"""Query current TCP offset in mm [x, y, z].
diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py
index 2ff61b1..01002d4 100644
--- a/parol6/client/dry_run_client.py
+++ b/parol6/client/dry_run_client.py
@@ -507,6 +507,19 @@ def angles(self) -> list[float]:
steps_to_rad(self._state.Position_in, self._q_rad_buf)
return np.degrees(self._q_rad_buf).tolist()
+ def shapes(self):
+ """The preview's collision world by layer (mirrors the live query).
+
+ Explicit so a script's readback never falls into the generic command
+ dispatch, which has no query path.
+ """
+ from waldoctl import ShapeWorld
+
+ return ShapeWorld(
+ installation=tuple(PAROL6_ROBOT.installation_shapes()),
+ program=tuple(PAROL6_ROBOT.program_shapes()),
+ )
+
def pose(self) -> list[float]:
"""Return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]."""
se3 = get_fkine_se3(self._state)
diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py
index 8ce489f..8c28e85 100644
--- a/parol6/client/sync_client.py
+++ b/parol6/client/sync_client.py
@@ -324,9 +324,18 @@ def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int:
return _run(self._inner.set_tcp_offset(x=x, y=y, z=z))
def set_shapes(self, shapes: list) -> int:
- """Replace the workspace collision-world shapes (keep-out barriers)."""
+ """Replace the program-layer collision-world shapes (keep-out barriers).
+
+ Acknowledged: 1 = confirmed applied, 0 = timeout; raises MotionError
+ on rejection. Installation-layer shapes are unaffected.
+ """
return _run(self._inner.set_shapes(shapes))
+ def shapes(self):
+ """The collision world the controller is enforcing (ShapeWorld), by
+ layer; None if unreachable."""
+ return _run(self._inner.shapes())
+
def tcp_offset(self) -> list[float]:
"""Query current TCP offset in mm [x, y, z]."""
return _run(self._inner.tcp_offset())
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index eb81a0c..beef229 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -92,6 +92,9 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array
def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
+ # Reporting vocabulary (URDF link names / shape: / install: / tool:),
+ # never checker-internal geometry identifiers.
+ pairs = PAROL6_ROBOT.display_pairs(pairs)
exc = TrajectoryPlanningError(
make_error(
ErrorCode.SYS_SELF_COLLISION,
diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py
index 0209b49..16c76c6 100644
--- a/parol6/commands/basic_commands.py
+++ b/parol6/commands/basic_commands.py
@@ -234,7 +234,9 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
if collision_blocked(checker, pos_rad, la):
logger.warning("[JOGJ] collision predicted - stopping jog")
# Allocate only here (the rare stop), never on the clean tick.
- state.collision_pairs = tuple(checker.colliding_pairs(la))
+ state.collision_pairs = tuple(
+ PAROL6_ROBOT.display_pairs(checker.colliding_pairs(la))
+ )
state.collision_active = True
se.active = False
self.finish()
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 0dc3e3b..64fcf2d 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -156,10 +156,15 @@ 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:
- # Don't stream a self-colliding config during the release
- # deceleration; skip the send and let the CSE finish stopping.
+ # Don't stream a colliding config during the release
+ # deceleration — but keep streaming while ESCAPING from
+ # inside a keep-out (same semantics as the held phase),
+ # else the target freezes at the release point and the
+ # arm jerks instead of coasting to a stop.
checker = PAROL6_ROBOT.collision
- if checker is None or not checker.in_collision(ik_result.q):
+ if checker is None or not collision_blocked(
+ checker, self._q_commanded, ik_result.q
+ ):
self._track_and_send(state, ik_result.q)
return ExecutionStatusCode.EXECUTING
@@ -241,7 +246,9 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
"[CARTJOG] collision predicted - initiating stop",
)
# Capture once on the stop transition (not every decel tick).
- state.collision_pairs = tuple(checker.colliding_pairs(ik_result.q))
+ state.collision_pairs = tuple(
+ PAROL6_ROBOT.display_pairs(checker.colliding_pairs(ik_result.q))
+ )
state.collision_active = True
cse.stop()
self._ik_stopping = True
diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py
index 2d99b07..80e6353 100644
--- a/parol6/commands/query_commands.py
+++ b/parol6/commands/query_commands.py
@@ -34,6 +34,9 @@
ReachableCmd,
IsSimulatorCmd,
IsSimulatorResultStruct,
+ ShapesCmd,
+ ShapesResultStruct,
+ ShapeWire,
SpeedsResultStruct,
TcpOffsetCmd,
TcpOffsetResultStruct,
@@ -356,6 +359,33 @@ def compute(self, state: "ControllerState") -> bytes:
return pack_response(IsSimulatorResultStruct(active=is_simulation_mode()))
+@register_command(CmdType.SHAPES)
+class ShapesCommand(QueryCommand[ShapesCmd]):
+ """Query the applied collision world by layer (readback truth).
+
+ The codec boundary in the reply direction: state holds waldoctl Shapes;
+ they are flattened to the wire form only here.
+ """
+
+ PARAMS_TYPE = ShapesCmd
+ QUERY_TYPE = QueryType.SHAPES
+
+ __slots__ = ()
+
+ def compute(self, state: "ControllerState") -> bytes:
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ return pack_response(
+ ShapesResultStruct(
+ installation=[
+ ShapeWire(*s.to_wire()) for s in PAROL6_ROBOT.installation_shapes()
+ ],
+ program=[ShapeWire(*s.to_wire()) for s in state.shapes],
+ epoch=state.shapes_version,
+ )
+ )
+
+
@register_command(CmdType.TCP_OFFSET)
class TcpOffsetCommand(QueryCommand[TcpOffsetCmd]):
"""Query current TCP offset in mm."""
diff --git a/parol6/commands/shape_commands.py b/parol6/commands/shape_commands.py
index 989e1df..eecdf0a 100644
--- a/parol6/commands/shape_commands.py
+++ b/parol6/commands/shape_commands.py
@@ -4,6 +4,8 @@
from typing import TYPE_CHECKING
+from waldoctl import shape_from_wire
+
from parol6.commands.base import ExecutionStatusCode, SystemCommand
from parol6.protocol.wire import CmdType, SetShapesCmd
from parol6.server.command_registry import register_command
@@ -14,12 +16,17 @@
@register_command(CmdType.SET_SHAPES)
class SetShapesCommand(SystemCommand[SetShapesCmd]):
- """Replace the workspace keep-out shapes on the collision checkers.
+ """Replace the program-layer keep-out shapes on the collision checkers.
A SystemCommand (like SELECT_PROFILE): safety configuration applies
immediately at intake — never enabled-gated, never queued behind (or
dropped with) motion. The controller mirrors the applied shapes to the
planner subprocess via ``sync_shapes`` after this executes.
+
+ This is the codec boundary: the wire form is rebuilt into waldoctl
+ ``Shape`` objects here (running their construction-time validation), and
+ everything downstream — state, checkers, subprocess syncs, readback —
+ speaks ``Shape`` only.
"""
PARAMS_TYPE = SetShapesCmd
@@ -27,6 +34,10 @@ class SetShapesCommand(SystemCommand[SetShapesCmd]):
__slots__ = ()
def execute_step(self, state: ControllerState) -> ExecutionStatusCode:
- state.set_shapes(self.p.shapes)
+ shapes = [
+ shape_from_wire(w.kind, w.params, w.pose, w.collision, w.margin, w.name)
+ for w in self.p.shapes
+ ]
+ state.set_shapes(shapes)
self.finish()
return ExecutionStatusCode.COMPLETED
diff --git a/parol6/config.py b/parol6/config.py
index 1ddb8e2..873f142 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -601,10 +601,25 @@ def _build_cart_kinodynamic(
os.getenv("PAROL6_COLLISION_JOG_LOOKAHEAD_S", "0.15")
)
+# Installation-layer keep-out shapes: standing restrictions of this robot's
+# installation (walls, tables, fixtures), declared as waldoctl Shapes right
+# here in robot config alongside the other installation limits. Every program
+# inherits them; ``set_shapes`` (the program layer) cannot remove them. Loaded
+# at import in every process — controller, planner, IK worker, dry-run — so
+# no runtime sync is needed.
+#
+# Example:
+# from waldoctl import Box
+# INSTALLATION_SHAPES = [
+# Box(name="table", x=0.8, y=0.8, z=0.02, pose=(0.3, 0, -0.01, 0, 0, 0)),
+# ]
+INSTALLATION_SHAPES: list = []
+
# Populate PAROL6_ROBOT.collision now that the config knobs are defined.
PAROL6_ROBOT._init_collision_checker(
COLLISION_CHECK_ENABLED, COLLISION_SRDF_PATH, COLLISION_CLEARANCE_M
)
+PAROL6_ROBOT.apply_installation_shapes(INSTALLATION_SHAPES)
# -----------------------------------------------------------------------------
diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py
index da8f555..eac9e09 100644
--- a/parol6/protocol/wire.py
+++ b/parol6/protocol/wire.py
@@ -9,7 +9,7 @@
Wire format uses msgpack arrays with integer type codes:
- OK: MsgType.OK (just the integer)
- ERROR: [MsgType.ERROR, message]
-- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs]
+- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch]
- RESPONSE: [MsgType.RESPONSE, query_type, value]
- COMMAND: [CmdType.XXX, ...params]
"""
@@ -90,6 +90,7 @@ class QueryType(IntEnum):
TCP_SPEED = auto()
IS_SIMULATOR = auto()
TCP_OFFSET = auto()
+ SHAPES = auto()
class CmdType(IntEnum):
@@ -154,6 +155,8 @@ class CmdType(IntEnum):
IS_SIMULATOR = auto()
# Workspace collision-world shapes (keep-out geometry)
SET_SHAPES = auto()
+ # Collision-world readback query (installation + program layers)
+ SHAPES = auto()
# =============================================================================
@@ -749,6 +752,18 @@ class TcpOffsetCmd(
pass
+class ShapesCmd(
+ msgspec.Struct,
+ tag=int(CmdType.SHAPES),
+ array_like=True,
+ frozen=True,
+ gc=False,
+):
+ """SHAPES: query the applied collision world (readback)."""
+
+ pass
+
+
# Query commands (no params, just the tag)
class PingCmd(
msgspec.Struct, tag=int(CmdType.PING), array_like=True, frozen=True, gc=False
@@ -1154,6 +1169,20 @@ class TcpOffsetResultStruct(
z: float
+class ShapesResultStruct(
+ msgspec.Struct,
+ tag=int(QueryType.SHAPES),
+ array_like=True,
+ frozen=True,
+ gc=False,
+):
+ """The applied collision world by layer, plus the epoch it represents."""
+
+ installation: list[ShapeWire]
+ program: list[ShapeWire]
+ epoch: int
+
+
# Tagged Union for responses
Response = (
StatusResultStruct
@@ -1173,6 +1202,7 @@ class TcpOffsetResultStruct(
| TcpSpeedResultStruct
| IsSimulatorResultStruct
| TcpOffsetResultStruct
+ | ShapesResultStruct
)
@@ -1306,6 +1336,7 @@ def pack_status(
simulator_active: bool = False,
collision_active: bool = False,
collision_pairs: tuple[tuple[str, str], ...] = (),
+ scene_epoch: int = 0,
) -> bytes:
"""Pack a status broadcast message.
@@ -1347,6 +1378,7 @@ def pack_status(
simulator_active,
collision_active,
collision_pairs,
+ scene_epoch,
),
option=ormsgpack.OPT_SERIALIZE_NUMPY,
)
@@ -1386,6 +1418,7 @@ class StatusBuffer:
simulator_active: bool = False
collision_active: bool = False
collision_pairs: list[tuple[str, str]] = field(default_factory=list)
+ scene_epoch: int = 0
def __post_init__(self) -> None:
self._cart_en_dict: dict[str, np.ndarray] = {
@@ -1431,6 +1464,7 @@ def copy(self) -> "StatusBuffer":
simulator_active=self.simulator_active,
collision_active=self.collision_active,
collision_pairs=list(self.collision_pairs),
+ scene_epoch=self.scene_epoch,
)
@@ -1442,7 +1476,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
executing_index, completed_index, last_checkpoint,
error, queued_segments, queued_duration, action_params,
tool_status_tuple, tcp_speed, simulator_active,
- collision_active, collision_pairs]
+ collision_active, collision_pairs, scene_epoch]
Args:
data: Raw msgpack bytes
@@ -1510,6 +1544,8 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
if raw_pairs:
for p in raw_pairs:
cp.append((p[0], p[1]))
+ if len(msg) > 22:
+ buf.scene_epoch = int(msg[22])
return True
except Exception as e:
@@ -1773,6 +1809,7 @@ def unpack_rx_frame_into(
"ErrorCmd",
"TcpSpeedCmd",
"TcpOffsetCmd",
+ "ShapesCmd",
"PingCmd",
"StatusCmd",
"AnglesCmd",
@@ -1805,6 +1842,7 @@ def unpack_rx_frame_into(
"TcpSpeedResultStruct",
"IsSimulatorResultStruct",
"TcpOffsetResultStruct",
+ "ShapesResultStruct",
"Response",
# Message types
"OkMsg",
diff --git a/parol6/robot.py b/parol6/robot.py
index 01a279b..c1beef6 100644
--- a/parol6/robot.py
+++ b/parol6/robot.py
@@ -825,18 +825,20 @@ def check_trajectory(self, q_path_rad: NDArray[np.float64]) -> int:
return c.check_path(np.ascontiguousarray(q_path_rad, dtype=np.float64))
def colliding_pairs(self, q_rad: NDArray[np.float64]) -> list[tuple[str, str]]:
- """Return list of (name, name) geometry pairs in collision at `q_rad`.
+ """Return list of (name, name) 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, or
- ``"shape:"`` for a workspace keep-out shape).
+ Names use the reporting vocabulary: URDF link names for arm geometry
+ (e.g. ``"L4"``), ``shape:`` / ``install:`` for keep-outs,
+ and ``tool::`` for attached tool geometry — never
+ checker-internal identifiers.
"""
c = self._collision_checker
if c is None:
return []
self._load_q_buf(q_rad)
- return c.colliding_pairs(self._q_buf)
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ return PAROL6_ROBOT.display_pairs(c.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`.
@@ -854,12 +856,12 @@ def apply_shapes(self, shapes) -> None:
"""Apply keep-out shapes to this process's checker (preview/editing viz).
Local-only twin of the client's ``set_shapes`` (which updates the
- server's checkers). Accepts waldoctl ``Shape`` objects.
+ server's checkers). Accepts waldoctl ``Shape`` objects — the canonical
+ in-process type everywhere; no wire form is involved.
"""
import parol6.PAROL6_ROBOT as PAROL6_ROBOT
- from parol6.protocol.wire import ShapeWire
- PAROL6_ROBOT.apply_shapes([ShapeWire(*s.to_wire()) for s in shapes])
+ PAROL6_ROBOT.apply_shapes(list(shapes))
def ik_batch(
self,
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index e0eb763..08efeda 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -52,7 +52,7 @@ class SyncTool:
@dataclass(frozen=True)
class SyncShapes:
- """Sync the worker checker's workspace keep-out shapes (ShapeWire tuple)."""
+ """Sync the worker checker's program-layer shapes (waldoctl Shape tuple)."""
shapes: tuple
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index b809336..9454bac 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -121,7 +121,7 @@ class SyncTool:
@dataclass
class SyncShapes:
- """Replace the planner checker's workspace keep-out shapes (ShapeWire list)."""
+ """Replace the planner checker's program-layer shapes (waldoctl Shape list)."""
shapes: list
@@ -485,7 +485,9 @@ def _handle_inline(self, command_index: int, params: object) -> None:
elif isinstance(params, SetShapesCmd):
# Only reachable via the DRY-RUN planner: a script's set_shapes()
# must shape its preview world. The live path routes SET_SHAPES as
- # a SystemCommand + SyncShapes, never through process().
+ # a SystemCommand + SyncShapes, never through process(). Here the
+ # cmd carries raw waldoctl Shapes (the dry-run client never
+ # touches the wire form), which is exactly what apply_shapes takes.
self._robot_module.apply_shapes(params.shapes)
diff --git a/parol6/server/state.py b/parol6/server/state.py
index 3cb16a1..997adaa 100644
--- a/parol6/server/state.py
+++ b/parol6/server/state.py
@@ -412,10 +412,12 @@ def set_tool(self, tool_name: str, variant_key: str = "") -> None:
logger.info(f"Tool changed to {label}")
def set_shapes(self, shapes: list) -> None:
- """Apply the workspace collision-world shapes to the control-loop checker.
+ """Apply the program-layer shapes (waldoctl ``Shape`` list) to the
+ control-loop checker.
Also retained (with a version bump) so the status cache can mirror them
- to the IK worker's checker for enablement greying.
+ to the IK worker's checker for enablement greying; the version doubles
+ as the ``scene_epoch`` broadcast in status so displays re-query.
"""
PAROL6_ROBOT.apply_shapes(shapes)
self.shapes = list(shapes)
diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py
index 9a63072..767c7b1 100644
--- a/parol6/server/status_cache.py
+++ b/parol6/server/status_cache.py
@@ -419,6 +419,8 @@ def update_from_state(self, state: ControllerState) -> None:
if state.shapes_version != self._last_shapes_version:
self._last_shapes_version = state.shapes_version
self._sync_ik_geometry(SyncShapes(shapes=tuple(state.shapes)))
+ # World changed → broadcast the new epoch so displays re-query.
+ self._binary_dirty = True
if pos_changed or tool_changed:
self.pose[:] = get_fkine_flat_mm(state)
@@ -555,6 +557,7 @@ def to_binary(self) -> bytes:
simulator_active=is_simulation_mode(),
collision_active=self._collision_active,
collision_pairs=self._collision_pairs,
+ scene_epoch=self._last_shapes_version,
)
self._binary_dirty = False
return self._binary_cache
diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py
new file mode 100644
index 0000000..9e8e84e
--- /dev/null
+++ b/tests/integration/test_shapes_e2e.py
@@ -0,0 +1,46 @@
+"""SET_SHAPES / SHAPES end-to-end: real client ↔ real (fake-serial) server.
+
+The ack contract under test is the waldoctl convention: 1 = confirmed applied,
+0 = unconfirmed (timeout), raise = rejected. The pre-fix client treated
+SET_SHAPES as fire-and-forget and returned 1 unconditionally — every assert
+here except the plain success one fails against that behavior.
+"""
+
+import pytest
+
+from parol6 import MotionError, RobotClient
+from waldoctl import Box
+
+pytestmark = pytest.mark.integration
+
+
+def test_set_shapes_ack_readback_rejection_and_timeout(
+ server_proc, client: RobotClient, ports
+):
+ box = Box(name="table", x=0.6, y=0.4, z=0.02, pose=(0.9, 0.9, -0.01, 0, 0, 0))
+ try:
+ # Confirmed apply → 1, and readback reports the applied program layer.
+ assert client.set_shapes([box]) == 1
+ world = client.shapes()
+ assert world is not None
+ assert tuple(s.name for s in world.program) == ("table",)
+ assert world.program[0] == box # full round-trip, not just the name
+
+ # Server rejection (duplicate names) → ERROR reply → raises; the
+ # previously-applied world must survive the rejected call.
+ with pytest.raises(MotionError, match="Duplicate"):
+ client.set_shapes([box, Box(name="table", x=0.1, y=0.1, z=0.1)])
+ world = client.shapes()
+ assert world is not None
+ assert tuple(s.name for s in world.program) == ("table",)
+
+ # Unreachable controller → unconfirmed (0), never a fake success.
+ dead = RobotClient(
+ host=ports.server_ip, port=ports.server_port + 91, timeout=0.3
+ )
+ assert dead.set_shapes([box]) == 0
+ assert dead.shapes() is None
+ finally:
+ assert client.set_shapes([]) == 1
+ world = client.shapes()
+ assert world is not None and world.program == ()
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index 7f902aa..af52f40 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -16,6 +16,7 @@
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.commands.base import ExecutionStatusCode
from parol6.utils.error_codes import ErrorCode
from parol6.utils.errors import TrajectoryPlanningError
@@ -57,29 +58,34 @@ def test_guard_joint_path_clear_returns_none():
guard_joint_path(positions)
-def test_guard_joint_path_raises_on_explicit_collision(monkeypatch):
- """Force a fake collision by patching the singleton checker temporarily.
- monkeypatch auto-restores the real checker at teardown."""
+def test_guard_joint_path_raises_with_display_vocabulary():
+ """Drive the REAL checker into a real keep-out: the guard must raise and
+ report pairs in the display vocabulary (URDF link names + shape:),
+ never checker-internal identifiers like ``L4_0``."""
+ from waldoctl import Box
- class FakeChecker:
- def in_collision(self, q):
- return True
-
- def check_path(self, q):
- return 2
-
- def colliding_pairs(self, q):
- return [("ssg48_body_simplified.stl", "L4_0")]
-
- monkeypatch.setattr(PAROL6_ROBOT, "collision", FakeChecker())
-
- positions = np.zeros((5, 6))
- with pytest.raises(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
+ # At q=zeros the wrist sits at (0.16, 0, 0.324) — this box envelops it;
+ # with the base rotated 90° the arm swings to +Y and is clear.
+ PAROL6_ROBOT.apply_shapes(
+ [Box(name="block", x=0.25, y=0.25, z=0.25, pose=(0.16, 0.0, 0.324, 0, 0, 0))]
+ )
+ try:
+ q_clear = np.zeros(6)
+ q_clear[0] = np.pi / 2
+ assert PAROL6_ROBOT.collision.in_collision(q_clear) is False
+ assert PAROL6_ROBOT.collision.in_collision(np.zeros(6)) is True
+
+ positions = np.linspace(q_clear, np.zeros(6), 12)
+ 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)
+ assert "shape:block" in err.cause
+ assert "_0" not in err.cause # no Pinocchio geometry suffixes leak
+ pairs = exc_info.value.colliding_pairs
+ assert all("shape:block" in p or not p[0].endswith("_0") for p in pairs)
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
def test_guard_disabled_when_checker_is_none(monkeypatch):
@@ -195,43 +201,206 @@ def pct(a: list[int], p: float) -> float:
print("servo tick budget: 10000 us (100 Hz)")
-def _shape_wire(name: str, **overrides):
- from parol6.protocol.wire import ShapeWire
+def _box(name: str, **overrides):
+ from waldoctl import Box
- fields = dict(
- kind="box",
- params=[0.1, 0.1, 0.1],
- pose=[1.0, 1.0, 1.0, 0, 0, 0],
- collision=True,
- margin=None,
- name=name,
- )
- fields.update(overrides)
- return ShapeWire(**fields)
+ kwargs = dict(x=0.1, y=0.1, z=0.1, pose=(1.0, 1.0, 1.0, 0, 0, 0))
+ kwargs.update(overrides)
+ return Box(name=name, **kwargs)
-def test_apply_shapes_rejects_invalid_input_without_mutation():
- """Any invalid shape fails fast — never a half-applied collision world.
+def test_apply_shapes_rejects_bad_sets_without_mutation():
+ """Set-level rejection fails fast — never a half-applied collision world.
- Critically, the OLD world must survive a rejected call: the remove-then-add
- loop only runs after validation, so an error reply can't silently disarm
- every barrier.
+ The OLD world must survive a rejected call: the remove-then-add loop only
+ runs after validation, so an error reply can't silently disarm barriers.
+ (Per-shape value validation lives in Shape construction — the decode gate
+ below.)
"""
bad_sets = [
- ("Duplicate shape name", [_shape_wire("b"), _shape_wire("b")]),
+ ("Duplicate shape name", [_box("b"), _box("b")]),
# A visual-only marker sharing a keep-out's name shadows it in the
# frontend's highlight mapping — rejected too.
- ("Duplicate shape name", [_shape_wire("b"), _shape_wire("b", collision=False)]),
- ("unknown kind", [_shape_wire("b", kind="torus")]),
- ("takes 3 param", [_shape_wire("b", params=[0.1])]),
- ("6 finite numbers", [_shape_wire("b", pose=[0, 0, 0, 0, 0])]),
- ("6 finite numbers", [_shape_wire("b", pose=[0, 0, float("nan"), 0, 0, 0])]),
+ ("Duplicate shape name", [_box("b"), _box("b", collision=False)]),
]
try:
- PAROL6_ROBOT.apply_shapes([_shape_wire("a")])
+ PAROL6_ROBOT.apply_shapes([_box("a")])
for match, shapes in bad_sets:
with pytest.raises(ValueError, match=match):
PAROL6_ROBOT.apply_shapes(shapes)
assert PAROL6_ROBOT._active_shape_names == ["shape:a"]
finally:
PAROL6_ROBOT.apply_shapes([])
+
+
+def test_decode_gate_rejects_malformed_wire():
+ """The server's codec boundary (shape_from_wire at SET_SHAPES intake)
+ rejects malformed and degenerate wire data — cases chosen from the
+ requirement (NaN/inf/negative/zero/count), not from what the code handles."""
+ from waldoctl import shape_from_wire
+
+ good = ("box", [0.1, 0.1, 0.1], [0, 0, 0, 0, 0, 0], True, None, "b")
+ assert shape_from_wire(*good).name == "b"
+ bad_wires = [
+ ("box", [0.1], [0, 0, 0, 0, 0, 0], True, None, "b"), # wrong count
+ ("box", [float("nan"), 0.1, 0.1], [0, 0, 0, 0, 0, 0], True, None, "b"),
+ ("box", [float("inf"), 0.1, 0.1], [0, 0, 0, 0, 0, 0], True, None, "b"),
+ ("box", [-0.1, 0.1, 0.1], [0, 0, 0, 0, 0, 0], True, None, "b"),
+ ("sphere", [0.0], [0, 0, 0, 0, 0, 0], True, None, "b"), # zero dim
+ ("box", [0.1] * 3, [0, 0, float("nan"), 0, 0, 0], True, None, "b"),
+ ("box", [0.1] * 3, [0, 0, 0, 0, 0], True, None, "b"), # short pose
+ ("box", [0.1] * 3, [0, 0, 0, 0, 0, 0], True, -0.01, "b"), # neg margin
+ ("plane", [0, 0, 0, 0.5], [0, 0, 0, 0, 0, 0], True, None, "b"), # 0 normal
+ ]
+ for wire in bad_wires:
+ with pytest.raises(ValueError):
+ shape_from_wire(*wire)
+
+
+def test_per_shape_margin_blocks_at_standoff():
+ """A margined keep-out trips ``in_collision`` at its standoff distance,
+ not at contact. Real checker, real geometry: the sphere hovers ~8 cm off
+ the wrist — clear without a margin, colliding with a 15 cm one — and the
+ reported pair names the shape, proving the margin applied to ITS pairs."""
+ from waldoctl import Sphere
+
+ q = np.zeros(6)
+ checker = PAROL6_ROBOT.collision
+ above_wrist = (0.16, 0.0, 0.424, 0, 0, 0)
+ try:
+ PAROL6_ROBOT.apply_shapes([Sphere(name="s", radius=0.02, pose=above_wrist)])
+ assert checker.in_collision(q) is False
+
+ PAROL6_ROBOT.apply_shapes(
+ [Sphere(name="s", radius=0.02, pose=above_wrist, margin=0.15)]
+ )
+ assert checker.in_collision(q) is True # margin closes the gap
+ pairs = PAROL6_ROBOT.display_pairs(checker.colliding_pairs(q))
+ assert any("shape:s" in p for p in pairs)
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
+ assert checker.in_collision(q) is False
+
+
+def test_installation_layer_survives_program_clear():
+ """Installation shapes (robot config) are enforced alongside the program
+ layer and are untouched by ``apply_shapes`` — including a full clear."""
+ from waldoctl import Box
+
+ q = np.zeros(6)
+ checker = PAROL6_ROBOT.collision
+ try:
+ PAROL6_ROBOT.apply_installation_shapes(
+ [Box(name="wall", x=0.25, y=0.25, z=0.25, pose=(0.16, 0.0, 0.324, 0, 0, 0))]
+ )
+ assert checker.in_collision(q) is True
+ assert [s.name for s in PAROL6_ROBOT.installation_shapes()] == ["wall"]
+
+ PAROL6_ROBOT.apply_shapes([]) # program clear must not disarm install
+ assert checker.in_collision(q) is True
+ assert PAROL6_ROBOT.display_pairs(checker.colliding_pairs(q))[0][1] == (
+ "install:wall"
+ )
+ finally:
+ PAROL6_ROBOT.apply_installation_shapes([])
+ assert checker.in_collision(q) is False
+
+
+def test_pose_to_matrix_is_extrinsic_xyz_rpy():
+ """The waldoctl Shape.pose contract is extrinsic-XYZ RPY (R = Rz·Ry·Rx).
+ Tripwire against swapping in ``pinokin.se3_from_rpy`` (Rx·Ry·Rz), which
+ would silently mis-orient every multi-axis-tilted shape versus its render."""
+ rx, ry, rz = 0.3, -0.5, 1.1
+ cx, sx = np.cos(rx), np.sin(rx)
+ cy, sy = np.cos(ry), np.sin(ry)
+ cz, sz = np.cos(rz), np.sin(rz)
+ Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
+ Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
+ Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
+ T = PAROL6_ROBOT._pose_to_matrix([0.1, 0.2, 0.3, rx, ry, rz])
+ assert np.allclose(T[:3, :3], Rz @ Ry @ Rx)
+ assert not np.allclose(T[:3, :3], Rx @ Ry @ Rz) # the tempting wrong order
+ assert np.allclose(T[:3, 3], [0.1, 0.2, 0.3])
+
+
+def test_jogl_release_decel_streams_while_escaping():
+ """Releasing a Cartesian jog while ESCAPING from inside a keep-out must
+ keep streaming the deceleration (escape-aware gate) — the pre-fix bare
+ ``in_collision`` skipped every decel send, freezing the target at the
+ release point. Real command, real state, real checker, real IK."""
+ import time as _time
+
+ from waldoctl import Box
+
+ from parol6.commands.cartesian_commands import JogLCommand
+ from parol6.protocol.wire import JogLCmd
+ from parol6.server.state import ControllerState
+
+ from parol6.config import deg_to_steps
+
+ state = ControllerState()
+ # IK-friendly physical home (wrist at ~(0.237, 0, 0.334)); q=zeros is an
+ # IK danger zone where the jog never streams.
+ q_home_deg = np.array([0.0, -90.0, 180.0, 0.0, 0.0, 180.0])
+ deg_to_steps(q_home_deg, state.Position_in)
+ try:
+ # Box centred 5 cm below the wrist: +Z is unambiguously the escape.
+ PAROL6_ROBOT.apply_shapes(
+ [
+ Box(
+ name="cage",
+ x=0.15,
+ y=0.15,
+ z=0.15,
+ pose=(0.237, 0.0, 0.284, 0, 0, 0),
+ )
+ ]
+ )
+ assert PAROL6_ROBOT.collision.in_collision(np.radians(q_home_deg)) is True
+
+ cmd = JogLCommand(JogLCmd(velocities=[0.0, 0.0, 1.0, 0, 0, 0], duration=5.0))
+ cmd.setup(state)
+ for _ in range(30): # held phase: build real velocity (CSE dt=0.01/tick)
+ cmd.execute_step(state)
+ assert state.Command_out != 0 # held phase streamed (escape allowed)
+ cmd._t_end = _time.perf_counter() - 1.0 # deterministic release
+
+ code = cmd.execute_step(state)
+ pos0 = state.Position_out.copy()
+ moved = False
+ for _ in range(500):
+ if code != ExecutionStatusCode.EXECUTING:
+ break
+ code = cmd.execute_step(state)
+ if not np.array_equal(state.Position_out, pos0):
+ moved = True
+ assert code == ExecutionStatusCode.COMPLETED
+ assert moved, "decel sends were skipped — target frozen at release point"
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
+
+
+def test_dry_run_script_set_shapes_applies_and_replays():
+ """A script's ``set_shapes`` through the REAL dry-run dispatch: applies the
+ world (raw waldoctl Shapes end-to-end — the pre-fix path crashed with
+ ``TypeError: object of type 'method' has no len()``), reads back via
+ ``shapes()``, and a subsequent world change replaces it."""
+ from waldoctl import Box
+
+ from parol6.client.dry_run_client import DryRunRobotClient
+
+ try:
+ c = DryRunRobotClient(initial_joints_deg=[0, -90, 180, 0, 0, 180])
+ c.set_shapes(
+ [Box(name="bar", x=0.1, y=0.1, z=0.1, pose=(0.9, 0.9, 0.9, 0, 0, 0))]
+ )
+ assert PAROL6_ROBOT._active_shape_names == ["shape:bar"]
+ world = c.shapes()
+ assert [s.name for s in world.program] == ["bar"]
+
+ c.set_shapes(
+ [Box(name="bar2", x=0.1, y=0.1, z=0.1, pose=(0.9, 0.9, 0.9, 0, 0, 0))]
+ )
+ assert PAROL6_ROBOT._active_shape_names == ["shape:bar2"]
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
From 1ecb605ea71601082b5bc467035bb7cf35ca976f Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 7 Jul 2026 18:24:12 -0400
Subject: [PATCH 30/36] Round-5: escape new-pairs parity, tool geom vocabulary,
MoveIt-style invalidation
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- collision_blocked and both ik-worker escape gates now also block when the
step introduces a colliding pair absent at the current config, matching
guard_joint_path: the global min-distance trend alone cannot distinguish an
improving start-collision from a new shallower one. pairs_now is hoisted
once per gate pass; allocations stay confined to the rare in-collision branch.
- Attached tool geometry is named tool:{key}:{role} (per-role dedup suffix)
instead of leaking raw STL basenames into error text and collision_pairs.
- SegmentPlayer re-guards committed motion against the current world: a
shapes_version bump re-validates the streaming trajectory's remaining rows
(zero-alloc int compare on the unchanged path), and every TrajectorySegment
is guarded at activation — closing the planner-FIFO race where a segment
planned against the old world arrives after SET_SHAPES applied. Violations
halt exactly like an ErrorSegment via guard_joint_path's escape-aware,
display-vocabulary error. Inline segments are not trajectory playback and
are not re-guarded.
Red-first e2e coverage: mid-flight set_shapes halts the streaming move,
rejects a queued move at activation, and leaves off-path motion undisturbed;
unit coverage for the new-pair gate greying and tool-role pair vocabulary.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
parol6/PAROL6_ROBOT.py | 13 ++-
parol6/commands/_collision_guard.py | 14 ++-
parol6/commands/shape_commands.py | 6 ++
parol6/server/ik_worker.py | 25 +++--
parol6/server/segment_player.py | 81 +++++++++++++-
tests/integration/test_shapes_e2e.py | 131 +++++++++++++++++++++++
tests/unit/test_collision_enablement.py | 60 ++++++++++-
tests/unit/test_collision_integration.py | 81 ++++++++++++++
8 files changed, 389 insertions(+), 22 deletions(-)
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index a1e2f51..95b1dcc 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -214,6 +214,7 @@ def _refresh_collision_tool_geometry(
meshes = v.meshes
break
mesh_root = Path(_mesh_dir) / "meshes"
+ role_counts: dict[str, int] = {}
try:
for spec in meshes:
path = mesh_root / spec.file
@@ -222,10 +223,14 @@ def _refresh_collision_tool_geometry(
# rotation branch here when a non-identity rpy appears.
T = np.eye(4, dtype=np.float64)
T[:3, 3] = spec.origin
- # tool: namespace so colliding-pair reports speak the defined
- # vocabulary (link names / shape: / install: / tool:) instead
- # of raw mesh filenames.
- geom_name = f"tool:{tool_key}:{spec.file}"
+ # tool: namespace with the mesh's semantic role so colliding-
+ # pair reports speak the defined vocabulary (link names /
+ # shape: / install: / tool:), never raw mesh filenames.
+ # Repeated roles (two jaws) get a positional suffix for the
+ # attach/detach bookkeeping's name uniqueness.
+ role = spec.role.name.lower()
+ n = role_counts[role] = role_counts.get(role, 0) + 1
+ geom_name = f"tool:{tool_key}:{role}" + (f"_{n}" if n > 1 else "")
collision.attach_mesh_to_frame(
geom_name,
str(path),
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index beef229..0455c72 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -29,12 +29,18 @@ def collision_blocked(checker, current_q, target_q) -> bool:
"""Whether streaming toward ``target_q`` must stop.
Approaching: blocked when ``target_q`` collides. Already colliding (a
- keep-out placed over the arm, or a tool-attach overlap): blocked only when
- the motion goes *deeper* — escaping is allowed, mirroring
- :func:`guard_joint_path`'s start-in-collision rule.
+ keep-out placed over the arm, or a tool-attach overlap): blocked when the
+ motion contacts anything *new* or goes *deeper* — escaping is allowed,
+ mirroring :func:`guard_joint_path`'s start-in-collision rule (a global
+ min-distance trend alone can't tell an improving start-collision from a
+ new shallower one, so pairs are checked too). The pair sets allocate, but
+ only in this already-colliding branch — never in the steady-state path.
"""
if checker.in_collision(current_q):
- return bool(
+ new_pairs = set(checker.colliding_pairs(target_q)) - set(
+ checker.colliding_pairs(current_q)
+ )
+ return bool(new_pairs) or bool(
checker.min_distance(target_q)
< checker.min_distance(current_q) - _ESCAPE_TOL
)
diff --git a/parol6/commands/shape_commands.py b/parol6/commands/shape_commands.py
index eecdf0a..b16e8e1 100644
--- a/parol6/commands/shape_commands.py
+++ b/parol6/commands/shape_commands.py
@@ -23,6 +23,12 @@ class SetShapesCommand(SystemCommand[SetShapesCmd]):
dropped with) motion. The controller mirrors the applied shapes to the
planner subprocess via ``sync_shapes`` after this executes.
+ A world change also invalidates committed motion that now violates it
+ (MoveIt-style): the segment player re-guards the streaming trajectory's
+ remaining waypoints on the version bump, and every trajectory again at
+ activation, halting with the collision error instead of driving into the
+ new keep-out.
+
This is the codec boundary: the wire form is rebuilt into waldoctl
``Shape`` objects here (running their construction-time validation), and
everything downstream — state, checkers, subprocess syncs, readback —
diff --git a/parol6/server/ik_worker.py b/parol6/server/ik_worker.py
index 08efeda..e14399e 100644
--- a/parol6/server/ik_worker.py
+++ b/parol6/server/ik_worker.py
@@ -294,11 +294,12 @@ def gate_joint_enable_collision(checker, q_rad, joint_en, q_step, qlim=None) ->
collides — self, tool, or keep-out shape. Proximity-gated (skip the
per-direction checks when the arm is farther than ``_ENABLE_NEAR_M`` from
collision) so it stays cheap. When the arm is ALREADY colliding (a keep-out
- placed over it), escaping directions stay enabled — grey only those that go
- deeper — mirroring the jog/planner escape semantics; otherwise every button
- would grey with no way out. ``qlim`` is optional ``(low, high)`` rows: the
- probe is clamped so a pose past the mechanical stop (which the jog itself
- can never reach) can't grey the button. Not njit — it calls the C++ checker.
+ placed over it), escaping directions stay enabled — grey those that go
+ deeper or contact anything new — mirroring the jog/planner escape
+ semantics; otherwise every button would grey with no way out. ``qlim`` is
+ optional ``(low, high)`` rows: the probe is clamped so a pose past the
+ mechanical stop (which the jog itself can never reach) can't grey the
+ button. Not njit — it calls the C++ checker.
"""
if checker is None:
return
@@ -306,6 +307,7 @@ def gate_joint_enable_collision(checker, q_rad, joint_en, q_step, qlim=None) ->
if md_now >= _ENABLE_NEAR_M:
return
inside = checker.in_collision(q_rad)
+ pairs_now = set(checker.colliding_pairs(q_rad)) if inside else None
for j in range(6):
for slot, sign in ((2 * j, 1.0), (2 * j + 1, -1.0)):
if joint_en[slot]:
@@ -316,8 +318,10 @@ def gate_joint_enable_collision(checker, q_rad, joint_en, q_step, qlim=None) ->
q_step[j] = qlim[0][j]
elif q_step[j] > qlim[1][j]:
q_step[j] = qlim[1][j]
- if inside:
- if checker.min_distance(q_step) < md_now - _ESCAPE_TOL:
+ if pairs_now is not None:
+ if set(checker.colliding_pairs(q_step)) - pairs_now or (
+ checker.min_distance(q_step) < md_now - _ESCAPE_TOL
+ ):
joint_en[slot] = 0
elif checker.in_collision(q_step):
joint_en[slot] = 0
@@ -420,6 +424,7 @@ def _compute_cart_enable(
near = md_now < _ENABLE_NEAR_M
# Already colliding: keep escaping directions enabled (see the joint gate).
inside = near and checker is not None and checker.in_collision(q_rad)
+ pairs_now = set(checker.colliding_pairs(q_rad)) if inside else None
# Check IK (and, when near collision, the solved config) for each target
for i in range(12):
@@ -427,8 +432,10 @@ def _compute_cart_enable(
ik = solve_ik(robot, targets[i], q_rad, quiet_logging=True)
ok = bool(ik.success)
if ok and near and checker is not None:
- if inside:
- ok = checker.min_distance(ik.q) >= md_now - _ESCAPE_TOL
+ if pairs_now is not None:
+ ok = checker.min_distance(ik.q) >= md_now - _ESCAPE_TOL and not (
+ set(checker.colliding_pairs(ik.q)) - pairs_now
+ )
elif checker.in_collision(ik.q):
ok = False
out[i] = 1 if ok else 0
diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py
index f7e33ea..3eb9ef5 100644
--- a/parol6/server/segment_player.py
+++ b/parol6/server/segment_player.py
@@ -16,10 +16,12 @@
from collections import deque
from typing import TYPE_CHECKING
+import numpy as np
from pinokin import arrays_equal_n
+from parol6.commands._collision_guard import guard_joint_path
from parol6.commands.base import CommandBase, ExecutionStatusCode
-from parol6.config import SETTLE_MAX_TICKS
+from parol6.config import COLLISION_PATH_SAMPLES, SETTLE_MAX_TICKS, steps_to_rad
from parol6.protocol.wire import CommandCode
from parol6.server.command_executor import _format_cmd_params
from parol6.server.command_registry import create_command_from_struct
@@ -32,6 +34,7 @@
)
from parol6.utils.error_catalog import RobotError, make_error
from parol6.utils.error_codes import ErrorCode
+from parol6.utils.errors import TrajectoryPlanningError
from waldoctl import ActionState
if TYPE_CHECKING:
@@ -57,6 +60,7 @@ class SegmentPlayer:
"_inline_activated",
"_settling",
"_settle_ticks",
+ "_last_shapes_version",
)
def __init__(self, planner: MotionPlanner) -> None:
@@ -68,6 +72,7 @@ def __init__(self, planner: MotionPlanner) -> None:
self._inline_activated: bool = False
self._settling: bool = False
self._settle_ticks: int = 0
+ self._last_shapes_version: int = 0
@property
def active(self) -> bool:
@@ -89,6 +94,18 @@ def tick(self, state: ControllerState) -> bool:
state.queued_duration += seg.duration
seg = self._planner.poll_segment()
+ # MoveIt-style invalidation: a world change (SET_SHAPES bumps
+ # shapes_version) re-guards the streaming trajectory's remaining
+ # waypoints. The unchanged path pays only this int compare.
+ if state.shapes_version != self._last_shapes_version:
+ self._last_shapes_version = state.shapes_version
+ if isinstance(self._active, TrajectorySegment):
+ start = self._step - 1 if self._step > 0 else 0
+ if not self._world_guard(
+ self._active, self._active.trajectory_steps[start:], state
+ ):
+ return False
+
# Process active segment or activate next
max_immediate = 8 # prevent infinite recursion on back-to-back instant commands
for _ in range(max_immediate):
@@ -97,6 +114,8 @@ def tick(self, state: ControllerState) -> bool:
if not self._buffer:
return False
self._activate_next(state)
+ if self._active is None:
+ continue # activation-time world guard rejected the segment
active = self._active
@@ -162,8 +181,19 @@ def tick(self, state: ControllerState) -> bool:
return self._active is not None
def _activate_next(self, state: ControllerState) -> None:
- """Promote next buffered segment to active."""
- self._active = self._buffer.popleft()
+ """Promote next buffered segment to active.
+
+ Trajectory segments are re-guarded against the *current* world first:
+ their plan-time guard may predate a world change — the planner FIFO
+ orders plans and shape syncs by submission, so a segment planned
+ against the old world can arrive here after SET_SHAPES applied.
+ """
+ seg = self._buffer.popleft()
+ if isinstance(seg, TrajectorySegment) and not self._world_guard(
+ seg, seg.trajectory_steps, state
+ ):
+ return
+ self._active = seg
self._step = 0
self._inline_cmd = None
self._inline_activated = False
@@ -245,6 +275,51 @@ def _on_failure(
self._planner.cancel()
self._drain_planner_queue(state)
+ def _world_guard(
+ self, seg: Segment, steps: np.ndarray, state: ControllerState
+ ) -> bool:
+ """Validate trajectory waypoints (motor steps) against the current
+ collision world; on violation, halt playback like an ErrorSegment.
+
+ Runs only at segment activation and on a world change, never per-tick;
+ rows are subsampled *before* the steps→rad conversion so the cost stays
+ ~COLLISION_PATH_SAMPLES checker calls regardless of trajectory length.
+ ``guard_joint_path`` keeps the escape semantics — a keep-out dropped
+ onto the arm still permits the escaping remainder. Inline segments
+ (Home/Gripper) are not trajectory playback and are not re-guarded.
+ """
+ n = len(steps)
+ if n == 0:
+ return True
+ target = max(2, COLLISION_PATH_SAMPLES + 2)
+ if n > target:
+ idx = np.unique(np.linspace(0, n - 1, target).round().astype(int))
+ steps = steps[idx]
+ q = np.empty((len(steps), 6), dtype=np.float64)
+ for i in range(len(steps)):
+ steps_to_rad(steps[i], q[i])
+ try:
+ guard_joint_path(q)
+ except TrajectoryPlanningError as exc:
+ logger.error(
+ "Command %d invalidated by world change: %s",
+ seg.command_index,
+ exc.robot_error,
+ )
+ state.error = exc.robot_error
+ pairs = exc.colliding_pairs
+ state.collision_active = bool(pairs)
+ state.collision_pairs = tuple(pairs) if pairs else ()
+ state.action_state = ActionState.ERROR
+ state.action_current = ""
+ state.action_params = ""
+ self._active = None
+ self._buffer.clear()
+ self._planner.cancel()
+ self._drain_planner_queue(state)
+ return False
+ return True
+
def cancel(self, state: ControllerState) -> None:
"""Clear buffer, drain stale segments, and stop playback."""
self._active = None
diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py
index 9e8e84e..9a8b101 100644
--- a/tests/integration/test_shapes_e2e.py
+++ b/tests/integration/test_shapes_e2e.py
@@ -4,8 +4,16 @@
0 = unconfirmed (timeout), raise = rejected. The pre-fix client treated
SET_SHAPES as fire-and-forget and returned 1 unconditionally — every assert
here except the plain success one fails against that behavior.
+
+The invalidation tests cover the MoveIt-style contract: a world change
+re-guards the streaming trajectory's remaining waypoints and every queued
+trajectory at activation — committed motion never sails into a keep-out
+declared after it was planned.
"""
+import time
+
+import numpy as np
import pytest
from parol6 import MotionError, RobotClient
@@ -13,6 +21,38 @@
pytestmark = pytest.mark.integration
+# The HOME command parks the arm at J1=90; invalidation moves sweep J1 downward.
+HOME_J1 = 90.0
+
+
+def _wrist_box(target_deg: list[float], name: str) -> Box:
+ """A keep-out enveloping the wrist position of ``target_deg``."""
+ import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+ p = PAROL6_ROBOT.robot.fkine(np.radians(target_deg))[:3, 3]
+ return Box(
+ name=name,
+ x=0.25,
+ y=0.25,
+ z=0.25,
+ pose=(float(p[0]), float(p[1]), float(p[2]), 0, 0, 0),
+ )
+
+
+def _wait_until(pred, timeout: float, msg: str) -> None:
+ deadline = time.monotonic() + timeout
+ while time.monotonic() < deadline:
+ if pred():
+ return
+ time.sleep(0.02)
+ pytest.fail(msg)
+
+
+def _j1(client: RobotClient) -> float:
+ angles = client.angles()
+ assert angles is not None
+ return angles[0]
+
def test_set_shapes_ack_readback_rejection_and_timeout(
server_proc, client: RobotClient, ports
@@ -44,3 +84,94 @@ def test_set_shapes_ack_readback_rejection_and_timeout(
assert client.set_shapes([]) == 1
world = client.shapes()
assert world is not None and world.program == ()
+
+
+def test_set_shapes_mid_flight_halts_streaming_move(client: RobotClient, server_proc):
+ """A keep-out declared over the *remaining* path of a streaming move halts
+ it with the collision error instead of letting committed motion sail into
+ the new keep-out (pre-fix: the segment player never re-checked)."""
+ target = [0.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+ try:
+ idx = client.move_j(target, duration=4.0, wait=False)
+ assert idx >= 0
+ _wait_until(
+ lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming"
+ )
+
+ assert client.set_shapes([_wrist_box(target, "blocker")]) == 1
+ _wait_until(
+ lambda: client.error() is not None,
+ 3.0,
+ "world change never halted the move",
+ )
+ err = client.error()
+ assert err is not None and "shape:blocker" in err.cause
+
+ j1_stop = _j1(client)
+ assert j1_stop > 30.0, f"arm reached the keep-out region (J1={j1_stop:.1f})"
+ time.sleep(0.3)
+ assert abs(_j1(client) - j1_stop) < 0.5, "arm kept moving after the halt"
+ finally:
+ client.reset()
+ assert client.set_shapes([]) == 1
+
+
+def test_set_shapes_mid_flight_rejects_queued_move_at_activation(
+ client: RobotClient, server_proc
+):
+ """A queued move planned against the old world is re-guarded when it
+ activates: the first (clear) move completes, the second (now blocked)
+ never streams."""
+ t1 = [60.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+ t2 = [0.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+ try:
+ i1 = client.move_j(t1, duration=1.5, wait=False)
+ i2 = client.move_j(t2, duration=2.0, wait=False)
+ assert i1 >= 0 and i2 >= 0
+ _wait_until(
+ lambda: _j1(client) < HOME_J1 - 2.0, 10.0, "first move never started"
+ )
+
+ assert client.set_shapes([_wrist_box(t2, "late-wall")]) == 1
+
+ _wait_until(
+ lambda: client.error() is not None,
+ 10.0,
+ "queued move was never invalidated",
+ )
+ err = client.error()
+ assert err is not None and "shape:late-wall" in err.cause
+ # The clear first move finished; the blocked second never streamed.
+ _wait_until(
+ lambda: abs(_j1(client) - 60.0) < 2.0,
+ 10.0,
+ f"arm not at the first target (J1={_j1(client):.1f})",
+ )
+ time.sleep(0.3)
+ assert abs(_j1(client) - 60.0) < 2.0, "second move streamed despite the wall"
+ finally:
+ client.reset()
+ assert client.set_shapes([]) == 1
+
+
+def test_set_shapes_mid_flight_off_path_does_not_disturb_motion(
+ client: RobotClient, server_proc
+):
+ """The re-guard must not manufacture failures: a mid-flight world change
+ that stays clear of the path leaves the move to complete normally."""
+ target = [30.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+ try:
+ idx = client.move_j(target, duration=2.5, wait=False)
+ assert idx >= 0
+ _wait_until(
+ lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming"
+ )
+
+ far = Box(name="far", x=0.1, y=0.1, z=0.1, pose=(0.9, 0.9, 0.9, 0, 0, 0))
+ assert client.set_shapes([far]) == 1
+
+ assert client.wait_command(idx, timeout=10.0), "move did not complete"
+ assert client.error() is None
+ assert abs(_j1(client) - 30.0) < 1.0
+ finally:
+ assert client.set_shapes([]) == 1
diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py
index 3581bcd..763f26b 100644
--- a/tests/unit/test_collision_enablement.py
+++ b/tests/unit/test_collision_enablement.py
@@ -19,11 +19,14 @@
class _FakeChecker:
- """``distance`` may be a float or a callable(q) for escape-semantics tests."""
+ """``distance``/``pairs`` may be values or callables(q) for escape-semantics
+ tests. ``pairs`` feeds the new-contact half of the escape rule; the default
+ (no pairs anywhere) leaves the distance-trend half in charge."""
- def __init__(self, distance, collides):
+ def __init__(self, distance, collides, pairs=()):
self._distance = distance
self._collides = collides
+ self._pairs = pairs
self.queries = 0
def min_distance(self, q):
@@ -33,6 +36,9 @@ def in_collision(self, q):
self.queries += 1
return self._collides(q)
+ def colliding_pairs(self, q):
+ return list(self._pairs(q)) if callable(self._pairs) else list(self._pairs)
+
def test_gate_skips_per_direction_checks_when_far():
joint_en = np.ones(12, dtype=np.uint8)
@@ -81,6 +87,22 @@ def test_gate_keeps_escaping_directions_when_already_inside():
assert joint_en[2] == 1 and joint_en[3] == 1 # no-change dirs stay enabled
+def test_gate_greys_direction_entering_new_pair_while_inside():
+ """Inside keep-out A, a step that leaves the global min improving but
+ contacts a NEW pair must grey — parity with the jog guard's pair diff."""
+ joint_en = np.ones(12, dtype=np.uint8)
+ checker = _FakeChecker(
+ lambda q: -0.01 + q[0], # A dominates; J1+ improves, J1- deepens
+ lambda q: True,
+ pairs=lambda q: [("L6", "shape:A")]
+ + ([("L4", "shape:B")] if q[1] > 0.001 else []),
+ )
+ gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6))
+ assert joint_en[0] == 1 # J1+ pure escape -> enabled
+ assert joint_en[2] == 0 # J2+ min unchanged but contacts B -> greyed
+ assert joint_en[3] == 1 # J2- no new contact, min unchanged -> enabled
+
+
def test_collision_blocked_escape_semantics():
"""Shared jog/guard decision: approach blocks, escape is allowed."""
# Approaching: current clear, target colliding -> blocked.
@@ -93,6 +115,22 @@ def test_collision_blocked_escape_semantics():
assert collision_blocked(inside, np.zeros(6), np.full(6, -0.1)) is True
+def test_collision_blocked_new_pair_blocks_even_when_global_min_improves():
+ """Escaping keep-out A while contacting a NEW pair B must block: the pair
+ diff catches what the global min-distance trend (still dominated by the
+ deeper A) cannot — the exact rule guard_joint_path already applies."""
+ inside = _FakeChecker(
+ lambda q: -0.05 + q[0], # A dominates the global min; +x improves it
+ lambda q: True,
+ pairs=lambda q: [("L6", "shape:A")]
+ + ([("L5", "shape:B")] if q[0] > 0.05 else []),
+ )
+ # Pure escape (no new contact) stays allowed…
+ assert collision_blocked(inside, np.zeros(6), np.full(6, 0.01)) is False
+ # …but the same improving trend with a new pair at the target blocks.
+ assert collision_blocked(inside, np.zeros(6), np.full(6, 0.1)) is True
+
+
@dataclass
class _FakeIK:
success: bool
@@ -152,6 +190,24 @@ def solve_ik(robot, target, q_seed, quiet_logging=True):
assert out[1] == 0 # X- deeper -> greyed
+def test_cart_gate_greys_direction_entering_new_pair_while_inside():
+ """Inside keep-out A, a Cartesian direction whose solved config contacts a
+ NEW pair greys even though the global min-distance holds steady."""
+
+ def solve_ik(robot, target, q_seed, quiet_logging=True):
+ return _FakeIK(True, target[:3, 3].repeat(2)) # q = [x, x, y, y, z, z]
+
+ checker = _FakeChecker(
+ lambda q: -0.01 + q[0], # A dominates; only x motion changes it
+ lambda q: True,
+ pairs=lambda q: [("L6", "shape:A")] + ([("L4", "shape:B")] if q[2] > 0 else []),
+ )
+ out = _cart_enable(checker, solve_ik)
+ assert out[0] == 1 # X+ pure escape -> enabled
+ assert out[2] == 0 # Y+ min unchanged but contacts B -> greyed
+ assert out[3] == 1 # Y- no new contact -> enabled
+
+
def test_drain_sync_applies_tool_and_shapes():
applied = []
diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py
index af52f40..59269cd 100644
--- a/tests/unit/test_collision_integration.py
+++ b/tests/unit/test_collision_integration.py
@@ -380,6 +380,87 @@ def test_jogl_release_decel_streams_while_escaping():
PAROL6_ROBOT.apply_shapes([])
+def test_jogl_escape_never_streams_into_a_second_keepout():
+ """Escaping keep-out A (jog +Z out of a cage around the wrist) must not
+ stream configs into a DIFFERENT keep-out B above: pre-fix the escape gate
+ compared only the global min-distance — still dominated by A — so the new
+ contact with B never registered. Real command, real CSE, real IK, real
+ checker."""
+ from waldoctl import Box
+
+ from parol6.commands.cartesian_commands import JogLCommand
+ from parol6.config import deg_to_steps, steps_to_rad
+ from parol6.protocol.wire import JogLCmd
+ from parol6.server.state import ControllerState
+
+ state = ControllerState()
+ q_home_deg = np.array([0.0, -90.0, 180.0, 0.0, 0.0, 180.0])
+ deg_to_steps(q_home_deg, state.Position_in)
+ state.Position_out[:] = state.Position_in
+ checker = PAROL6_ROBOT.collision
+ q0 = np.radians(q_home_deg)
+ try:
+ # A is tall enough that the wrist is still inside it when it reaches
+ # B's underside — the block must come from the pair diff, not from the
+ # (clear-of-A) plain approach gate.
+ PAROL6_ROBOT.apply_shapes(
+ [
+ Box(name="A", x=0.15, y=0.15, z=0.25, pose=(0.237, 0.0, 0.30, 0, 0, 0)),
+ Box(name="B", x=0.10, y=0.10, z=0.04, pose=(0.237, 0.0, 0.43, 0, 0, 0)),
+ ]
+ )
+ start = {n for pair in checker.colliding_pairs(q0) for n in pair}
+ assert "shape:A" in start and "shape:B" not in start
+
+ cmd = JogLCommand(JogLCmd(velocities=[0.0, 0.0, 1.0, 0, 0, 0], duration=5.0))
+ cmd.setup(state)
+ q_buf = np.zeros(6)
+ hit_b_streamed = False
+ for _ in range(500):
+ code = cmd.execute_step(state)
+ steps_to_rad(state.Position_out, q_buf)
+ streamed = {n for pair in checker.colliding_pairs(q_buf) for n in pair}
+ if "shape:B" in streamed:
+ hit_b_streamed = True
+ break
+ if code != ExecutionStatusCode.EXECUTING:
+ break
+ assert not hit_b_streamed, (
+ "jog streamed the arm into keep-out B while escaping A"
+ )
+ assert state.collision_active is True
+ assert any("shape:B" in name for pair in state.collision_pairs for name in pair)
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
+
+
+def test_tool_pair_names_use_role_vocabulary_not_stl_basenames():
+ """Attached-tool collision geometry reports ``tool::`` — never
+ a raw mesh filename. A keep-out enveloping the wrist+tool forces real
+ tool pairs through display_pairs."""
+ from waldoctl import Box
+
+ q = np.radians(np.array([0.0, -90.0, 180.0, 0.0, 0.0, 180.0]))
+ checker = PAROL6_ROBOT.collision
+ try:
+ PAROL6_ROBOT.apply_tool("SSG-48")
+ assert PAROL6_ROBOT._active_tool_geom_names == [
+ "tool:SSG-48:body",
+ "tool:SSG-48:jaw",
+ "tool:SSG-48:jaw_2",
+ ]
+ PAROL6_ROBOT.apply_shapes(
+ [Box(name="blk", x=0.3, y=0.3, z=0.3, pose=(0.237, 0.0, 0.334, 0, 0, 0))]
+ )
+ pairs = PAROL6_ROBOT.display_pairs(checker.colliding_pairs(q))
+ flat = [name for pair in pairs for name in pair]
+ assert any(n.startswith("tool:SSG-48:") for n in flat)
+ assert not any(".stl" in n for n in flat)
+ finally:
+ PAROL6_ROBOT.apply_shapes([])
+ PAROL6_ROBOT.apply_tool("NONE")
+
+
def test_dry_run_script_set_shapes_applies_and_replays():
"""A script's ``set_shapes`` through the REAL dry-run dispatch: applies the
world (raw waldoctl Shapes end-to-end — the pre-fix path crashed with
From f31d47fed69f26307410ec5213a6e60003c48534 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 00:18:51 -0400
Subject: [PATCH 31/36] CI: pre-install ruckig from fixed upstream SHA
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
ruckig 0.17.3 on PyPI is sdist-only and its build config still uses
cmake.targets, which scikit-build-core 1.0 rejects as a hard error —
every CI job has died at dependency install since the 1.0 release.
Upstream fixed the rename on main (pantor/ruckig#262) but has not cut
a release, so pre-install that commit before ".[dev]" in both the lint
and test jobs; its version (0.17.3) satisfies the ruckig>=0.12.2 pin,
so the dev install leaves it in place. Drop the override once a fixed
ruckig release lands.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
.github/workflows/tests.yml | 10 ++++++++++
1 file changed, 10 insertions(+)
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 2e3b064..e417488 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -24,6 +24,11 @@ jobs:
shell: bash
run: |
python -m pip install --upgrade pip
+ # ruckig 0.17.3 (sdist-only) fails to build under scikit-build-core
+ # 1.0 (cmake.targets was renamed to build.targets); pre-install the
+ # fixed upstream commit until a release lands.
+ # https://github.com/pantor/ruckig/pull/262
+ pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff"
pip install -e ".[dev]"
# Override the pinned waldoctl with the matching feature branch if one
# exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag
@@ -106,6 +111,11 @@ jobs:
- name: Install package
run: |
python -m pip install --upgrade pip
+ # ruckig 0.17.3 (sdist-only) fails to build under scikit-build-core
+ # 1.0 (cmake.targets was renamed to build.targets); pre-install the
+ # fixed upstream commit until a release lands.
+ # https://github.com/pantor/ruckig/pull/262
+ pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff"
pip install -e ".[dev]" pytest-timeout
# Override the pinned waldoctl with the matching feature branch if one
# exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag
From 6d2d7f6414a192c8ef5fee8ab9bea9a9aa9da102 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 00:56:53 -0400
Subject: [PATCH 32/36] ty: assert IK limit caches non-None before the njit
safety check
numba 0.66 ships a _dispatcher.pyi stub, so ty now type-checks njit
call arguments; the four module-level limit caches are ndarray | None
and need the same narrowing the solver/result caches already get.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
parol6/utils/ik.py | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/parol6/utils/ik.py b/parol6/utils/ik.py
index b856e23..c8f1f38 100644
--- a/parol6/utils/ik.py
+++ b/parol6/utils/ik.py
@@ -169,6 +169,10 @@ def solve_ik(
_ensure_cache(robot)
assert _cached_solver is not None
assert _cached_result is not None
+ assert _cached_buffered_min is not None
+ assert _cached_buffered_max is not None
+ assert _cached_qlim_min is not None
+ assert _cached_qlim_max is not None
solver = _cached_solver
result = _cached_result
From fe9dd81195e39b2d6e704019e7c86ded201e3d11 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 02:30:58 -0400
Subject: [PATCH 33/36] deps: bump waldoctl to v0.5.0, pinokin to v0.1.7
release wheels
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Wheel URLs written from the actual v0.1.7 assets — the cp312-cp314
macOS wheels moved to macosx_26_0_arm64 in this build.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
pyproject.toml | 36 ++++++++++++++++++------------------
1 file changed, 18 insertions(+), 18 deletions(-)
diff --git a/pyproject.toml b/pyproject.toml
index e11c840..bf5369d 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -10,31 +10,31 @@ description = "Python library for controlling PAROL6 robot arms"
requires-python = ">=3.11"
dependencies = [
# pinokin: Pinocchio-based FK/IK bindings
- # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.6
+ # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.7
# macOS ARM64 (Apple Silicon)
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-macosx_26_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-macosx_26_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-macosx_26_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
# Windows AMD64
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
# Linux x86_64
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
# Linux aarch64 (ARM64)
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
"pyserial>=3.4",
"scipy>=1.11.4",
@@ -46,7 +46,7 @@ dependencies = [
"psutil>=5.9",
"msgspec>=0.18",
"ormsgpack>=1.4.0",
- "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.4.0",
+ "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.5.0",
]
[tool.setuptools.packages.find]
From 6b6391a09518db21d93d6c274006b75bc290f76d Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 22:30:43 -0400
Subject: [PATCH 34/36] deps: bump pinokin to v0.1.8 (deterministic macosx_15_0
wheels)
v0.1.7's cp312-314 macOS wheels were tagged macosx_26_0 (built on the
macos-latest pool mid-rollout), which pip rejects on macOS 15 runners.
v0.1.8 pins MACOSX_DEPLOYMENT_TARGET=15.0 in the wheel build, so every
macOS wheel is macosx_15_0_arm64.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
pyproject.toml | 34 +++++++++++++++++-----------------
1 file changed, 17 insertions(+), 17 deletions(-)
diff --git a/pyproject.toml b/pyproject.toml
index bf5369d..2d19233 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -10,31 +10,31 @@ description = "Python library for controlling PAROL6 robot arms"
requires-python = ">=3.11"
dependencies = [
# pinokin: Pinocchio-based FK/IK bindings
- # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.7
+ # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.8
# macOS ARM64 (Apple Silicon)
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-macosx_26_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-macosx_26_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-macosx_26_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
# Windows AMD64
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
# Linux x86_64
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
# Linux aarch64 (ARM64)
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
- "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.7/pinokin-0.1.7-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+ "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
"pyserial>=3.4",
"scipy>=1.11.4",
From 292d298771af87b6a06c5ce232fd6340b268c46b Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 23:05:08 -0400
Subject: [PATCH 35/36] StatusBuffer: cart_en as a declared field, not a
property
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
waldoctl's StatusBuffer protocol declares cart_en as a plain attribute;
ty 0.0.57 enforces that a read-only property doesn't satisfy a mutable
protocol member. The dict is still built once in __post_init__, aliasing
the two enable arrays the decoder mutates in place — nothing per-tick.
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
parol6/protocol/wire.py | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py
index eac9e09..2e15c02 100644
--- a/parol6/protocol/wire.py
+++ b/parol6/protocol/wire.py
@@ -1419,18 +1419,18 @@ class StatusBuffer:
collision_active: bool = False
collision_pairs: list[tuple[str, str]] = field(default_factory=list)
scene_epoch: int = 0
+ # Frame name → (12,) int32 Cartesian enable envelope. A plain attribute
+ # (not a property) because waldoctl's StatusBuffer protocol declares it
+ # as one; built once in __post_init__ aliasing the two arrays, which are
+ # mutated in place by the decoder — never rebuilt per tick.
+ cart_en: dict[str, np.ndarray] = field(init=False, repr=False, compare=False)
def __post_init__(self) -> None:
- self._cart_en_dict: dict[str, np.ndarray] = {
+ self.cart_en = {
"WRF": self.cart_en_wrf,
"TRF": self.cart_en_trf,
}
- @property
- def cart_en(self) -> dict[str, np.ndarray]:
- """Frame name → (12,) int32 Cartesian enable envelope."""
- return self._cart_en_dict
-
def copy(self) -> "StatusBuffer":
"""Return a deep copy with all arrays copied."""
ts = self.tool_status
From 1c87409c422dc4d9ab67c1ae49f0b04d71ae7a52 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 8 Jul 2026 23:32:01 -0400
Subject: [PATCH 36/36] Comments: trim narration to one-line whys
Co-Authored-By: Claude Fable 5
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
---
.github/workflows/tests.yml | 12 ++----
parol6/PAROL6_ROBOT.py | 60 +++++++++------------------
parol6/commands/_collision_guard.py | 19 ++++-----
parol6/commands/cartesian_commands.py | 25 ++++-------
parol6/protocol/wire.py | 6 +--
5 files changed, 40 insertions(+), 82 deletions(-)
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index e417488..75cc487 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -24,10 +24,8 @@ jobs:
shell: bash
run: |
python -m pip install --upgrade pip
- # ruckig 0.17.3 (sdist-only) fails to build under scikit-build-core
- # 1.0 (cmake.targets was renamed to build.targets); pre-install the
- # fixed upstream commit until a release lands.
- # https://github.com/pantor/ruckig/pull/262
+ # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0;
+ # pre-install the fixed commit until a release lands (pantor/ruckig#262).
pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff"
pip install -e ".[dev]"
# Override the pinned waldoctl with the matching feature branch if one
@@ -111,10 +109,8 @@ jobs:
- name: Install package
run: |
python -m pip install --upgrade pip
- # ruckig 0.17.3 (sdist-only) fails to build under scikit-build-core
- # 1.0 (cmake.targets was renamed to build.targets); pre-install the
- # fixed upstream commit until a release lands.
- # https://github.com/pantor/ruckig/pull/262
+ # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0;
+ # pre-install the fixed commit until a release lands (pantor/ruckig#262).
pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff"
pip install -e ".[dev]" pytest-timeout
# Override the pinned waldoctl with the matching feature branch if one
diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py
index 95b1dcc..a4a10b4 100644
--- a/parol6/PAROL6_ROBOT.py
+++ b/parol6/PAROL6_ROBOT.py
@@ -63,18 +63,13 @@
# Current robot instance (tool transform applied in-place)
robot: Robot = Robot(_urdf_path)
-# Self-collision checker bound to the same pinokin Robot. Built eagerly when
-# ``parol6.config`` is imported (config.py calls ``_init_collision_checker``),
-# i.e. on any ``import parol6``; stays None when collision checking is disabled
-# or geometry fails to load. Treat None as "checks disabled" everywhere.
-# TODO: defer construction to a server-side ``ensure_collision_checker()`` so
-# pure RobotClient script subprocesses don't pay the URDF-rewrite + BVH build.
+# Built at import via config._init_collision_checker; None means checks disabled.
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.
+ absolute 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
@@ -88,11 +83,8 @@ 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` -> 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.
+ # Plain absolute path, not file://: coal strips the scheme naively, which
+ # on Windows yields an invalid `/D:/...`.
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:
@@ -123,8 +115,6 @@ def _init_collision_checker(
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, clearance_margin=clearance_margin
@@ -138,8 +128,8 @@ def _init_collision_checker(
c.num_geometry_objects,
)
except Exception as e: # noqa: BLE001
- # Enabled but failed to build: fail loud. Silently running the arm with
- # no collision checking is unsafe; require an explicit opt-out.
+ # Silently running 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 "
@@ -155,20 +145,15 @@ def _init_collision_checker(
) from e
-# Geometry-object names for meshes attached to the collision checker on
-# behalf of the currently-active tool, plus the (tool, variant) they were
-# attached for so an unchanged re-apply can skip the disk reload.
+# Active tool's checker geometry; the key lets an unchanged re-apply skip reload.
_active_tool_geom_names: list[str] = []
_active_tool_geom_key: tuple[str, str | None] | None = None
-# Geometry-object names for user-placed workspace shapes (keep-out barriers)
-# currently on this process's collision checker, plus the applied Shape list
-# itself for readback.
+# Program-layer keep-out shapes on this process's checker (+ list for readback).
_active_shape_names: list[str] = []
_program_shapes: list = []
-# Installation-layer shapes (from robot config) and their geometry names.
-# Every program inherits these; set_shapes cannot change them.
+# Installation-layer shapes; every program inherits these, set_shapes can't touch.
_installation_shapes: list = []
_installation_geom_names: list[str] = []
@@ -192,9 +177,8 @@ def _refresh_collision_tool_geometry(
key = (tool_key, variant_key)
if key == _active_tool_geom_key:
return
- # Clear the previous tool's geometry. Mark the key inconsistent until the
- # new attaches finish, so a mid-loop failure self-repairs on the next call
- # (otherwise the early-return above would skip a partial attach forever).
+ # Key stays unset until the attaches finish so a mid-loop failure
+ # self-repairs on the next call.
for name in _active_tool_geom_names:
collision.remove_geometry_by_name(name)
_active_tool_geom_names.clear()
@@ -204,9 +188,8 @@ def _refresh_collision_tool_geometry(
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).
+ # An empty variant deliberately falls back to cfg.meshes (unlike WC's
+ # swap_tool_mesh).
meshes = cfg.meshes
if variant_key:
for v in cfg.variants:
@@ -218,16 +201,12 @@ def _refresh_collision_tool_geometry(
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.
+ # rpy is (0,0,0) for all current MeshSpecs — rotation is baked
+ # into the STL (see _MESH_RPY in tools.py).
T = np.eye(4, dtype=np.float64)
T[:3, 3] = spec.origin
- # tool: namespace with the mesh's semantic role so colliding-
- # pair reports speak the defined vocabulary (link names /
- # shape: / install: / tool:), never raw mesh filenames.
- # Repeated roles (two jaws) get a positional suffix for the
- # attach/detach bookkeeping's name uniqueness.
+ # Pair reports speak the tool:{key}:{role} vocabulary, never
+ # raw mesh filenames; repeated roles get a positional suffix.
role = spec.role.name.lower()
n = role_counts[role] = role_counts.get(role, 0) + 1
geom_name = f"tool:{tool_key}:{role}" + (f"_{n}" if n > 1 else "")
@@ -310,9 +289,8 @@ def _pose_to_matrix(pose: "Sequence[float]") -> np.ndarray:
return T
-# Kinds pinokin's add_obstacle supports. Guards atomicity across a
-# waldoctl/pinokin version skew: a kind the checker would reject raises here,
-# BEFORE the old collision world is removed.
+# Kinds pinokin's add_obstacle supports; unknown kinds raise BEFORE the old
+# collision world is removed.
_SHAPE_KINDS = frozenset(
{"box", "sphere", "cylinder", "capsule", "cone", "ellipsoid", "plane"}
)
diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py
index 0455c72..d6e0d27 100644
--- a/parol6/commands/_collision_guard.py
+++ b/parol6/commands/_collision_guard.py
@@ -19,9 +19,8 @@
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-check tolerance (m): min-distance drops within this count as "not
+# deeper" (absorbs signed-distance jitter).
_ESCAPE_TOL = 1e-4
@@ -86,9 +85,8 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
if n == 0:
return
- # 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.
+ # n > target guarantees linspace spacing > 1, so rounded indices are
+ # strictly increasing; np.unique is cheap insurance.
target = max(2, COLLISION_PATH_SAMPLES + 2)
if n <= target:
idx = None
@@ -98,8 +96,7 @@ def guard_joint_path(positions: NDArray[np.float64]) -> None:
sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array
def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
- # Reporting vocabulary (URDF link names / shape: / install: / tool:),
- # never checker-internal geometry identifiers.
+ # Reporting vocabulary, never checker-internal geometry identifiers.
pairs = PAROL6_ROBOT.display_pairs(pairs)
exc = TrajectoryPlanningError(
make_error(
@@ -121,10 +118,8 @@ def _raise(sample: int, pairs: list[tuple[str, str]]) -> None:
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.)
+ # Already in collision at the start: permit an escaping move (no new
+ # colliding pair, no deeper than the start) so the arm isn't trapped.
d0 = checker.min_distance(pos[0])
start_pairs = set(checker.colliding_pairs(pos[0]))
for j in range(1, sub.shape[0]):
diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py
index 64fcf2d..281b05e 100644
--- a/parol6/commands/cartesian_commands.py
+++ b/parol6/commands/cartesian_commands.py
@@ -156,11 +156,8 @@ 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:
- # Don't stream a colliding config during the release
- # deceleration — but keep streaming while ESCAPING from
- # inside a keep-out (same semantics as the held phase),
- # else the target freezes at the release point and the
- # arm jerks instead of coasting to a stop.
+ # Keep streaming while escaping from inside a keep-out,
+ # else the target freezes at release and the arm jerks.
checker = PAROL6_ROBOT.collision
if checker is None or not collision_blocked(
checker, self._q_commanded, ik_result.q
@@ -192,11 +189,8 @@ 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). 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).
+ # While stopping, leave the CSE target at zero — re-commanding full
+ # velocity every tick would defeat cse.stop()'s deceleration.
if not self._ik_stopping:
if self.p.frame == "WRF":
cse.set_jog_velocity_1dof_wrf(
@@ -231,11 +225,9 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
return ExecutionStatusCode.COMPLETED
return ExecutionStatusCode.EXECUTING
- # 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. When the arm is
- # ALREADY inside (a keep-out placed over it), escaping motion is
- # allowed, mirroring the planner guard's start-in-collision semantics.
+ # Predicted collision decelerates like an IK failure (no mid-jog
+ # raise); escaping from inside a keep-out stays allowed, mirroring the
+ # planner guard.
checker = PAROL6_ROBOT.collision
if checker is not None and collision_blocked(
checker, self._q_commanded, ik_result.q
@@ -254,8 +246,7 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
self._ik_stopping = True
return ExecutionStatusCode.EXECUTING
- # Reachable + collision-free again — if we were stopping (IK failure or
- # predicted self-collision), recover by resuming jogging.
+ # Reachable + collision-free again — resume jogging.
if self._ik_stopping:
logger.info("[CARTJOG] constraint cleared - resuming jog")
state.clear_collision()
diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py
index 2e15c02..00210e7 100644
--- a/parol6/protocol/wire.py
+++ b/parol6/protocol/wire.py
@@ -1419,10 +1419,8 @@ class StatusBuffer:
collision_active: bool = False
collision_pairs: list[tuple[str, str]] = field(default_factory=list)
scene_epoch: int = 0
- # Frame name → (12,) int32 Cartesian enable envelope. A plain attribute
- # (not a property) because waldoctl's StatusBuffer protocol declares it
- # as one; built once in __post_init__ aliasing the two arrays, which are
- # mutated in place by the decoder — never rebuilt per tick.
+ # Built once in __post_init__, aliasing the two enable arrays the decoder
+ # mutates in place.
cart_en: dict[str, np.ndarray] = field(init=False, repr=False, compare=False)
def __post_init__(self) -> None: