Skip to content

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/10_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.

The persistent floor does not increase merely because a pair moves above nominal clearance. Position and position-step rejection check the effective per-pair floor, including pair overrides and command-local anchors, rather than applying the global nominal distance a second time. An independent penetration guard remains active. With the structural policy disabled, configured nominal pair clearances remain enforced.

To retain startup clearance rather than initialize from the structural floor, initialize the existing recovery-floor map once, before the first solve:

solver.set_non_worsening_collision_floor_enabled(True)
solver.initialize_collision_recovery_floors(q_start)

For every currently allowed and runtime-active pair this stores min(signed_distance(q_start), active_pair_minimum). For example, with a 40 mm nominal margin, a 20 mm startup gap and a 5 mm structural floor, the initialized target is 20 mm. The default structural policy would permit loss toward 5 mm. Nominal margins, active pair overrides, actuator acceleration settings and robot configuration/velocity are preserved. A signed negative startup distance remains negative in this map; initialization does not establish that a configuration is collision-free. Applications requiring a nonpenetrating startup state must reject negative startup distances before initialization.

At a satisfied recovery floor, the velocity damper permits stationary motion. Its numerical reserve forms a no-approach band above the floor rather than requiring separation from an already satisfied boundary. Recovery below the declared floor remains active, including below a negative initialized floor.

Initialization requires enabled collision avoidance and non-worsening recovery, an empty recovery-floor map, no trajectory-floor anchor, and no pending pair overrides. Missing/invalid pair evidence rejects the whole initialization; floors are committed only after the complete cold query succeeds. Initialization after any recovery floors have been seeded is rejected; callers must initialize before their first solve. Explicit later reconfiguration, pair/catalog or geometry changes can clear or change floors, so applications retaining a startup contract must freeze that context and avoid reseeding on target changes.

This CPU API keeps the ordinary native floating-point and normalized-row acceptance tolerances. It does not promise a particular nanometre threshold or continuous path clearance. Applications needing a stricter sampled criterion must independently check the original floor map before adopting a command.

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

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

Fresh per-pair endpoint evaluation

solver.evaluate_collision_pair_distances(q) evaluates every currently allowed and active collision geometry pair at a finite, full-size configuration. It returns read-only CollisionPairDistance records with geometry_a, geometry_b, distance, minimum_distance, and persistent_floor_m. Each minimum is the currently active pair override or configured global margin. Pending overrides and recovery floors are not activated by this diagnostic query.

persistent_floor_m reports the initialized persistent recovery floor, capped by the active pair minimum. It is None while that policy is disabled or the pair has not been seeded by solving. Queries neither seed nor ratchet floors. This value does not include command-local anchors or certify a candidate or swept trajectory; retain those checks separately. A persistent structural floor can permit clearance loss above that floor, so a per-step no-clearance-loss test measures a different policy.

records = solver.evaluate_collision_pair_distances(q_command)
if records is not None:
    for pair in records:
        print(pair.geometry_a, pair.geometry_b, pair.distance, pair.minimum_distance)

The query preserves robot configuration, velocity, collision policy and shared geometry results. It uses local geometry data for fresh distances. Missing geometry, a disabled collision policy, or no active allowed pairs returns None; invalid configurations or nonfinite distances raise an error. It does not limit evaluation to the constraint row budget or a cached closest pair.

For a clearance-policy snapshot without geometric evaluation, use solver.get_collision_pair_floors(). It returns read-only CollisionPairFloor records in the same active/allowed pair order, with geometry_a, geometry_b, minimum_distance, and persistent_floor_m. Values are read fresh on each call; the getter performs no forward kinematics, distance queries, floor initialization, or solver-state mutation. Missing geometry, disabled policy, or an empty active catalog returns None. A trajectory-floor anchor takes precedence over the ordinary persistent floor, as in the distance observer.

This metadata query is useful for detecting policy changes. It does not detect invalid geometric distances or replace collision validation at a configuration.

When checking a command modified after solving, compare complete previous and candidate pair snapshots. Require each pair's configured margin; for a pair already inside its margin, require nonworsening clearance relative to that pair's previous distance. A single minimum distance and the largest override margin cannot establish this per-pair condition.

Distance evidence consistency

A finite narrow-phase result can still be numerically inconsistent. When initialized enclosing AABBs prove positive separation, the shared distance query checks that its result respects a conservative world-space lower bound. The comparison accounts for the query's stated numerical precision without changing its reported distance. Requests with nonzero abs_err or rel_err skip this auxiliary bound comparison and retain the existing finite-result checks. Contradictory evidence triggers the existing single cold retry with an alternate initial guess; if that evidence is also invalid, the query fails. The returned distance and closest points come from the full narrow-phase query, never from the enclosing bound. Overlapping or unavailable bounds do not restrict valid signed penetration. This changes neither collision floors nor the scope of sampled or swept trajectory guarantees.

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.