diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 7a06f83..75cc487 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -24,6 +24,9 @@ jobs: shell: bash run: | python -m pip install --upgrade pip + # 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 # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag @@ -36,21 +39,67 @@ jobs: fi - name: Run pre-commit uses: pre-commit/action@v3.0.1 + test: name: ${{ matrix.os }} / Python ${{ matrix.python-version }} runs-on: ${{ matrix.os }} - timeout-minutes: 30 + timeout-minutes: 45 strategy: fail-fast: false matrix: os: [ubuntu-latest, windows-latest, macos-latest] python-version: ['3.11', '3.12', '3.13', '3.14'] + defaults: + run: + shell: bash -l {0} + steps: - name: Checkout repository (with submodules) uses: actions/checkout@v4 - - name: Setup Python + - name: Detect matching pinokin branch + id: pinokin + shell: bash + run: | + BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}" + if git ls-remote --heads https://github.com/Jepson2k/pinokin.git "$BRANCH" 2>/dev/null | grep -q .; then + echo "matching=true" >> "$GITHUB_OUTPUT" + echo "branch=$BRANCH" >> "$GITHUB_OUTPUT" + else + echo "matching=false" >> "$GITHUB_OUTPUT" + fi + + # ---------------------------------------------------------------------- + # Path A: matching pinokin branch -> conda env (provides libpinocchio + + # libcoal headers/libs) so pinokin can be built from source. We install + # PAROL6 first (which pulls in the pinned pinokin v0.1.6 wheel) and + # then override pinokin with the source-built wheel from the matching + # branch. This is the only way to exercise unreleased pinokin features + # before a release lands. + # ---------------------------------------------------------------------- + - name: Setup Miniforge (matching pinokin branch) + if: steps.pinokin.outputs.matching == 'true' + uses: conda-incubator/setup-miniconda@v3 + with: + miniforge-version: latest + python-version: ${{ matrix.python-version }} + conda-remove-defaults: true + activate-environment: parol6-test + + - name: Clone matching pinokin branch + if: steps.pinokin.outputs.matching == 'true' + run: | + git clone --depth=1 --branch="${{ steps.pinokin.outputs.branch }}" https://github.com/Jepson2k/pinokin.git pinokin-src + conda env update -n parol6-test -f pinokin-src/environment.yml + + # ---------------------------------------------------------------------- + # Path B: no matching pinokin branch -> plain pip with the pinned + # release wheel. Anything in PAROL6 that requires a newer pinokin + # feature will fail loudly here (intended). + # ---------------------------------------------------------------------- + - name: Setup Python (pinned pinokin) + if: steps.pinokin.outputs.matching != 'true' uses: actions/setup-python@v5 with: python-version: ${{ matrix.python-version }} @@ -58,9 +107,11 @@ jobs: cache-dependency-path: pyproject.toml - name: Install package - shell: bash run: | python -m pip install --upgrade pip + # 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 # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag @@ -72,6 +123,16 @@ jobs: pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}" fi + # Override the pinned pinokin v0.1.6 wheel with the matching-branch + # source build. --force-reinstall + --no-deps swaps just pinokin + # without disturbing other resolved dependencies. + - name: Override pinokin with matching-branch source build + if: steps.pinokin.outputs.matching == 'true' + run: | + cd pinokin-src + pip install . --no-build-isolation --force-reinstall --no-deps + python -c "from pinokin import CollisionChecker; print('CollisionChecker available')" + - name: Show environment run: | python -V 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 28b1ede..a4a10b4 100644 --- a/parol6/PAROL6_ROBOT.py +++ b/parol6/PAROL6_ROBOT.py @@ -2,13 +2,15 @@ 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 -from pinokin import Robot +from pinokin import CollisionChecker, Robot from parol6.tools import get_tool_transform @@ -56,10 +58,174 @@ _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) +# 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 paths so pinokin's mesh loader can resolve them. + + The PAROL6 URDF was authored for a ROS package layout (meshes at + `parol6/meshes/`) but the Python package places them at + `parol6/urdf_model/meshes/`. Rewriting at runtime keeps the source + URDF unchanged and avoids fragile symlink farms. + + Writes a fresh temp file each call and cleans it up at interpreter exit. + """ + import tempfile + + src = Path(_urdf_path) + text = src.read_text() + mesh_root = Path(_mesh_dir) / "meshes" + # 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: + f.write(rewritten) + + @atexit.register + def _cleanup_tmp_urdf() -> None: + try: + os.unlink(tmp_path) + except OSError: + pass + + return tmp_path + + +def _init_collision_checker( + enabled: bool, srdf_path: str, clearance_margin: float = 0.0 +) -> None: + """Build the singleton CollisionChecker when *enabled*. + + Config values are passed in (by ``parol6.config`` after its knobs are + defined) rather than imported here, keeping the dependency one-directional + — ``config`` imports ``PAROL6_ROBOT``, not the other way around. + """ + global collision + if not enabled: + collision = None + return + + try: + urdf_for_collision = _resolved_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 + logger.info( + "Collision checker loaded: %d pairs, %d geometry objects", + c.num_collision_pairs, + c.num_geometry_objects, + ) + except Exception as e: # noqa: BLE001 + # 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 " + "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 + + +# 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 + +# Program-layer keep-out shapes on this process's checker (+ list for readback). +_active_shape_names: list[str] = [] +_program_shapes: list = [] + +# Installation-layer shapes; every program inherits these, set_shapes can't touch. +_installation_shapes: list = [] +_installation_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 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 + # 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() + _active_tool_geom_key = None + + from parol6.tools import get_registry + + cfg = None if tool_key == "NONE" else get_registry().get(tool_key) + if cfg is not None: + # 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: + if v.key == variant_key and v.meshes: + 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 + # 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 + # 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 "") + collision.attach_mesh_to_frame( + geom_name, + str(path), + parent_frame="L6", + placement=T, + ) + _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: + collision.remove_geometry_by_name(name) + _active_tool_geom_names.clear() + raise + + _active_tool_geom_key = key + def apply_tool( tool_name: str, @@ -95,6 +261,136 @@ 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) + + +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). + + 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) + 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 + + +# 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"} +) + + +def _validate_shapes(shapes: "Iterable[Any]") -> "list[Any]": + """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. Raises before + any mutation so an error can never leave a half-applied world. + """ + 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: + if s.kind not in _SHAPE_KINDS: + raise ValueError(f"Shape {s.name!r}: unknown kind {s.kind!r}") + return shapes + + +def apply_shapes(shapes: "Iterable[Any]") -> None: + """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, _program_shapes + shapes = _validate_shapes(shapes) + _program_shapes = shapes + 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, 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 ec66551..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, 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 @@ -68,7 +69,11 @@ ResponseMsg, SelectProfileCmd, SelectToolCmd, + SetShapesCmd, + ShapesCmd, + ShapesResultStruct, SetTcpOffsetCmd, + ShapeWire, ServoJCmd, ServoJPoseCmd, ServoLCmd, @@ -931,6 +936,52 @@ 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 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 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 + + 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 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 0febb7e..8c28e85 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -323,6 +323,19 @@ 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 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 new file mode 100644 index 0000000..d6e0d27 --- /dev/null +++ b/parol6/commands/_collision_guard.py @@ -0,0 +1,133 @@ +"""Shared self-collision pre-flight check used by motion commands. + +`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. +""" + +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 TrajectoryPlanningError + +# Escape-check tolerance (m): min-distance drops within this count as "not +# deeper" (absorbs signed-distance jitter). +_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 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): + 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 + ) + 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. + + 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_joint_path(positions: NDArray[np.float64]) -> None: + """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: + 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 + + # 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 + sub = pos # already contiguous float64 — no copy needed + else: + idx = np.unique(np.linspace(0, n - 1, target).round().astype(int)) + sub = pos[idx] # fancy indexing yields a fresh contiguous float64 array + + def _raise(sample: int, pairs: list[tuple[str, str]]) -> None: + # Reporting vocabulary, never checker-internal geometry identifiers. + pairs = PAROL6_ROBOT.display_pairs(pairs) + exc = TrajectoryPlanningError( + make_error( + ErrorCode.SYS_SELF_COLLISION, + sample=str(sample), + total=str(n), + 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: + 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])) + + # 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]): + 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/basic_commands.py b/parol6/commands/basic_commands.py index 59d0f52..16c76c6 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, @@ -24,11 +25,14 @@ 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 from parol6.server.transports.transport_factory import is_simulation_mode +import parol6.PAROL6_ROBOT as PAROL6_ROBOT # noqa: N811 + from .base import ( ExecutionStatusCode, MotionCommand, @@ -36,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])) | ( @@ -127,6 +149,7 @@ class JogJCommand(MotionCommand[JogJCmd]): "speeds_out", "_jog_initialized", "_jog_vel_rad", + "_lookahead_buf", ) def __init__(self, p: JogJCmd): @@ -134,6 +157,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.""" @@ -187,6 +211,37 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: se.set_jog_velocity(self._jog_vel_rad) pos_rad, _vel, _finished = se.tick() + + # 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: + # 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 + 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. + state.collision_pairs = tuple( + PAROL6_ROBOT.display_pairs(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) self.set_move_position(state, self._steps_buf) diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py index 54f4169..281b05e 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 collision_blocked, guard_joint_path from parol6.config import ( CART_ANG_JOG_MIN, CART_LIN_JOG_MIN, @@ -155,7 +156,13 @@ 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) + # 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 + ): + self._track_and_send(state, ik_result.q) return ExecutionStatusCode.EXECUTING cse.active = False @@ -182,11 +189,15 @@ 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) + # 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( + 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() @@ -214,9 +225,31 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED return ExecutionStatusCode.EXECUTING - # IK succeeded - if we were stopping, recover by resuming jogging + # 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 + ): + if not self._ik_stopping: + _ik_warn( + logger, + "[CARTJOG] collision predicted - initiating stop", + ) + # Capture once on the stop transition (not every decel tick). + state.collision_pairs = tuple( + PAROL6_ROBOT.display_pairs(checker.colliding_pairs(ik_result.q)) + ) + state.collision_active = True + cse.stop() + self._ik_stopping = True + return ExecutionStatusCode.EXECUTING + + # Reachable + collision-free again — resume jogging. if self._ik_stopping: - logger.info("[CARTJOG] IK recovered - resuming jog") + 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 @@ -285,6 +318,9 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: stop_on_failure=stop_on_failure, ) + if not joint_path.is_partial: + guard_joint_path(joint_path.positions) + if joint_path.is_partial: ik_valid = joint_path.valid assert ik_valid is not None @@ -331,7 +367,7 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: ) def _compute_target_pose(self, state: "ControllerState") -> None: - """Compute target pose — absolute or relative based on rel flag.""" + """Compute target pose - absolute or relative based on rel flag.""" pose = self.p.pose if self.p.rel: @@ -443,6 +479,8 @@ def do_setup_with_blend( self.do_setup(state) return 0 + guard_joint_path(joint_path.positions) + # Use minimum speed/accel across chain, sum durations when all duration-based min_speed = self.p.resolved_speed min_accel = self.p.accel diff --git a/parol6/commands/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, diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py index 2894127..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,6 +87,7 @@ def do_setup(self, state: ControllerState) -> None: current_rad = self._q_rad_buf joint_path = JointPath.interpolate(current_rad, target_rad, n_samples=50) + guard_joint_path(joint_path.positions) builder = TrajectoryBuilder( joint_path=joint_path, profile=state.motion_profile, @@ -194,6 +196,7 @@ def do_setup_with_blend( return 0 joint_path = JointPath(positions=positions) + 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/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 new file mode 100644 index 0000000..b16e8e1 --- /dev/null +++ b/parol6/commands/shape_commands.py @@ -0,0 +1,49 @@ +"""Workspace collision-world shapes — the SET_SHAPES command.""" + +from __future__ import annotations + +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 + +if TYPE_CHECKING: + from parol6.server.state import ControllerState + + +@register_command(CmdType.SET_SHAPES) +class SetShapesCommand(SystemCommand[SetShapesCmd]): + """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. + + 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 — + speaks ``Shape`` only. + """ + + PARAMS_TYPE = SetShapesCmd + + __slots__ = () + + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: + 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 476b9c6..873f142 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -564,6 +564,63 @@ 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. +# 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 +# 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 "", +) + +# 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") +) + +# 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) + # ----------------------------------------------------------------------------- # Utility Functions diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index fba49c8..00210e7 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, 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): @@ -152,6 +153,10 @@ class CmdType(IntEnum): # Simulator state query IS_SIMULATOR = auto() + # Workspace collision-world shapes (keep-out geometry) + SET_SHAPES = auto() + # Collision-world readback query (installation + program layers) + SHAPES = auto() # ============================================================================= @@ -616,6 +621,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), @@ -724,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 @@ -1129,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 @@ -1148,6 +1202,7 @@ class TcpOffsetResultStruct( | TcpSpeedResultStruct | IsSimulatorResultStruct | TcpOffsetResultStruct + | ShapesResultStruct ) @@ -1279,6 +1334,9 @@ 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], ...] = (), + scene_epoch: int = 0, ) -> bytes: """Pack a status broadcast message. @@ -1318,6 +1376,9 @@ def pack_status( else None, tcp_speed, simulator_active, + collision_active, + collision_pairs, + scene_epoch, ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1355,18 +1416,19 @@ 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) + scene_epoch: int = 0 + # 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: - 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 @@ -1398,6 +1460,9 @@ 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), + scene_epoch=self.scene_epoch, ) @@ -1408,7 +1473,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, scene_epoch] Args: data: Raw msgpack bytes @@ -1465,6 +1531,20 @@ 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])) + if len(msg) > 22: + buf.scene_epoch = int(msg[22]) + return True except Exception as e: logger.debug("decode_status_bin_into: %s", e) @@ -1727,6 +1807,7 @@ def unpack_rx_frame_into( "ErrorCmd", "TcpSpeedCmd", "TcpOffsetCmd", + "ShapesCmd", "PingCmd", "StatusCmd", "AnglesCmd", @@ -1759,6 +1840,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 6e3b9e3..c1beef6 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.) @@ -489,7 +489,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 @@ -504,7 +504,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:: @@ -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 @@ -667,6 +673,10 @@ 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. + + 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 @@ -689,6 +699,21 @@ def set_active_tool( else: self._pinokin.clear_tool_transform() + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + # 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 ) -> NDArray[np.float64]: @@ -769,6 +794,75 @@ def fk_batch(self, joint_path_rad: NDArray[np.float64]) -> NDArray[np.float64]: result[i, 5] = rpy[2] return result + @property + def _collision_checker(self): + """The process-global checker, or None when collision checking is off. + + 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 + + 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 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. + """ + c = self._collision_checker + if c is None: + return -1 + 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) pairs in collision at `q_rad`. + + 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) + 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`. + + Positive => separation; negative => penetration depth. + Returns +inf when collision checking is disabled. + """ + c = self._collision_checker + if c is None: + return float("inf") + 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 — the canonical + in-process type everywhere; no wire form is involved. + """ + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + PAROL6_ROBOT.apply_shapes(list(shapes)) + def ik_batch( self, poses: NDArray[np.float64], diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2d42365..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") @@ -686,6 +688,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: @@ -713,6 +717,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 +749,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 @@ -801,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: @@ -823,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/ik_worker.py b/parol6/server/ik_worker.py index d8d91a9..e14399e 100644 --- a/parol6/server/ik_worker.py +++ b/parol6/server/ik_worker.py @@ -7,14 +7,18 @@ 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 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, @@ -27,26 +31,78 @@ unregister_shm, ) +if TYPE_CHECKING: + import multiprocessing + logger = logging.getLogger(__name__) +# 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 program-layer shapes (waldoctl Shape 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. + + 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 + 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( 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) @@ -102,25 +158,44 @@ 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 + 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 - # 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): @@ -133,10 +208,15 @@ 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) + checker = PAROL6_ROBOT.collision + gate_joint_enable_collision( + checker, q_rad, joint_en, q_step, qlim=qlim_rows + ) # Compute cartesian enablement for both frames _compute_cart_enable( @@ -148,6 +228,7 @@ def ik_enablement_worker_main( _AXIS_DIRS, cart_targets, cart_en_wrf, + checker=checker, ) _compute_cart_enable( T_matrix, @@ -158,6 +239,7 @@ def ik_enablement_worker_main( _AXIS_DIRS, cart_targets, cart_en_trf, + checker=checker, ) # Output data written directly via views, just update version @@ -207,6 +289,44 @@ 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, 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 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 + md_now = checker.min_distance(q_rad) + 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]: + 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 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 + + # 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( @@ -283,10 +403,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- """ @@ -296,10 +420,24 @@ 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 + 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) + 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): 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 is not None: + 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 except Exception: out[i] = 0 diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 6dd921f..9454bac 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 @@ -69,6 +75,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] @@ -112,12 +119,21 @@ class SyncTool: tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) +@dataclass +class SyncShapes: + """Replace the planner checker's program-layer shapes (waldoctl Shape 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 +] # --------------------------------------------------------------------------- @@ -240,6 +256,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( @@ -386,6 +406,7 @@ def _emit_error( error=robot_error, cartesian_path=cartesian_path, ik_valid=ik_valid, + colliding_pairs=getattr(exc, "colliding_pairs", None), ) ) @@ -461,6 +482,13 @@ 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(). 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) # --------------------------------------------------------------------------- @@ -513,6 +541,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 @@ -597,6 +629,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) @@ -746,6 +782,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/parol6/server/segment_player.py b/parol6/server/segment_player.py index e01b131..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 @@ -140,6 +159,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 = "" @@ -159,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 @@ -242,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/parol6/server/state.py b/parol6/server/state.py index a45ca5f..997adaa 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -251,6 +251,17 @@ 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], ...] = () + + # 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 @@ -300,6 +311,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 +370,7 @@ def reset(self) -> None: # Error and pipeline depth self.error = None + self.clear_collision() self.queued_segments = 0 self.queued_duration = 0.0 @@ -394,6 +411,18 @@ 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 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; the version doubles + as the ``scene_epoch`` broadcast in status so displays re-query. + """ + PAROL6_ROBOT.apply_shapes(shapes) + self.shapes = list(shapes) + self.shapes_version += 1 + @property def tcp_offset_m(self) -> tuple[float, float, float]: """Current TCP offset in meters (tool-local frame).""" diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 0264f0a..767c7b1 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -30,7 +30,11 @@ SHM_EXTRA_KWARGS, unregister_shm, ) -from parol6.server.ik_worker import ik_enablement_worker_main +from parol6.server.ik_worker import ( + SyncShapes, + SyncTool, + 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 @@ -159,6 +163,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 @@ -188,6 +196,9 @@ def __init__(self) -> None: 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 @@ -248,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=( @@ -255,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", @@ -270,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(): @@ -300,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). @@ -383,6 +408,19 @@ 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))) + # 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) @@ -460,6 +498,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 +525,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 +555,9 @@ 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, + scene_epoch=self._last_shapes_version, ) self._binary_dirty = False return self._binary_cache diff --git a/parol6/tools.py b/parol6/tools.py index 42f0b2f..da015ec 100644 --- a/parol6/tools.py +++ b/parol6/tools.py @@ -508,9 +508,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( @@ -524,9 +528,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( @@ -581,15 +591,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( @@ -646,21 +656,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( @@ -716,7 +726,9 @@ def _make_tcp_transform( name="Vacuum Gripper", description="Vacuum gripper (pneumatic valve I/O)", transform=_make_tcp_transform(z=-0.037), - meshes=(MeshSpec(file="vacuum_gripper_body.stl", role=MeshRole.BODY),), + meshes=( + MeshSpec(file="vacuum_gripper_body_simplified.stl", role=MeshRole.BODY), + ), motions=(), io_port=1, ), diff --git a/parol6/urdf_model/srdf/PAROL6.srdf b/parol6/urdf_model/srdf/PAROL6.srdf new file mode 100644 index 0000000..e8026e7 --- /dev/null +++ b/parol6/urdf_model/srdf/PAROL6.srdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + diff --git a/parol6/urdf_model/urdf/PAROL6.urdf b/parol6/urdf_model/urdf/PAROL6.urdf index ac81a5f..dce672b 100644 --- a/parol6/urdf_model/urdf/PAROL6.urdf +++ b/parol6/urdf_model/urdf/PAROL6.urdf @@ -49,7 +49,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/base_link_simplified.stl" /> @@ -89,7 +89,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L1_simplified.stl" /> @@ -147,7 +147,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L2_simplified.stl" /> @@ -205,7 +205,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L3_simplified.stl" /> @@ -263,7 +263,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L4_simplified.stl" /> @@ -321,7 +321,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L5_simplified.stl" /> @@ -379,7 +379,7 @@ rpy="0 0 0" /> + filename="package://parol6/meshes/L6_simplified.stl" /> diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 94b61dc..7ab38fd 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -165,6 +165,12 @@ class _ErrorTemplate: effect="Profile not changed.", remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, TRAPEZOID, LINEAR.", ), + ErrorCode.SYS_SELF_COLLISION: _ErrorTemplate( + title="Self-collision predicted", + cause="Planned configuration would self-collide at sample {sample} of {total}: {pairs}", + effect="Motion command rejected before dispatch.", + remedy="Choose a different target, add intermediate waypoints, or disable via PAROL6_COLLISION_CHECK=0.", + ), } diff --git a/parol6/utils/error_codes.py b/parol6/utils/error_codes.py index 5e32b39..6b6eaec 100644 --- a/parol6/utils/error_codes.py +++ b/parol6/utils/error_codes.py @@ -38,3 +38,4 @@ class ErrorCode(IntEnum): SYS_ESTOP_ACTIVE = 51 SYS_PORT_SAVE_FAILED = 52 SYS_PROFILE_INVALID = 53 + SYS_SELF_COLLISION = 54 diff --git a/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)) 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 diff --git a/pyproject.toml b/pyproject.toml index 1306216..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.6 + # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.8 # 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.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.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.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.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.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.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.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", @@ -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] @@ -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 diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py new file mode 100644 index 0000000..9a8b101 --- /dev/null +++ b/tests/integration/test_shapes_e2e.py @@ -0,0 +1,177 @@ +"""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. + +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 +from waldoctl import Box + +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 +): + 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 == () + + +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 new file mode 100644 index 0000000..763f26b --- /dev/null +++ b/tests/unit/test_collision_enablement.py @@ -0,0 +1,259 @@ +"""Directional collision enablement gates + geometry sync in the IK worker.""" + +import multiprocessing +import time +from dataclasses import dataclass + +import numpy as np + +from parol6.commands._collision_guard import collision_blocked +from parol6.server.ik_worker import ( + _AXIS_DIRS, + _ENABLE_NEAR_M, + SyncShapes, + SyncTool, + _compute_cart_enable, + _drain_sync, + gate_joint_enable_collision, +) + + +class _FakeChecker: + """``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, pairs=()): + self._distance = distance + self._collides = collides + self._pairs = pairs + self.queries = 0 + + def min_distance(self, q): + return self._distance(q) if callable(self._distance) else self._distance + + 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) + 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 + + +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) + # 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_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. + 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 + + +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 + 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_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_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 = [] + + 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() + + +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() diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py new file mode 100644 index 0000000..59269cd --- /dev/null +++ b/tests/unit/test_collision_integration.py @@ -0,0 +1,487 @@ +"""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 TrajectoryPlanningError 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.commands.base import ExecutionStatusCode +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import TrajectoryPlanningError + + +def test_singleton_checker_initialized(): + assert PAROL6_ROBOT.collision is not None + assert PAROL6_ROBOT.collision.num_geometry_objects > 0 + assert PAROL6_ROBOT.collision.num_collision_pairs > 0 + + +def test_srdf_disabled_pairs_reduce_pair_count(): + # Without SRDF: 7 link geometries -> 21 pairs minus 6 parent/child adjacent = 15. + # The bundled SRDF disables 7 more pairs (base<->L4/L5/L6, L1<->L4/L5/L6, L2<->L4). + assert PAROL6_ROBOT.collision.num_collision_pairs == 8 + + +def test_home_is_clear(): + q = np.zeros(PAROL6_ROBOT.robot.nq) + assert PAROL6_ROBOT.collision.in_collision(q) is False + + +def test_robot_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() + 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(): + positions = np.zeros((10, 6)) + # No exception means no collision detected. + guard_joint_path(positions) + + +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 + + # 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): + monkeypatch.setattr(PAROL6_ROBOT, "collision", None) + positions = np.zeros((5, 6)) + # No exception, returns None (no-op). + assert guard_joint_path(positions) is None + + +def test_no_spurious_self_overlap_at_home_or_joint_limits(): + """Audit the bundled simplified collision STLs against the SRDF. + + Asserts that the checker reports no colliding pairs at home, at each + joint's lower/upper limit (single-axis), at every (low, high) corner + of the joint-limit hypercube, and across a handful of seeded random + configs. If this fails, the assertion message identifies the named + pair — add it to parol6/urdf_model/srdf/PAROL6.srdf and re-run. + """ + import itertools + + lo = PAROL6_ROBOT._joint_limits_radian[:, 0] + hi = PAROL6_ROBOT._joint_limits_radian[:, 1] + checker = PAROL6_ROBOT.collision + + configs: list[tuple[str, np.ndarray]] = [("home", np.zeros(6))] + for j in range(6): + q_lo = np.zeros(6) + q_lo[j] = lo[j] + q_hi = np.zeros(6) + q_hi[j] = hi[j] + configs.append((f"J{j}=low", q_lo)) + configs.append((f"J{j}=high", q_hi)) + + for bits in itertools.product((0, 1), repeat=6): + q = np.where(np.array(bits, dtype=bool), hi, lo) + configs.append((f"corner_{''.join(map(str, bits))}", q)) + + rng = np.random.default_rng(0xC011) + for k in range(20): + configs.append((f"rand_{k}", rng.uniform(lo, hi))) + + failures: list[str] = [] + for label, q in configs: + pairs = checker.colliding_pairs(np.ascontiguousarray(q, dtype=np.float64)) + if pairs: + failures.append(f"{label}: {pairs}") + + assert not failures, ( + "Spurious self-collision in the bundled simplified STLs. Add these " + "pairs to parol6/urdf_model/srdf/PAROL6.srdf and re-run:\n" + + "\n".join(failures[:10]) + + (f"\n... ({len(failures) - 10} more)" if len(failures) > 10 else "") + ) + + +def test_collision_check_speed_diagnostic(capsys): + """Time in_collision and colliding_pairs across a sampled workspace. + + Diagnostic only — does not assert a threshold. 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 + """ + 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)") + + +def _box(name: str, **overrides): + from waldoctl import Box + + 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_bad_sets_without_mutation(): + """Set-level rejection fails fast — never a half-applied collision world. + + 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", [_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", [_box("b"), _box("b", collision=False)]), + ] + try: + 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_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 + ``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([])