Skip to content

Solver robustness and recovery

EmbodiK keeps recovery policy in the C++ solver, not in example-side guard code. The same KinematicsSolver that tracks your end-effector also decides how to unstick near joint limits, collision margins, and singular layouts — while still respecting hard constraints.

Related guides

Collision tuning (Speed / Balanced / Precise, bounds, post-step guards) → Collision Constraints & Tuning. Guide map and reading order → Guides overview. Runnable teleop → Teleop IK, Collision-Aware IK.

Solver-owned vs app-owned

Concern Typical naive stack EmbodiK
Stuck at joint limit App lowers gains or nudges joints manually Elastic band expands limit margins temporarily; SCALE_ELASTIC task mode bundles this
Stuck at collision margin App disables collision or shrinks targets Stall handler relaxes margin only when collision rows bind; non-worsening floors for structural pairs
Pose task overconstrained App splits position/orientation tasks by hand Auto task layout switches merged ↔ split at solve boundaries
Priority stack infeasible App drops tasks or switches to Jacobian transpose Weighted fallback accepts a constrained weighted candidate under the same hard limits
Large marker jump App retunes dt or gains per scene Adaptive dt scales integration step with error (capped near collision)
Penetration creep Hope QP constraints are enough Post-step acceptance rejects deepening penetration (see collision guide)

Hard constraints stay in C++ for all of the above: joint limits, collision, CoM polygon, contact projection, and linear inequalities.

Runtime policy (SolverRuntimeConfig)

Most interactive examples call configure_solver_runtime_policy(solver) to enable the default robust teleop bundle:

import embodik as eik

solver = eik.KinematicsSolver(robot)
cfg = solver.runtime_config()
cfg.enable_auto_task_layout = True
cfg.weighted_fallback_enabled = True
cfg.adaptive_dt = True  # optional; stamp into PositionStepOptions via make_position_step_options()
solver.configure_runtime(cfg)
Flag Default What it does
weighted_fallback_enabled on After a non-success prioritized solve, may accept a constrained weighted MIN_ERROR candidate that still satisfies collision, limits, CoM, etc.
enable_auto_task_layout off in raw solver; on in examples Toggles merged 6D pose vs split position + orientation tasks when binding score says one layout fits better
adaptive_dt off When stamped into PositionStepOptions, scales integration dt with position error
weighted_advisor_enabled off Computes weighted candidate every step for diagnostics without changing authoritative output

Prioritized SNS remains authoritative on success. Fallback and layout switches happen at solve boundaries — the solver does not silently rewrite your registered task list mid-tick.

Auto task layout

PoseTaskGroup adapters can register both a merged FRAME_POSE task and split position/orientation tasks. With enable_auto_task_layout:

  1. Start on merged layout (fewer rows, faster when it works).
  2. Switch to split when the previous constrained solve’s binding score exceeds auto_layout_binding_threshold_high (default 0.30).
  3. Switch back to merged after auto_layout_cooldown_ticks consecutive scores below auto_layout_binding_threshold_low (default 0.15).

Useful when orientation rows fight position rows near singular or boxy workspaces — the solver picks the layout, not the teleop script.

Weighted fallback

When the priority stack cannot make progress (INFEASIBLE, NO_PROGRESS, etc.), the solver may evaluate a weighted stacked velocity solve under the same hard constraints. If that candidate is feasible, it can replace the failed prioritized step (recovery_stage reports WEIGHTED_FALLBACK in diagnostics).

Disable only for strict-priority A/B benchmarks — not for production teleop.

Task solve modes

Per-task TaskSolveMode controls how strictly a frame task must be met each velocity step:

Mode Behavior Typical use
SCALE Task rows scale down under conflict (elastic priority) Default teleop
MIN_ERROR Minimum-error objective under hard constraints Recovery near body; stall unfreeze
SCALE_ELASTIC Like SCALE + automatic elastic band on joint limits Whole-body teleop near limit saturation

PositionStepOptions.primary_allow_min_error_fallback = True retries a stalled primary SCALE / SCALE_ELASTIC step once with MIN_ERROR while keeping collision and CoM active — see Collision-Aware IK.

Adaptive integration timestep

Large marker jumps need larger effective steps; near the target, small steps prevent overshoot. With adaptive_dt=True:

effective_dt = dt × clamp(pos_error / reference_distance, 1.0, max_scale)

Defaults: reference_distance=0.05 m, max_scale=5.0 (override via SolverRuntimeConfig or per-call PositionStepOptions).

Near collision: if clearance to the active constraint margin is tight, adaptive scaling is capped so a larger dt cannot outrun what the margin allows — avoiding “teleport through” the safety shell.

Enable in teleop:

opts = solver.make_position_step_options()
opts.adaptive_dt = True
result = solver.solve_position_step(q, target, "ee_task", opts)

Elastic band joint limits

When several joints sit on their limits, the QP can lose degrees of freedom and collapse task scale. Elastic band temporarily widens position limit margins on saturated joints:

  • Expands after repeated limit-dominated stalls (expand_rate, stall_threshold).
  • Decays when solves are healthy again (decay_rate).
  • Capped per joint (delta_max, default 0.05 rad).

Enable explicitly:

solver.enable_elastic_band(0.05)
# or per step:
opts.elastic_band = True

Or use TaskSolveMode.SCALE_ELASTIC on frame tasks for the same mechanism with tuned defaults.

Stall handler (collision margin recovery)

Distinct from solver stall status and from eval harness Hold%:

solver.enable_stall_handler(nominal_min_distance=0.04)
opts.stall_recovery = True  # persists across solve_position_step calls

After consecutive near-zero-velocity failed solves, the handler may ratchet down effective min_distance — but only when a collision constraint row is actually binding. Joint-limit stalls do not open collision margin (prevents driving the arm into the body).

Margin restores gradually toward nominal when solves succeed again.

Pair with non-worsening floors for structurally close pairs (Collision Constraints).

CoM support polygon

configure_com_constraint() adds half-plane velocity inequalities so the projected CoM stays inside a support polygon, with:

  • Margin shrink via char_size (mean centroid-to-vertex distance).
  • Velocity and acceleration caps near the boundary (smooth approach, reduced overshoot).
  • Optional proximity activation — rows enter the QP only when CoM slack is within a fraction of polygon inradius (cheaper when the CoM is comfortably inside).

See CoM Constraint Example.

Diagnostics worth logging

VelocitySolverResult / PositionIKResult expose solver state for UI and tests:

Field Meaning
condition_number Worst task Jacobian condition number this step — high ⇒ sensitive posture
binding_score How hard inequality rows (limits, collision, CoM) constrained the step
recovery_stage PRIORITIZED vs WEIGHTED_FALLBACK
task_scales Per-task scale under SCALE modes
collision_rejection_count Post-step guards rejected integration
stall_escape_count Jacobian escape nudges out of penetration

Use these in Viser panels and regression tests instead of guessing from motion alone.

Batch and parallel workloads

Real-time control uses one solver per loop. Offline reachability and morphology sweeps scale with process pools — each worker owns an EmbodiK instance. GPU batch IK is separate; see GPU Solvers.