Collision checking: keep-out shapes, enablement gates, MoveIt-style invalidation#24
Merged
Conversation
Pre-flight self/world collision validation is applied before any motion command leaves the host. Failure raises MotionError(SYS_SELF_COLLISION) for offline planners (MoveJ, MoveJPose, MoveL, blended chains) and mirrors the IK-failure graceful-stop pathway for streaming JogL. Changes - parol6/PAROL6_ROBOT.py: module-level collision: CollisionChecker | None singleton alongside the existing robot singleton. Lazy init via _init_collision_checker() called at the end of parol6/config.py once the COLLISION_* knobs exist. Geometry-load failures degrade to a warning + None so existing scripts keep running. - parol6/PAROL6_ROBOT.py: _resolved_urdf_for_collision() rewrites package://parol6/meshes/... URIs to absolute file:// paths in a temp URDF. The bundled URDF was authored for a ROS package layout (meshes at parol6/meshes/) but the Python package places them at parol6/urdf_model/meshes/; rewriting keeps the source URDF untouched. - parol6/config.py: COLLISION_CHECK_ENABLED, COLLISION_PATH_SAMPLES (default 16, needs benchmarking once scene/world geometry is added), COLLISION_SRDF_PATH (defaults to bundled PAROL6.srdf). - parol6/urdf_model/srdf/PAROL6.srdf: disables 5 non-adjacent pairs (base<->L4/L5/L6, L1<->L5/L6) that are physically unreachable. Reduces enabled pair count from 15 to 10. - parol6/utils/error_codes.py + error_catalog.py: SYS_SELF_COLLISION (code 54) with title/cause/effect/remedy template. - parol6/commands/_collision_guard.py: shared guard_joint_path helper subsamples per COLLISION_PATH_SAMPLES, calls check_path, raises on first hit. guard_config for single-point checks. - parol6/commands/joint_commands.py: guard MoveJ/MoveJPose in do_setup after JointPath.interpolate and do_setup_with_blend after build_composite_joint_path. - parol6/commands/cartesian_commands.py: guard MoveL in _precompute_trajectory after JointPath.from_poses and in do_setup_with_blend. For JogL, check ik_result.q per tick before _track_and_send; on collision predict, call cse.stop() and set _ik_stopping=True exactly like IK failure does (graceful deceleration; no exception mid-jog). - parol6/robot.py: public Robot.in_collision, check_trajectory, colliding_pairs, min_distance methods delegate to the singleton. All return safe defaults when checker is None. - tests/unit/test_collision_integration.py: covers singleton init, SRDF effect on pair count, home-is-clear, the four public Robot methods, and guard raise/no-op behavior. Opt-out via PAROL6_COLLISION_CHECK=0. Failure-to-init logs a warning and disables checking; existing motion behavior is unchanged. https://claude.ai/code/session_01TiEkni9M9ZJC88LmvJwUyf
… + collision integration tests
Released pinokin (v0.1.6) doesn't ship CollisionChecker, so the singleton stays None on those installs. Mark the integration assertions skip-on-None rather than fail; the universal guard tests still exercise the PAROL6 wiring against a fake checker.
…ion) CI's test matrix detects when a pinokin branch with the same name as the PAROL6 PR branch exists. If so, conda is set up with libpinocchio + libcoal so pinokin can be built from source from that branch, then overrides the pinned v0.1.6 wheel via --force-reinstall --no-deps. This exercises the actual cross-repo collision integration before pinokin v0.1.7 ships. If no matching branch exists, the existing plain-pip flow runs and any PAROL6 code that requires unreleased pinokin features fails loudly (intended - that's the contract). Reverts the prior graceful-degradation experiment in test_collision_integration.py: the integration tests now hard-assert on collision being available rather than silently skipping when it isn't.
Closes the gaps from the parol6-vision (python-fcl) comparison so the runtime safety net covers gripper-vs-environment and surfaces readable diagnostics, while staying within the servo budget. - URDF + tool registry switched to bundled *_simplified.stl twins (parol6/urdf_model/urdf/PAROL6.urdf collision blocks; every MeshSpec.file in parol6/tools.py). Visual rendering is unaffected (URDF visual blocks untouched; WC's urdf_scene already prefers the same _simplified meshes via _stl_to_url). Collision queries on simplified BVH are dramatically faster (p99 ~38 us on Pi, ~0.4% of a 10 ms servo tick). - Robot.colliding_pairs and the underlying CollisionChecker call now return list[tuple[str, str]] of geometry-object names rather than opaque indices. _collision_guard.py renders pairs as "ssg48_body_simplified.stl vs L4_0" in the SYS_SELF_COLLISION error cause, so operators see what hit what without a separate name lookup. - PAROL6_ROBOT.apply_tool now refreshes the singleton collision scene with the active tool's BODY/JAW meshes (attached to the L6 frame). Tracks attached names in _active_tool_geom_names so the next tool change cleans up before the new one is attached. Variant resolution mirrors WC's swap_tool_mesh semantics. The hook lives only at apply_tool (NOT Robot.set_active_tool, which mutates a per-instance pinokin and never touches the global scene - note added to its docstring). - SRDF (parol6/urdf_model/srdf/PAROL6.srdf) expanded to the defensive union of the original inspection set and the measured simplified-mesh overlap set: base_link <-> L4/L5/L6, L1 <-> L4/L5/L6, L2 <-> L4. - New test_no_spurious_self_overlap_at_home_or_joint_limits audits the bundled simplified STLs across home + every single-axis joint limit + all 64 corners of the limit hypercube + 20 seeded random configs. Fails loud with the named pair when anything new slips through; that is the answer to "which pairs overlap on these meshes" - measurement, not inspection. - New test_collision_check_speed_diagnostic prints per-call percentiles for in_collision and colliding_pairs, so the trajectory-build pre-flight and the JogLCommand mid-motion check can be evaluated against the 100 Hz tick budget. Measured here: p99 ~38 us for both, well inside budget. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
"Planned configuration would self-collide at sample 7/50: pairs=ssg48_body_simplified.stl vs L4_0"
reads awkwardly because "pairs=" is dropped into the middle of the
sentence. Phrase as
"Planned configuration would self-collide at sample 7 of 50: ssg48_body_simplified.stl vs L4_0"
instead. {pairs} is now substituted directly without a key= prefix.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Safety coverage: - JogJ stops before streaming a self-colliding configuration (only fires in the collision case, so normal jogging is unaffected; an abrupt stop beats driving the arm into itself — JogJ has no CSE smoother for a graceful stop). - JogL release-deceleration skips streaming a colliding config and lets the CSE finish stopping (was sending unchecked IK configs after timer expiry). Efficiency / cleanup: - _refresh_collision_tool_geometry skips the STL reload + BVH rebuild when the (tool, variant) is unchanged — a TCP-offset-only apply_tool no longer reloads geometry on the control-loop thread (placement depends only on spec.origin). - guard_joint_path: drop the dead concatenate/endpoint subsample work and the no-op inner ascontiguousarray (the entry conversion stays — load-bearing); hoist the four function-level guard imports to module top (cycle-free). - _init_collision_checker takes (enabled, srdf_path) instead of importing config back (keeps the dependency one-directional); drop the dead package_dirs arg (all mesh URIs are rewritten to file://); reuse _mesh_dir. - guard raises TrajectoryPlanningError (server-side planning convention), not the client-side MotionError. Delete dead guard_config. Docs/comments: honest "Lazy" comment (+ TODO to actually defer the build off import); SRDF final-state phrasing; cost estimate uses the measured ~38us/check; neutral jog-recovery log; Robot collision-method docstrings note the process-global checker; speed-diagnostic docstring reflects the shipped check. Tests: TrajectoryPlanningError assertion; merged the three Robot() collision tests into one shared-instance test; dropped the no-op try/finally del and the redundant manual monkeypatch restore. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
…decel + attach fixes - MoveC/S/P now call guard_joint_path (were unchecked) [S4-2] - guard_joint_path permits an escaping move when the start is already in collision (no new pair, no deeper) instead of trapping the arm [S4-1] - collision-init failure raises unless PAROL6_ALLOW_NO_COLLISION set [S4-4] - mesh URIs via Path.as_uri() so Windows file:// is valid [S4-6] - tool geom-key set only after all attaches succeed, with rollback [S4-5] - CartesianJog stops re-commanding jog velocity while _ik_stopping so cse.stop() actually decelerates [S4-3] Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
NumPy 2.5.0 (released 2026-06) violates numba 0.6x's numpy<2.5 pin, breaking collection on Python 3.12+ (3.11 resolves an older numpy and passes). Bound numpy to what numba supports. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
- Windows: rewrite package:// mesh refs to a plain POSIX path, not a file:// URI. coal strips the scheme naively, leaving an invalid '/D:/...' (leading slash before the drive letter) so assimp couldn't open the meshes. as_posix() yields '/abs/...' on POSIX and 'D:/abs/...' on Windows — both openable. - Lint: scope a ty unresolved-import override to PAROL6_ROBOT.py; the lint env's pinokin build can predate CollisionChecker (resolves fine at runtime). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
….yml main (post-panels) needs waldoctl CameraSpec/ToolsCollection; pin v0.4.0. tests.yml: take main's single Install-package step (job already defaults to bash); the separate cross-repo step was redundant with the force-reinstall. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
The matching-pinokin path builds into a conda env; the job defaults to the conda-aware login shell (bash -l). An explicit shell:bash on Install package made pip install into base python while pytest ran in the conda env -> numba ModuleNotFoundError. Use the job-default login shell instead. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
When a move is blocked or a continuous jog is stopped by a predicted self-collision, capture the colliding geometry/link pairs at the predicted colliding config and surface them on the status broadcast so the frontend can highlight the offending parts. - ControllerState gains collision_active/collision_pairs + clear_collision(); reset() clears them. - Move path: the guard attaches structured pairs to TrajectoryPlanningError; they ride the pickled ErrorSegment across the planner subprocess and land on state in the segment player. - Jog path: JogJ/JogL capture pairs on the (rare) stop branch only — never on the clean per-tick path, preserving the 100 Hz no-alloc rule. Cleared on a fresh jog, on JogL recovery, and at command intake. - Wire: collision_active/collision_pairs appended to the STATUS tuple (len-guarded decode → backward compatible); cached + dirty-checked. - Robot gains has_collision_checking to satisfy the new waldoctl ABC flag. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
A SetShapesCmd carries the generic shape wire form (kind/params/pose/collision/ margin/name, matching waldoctl Shape.to_wire). Like SELECT_TOOL it rides the planner→inline→segment-player path, so PAROL6_ROBOT.apply_shapes runs in both the controller and planner processes against each one's collision checker. Collision-enabled shapes become world obstacles via the generic add_obstacle; visual-only shapes are skipped. async client gains set_shapes. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…ying - COLLISION_CLEARANCE_M (default 5mm) sets the checker's fixed clearance at init, so all three process checkers keep a uniform near-miss buffer. - JogJ now checks a velocity-scaled lookahead (COLLISION_JOG_LOOKAHEAD_S), so faster jogs stop further from contact; composes with the clearance. In-place to keep the 100Hz path allocation-free. - The IK worker greys a joint direction whose small step self-collides (_gate_joint_enable_collision, proximity-gated), feeding the existing can_jog_* path so the jog buttons disable toward a self-collision. Cartesian and tool/shape greying need the ik-worker geometry sync (a follow-up). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Move the directional self-collision gate from the ik-worker (links-only) to the controller's status_cache _poll_ik_results, which runs against the process-global checker carrying the tool (apply_tool) and keep-out shapes (set_shapes). So the jog/move joint buttons now grey for tool and shape collisions, not just link-vs-link. gate_joint_enable_collision is promoted to a shared helper. Cartesian-direction greying needs the per-direction IK targets and is a follow-up. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
A _collision_checker property folds the repeated lazy import + None check used by in_collision / check_trajectory / colliding_pairs / min_distance. Zero behavior change. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…mment Streamable intake (servo/jog/teleport) now clears the collision viz like the tool-action and planner intakes — a jog-collision red no longer outlives a subsequent clean servo. The per-jog do_setup clears are dropped (intake covers all streamables). _pose_to_matrix documents why Rz·Ry·Rx is deliberate (matches the frontend's reversed-Euler render order; se3_from_rpy would mis-orient shapes). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Per the collision plan: the worker's checker is kept in sync with the controller's geometry (SyncTool/SyncShapes over a new command queue, driven by tool-change detection + a shapes_version on state), the joint gate moves from status_cache into the worker loop, and _compute_cart_enable now also requires the solved config to be collision-free (proximity-gated) — cartesian jog buttons grey toward blocked directions. A geometry sync recomputes at the last requested pose, so greying updates even while the arm is stationary. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
set_active_tool now refreshes the global checker's tool meshes and apply_shapes mirrors the client's set_shapes locally, so host-side collision queries (editing-pose highlight, dry-run preview) see the same collision world as the server. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
…allow escape JogJ only sampled the velocity-scaled lookahead, so anything inside the first lookahead window at jog start was tunneled through unchecked; the lookahead also evaluated poses past the joint limits, phantom-tripping near mechanical stops. Both jogs now use a shared collision_blocked() in _collision_guard: blocked when the next config collides, except when the arm is ALREADY inside (a keep-out placed over it) — then only deeper motion is blocked, mirroring guard_joint_path's start-in-collision escape rule. JogJ clamps the lookahead to qlim in place. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
…ill the worker Inside a colliding config, the joint/cartesian gates greyed all 24 directions, locking the UI with no way out — they now grey only directions that go deeper. _drain_sync applies each geometry sync under its own exception guard: one bad tool mesh or plugin import must not permanently kill the enablement worker. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
…r; dup-name validation Shapes are safety configuration, not motion: as a MotionCommand they were rejected while disabled (e-stop) and applied controller-side via an InlineSegment that any segment-player cancel — including every streamed jog packet's intake — silently drained, splitting the planner and control-loop checkers with no error. Now modeled on SELECT_PROFILE: immediate at intake, mirrored to the planner via a SyncShapes message, re-synced at e-stop/reset alongside sync_tool. The sync client gains the missing set_shapes wrapper, and apply_shapes validates duplicate names before mutating (pinokin raises mid-add, which would leave a half-applied collision world). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
…cs, gate clamp - apply_shapes fully pre-validates (kind, param count, finite 6-pose, name uniqueness across ALL shapes incl. visual-only) before the checker-None return and before removing the old world — a malformed SET_SHAPES could previously reply error while silently disarming every enforced barrier. - The dry-run planner re-applies SetShapesCmd inline (only reachable there; the live path routes SET_SHAPES as a SystemCommand + SyncShapes), so a script's set_shapes() shapes its preview world instead of being swallowed. - set_active_tool guards the collision-geometry refresh: a plugin tool whose meshes live outside parol6's mesh root must not break tool selection after the kinematic transform applied. - JogJ joint limits come from a once-per-process module cache — robot.qlim allocates a fresh matrix per access and do_setup re-runs per streamed packet, and the per-tick row indexing allocated view objects. - The enablement gate's ±2° probe is clamped to qlim (like JogJ's lookahead), so geometry just past a mechanical stop can't grey a direction the jog itself would permit. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
…llation layer, margin, display vocabulary - SET_SHAPES decodes to waldoctl Shapes at intake (the codec boundary); apply_shapes / SyncShapes / the planner all speak Shape — a preview script's set_shapes works end-to-end (previously TypeError) - SET_SHAPES joins SYSTEM_CMD_TYPES: 1 confirmed / 0 timeout, raises on rejection; new SHAPES readback query; scene_epoch in the STATUS wire - config INSTALLATION_SHAPES: an install: layer every program inherits, untouched by set_shapes, loaded per-process at import - apply_shapes plumbs the per-shape margin into the checker; set-level validation only (values enforced at Shape construction) - colliding pairs report URDF link names + shape:/install:/tool: names, never checker-internal identifiers; tool geometry attached as tool:<key>:<part> - JogL release deceleration uses the escape-aware gate — the target no longer freezes when releasing while escaping a keep-out - _pose_to_matrix documented against the waldoctl RPY contract - CLAUDE.md: no-testing-theatre rules, no declared-but-unimplemented API Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
…invalidation
- collision_blocked and both ik-worker escape gates now also block when the
step introduces a colliding pair absent at the current config, matching
guard_joint_path: the global min-distance trend alone cannot distinguish an
improving start-collision from a new shallower one. pairs_now is hoisted
once per gate pass; allocations stay confined to the rare in-collision branch.
- Attached tool geometry is named tool:{key}:{role} (per-role dedup suffix)
instead of leaking raw STL basenames into error text and collision_pairs.
- SegmentPlayer re-guards committed motion against the current world: a
shapes_version bump re-validates the streaming trajectory's remaining rows
(zero-alloc int compare on the unchanged path), and every TrajectorySegment
is guarded at activation — closing the planner-FIFO race where a segment
planned against the old world arrives after SET_SHAPES applied. Violations
halt exactly like an ErrorSegment via guard_joint_path's escape-aware,
display-vocabulary error. Inline segments are not trajectory playback and
are not re-guarded.
Red-first e2e coverage: mid-flight set_shapes halts the streaming move,
rejects a queued move at activation, and leaves off-path motion undisturbed;
unit coverage for the new-pair gate greying and tool-role pair vocabulary.
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
ruckig 0.17.3 on PyPI is sdist-only and its build config still uses cmake.targets, which scikit-build-core 1.0 rejects as a hard error — every CI job has died at dependency install since the 1.0 release. Upstream fixed the rename on main (pantor/ruckig#262) but has not cut a release, so pre-install that commit before ".[dev]" in both the lint and test jobs; its version (0.17.3) satisfies the ruckig>=0.12.2 pin, so the dev install leaves it in place. Drop the override once a fixed ruckig release lands. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
numba 0.66 ships a _dispatcher.pyi stub, so ty now type-checks njit call arguments; the four module-level limit caches are ndarray | None and need the same narrowing the solver/result caches already get. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
Wheel URLs written from the actual v0.1.7 assets — the cp312-cp314 macOS wheels moved to macosx_26_0_arm64 in this build. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
v0.1.7's cp312-314 macOS wheels were tagged macosx_26_0 (built on the macos-latest pool mid-rollout), which pip rejects on macOS 15 runners. v0.1.8 pins MACOSX_DEPLOYMENT_TARGET=15.0 in the wheel build, so every macOS wheel is macosx_15_0_arm64. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
waldoctl's StatusBuffer protocol declares cart_en as a plain attribute; ty 0.0.57 enforces that a read-only property doesn't satisfy a mutable protocol member. The dict is still built once in __post_init__, aliasing the two enable arrays the decoder mutates in place — nothing per-tick. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01M7tEqGhaTqJV6ZxCXgMpFU
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Implements the waldoctl collision contract end-to-end (35 commits, 5 review rounds):
CollisionChecker: status reporting, clearance config, jog speed-buffer, directional enablement (joint/cartesian button greying via the ik-worker gates)SET_SHAPESSystemCommand with explicit planner sync, canonical waldoctlShapein-process (codec confined to the wire), installation layer + program layer (last-write-wins), controller readback, per-shape margin plumbed to pinokincollision_blocked)SegmentPlayerre-guards streaming trajectory rows onshapes_versionchange and guards everyTrajectorySegmentat activationtool:{key}:{role}Depends on waldoctl v0.5.0 + pinokin v0.1.7 (pins to be bumped on this branch before merge).
Replaces #17 (same work, branch renamed to feat/collision so cross-repo CI branch-matching finds the waldoctl/pinokin feature branches).
🤖 Generated with Claude Code
https://claude.ai/code/session_01M7tEqGhaTqJV6xCXgMpFU