Add self-collision checking with pinokin CollisionChecker#5
Closed
Jepson2k wants to merge 11 commits into
Closed
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>
Owner
Author
|
Moved to the canonical PCrnjak repo: PCrnjak#17 The collision branch was rebased onto PCrnjak/main (v0.2.7 + setuptools_scm) and pushed there. Closing this PR. Comment written by Claude on the author's behalf. |
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.
Summary
Wires pinokin's
CollisionCheckerinto PAROL6 as a runtime safety net. Self-collision and gripper-vs-world checks run as pre-flight guards at trajectory build time on every motion command, and as a mid-motion graceful-stop trigger inside cartesian jog. Measuredin_collisionp99 is ~38 µs on a Pi 5 with simplified collision meshes — ~0.4% of a 10 ms servo tick.Key pieces
PAROL6_ROBOT.py, bound to the global pinokinRobot. Enabled by default; disable withPAROL6_COLLISION_CHECK=0. Lazy init fromconfig.py; gracefully no-ops if the build fails.<collision>blocks and everyMeshSpec.filein the tool registry reference the bundled*_simplified.stltwins. Visual rendering is unaffected (Waldo Commander'surdf_scene._stl_to_urlalready prefers simplified meshes for visuals; URDF<visual>blocks untouched).apply_tool(tool_key, variant_key, …)refreshes the global collision scene with the active tool's BODY/JAW meshes, parented to theL6flange frame. Variant resolution mirrors WC'sswap_tool_mesh(variant.mesheswholesale replacescfg.meshes). Attached names are tracked in_active_tool_geom_namesso the next tool change cleans up before the new one is attached.Robot.set_active_tooldeliberately does not touch the global scene (per-instance pinokin only).config.py):COLLISION_CHECK_ENABLED(env:PAROL6_COLLISION_CHECK),COLLISION_PATH_SAMPLES(env:PAROL6_COLLISION_PATH_SAMPLES),COLLISION_SRDF_PATH(env:PAROL6_COLLISION_SRDF, defaults to the bundledPAROL6.srdf).Public Robot API
in_collision(q),check_trajectory(q_path),min_distance(q).colliding_pairs(q) -> list[tuple[str, str]]. Pairs are link names (URDF) and user-supplied names (runtime-attached, e.g."ssg48_body_simplified.stl").Motion-command wiring
joint_commands.py:guard_joint_path()runs in bothJointMove.do_setup()paths.cartesian_commands.py:guard_joint_path()runs inCartesianMove._precompute_trajectory()anddo_setup_with_blend();CartesianJog.execute_step()runs a real-timein_collision()check on the streamed IK target and decelerates viacse.stop()on predicted collision (mirrors the IK-failure graceful-stop pathway).SRDF
parol6/urdf_model/srdf/PAROL6.srdflists pairs disabled as physically-impossible or simplified-mesh artifacts:base_link ↔ {L4, L5, L6},L1 ↔ {L4, L5, L6},L2 ↔ L4. Adjacent (parent/child) pairs are auto-disabled byCollisionCheckerand never appear here.Error surface
New
ErrorCode.SYS_SELF_COLLISION(54). The cause template renders the named pair list directly: "Planned configuration would self-collide at sample 7 of 50: ssg48_body_simplified.stl vs L4_0".Tests
tests/unit/test_collision_integration.py: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 if anything slips through; clean as shipped.test_collision_check_speed_diagnostic: printsin_collisionandcolliding_pairspercentiles so the trajectory-build pre-flight andJogLCommandmid-motion check can be evaluated against the 100 Hz tick budget. Measured on Pi: ~38 µs p99 for both.PR description written by Claude on the author's behalf.