KinematicsSolver
KinematicsSolver provides high-level inverse kinematics entry points:
solve_velocity()for one-step velocity IK over registered tasks.solve_position()for iterative position IK with an internal objective stack.solve_position_step()for marker/teleop loops using registered tasks.
Result Diagnostics
Solver result objects include condition_number, the worst Jacobian condition
number observed by the singularity-robust inverse during that solve. Values near
1.0 indicate well-conditioned task Jacobians; larger values mean the solve is
more sensitive to target changes, numerical tolerances, or near-singular robot
postures.
Use this field for logging, test assertions, and tuning UI feedback. It is not a
manipulability metric and does not indicate a separate solver mode. A high but
finite value can explain weak or unstable-looking motion even when the solve
returns SUCCESS.
Runtime Policy
SolverRuntimeConfig stores runtime defaults for interactive loops. See
Solver Robustness for the full picture (adaptive dt, elastic band,
stall handler, auto layout, weighted fallback). Summary:
cfg = solver.runtime_config()
cfg.enable_auto_task_layout = True
cfg.weighted_fallback_enabled = True
solver.configure_runtime(cfg)
enable_auto_task_layout applies to PoseTaskGroup adapters. It lets the solver
try the merged pose layout first, then switch to split position/orientation
tasks when the merged rows are binding poorly. The switch happens at solve
boundaries, not by mutating the caller's configuration after a rejected attempt.
weighted_fallback_enabled keeps the prioritized solver authoritative on
success. If the prioritized path does not find a useful step, the solver may
accept a constrained weighted candidate. The candidate uses the same hard
constraint machinery, so it is still subject to configured joint limits,
collision, CoM, relative-pose, contact projection, and linear constraints.
Disable weighted_fallback_enabled only when you are running an A/B benchmark
or need to reproduce historical strict-priority behavior. Disable
enable_auto_task_layout when you need a fixed merged or split pose-task layout.
Adaptive dt, elastic band, stall handler
- Adaptive dt —
PositionStepOptions.adaptive_dtscales integration step with position error; capped when collision clearance is tight. Configure defaults viaSolverRuntimeConfig. - Elastic band —
enable_elastic_band()orTaskSolveMode.SCALE_ELASTICtemporarily widens joint limit margins when limit-dominated stalls collapse task scale. - Stall handler —
enable_stall_handler(nominal_min_distance)+stall_recovery=Truerelaxes collision margin only when collision rows bind (not on joint-limit stalls).
See Solver Robustness.
Collision recovery floor
Whole-body robots often include link pairs that rest closer than the configured
collision clearance. Enable the non-worsening floor when those structural pairs
should not trigger an infeasible push to the global min_distance:
solver.set_non_worsening_collision_floor_enabled(True)
solver.set_collision_structural_floor(0.005) # metres; default 5 mm
The default 5 mm floor preserves the historical non-worsening behavior for positive structural clearances: pairs that already rest above the floor keep their observed clearance rather than being pushed to the global collision margin. When an app deliberately raises the floor, first-seen positive pairs below that raised floor recover toward the floor; first-seen penetrating pairs also recover toward the floor, capped by the active collision margin.
The floor is off by default. Getter/setter pairs:
get_non_worsening_collision_floor_enabled() and
get_collision_structural_floor().
Collision tuning default
Fresh KinematicsSolver instances default to CollisionTuningMode.BALANCED
(sphere broadphase + conservative pair cache). Override with
set_collision_tuning_mode() when benchmarking or reproducing older behavior.
Three presets trade latency vs distance fidelity:
| Mode | Typical use | Character |
|---|---|---|
SPEED |
High-rate teleop | Aggressive cache + bounded exact-refinement budget (~300 µs) |
BALANCED |
Default interactive IK | Conservative cache, no refinement early-stop |
PRECISE |
Debug / regression | Cache off; full exact checks every step |
EmbodiK combines these tuned hot paths with post-step penetration guards and optional non-worsening floors so fast modes do not silently accept deepening penetration. See the Collision Constraints guide for tuning walkthroughs, batch parallelization notes, and measured Speed vs Precise timings.
Position-step options (teleop)
PositionStepOptions fields used by marker/teleop loops:
max_steps— inner IK iterations per control tick (bimanual teleop default:2)primary_solve_mode— mirrors registered EE task solve mode for the primary bandprimary_allow_min_error_fallback— whenTrue, retry a stalled SCALE/SCALE_ELASTIC primary solve once with MIN_ERROR before accepting freeze
See docs/examples/collision_aware_ik.md for collision-floor, adaptive dt, elastic band, and
fallback interaction with configure_collision_constraint().
Position IK Objective Order
solve_position() now supports a three-level stack:
- Primary end-effector frame objective (priority
0) - Optional torso orientation/pose objective (priority
1) - Optional nullspace posture bias (priority
2when torso is enabled, else1)
PositionIKOptions (torso + nullspace)
import numpy as np
import embodik as eik
opts = eik.PositionIKOptions()
opts.max_iterations = 20
opts.position_gain = 40.0
opts.orientation_gain = 40.0
# Tertiary nullspace bias
opts.nullspace_bias = q_bias
opts.nullspace_gain = 0.01
opts.nullspace_active_joints = [0, 1, 2, 3, 4, 5, 6]
opts.nullspace_joint_weights = np.ones(len(opts.nullspace_active_joints))
# Secondary torso orientation objective
opts.torso_constraint.enabled = True
opts.torso_constraint.frame_name = "base_link"
opts.torso_constraint.orientation_mask = np.array([1.0, 1.0, 0.0]) # roll/pitch only
opts.torso_constraint.orientation_gain = 0.05
# Optional torso pose box bounds (6D: x,y,z,rx,ry,rz)
# Units: x/y/z in meters, rx/ry/rz in radians.
# Bounds are relative to a fixed world-frame torso reference. Inside solve_position,
# that reference does not move across inner iterations. If you call solve_position
# every control tick with a new seed_q, set pose_bounds_reference_pose once (e.g.
# torso.homogeneous() at session start) so the box does not recentre each tick.
half_range = np.array([0.08, 0.08, 0.08, 0.20, 0.20, 0.20])
torso0 = robot.get_frame_pose("base_link")
opts.torso_constraint.pose_bounds_reference_pose = np.asarray(
torso0.homogeneous(), dtype=float
)
opts.torso_constraint.pose_lower_bounds = -half_range
opts.torso_constraint.pose_upper_bounds = half_range
opts.torso_constraint.pose_axis_mask = np.ones(6)
# Units: [m/s, m/s, m/s, rad/s, rad/s, rad/s]
opts.torso_constraint.velocity_limits = np.full(6, 0.5)
# Units: [m/s^2, m/s^2, m/s^2, rad/s^2, rad/s^2, rad/s^2]
opts.torso_constraint.acceleration_limits = np.full(6, 1.0)
Validation rules:
nullspace_biasmust matchrobot.nq.nullspace_active_jointsindices must be unique and in[0, robot.nv).nullspace_joint_weightslength must matchrobot.nv(all joints) orlen(nullspace_active_joints)(selected joints).- Torso pose bounds require both lower and upper vectors, each length
6. - Torso 6D ordering is
[x, y, z, rx, ry, rz]with units[m, m, m, rad, rad, rad]. - Optional
torso_constraint.pose_bounds_reference_pose(4x4 homogeneous): when set, pose bounds are measured vs this fixed transform; when unset, the reference is the torso frame atseed_qfor thatsolve_positioncall only.
solve_position Usage
target = np.eye(4)
target[:3, 3] = [0.5, 0.2, 0.3]
result = solver.solve_position(seed_q, target, "end_effector", opts)
API Reference
KinematicsSolver
High-level kinematics solver providing simple API for IK problems
Attributes
Functions
add_absolute_frame_task
Add an absolute frame task (weighted average of two frames)
add_collision_constraint
Convenience helper to enable collision avoidance using a specific set of link pairs.
add_contact_frame
Add a contact frame for contact-root Jacobian projection.
POINT_CONTACT constrains linear velocity only (3 rows). RIGID_CONTACT constrains full spatial velocity (6 rows).
add_frame_task
Add a frame tracking task
add_pose_task_group
add_pose_task_group(
name,
tcp_frame,
base_priority=0,
rotation_priority_offset=1,
merged_pose=False,
auto_switch=False,
)
Add a pose task group adapter backed by regular frame tasks
add_relative_frame_task
Add a relative frame task (tracks T_a^{-1} * T_b)
add_tight_frame_pose_constraint
add_tight_frame_pose_constraint(
frame_name, target_pose, position_epsilon=1e-05, orientation_epsilon=0.0001, axis_mask=...
)
Add a tight 6D frame pose epsilon-box constraint.
add_tight_point_constraint
Add a tight 3D point epsilon-box constraint.
append_linear_velocity_constraints
Append user-defined linear velocity constraints.
calculate_velocity_box_constraint
calculate_velocity_box_constraint(
position_margin_lower,
position_margin_upper,
velocity_limit,
acceleration_limit,
dt,
min_velocity_headroom=-1.0,
headroom_activation_margin=0.01,
)
Compute velocity bounds from position/velocity/acceleration limits.
clear_all_target_velocities
Clear direct target velocities on all registered tasks.
clear_collision_pair_min_distance
Remove per-pair min_distance overrides for link_a/link_b geometry pairs. Affected pairs revert to the global min_distance.
clear_joint_metric_weights
Clear the joint-space metric (restore unweighted solve)
clear_joint_velocity_limit_overrides
Clear all per-joint velocity-limit overrides
clear_linear_velocity_constraints
Clear all user-defined linear velocity constraints.
clear_tight_frame_pose_constraints
Clear all tight frame pose constraints.
configure_collision_constraint
configure_collision_constraint(
min_distance,
include_pairs=[],
exclude_pairs=[],
nearest_points_all_pairs=True,
max_constraints=1,
)
Enable collision avoidance with optional include/exclude geometry pair filters.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
min_distance
|
float
|
Minimum separation distance to enforce (metres). |
required |
include_pairs
|
Sequence[tuple[str, str]]
|
List of (geom_a, geom_b) tuples to consider (empty = all). |
[]
|
exclude_pairs
|
Sequence[tuple[str, str]]
|
List of (geom_a, geom_b) tuples to ignore. |
[]
|
nearest_points_all_pairs
|
bool
|
If False, compute nearest points only for the selected pair. |
True
|
max_constraints
|
int
|
Number of simultaneous QP constraint rows. Each row protects one of the closest pairs independently. Defaults to 1 (original behaviour). Values of 3-5 are recommended for complex robots with multiple tight-clearance regions (e.g. base/leg and arm/torso simultaneously). |
1
|
configure_com_constraint
configure_com_constraint(
support_polygon,
margin=0.0,
frame_name="world",
com_vel_max=0.4,
com_acc_max=0.1,
use_acceleration_limits=True,
proximity_fraction=0.0,
)
Configure a CoM support-polygon inequality constraint.
Keeps the 2D projection of the center of mass inside the given convex polygon. Velocity and acceleration limits are applied to smoothly saturate CoM velocity near the polygon boundary, bounding tipping energy.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
support_polygon
|
Annotated[NDArray[float64], dict(shape=(None, None), order=F)]
|
Nx2 or Nx3 array of polygon vertices in the XY plane of frame_name (Z column is ignored if Nx3). |
required |
margin
|
float
|
Fractional inward shrink in [0, 1]. Applied as margin * char_size (mean distance centroid→vertices). Matches feasibility check. |
0.0
|
frame_name
|
str
|
Frame in which vertices are expressed. |
'world'
|
com_vel_max
|
float
|
Maximum CoM velocity (m/s). |
0.4
|
com_acc_max
|
float
|
Maximum CoM acceleration (m/s²). |
0.1
|
use_acceleration_limits
|
bool
|
If True, clamp approach velocity by sqrt(2 * com_acc_max * margin) near boundary. |
True
|
proximity_fraction
|
float
|
Fraction of the polygon inradius used as the per-row activation distance. A half-plane row is only added to the QP when the CoM slack for that row is less than proximity_fraction * inradius. The inradius (minimum perpendicular distance from centroid to any edge) is computed automatically from the vertices. Set to 0 (default) to disable proximity filtering and always include every row. Use get_com_proximity_threshold() to read back the computed threshold in metres. |
0.0
|
configure_contact_frames
Clear and configure multiple contact frames with a shared contact type.
configure_elastic_band
configure_elastic_band(
delta_max=0.05, expand_rate=0.01, decay_rate=0.2, stall_threshold=3, expand_only_saturated=True
)
Configure elastic band tuning parameters.
configure_relative_pose_constraint
Configure a relative pose inequality constraint.
Constrains each masked axis of T_a^{-1} * T_b to stay within the given bounds. Uses relative Jacobian as QP inequality rows.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
frame_a
|
str
|
Reference frame name |
required |
frame_b
|
str
|
Target frame name |
required |
lower_bounds
|
Annotated[NDArray[float64], dict(shape=(None,), order=C)]
|
6D lower bounds (pos xyz + ori xyz) |
required |
upper_bounds
|
Annotated[NDArray[float64], dict(shape=(None,), order=C)]
|
6D upper bounds (pos xyz + ori xyz) |
required |
axis_mask
|
Annotated[NDArray[float64], dict(shape=(None,), order=C)]
|
6D mask (1=constrained, 0=free). Empty = all. |
...
|
configure_runtime
Apply bundled runtime defaults. Stamps damping immediately and stores position-step defaults for make_position_step_options().
configure_stall_handler
Configure stall handler tuning parameters.
disable_stall_handler
Disable the stall handler and restore nominal parameters.
elastic_band_max_delta
Return the maximum delta currently active across all joints.
enable_acceleration_limits
Enable or disable inter-tick acceleration limit constraints
enable_collision_pair_cache
enable_collision_pair_cache(
enable, full_refresh_interval=20, candidate_distance_margin=0.03, max_cached_candidates=128
)
Enable conservative collision pair candidate caching.
When enabled, collision distance queries are evaluated on cached active/near-active candidate pairs between periodic full scans. This is intended for teleop loops where active pairs evolve smoothly over time.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
enable
|
bool
|
Enable/disable candidate caching. |
required |
full_refresh_interval
|
int
|
Steps between mandatory full pair scans. |
20
|
candidate_distance_margin
|
float
|
Extra margin (m) above min_distance for retaining near-active pairs in the candidate cache. |
0.03
|
max_cached_candidates
|
int
|
Cap on cached pair indices. |
128
|
enable_elastic_band
Enable elastic band joint limit expansion for limit-dominated stalls. Temporarily expands joint limit margins to keep more DOFs active in the SNS solver.
enable_position_ik_debug
Enable verbose logging for position IK iterations
enable_saturation_exit_behavior
Enable velocity-box softening near limits (disabled by default).
enable_sphere_broadphase
Enable sphere-based broadphase culling for collision queries.
When enabled, cheap sphere-sphere distance bounds skip expensive GJK/EPA calls for pairs whose bounding spheres are far apart. Spheres are auto-computed from collision geometry AABBs.
enable_stall_handler
Enable the automatic stall handler. Detects consecutive stalled velocity solves (non-success with near-zero ||dq||) and applies collision margin relaxation/restoration to break out of stalls.
enable_timing_breakdown
Enable/disable detailed timing breakdown fields in VelocitySolverResult
evaluate_collision_debug
Evaluate collisions at the provided configuration and return debug info (side-effect free).
evaluate_min_collision_distance
Evaluate minimum collision distance at the given configuration. If current_q is empty, uses the current configuration.
evaluate_post_step_collision_distance
Evaluate the scalar collision distance used by post-step safety checks. Prefers cached / targeted collision data before falling back to a global scan.
get_active_collision_pairs
Return the list of collision pairs currently considered by the solver.
get_collision_constraint_activation_margin
Get effective collision-row activation margin in meters.
get_collision_constraint_activation_multiplier
Get collision-row activation multiplier.
get_collision_min_distance
Read the current collision min_distance. Returns -1 if no collision constraint is active.
get_collision_pair_min_distance_overrides
Return list of (pair_key, min_distance) tuples for all active per-pair overrides.
get_collision_refinement_time_budget_us
Get exact collision refinement budget per solve (microseconds).
get_collision_structural_floor
Return the structural collision recovery floor distance (m).
get_collision_tuning_mode
Get the active high-level collision tuning preset.
get_com_proximity_threshold
Return the proximity threshold (m) computed by the last call to configure_com_constraint(). Equals proximity_fraction * inradius where the inradius is the minimum perpendicular distance from the polygon centroid to any edge. Returns 0 if no constraint is set.
get_last_collision_debug
Retrieve debug information for the closest active collision pair after the last solve, if available. When max_constraints > 1, use get_last_collision_debug_list() for all active pairs.
get_last_collision_debug_list
Retrieve debug information for all active collision constraint pairs after the last solve (one entry per constraint row, up to max_constraints). Returns an empty list when no collision constraint is configured or no solve has been performed.
get_linear_velocity_constraint_rows
Return active user-defined linear constraint row count.
get_proximity_gated_collision_activation_enabled
Get whether proximity-gated collision-row activation is enabled.
make_position_step_options
Return fresh PositionStepOptions populated from runtime defaults.
reset_adaptive_state
Reset default-off stateful runtime adapters without changing the stored runtime configuration.
saturation_exit_behavior_enabled
Return whether saturation-exit softening is enabled.
set_base_orientation_bounds
Set floating-base orientation bounds (3D, in velocity space)
set_base_position_bounds
Set floating-base position bounds (3D)
set_collision_constraint_activation_multiplier
Set proximity-gated collision-row activation multiplier.
Effective activation margin is multiplier * min_distance. Values <= 0 disable gating and preserve legacy row-emission behavior.
set_collision_max_separation_speed_nonpenetrating
Max separation speed (m/s) for non-penetrating recovery. Default 0.15 m/s.
set_collision_min_distance
Update only the min_distance of an already-configured collision constraint without rebuilding pair masks. Returns True if updated, False if no constraint exists.
set_collision_pair_min_distance
Set a custom minimum distance for all collision geometry pairs whose parent frame name contains link_a and link_b respectively.
activate_when_clear=True (default): the override is pending until the pair first achieves the desired clearance during a solve, then latches on permanently. Safe to call from any starting configuration.
activate_when_clear=False: override takes effect immediately (use when the robot is already above the threshold and you need instant effect).
Call after configure_collision_constraint().
set_collision_recovery_scale
Fraction of desired recovery velocity applied inside min_distance. Default 0.2.
set_collision_refinement_time_budget_us
Set optional exact collision refinement budget per solve in microseconds. Values <= 0 disable budgeting.
set_collision_repulsion_deadband
Width (m) of the no-braking zone above min_distance. Default 0.003 m. Set to 0 to eliminate the discontinuity that causes boundary oscillation.
set_collision_structural_floor
Minimum clearance (m) maintained for structurally-close pairs when the non-worsening floor is enabled. Default 0.005 m.
set_collision_tuning_mode
Apply high-level collision tuning preset.
Modes
PRECISE - full exact checks (highest accuracy, highest cost) BALANCED - conservative cache cadence without time budget SPEED - fastest teleop-oriented path
set_constraint_tolerance
Set constraint violation deadband and COD pseudoinverse relative threshold (VelocitySolverConfig.epsilon).
set_joint_metric_weights
Soft per-joint joint-space metric (size nv): higher weight => that joint contributes less to the achieved task motion (weighted least-norm). All-ones is a no-op. Torso-vs-arm contribution knob.
set_joint_velocity_limit
Override one joint's velocity limit (nv index); throttles that joint so the solver recruits other DOFs to keep tracking
set_limit_exit_release_margin
Set release margin that relaxes tiny post-limit recovery forcing.
set_limit_recovery_hysteresis
Set enter/exit hysteresis epsilons for joint-limit recovery.
set_linear_velocity_constraints
Replace user-defined linear velocity constraints.
Enforces lower_bounds <= C @ dq <= upper_bounds.
set_non_worsening_collision_floor_enabled
Enable the per-pair non-worsening (ratcheting) collision recovery floor. When enabled, links that rest closer than min_distance are maintained at their achievable distance instead of triggering an infeasible recovery. Off by default on a new solver; the bimanual teleop app opts in.
set_proximity_gated_collision_activation_enabled
Enable/disable proximity-gated collision-row activation without changing multiplier or margin values.
set_regularization_epsilon
Alias of set_tolerance(): set singular-value damping threshold for the regularized pseudoinverse.
set_solver_recovery_enabled
Deprecated no-op. Recovery state machine has been removed.
set_tolerance
Set singular-value damping threshold for the regularized pseudoinverse (default 0.1).
solve_position
Solve position-level IK to reach target pose. With options.classify_stagnation_as_no_progress=True, stagnation exits return SolverStatus.NO_PROGRESS.
solve_position_in_tcp
Solve position-level IK with target relative to TCP frame
solve_velocity
Solve for joint velocities without integration. Returns velocities and identifies saturated joints. When stall_recovery=True, enables automatic stall detection and collision-margin relaxation / restoration. The handler stays active across calls so stall counts accumulate correctly in user loops.
solve_velocity_dq
Solve for joint velocities and return only dq as a NumPy array. When stall_recovery=True, enables automatic stall handler.
stall_handler_consecutive_stall_steps
Return the number of consecutive stall steps.
stall_handler_current_min_distance
Return the current effective collision min_distance.
stall_handler_floor_fraction
Return the current stall-handler floor fraction.
stall_handler_is_relaxed
Return True if collision margin is currently relaxed.
stall_handler_restore_rate
Return the current stall-handler restore rate.