Collision constraints and tuning

EmbodiK treats collision avoidance as a stack of mechanisms, not a single distance check. That design keeps dual-arm teleop and whole-body control (WBC) fast while still rejecting steps that deepen penetration.

Related guides

Recovery when stuck at margins (stall handler, floors, weighted fallback) → Solver Robustness & Recovery. Guide map → Guides overview. Runnable demo → Collision-Aware IK.

The WBC collision bottleneck

Whole-body control has to balance speed, smoothness, and safety at control rate. A common bottleneck is collision checking: evaluating many link–link (and link–obstacle) pairs every cycle. On dense humanoid or dual-arm models that can mean hundreds of pairwise distance queries per step — fine for offline planning, expensive for real-time WBC.

EmbodiK addresses this with conservative proximity bounds, pair caching, and three tuning presets so most cycles skip exact geometry work on clearly safe pairs.

Pairwise spatial coherence

The core intuition: if two shapes have not moved much relative to each other, we often do not need an exact distance query this cycle.

EmbodiK tracks how each candidate pair moved since the last exact check (relative translation and rotation). From that motion it maintains conservative bounds on how far the true signed distance could have changed:

Bound	Meaning	How EmbodiK uses it
Lower bound ℓ	Smallest distance the pair could now have	If ℓ is still above the safety margin, the pair is clearly safe → skip the expensive exact check
Upper / broadphase bounds	Pairs clearly far apart in space	Sphere broadphase and cache margins skip work on distant pairs

This is conservative: we only skip work when bounds prove the pair remains outside the danger zone. We do not underestimate collision risk to go faster.

Together with proximity-gated QP rows and an optional time budget on exact refinement (Speed mode), compute concentrates on pairs that are close, recently moved, or candidates for active constraints — not on the full combinatorial set every tick.

What this unlocks

• Large speedups on collision-heavy steps — often ~10–50× vs running full exact checks on every pair every cycle (see measured mode table below).
• Three operating modes — Speed / Balanced / Precise — accuracy vs compute without hand-tuning a dozen low-level flags.
• Conservative safety on skipped pairs — bound gates only elide checks when ℓ stays above margin; post-step guards still reject deepening penetration on accepted motion.
• Richer real-time motion — whole-body IK and teleop with complex self-collision geometry within latency budgets naive full-scan pipelines cannot meet.

flowchart LR
  subgraph each_step [Each control step]
    A[Joint motion Δq] --> B[Update pair bounds from relative motion]
    B --> C{ℓ still above margin?}
    C -->|yes| D[Skip exact GJK this cycle]
    C -->|no| E[Exact distance + QP constraint]
    E --> F[Integrate step]
    F --> G{Post-step deepen?}
    G -->|yes| H[Reject / recover]
    G -->|no| I[Accept SUCCESS]
  end

Why this is stronger than naive approaches

Many IK pipelines use one of the patterns below. EmbodiK composes them so you can choose speed without giving up incremental safety.

Naive pattern	Problem	EmbodiK
Full distance scan every step	Correct but too slow for teleop	Speed / Balanced presets: cache, broadphase, optional refinement budget
Inequality constraints only	Penetration creeps across integrated steps	Post-step acceptance rejects deepening penetration
Hard reject on any violation	Zero-motion freezes near margins	Non-worsening floors + bounded escape recovery
Fixed top‑K pairs forever	Misses newly tight pairs on large sweeps	Tunable max_constraints + eval harness for ground truth
Same ε for all pairs	Wrong trade-off for margin-floor recovery	Separate worsen tolerances when structural_floor ≥ margin

EmbodiK’s difference is composition: soft QP steering, preset-tuned pair monitoring, per-pair non-worsening memory, post-step guards, and explicit separation between solver stall and eval harness holds.

Speed, Balanced, and Precise

Three presets control cache cadence, sphere broadphase, proximity-gated constraint activation, and the exact-distance refinement budget. New solvers default to CollisionTuningMode.BALANCED.

Preset	Best for	Trade-off
SPEED	High-rate teleop, Viser loops	Time budget (~300 µs refinement cap); fastest median step time
BALANCED	Most examples and WBC replay harness	Conservative cache, no refinement early-stop; default
PRECISE	Regression debug, distance fidelity	Cache off, broadphase off; full exact checks

import embodik as eik

solver = eik.KinematicsSolver(robot)
solver.set_collision_tuning_mode(eik.CollisionTuningMode.BALANCED)
# eik.CollisionTuningMode.SPEED  — bounded latency for high-rate teleop
# eik.CollisionTuningMode.PRECISE — full exact checks for debug / regression

Maintained examples also expose apply_collision_tuning_mode(solver, "balanced") via examples/example_helpers/ik_common.py.

Measured step time (Panda, moving target)

On a representative Panda solve_position_step workload (see scripts/benchmark_collision_tuning_modes.py), median collision-inclusive step time typically looks like:

Mode	Median step time (indicative)	Role
SPEED	≲ 0.3 ms (often ~0.1 ms in repo baselines)	Teleop / WBC hot path
BALANCED	~0.4–0.5 ms	Default interactive mode
PRECISE	≳ 5 ms (often ~7–8 ms in repo baselines)	Ground-truth distance fidelity

