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:
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 = Trueretries 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
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.