The clip below shows the same tracking workload while switching collision presets — per-step elapsed time rises from Speed to Precise (with Balanced in between) while the solver continues to follow the target:

Re-run on your robot before quoting externally:

pixi run python scripts/benchmark_collision_tuning_modes.py

Performance vs naive full scan

CI regression tests (test/test_collision_tuning_perf.py) lock in that Speed stays under 3 ms median per Panda solve_position_step, Balanced under 4 ms, and Speed stays well below Precise — while all three modes still pass penetration safety checks.

A naive exact full pair scan after every integration step bypasses these optimizations and is much slower on the same workload. EmbodiK’s post-step guard uses the tuned evaluation path, not an unbounded full scan.

Parallel batch IK (offline / analysis)

Real-time WBC uses one solver per control loop, but offline workloads scale out: morphology studies, reachability grids, and parameter sweeps fire thousands of independent IK solves.

EmbodiK supports this pattern well: large IK reachability and morphology sweeps can use num_workers > 1 with a process pool so each worker owns an EmbodiK solver instance — no shared mutable solver state across processes. That combination (per-process solvers + conservative collision presets on hot paths) keeps sweep throughput practical while still using the same collision stack as teleop.

For GPU-scale batch trajectories inside EmbodiK itself, see GPU Solvers and examples/parallel_trajectory_tracking.py.

Configure collision avoidance

solver.configure_collision_constraint(
    min_distance=0.04,       # required clearance (metres)
    include_pairs=[],        # empty = all configured pairs
    exclude_pairs=exclusions,
    nearest_points_all_pairs=False,
    max_constraints=3,       # top-K active QP rows; raise for large sweeps
)

Non-worsening recovery floor

Whole-body models often rest closer than min_distance on structural pairs (upper arm vs torso). Enable the floor so recovery does not try to push those pairs to the global margin and freeze:

solver.set_non_worsening_collision_floor_enabled(True)
solver.set_collision_structural_floor(0.005)  # default 5 mm; raise to match WBC margin if needed

Penetrating vs close clearance: if a pair is first seen inside the mesh (negative signed distance), EmbodiK seeds recovery toward the configured floor — not zero clearance — so the arm can exit penetration without treating interpenetration as a valid rest pose. A pair first seen above the floor but below min_distance may move toward the floor instead of being pinned to its initial clearance, preserving safe tangential and whole-body motion.

See KinematicsSolver — Collision recovery floor.

Tangential sliding at an active boundary

When an unconstrained strict SCALE task velocity would violate an active collision row, EmbodiK separates the objective into two parts. The hard collision row owns affine normal recovery, while the task Jacobian and goal are projected into the joint-space collision tangent. Recomputing the goal through the projected Jacobian keeps every task row realizable. A rank-revealing row-space compression then removes equations made dependent by the projection, preserving the same achievable task residual while giving the strict solver the correct tangent rank. This retains tangent motion without scaling mandatory recovery toward zero. A task velocity that already provides enough separation is left intact. SCALE_ELASTIC retains its elastic-band behavior because applying the strict-scale transform there would change held-task priority and entry/release semantics.

Constrained weighted fallback uses the same tangent-filtered Jacobian but keeps the original full-row residual as a soft error. Collision inequalities and post-step validation remain hard, so weighted trade-offs cannot restore unsafe normal approach.

Task-local joint exclusions remain local to their tasks. A joint is removed from global collision rows only when every active task excludes it, or when the position-step options explicitly lock it. This lets decoupled arm tasks keep independent ownership while collision constraints still use the arm that can move the active pair.

Position-step safety and fallback

For teleop loops:

• Post-step checks reject SUCCESS when penetration deepens beyond tolerance.
• PositionStepOptions.primary_allow_min_error_fallback = True retries once with MIN_ERROR when collision rows collapse primary task scale — unfreezing motion near the body without abandoning hard constraints.

Eval harness vs solver

Segment validators and integrated WBC eval may apply a stricter actionable-pair gate (e.g. 10 mm on specific link classes) using full geometry queries. That is eval policy.

The solver reports NO_PROGRESS, COLLISION_VIOLATED, and collision_rejection_count separately from harness Hold% / Back%. Do not treat a safety hold as a solver stall.

Examples and diagnostics

• Runnable demo: Collision-Aware IK Example
• Recovery policy overview: Solver Robustness & Recovery
• Guide map: Guides overview

pixi run python scripts/diagnose_collision_pair_membership.py torso_link right_bicep_yaw_link
pixi run python scripts/prove_sweep_collision_metric.py
pixi run python scripts/validate_wbc_segments.py

API reference

• KinematicsSolver — configure_collision_constraint, tuning presets, floors
• RobotModel — collision pair configuration and distance queries

Acknowledgments

Collision bound gating and the Speed / Balanced / Precise time-or-accuracy preset idea draw on Daniel Rakita's Proxima proximity work (RSS 2022). EmbodiK implements its own Pinocchio/hpp-fcl stack on top of that idea; it is not a port of the Optima toolbox.