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.

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 2 when torso is enabled, else 1)

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_bias must match robot.nq.
nullspace_active_joints indices must be unique and in [0, robot.nv).
nullspace_joint_weights length must match robot.nv (all joints) or len(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 at seed_q for that solve_position call 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

doc `class-attribute`

__doc__ = 'High-level kinematics solver providing simple API for IK problems'

str(object='') -> str str(bytes_or_buffer[, encoding[, errors]]) -> str

Create a new string object from the given object. If encoding or errors is specified, then the object must expose a data buffer that will be decoded using the given encoding and error handler. Otherwise, returns the result of object.str() (if defined) or repr(object). encoding defaults to sys.getdefaultencoding(). errors defaults to 'strict'.

module `class-attribute`

__module__ = 'embodik._embodik_impl'

str(object='') -> str str(bytes_or_buffer[, encoding[, errors]]) -> str

Create a new string object from the given object. If encoding or errors is specified, then the object must expose a data buffer that will be decoded using the given encoding and error handler. Otherwise, returns the result of object.str() (if defined) or repr(object). encoding defaults to sys.getdefaultencoding(). errors defaults to 'strict'.

dt `property`

dt

Time step for velocity integration

robot `property`

robot

Get the robot model

tasks `property`

tasks

Get all tasks

Functions

init `method descriptor`

__init__()

init(self, robot: embodik._embodik_impl.RobotModel) -> None

Create a kinematics solver for the given robot model

new `builtin`

__new__(*args, **kwargs)

Create and return a new object. See help(type) for accurate signature.

repr `method descriptor`

__repr__()

repr(self) -> str

add_absolute_frame_task `method descriptor`

add_absolute_frame_task()

add_absolute_frame_task(self, name: str, frame_a: str, frame_b: str, alpha: float = 0.5) -> embodik._embodik_impl.AbsoluteFrameTask

Add an absolute frame task (weighted average of two frames)

add_collision_constraint `method descriptor`

add_collision_constraint()

add_collision_constraint(self, link_pairs: collections.abc.Sequence[tuple[str, str]], min_distance: float = 0.05) -> None

Convenience helper to enable collision avoidance using a specific set of link pairs.

add_com_task `method descriptor`

add_com_task()

add_com_task(self, name: str) -> embodik._embodik_impl.COMTask

Add a center of mass tracking task

add_frame_task `method descriptor`

add_frame_task()

add_frame_task(self, name: str, frame_name: str, task_type: embodik._embodik_impl.TaskType = TaskType.FRAME_POSE) -> embodik._embodik_impl.FrameTask

Add a frame tracking task

add_joint_task `method descriptor`

add_joint_task()

add_joint_task(self, name: str, joint_name: str, target_value: float = 0.0) -> embodik._embodik_impl.JointTask

Add a single joint tracking task

add_posture_task `method descriptor`

add_posture_task(name: str) -> PostureTask

add_posture_task(name: str, controlled_joints: Sequence[int]) -> PostureTask

add_posture_task()

add_posture_task(self, name: str) -> embodik._embodik_impl.PostureTask add_posture_task(self, name: str, controlled_joints: collections.abc.Sequence[int]) -> embodik._embodik_impl.PostureTask

Overloaded function.

add_posture_task(self, name: str) -> embodik._embodik_impl.PostureTask

Add a posture regularization task for all joints

add_posture_task(self, name: str, controlled_joints: collections.abc.Sequence[int]) -> embodik._embodik_impl.PostureTask

Add a posture regularization task for specific joints

add_relative_frame_task `method descriptor`

add_relative_frame_task()

add_relative_frame_task(self, name: str, frame_a: str, frame_b: str) -> embodik._embodik_impl.RelativeFrameTask

Add a relative frame task (tracks T_a^{-1} * T_b)

add_tight_frame_pose_constraint `method descriptor`

add_tight_frame_pose_constraint()

add_tight_frame_pose_constraint(self, frame_name: str, target_pose: numpy.ndarray[dtype=float64, shape=(4, 4), order='F'], position_epsilon: float = 1e-05, orientation_epsilon: float = 0.0001, axis_mask: numpy.ndarray[dtype=float64, shape=(*), order='C'] = array([], dtype=float64)) -> None

Add a tight 6D frame pose epsilon-box constraint.

add_tight_point_constraint `method descriptor`

add_tight_point_constraint()

add_tight_point_constraint(self, frame_name: str, target_point: numpy.ndarray[dtype=float64, shape=(3), order='C'], position_epsilon: float = 1e-05, axis_mask: numpy.ndarray[dtype=float64, shape=(3), order='C'] = array([1., 1., 1.])) -> None

Add a tight 3D point epsilon-box constraint.

append_linear_velocity_constraints `method descriptor`

append_linear_velocity_constraints()

append_linear_velocity_constraints(self, C: numpy.ndarray[dtype=float64, shape=(, ), order='F'], lower_bounds: numpy.ndarray[dtype=float64, shape=(), order='C'], upper_bounds: numpy.ndarray[dtype=float64, shape=(), order='C']) -> None

Append user-defined linear velocity constraints.

calculate_velocity_box_constraint `method descriptor`

calculate_velocity_box_constraint()

calculate_velocity_box_constraint(self, position_margin_lower: float, position_margin_upper: float, velocity_limit: float, acceleration_limit: float, dt: float, min_velocity_headroom: float = -1.0, headroom_activation_margin: float = 0.01) -> tuple[float, float]

Compute velocity bounds from position/velocity/acceleration limits.

clear_all_target_velocities `method descriptor`

clear_all_target_velocities()

clear_all_target_velocities(self) -> None

Clear direct target velocities on all registered tasks.

clear_base_bounds `method descriptor`

clear_base_bounds()

clear_base_bounds(self) -> None

Clear floating-base bounds (use unlimited bounds)

clear_collision_constraint `method descriptor`

clear_collision_constraint()

clear_collision_constraint(self) -> None

Disable collision avoidance constraint.

clear_collision_pair_min_distance `method descriptor`

clear_collision_pair_min_distance()

clear_collision_pair_min_distance(self, link_a: str, link_b: str) -> None

Remove per-pair min_distance overrides for link_a/link_b geometry pairs. Affected pairs revert to the global min_distance.

clear_com_constraint `method descriptor`

clear_com_constraint()

clear_com_constraint(self) -> None

Disable CoM support-polygon constraint.

clear_linear_velocity_constraints `method descriptor`

clear_linear_velocity_constraints()

clear_linear_velocity_constraints(self) -> None

Clear all user-defined linear velocity constraints.

clear_relative_pose_constraint `method descriptor`

clear_relative_pose_constraint()

clear_relative_pose_constraint(self) -> None

Disable relative pose constraint

clear_tasks `method descriptor`

clear_tasks()

clear_tasks(self) -> None

Remove all tasks

clear_tight_frame_pose_constraints `method descriptor`

clear_tight_frame_pose_constraints()

clear_tight_frame_pose_constraints(self) -> None

Clear all tight frame pose constraints.

clear_tight_point_constraints `method descriptor`

clear_tight_point_constraints()

clear_tight_point_constraints(self) -> None

Clear all tight point constraints.

configure_collision_constraint `method descriptor`

configure_collision_constraint()

configure_collision_constraint(self, min_distance: float, include_pairs: collections.abc.Sequence[tuple[str, str]] = [], exclude_pairs: collections.abc.Sequence[tuple[str, str]] = [], nearest_points_all_pairs: bool = True, max_constraints: int = 1) -> None

Enable collision avoidance with optional include/exclude geometry pair filters.

Parameters:

Name	Description	Default
`min_distance`	Minimum separation distance to enforce (metres).	required
`include_pairs`	List of (geom_a, geom_b) tuples to consider (empty = all).	required
`exclude_pairs`	List of (geom_a, geom_b) tuples to ignore.	required
`nearest_points_all_pairs`	If False, compute nearest points only for the selected pair.	required
`max_constraints`	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).	required

configure_com_constraint `method descriptor`

configure_com_constraint()

configure_com_constraint(self, support_polygon: numpy.ndarray[dtype=float64, shape=(, ), order='F'], margin: float = 0.0, frame_name: str = 'world', com_vel_max: float = 0.4, com_acc_max: float = 0.1, use_acceleration_limits: bool = True, proximity_fraction: float = 0.0) -> None

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	Description	Default
`support_polygon`	Nx2 or Nx3 array of polygon vertices in the XY plane of frame_name (Z column is ignored if Nx3).	required
`margin`	Fractional inward shrink in [0, 1]. Applied as margin * char_size (mean distance centroid→vertices). Matches feasibility check.	required
`frame_name`	Frame in which vertices are expressed.	required
`com_vel_max`	Maximum CoM velocity (m/s).	required
`com_acc_max`	Maximum CoM acceleration (m/s²).	required
`use_acceleration_limits`	If True, clamp approach velocity by sqrt(2 * com_acc_max * margin) near boundary.	required
`proximity_fraction`	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.	required

configure_elastic_band `method descriptor`

configure_elastic_band()

configure_elastic_band(self, delta_max: float = 0.05, expand_rate: float = 0.01, decay_rate: float = 0.2, stall_threshold: int = 3, expand_only_saturated: bool = True) -> None

Configure elastic band tuning parameters.

configure_relative_pose_constraint `method descriptor`

configure_relative_pose_constraint()

configure_relative_pose_constraint(self, frame_a: str, frame_b: str, lower_bounds: numpy.ndarray[dtype=float64, shape=(), order='C'], upper_bounds: numpy.ndarray[dtype=float64, shape=(), order='C'], axis_mask: numpy.ndarray[dtype=float64, shape=(*), order='C'] = array([], dtype=float64)) -> None

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	Description	Default
`frame_a`	Reference frame name	required
`frame_b`	Target frame name	required
`lower_bounds`	6D lower bounds (pos xyz + ori xyz)	required
`upper_bounds`	6D upper bounds (pos xyz + ori xyz)	required
`axis_mask`	6D mask (1=constrained, 0=free). Empty = all.	required

configure_stall_handler `method descriptor`

configure_stall_handler()

configure_stall_handler(self, stall_threshold: int = 5, restore_rate: float = 0.005, floor_fraction: float = 0.3) -> None

Configure stall handler tuning parameters.

disable_elastic_band `method descriptor`

disable_elastic_band()

disable_elastic_band(self) -> None

Disable elastic band and reset expansion state.

disable_stall_handler `method descriptor`

disable_stall_handler()

disable_stall_handler(self) -> None

Disable the stall handler and restore nominal parameters.

elastic_band_deltas `method descriptor`

elastic_band_deltas()

elastic_band_deltas(self) -> numpy.ndarray[dtype=float64, shape=(*), order='C']

Return per-joint delta vector (size == nv).

elastic_band_enabled `method descriptor`

elastic_band_enabled()

elastic_band_enabled(self) -> bool

Return True if elastic band is enabled.

elastic_band_is_expanded `method descriptor`

elastic_band_is_expanded()

elastic_band_is_expanded(self) -> bool

Return True if any joint has nonzero expansion.

elastic_band_max_delta `method descriptor`

elastic_band_max_delta()

elastic_band_max_delta(self) -> float

Return the maximum delta currently active across all joints.

enable_collision_pair_cache `method descriptor`

enable_collision_pair_cache()

enable_collision_pair_cache(self, enable: bool, full_refresh_interval: int = 20, candidate_distance_margin: float = 0.03, max_cached_candidates: int = 128) -> None

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	Description	Default
`enable`	Enable/disable candidate caching.	required
`full_refresh_interval`	Steps between mandatory full pair scans.	required
`candidate_distance_margin`	Extra margin (m) above min_distance for retaining near-active pairs in the candidate cache.	required
`max_cached_candidates`	Cap on cached pair indices.	required

enable_elastic_band `method descriptor`

enable_elastic_band()

enable_elastic_band(self, delta_max: float = 0.05) -> None

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 `method descriptor`

enable_position_ik_debug()

enable_position_ik_debug(self, enable: bool) -> None

Enable verbose logging for position IK iterations

enable_position_limits `method descriptor`

enable_position_limits()

enable_position_limits(self, enable: bool) -> None

Enable or disable position limit constraints

enable_saturation_exit_behavior `method descriptor`

enable_saturation_exit_behavior()

enable_saturation_exit_behavior(self, enable: bool) -> None

Enable velocity-box softening near limits (disabled by default).

enable_sphere_broadphase `method descriptor`

enable_sphere_broadphase()

enable_sphere_broadphase(self, enable: bool) -> None

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 `method descriptor`

enable_stall_handler()

enable_stall_handler(self, nominal_min_distance: float) -> None

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 `method descriptor`

enable_timing_breakdown()

enable_timing_breakdown(self, enable: bool) -> None

Enable/disable detailed timing breakdown fields in VelocitySolverResult

enable_velocity_limits `method descriptor`

enable_velocity_limits()

enable_velocity_limits(self, enable: bool) -> None

Enable or disable velocity limit constraints

evaluate_collision_debug `method descriptor`

evaluate_collision_debug()

evaluate_collision_debug(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'] = array([], dtype=float64)) -> embodik._embodik_impl.CollisionDebugInfo | None

Evaluate collisions at the provided configuration and return debug info (side-effect free).

evaluate_min_collision_distance `method descriptor`

evaluate_min_collision_distance()

evaluate_min_collision_distance(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'] = array([], dtype=float64)) -> float | None

Evaluate minimum collision distance at the given configuration. If current_q is empty, uses the current configuration.

evaluate_post_step_collision_distance `method descriptor`

evaluate_post_step_collision_distance()

evaluate_post_step_collision_distance(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C']) -> float | None

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 `method descriptor`

get_active_collision_pairs()

get_active_collision_pairs(self) -> list[tuple[str, str]]

Return the list of collision pairs currently considered by the solver.

get_collision_constraint_activation_margin `method descriptor`

get_collision_constraint_activation_margin()

get_collision_constraint_activation_margin(self) -> float

Get effective collision-row activation margin in meters.

get_collision_constraint_activation_multiplier `method descriptor`

get_collision_constraint_activation_multiplier()

get_collision_constraint_activation_multiplier(self) -> float

Get collision-row activation multiplier.

get_collision_max_separation_speed_nonpenetrating `method descriptor`

get_collision_max_separation_speed_nonpenetrating()

get_collision_max_separation_speed_nonpenetrating(self) -> float

get_collision_min_distance `method descriptor`

get_collision_min_distance()

get_collision_min_distance(self) -> float

Read the current collision min_distance. Returns -1 if no collision constraint is active.

get_collision_pair_min_distance_overrides `method descriptor`

get_collision_pair_min_distance_overrides()

get_collision_pair_min_distance_overrides(self) -> list[tuple[str, float]]

Return list of (pair_key, min_distance) tuples for all active per-pair overrides.

get_collision_recovery_scale `method descriptor`

get_collision_recovery_scale()

get_collision_recovery_scale(self) -> float

get_collision_refinement_time_budget_us `method descriptor`

get_collision_refinement_time_budget_us()

get_collision_refinement_time_budget_us(self) -> int

Get exact collision refinement budget per solve (microseconds).

get_collision_repulsion_deadband `method descriptor`

get_collision_repulsion_deadband()

get_collision_repulsion_deadband(self) -> float

get_collision_tuning_mode `method descriptor`

get_collision_tuning_mode()

get_collision_tuning_mode(self) -> embodik._embodik_impl.CollisionTuningMode

Get the active high-level collision tuning preset.

get_com_proximity_threshold `method descriptor`

get_com_proximity_threshold()

get_com_proximity_threshold(self) -> float

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 `method descriptor`

get_last_collision_debug()

get_last_collision_debug(self) -> embodik._embodik_impl.CollisionDebugInfo | None

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 `method descriptor`

get_last_collision_debug_list()

get_last_collision_debug_list(self) -> list[embodik._embodik_impl.CollisionDebugInfo]

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 `method descriptor`

get_linear_velocity_constraint_rows()

get_linear_velocity_constraint_rows(self) -> int

Return active user-defined linear constraint row count.

get_proximity_gated_collision_activation_enabled `method descriptor`

get_proximity_gated_collision_activation_enabled()

get_proximity_gated_collision_activation_enabled(self) -> bool

Get whether proximity-gated collision-row activation is enabled.

get_task `method descriptor`

get_task()

get_task(self, name: str) -> embodik._embodik_impl.Task

Get a task by name

remove_task `method descriptor`

remove_task()

remove_task(self, name: str) -> None

Remove a task by name

saturation_exit_behavior_enabled `method descriptor`

saturation_exit_behavior_enabled()

saturation_exit_behavior_enabled(self) -> bool

Return whether saturation-exit softening is enabled.

set_base_orientation_bounds `method descriptor`

set_base_orientation_bounds()

set_base_orientation_bounds(self, lower: numpy.ndarray[dtype=float64, shape=(3), order='C'], upper: numpy.ndarray[dtype=float64, shape=(3), order='C']) -> None

Set floating-base orientation bounds (3D, in velocity space)

set_base_position_bounds `method descriptor`

set_base_position_bounds()

set_base_position_bounds(self, lower: numpy.ndarray[dtype=float64, shape=(3), order='C'], upper: numpy.ndarray[dtype=float64, shape=(3), order='C']) -> None

Set floating-base position bounds (3D)

set_collision_constraint_activation_multiplier `method descriptor`

set_collision_constraint_activation_multiplier()

set_collision_constraint_activation_multiplier(self, multiplier: float) -> None

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 `method descriptor`

set_collision_max_separation_speed_nonpenetrating()

set_collision_max_separation_speed_nonpenetrating(self, mps: float) -> None

Max separation speed (m/s) for non-penetrating recovery. Default 0.15 m/s.

set_collision_min_distance `method descriptor`

set_collision_min_distance()

set_collision_min_distance(self, min_distance: float) -> bool

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 `method descriptor`

set_collision_pair_min_distance()

set_collision_pair_min_distance(self, link_a: str, link_b: str, min_distance: float, activate_when_clear: bool = True) -> None

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 `method descriptor`

set_collision_recovery_scale()

set_collision_recovery_scale(self, scale: float) -> None

Fraction of desired recovery velocity applied inside min_distance. Default 0.2.

set_collision_refinement_time_budget_us `method descriptor`

set_collision_refinement_time_budget_us()

set_collision_refinement_time_budget_us(self, budget_us: int) -> None

Set optional exact collision refinement budget per solve in microseconds. Values <= 0 disable budgeting.

set_collision_repulsion_deadband `method descriptor`

set_collision_repulsion_deadband()

set_collision_repulsion_deadband(self, metres: float) -> None

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_tuning_mode `method descriptor`

set_collision_tuning_mode()

set_collision_tuning_mode(self, mode: embodik._embodik_impl.CollisionTuningMode) -> None

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 `method descriptor`

set_constraint_tolerance()

set_constraint_tolerance(self, epsilon: float) -> None

Set constraint violation deadband and COD pseudoinverse relative threshold (VelocitySolverConfig.epsilon).

set_damping `method descriptor`

set_damping()

set_damping(self, damping: float) -> None

Set singularity robust damping factor

set_limit_exit_release_margin `method descriptor`

set_limit_exit_release_margin()

set_limit_exit_release_margin(self, margin: float) -> None

Set release margin that relaxes tiny post-limit recovery forcing.

set_limit_recovery_gain `method descriptor`

set_limit_recovery_gain()

set_limit_recovery_gain(self, gain: float) -> None

Set joint limit recovery gain in [0, 1]

set_limit_recovery_hysteresis `method descriptor`

set_limit_recovery_hysteresis()

set_limit_recovery_hysteresis(self, enter_epsilon: float, exit_epsilon: float) -> None

Set enter/exit hysteresis epsilons for joint-limit recovery.

set_linear_velocity_constraints `method descriptor`

set_linear_velocity_constraints()

set_linear_velocity_constraints(self, C: numpy.ndarray[dtype=float64, shape=(, ), order='F'], lower_bounds: numpy.ndarray[dtype=float64, shape=(), order='C'], upper_bounds: numpy.ndarray[dtype=float64, shape=(), order='C']) -> None

Replace user-defined linear velocity constraints.

Enforces lower_bounds <= C @ dq <= upper_bounds.

set_max_iterations `method descriptor`

set_max_iterations()

set_max_iterations(self, max_iter: int) -> None

Set maximum solver iterations

set_proximity_gated_collision_activation_enabled `method descriptor`

set_proximity_gated_collision_activation_enabled()

set_proximity_gated_collision_activation_enabled(self, enabled: bool) -> None

Enable/disable proximity-gated collision-row activation without changing multiplier or margin values.

set_regularization_epsilon `method descriptor`

set_regularization_epsilon()

set_regularization_epsilon(self, epsilon: float) -> None

Alias of set_tolerance(): set singular-value damping threshold for the regularized pseudoinverse.

set_solver_recovery_enabled `method descriptor`

set_solver_recovery_enabled()

set_solver_recovery_enabled(self, enable: bool) -> None

Deprecated no-op. Recovery state machine has been removed.

set_tolerance `method descriptor`

set_tolerance()

set_tolerance(self, tolerance: float) -> None

Set singular-value damping threshold for the regularized pseudoinverse (default 1e-6).

solve_position `method descriptor`

solve_position()

solve_position(self, seed_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], target_pose: numpy.ndarray[dtype=float64, shape=(4, 4), order='F'], frame_name: str, options: embodik._embodik_impl.PositionIKOptions = ) -> embodik._embodik_impl.PositionIKResult

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 `method descriptor`

solve_position_in_tcp()

solve_position_in_tcp(self, seed_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], relative_target: numpy.ndarray[dtype=float64, shape=(4, 4), order='F'], frame_name: str, options: embodik._embodik_impl.PositionIKOptions = ) -> embodik._embodik_impl.PositionIKResult

Solve position-level IK with target relative to TCP frame

solve_position_step `method descriptor`

solve_position_step(
    current_q: Annotated[NDArray[float64], dict(shape=(None,), order=C)],
    target_pose: Annotated[NDArray[float64], dict(shape=(4, 4), order=F)],
    frame_task_name: str,
    options: PositionStepOptions = ...,
) -> PositionIKResult

solve_position_step(
    current_q: Annotated[NDArray[float64], dict(shape=(None,), order=C)],
    target_pose: SE3,
    frame_task_name: str,
    options: PositionStepOptions = ...,
) -> PositionIKResult

solve_position_step(
    current_q: Annotated[NDArray[float64], dict(shape=(None,), order=C)],
    targets: Sequence[TaskTarget],
    options: PositionStepOptions = ...,
) -> PositionIKResult

solve_position_step()

solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(), order='C'], target_pose: numpy.ndarray[dtype=float64, shape=(4, 4), order='F'], frame_task_name: str, options: embodik._embodik_impl.PositionStepOptions = ) -> embodik._embodik_impl.PositionIKResult solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(), order='C'], target_pose: embodik._embodik_impl.SE3, frame_task_name: str, options: embodik._embodik_impl.PositionStepOptions = ) -> embodik._embodik_impl.PositionIKResult solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], targets: collections.abc.Sequence[embodik._embodik_impl.TaskTarget], options: embodik._embodik_impl.PositionStepOptions = ) -> embodik._embodik_impl.PositionIKResult

Overloaded function.

solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], target_pose: numpy.ndarray[dtype=float64, shape=(4, 4), order='F'], frame_task_name: str, options: embodik._embodik_impl.PositionStepOptions = <embodik._embodik_impl.PositionStepOptions object at 0x560264a62060>) -> embodik._embodik_impl.PositionIKResult

Stepping position IK using the solver's registered tasks. Sets the target pose on the named FrameTask, computes pose error internally, scales it by position_gain / orientation_gain, calls solve_velocity() up to max_steps times, integrating after each step. The task weight is left untouched. Unlike solve_position(), this does not create temporary tasks. The recovery state machine (stuck detection, collision homotopy, etc.) is automatically exercised. Optional no-progress detection can return SolverStatus.NO_PROGRESS.

solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], target_pose: embodik._embodik_impl.SE3, frame_task_name: str, options: embodik._embodik_impl.PositionStepOptions = <embodik._embodik_impl.PositionStepOptions object at 0x560264a85ba0>) -> embodik._embodik_impl.PositionIKResult

Stepping position IK overload that accepts SE3/Rt directly. Equivalent to passing target_pose.homogeneous().

solve_position_step(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'], targets: collections.abc.Sequence[embodik._embodik_impl.TaskTarget], options: embodik._embodik_impl.PositionStepOptions = <embodik._embodik_impl.PositionStepOptions object at 0x5602648ef690>) -> embodik._embodik_impl.PositionIKResult

Stepping position IK for multiple registered pose tasks. Each TaskTarget carries task_name, target_pose, and per-task position/orientation gains. For each step, all task target velocities are computed from pose errors, then solve_velocity() is called once to preserve coordinated multi-task behavior.

solve_velocity `method descriptor`

solve_velocity()

solve_velocity(self, current_q: numpy.ndarray[dtype=float64, shape=(*), order='C'] = array([], dtype=float64), apply_limits: bool = True, stall_recovery: bool = False) -> embodik._embodik_impl.VelocitySolverResult

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 `method descriptor`

solve_velocity_dq()

solve_velocity_dq(self, current_q: numpy.ndarray[dtype=float64, shape=(), order='C'] = array([], dtype=float64), apply_limits: bool = True, stall_recovery: bool = False) -> numpy.ndarray[dtype=float64, shape=(), order='C']

Solve for joint velocities and return only dq as a NumPy array. When stall_recovery=True, enables automatic stall handler.

solver_recovery_enabled `method descriptor`

solver_recovery_enabled()

solver_recovery_enabled(self) -> bool

Deprecated no-op. Always returns False.

sphere_broadphase_enabled `method descriptor`

sphere_broadphase_enabled()

sphere_broadphase_enabled(self) -> bool

Whether sphere broadphase culling is enabled.

stall_handler_consecutive_stall_steps `method descriptor`

stall_handler_consecutive_stall_steps()

stall_handler_consecutive_stall_steps(self) -> int

Return the number of consecutive stall steps.

stall_handler_current_min_distance `method descriptor`

stall_handler_current_min_distance()

stall_handler_current_min_distance(self) -> float

Return the current effective collision min_distance.

stall_handler_enabled `method descriptor`

stall_handler_enabled()

stall_handler_enabled(self) -> bool

Return True if the stall handler is enabled.

stall_handler_is_relaxed `method descriptor`

stall_handler_is_relaxed()

stall_handler_is_relaxed(self) -> bool

Return True if collision margin is currently relaxed.

KinematicsSolver

Position IK Objective Order

PositionIKOptions (torso + nullspace)

solve_position Usage

API Reference

KinematicsSolver

Attributes

__doc__ class-attribute

__module__ class-attribute

dt property

robot property

tasks property

Functions

__init__ method descriptor

__new__ builtin

__repr__ method descriptor

add_absolute_frame_task method descriptor

add_collision_constraint method descriptor

add_com_task method descriptor

add_frame_task method descriptor

add_joint_task method descriptor

add_posture_task method descriptor

add_relative_frame_task method descriptor

add_tight_frame_pose_constraint method descriptor

add_tight_point_constraint method descriptor

append_linear_velocity_constraints method descriptor

calculate_velocity_box_constraint method descriptor

clear_all_target_velocities method descriptor

clear_base_bounds method descriptor

clear_collision_constraint method descriptor

clear_collision_pair_min_distance method descriptor

clear_com_constraint method descriptor

clear_linear_velocity_constraints method descriptor

clear_relative_pose_constraint method descriptor

clear_tasks method descriptor

clear_tight_frame_pose_constraints method descriptor

clear_tight_point_constraints method descriptor

configure_collision_constraint method descriptor

configure_com_constraint method descriptor

configure_elastic_band method descriptor

configure_relative_pose_constraint method descriptor

configure_stall_handler method descriptor

disable_elastic_band method descriptor

disable_stall_handler method descriptor

elastic_band_deltas method descriptor

elastic_band_enabled method descriptor

elastic_band_is_expanded method descriptor

elastic_band_max_delta method descriptor

enable_collision_pair_cache method descriptor

enable_elastic_band method descriptor

enable_position_ik_debug method descriptor

enable_position_limits method descriptor

enable_saturation_exit_behavior method descriptor

enable_sphere_broadphase method descriptor

enable_stall_handler method descriptor

enable_timing_breakdown method descriptor

enable_velocity_limits method descriptor

evaluate_collision_debug method descriptor

evaluate_min_collision_distance method descriptor

evaluate_post_step_collision_distance method descriptor

get_active_collision_pairs method descriptor

get_collision_constraint_activation_margin method descriptor

get_collision_constraint_activation_multiplier method descriptor

get_collision_max_separation_speed_nonpenetrating method descriptor

get_collision_min_distance method descriptor

get_collision_pair_min_distance_overrides method descriptor

get_collision_recovery_scale method descriptor

get_collision_refinement_time_budget_us method descriptor

get_collision_repulsion_deadband method descriptor

get_collision_tuning_mode method descriptor

get_com_proximity_threshold method descriptor

get_last_collision_debug method descriptor

get_last_collision_debug_list method descriptor

get_linear_velocity_constraint_rows method descriptor

get_proximity_gated_collision_activation_enabled method descriptor

get_task method descriptor

remove_task method descriptor

saturation_exit_behavior_enabled method descriptor

set_base_orientation_bounds method descriptor

set_base_position_bounds method descriptor

doc `class-attribute`

module `class-attribute`

dt `property`

robot `property`

tasks `property`

init `method descriptor`

new `builtin`

repr `method descriptor`

add_absolute_frame_task `method descriptor`

add_collision_constraint `method descriptor`

add_com_task `method descriptor`

add_frame_task `method descriptor`

add_joint_task `method descriptor`

add_posture_task `method descriptor`

add_relative_frame_task `method descriptor`

add_tight_frame_pose_constraint `method descriptor`

add_tight_point_constraint `method descriptor`

append_linear_velocity_constraints `method descriptor`

calculate_velocity_box_constraint `method descriptor`

clear_all_target_velocities `method descriptor`

clear_base_bounds `method descriptor`

clear_collision_constraint `method descriptor`

clear_collision_pair_min_distance `method descriptor`

clear_com_constraint `method descriptor`

clear_linear_velocity_constraints `method descriptor`

clear_relative_pose_constraint `method descriptor`

clear_tasks `method descriptor`

clear_tight_frame_pose_constraints `method descriptor`

clear_tight_point_constraints `method descriptor`

configure_collision_constraint `method descriptor`

configure_com_constraint `method descriptor`

configure_elastic_band `method descriptor`

configure_relative_pose_constraint `method descriptor`

configure_stall_handler `method descriptor`

disable_elastic_band `method descriptor`

disable_stall_handler `method descriptor`

elastic_band_deltas `method descriptor`

elastic_band_enabled `method descriptor`

elastic_band_is_expanded `method descriptor`

elastic_band_max_delta `method descriptor`

enable_collision_pair_cache `method descriptor`

enable_elastic_band `method descriptor`

enable_position_ik_debug `method descriptor`

enable_position_limits `method descriptor`

enable_saturation_exit_behavior `method descriptor`

enable_sphere_broadphase `method descriptor`

enable_stall_handler `method descriptor`

enable_timing_breakdown `method descriptor`

enable_velocity_limits `method descriptor`

evaluate_collision_debug `method descriptor`

evaluate_min_collision_distance `method descriptor`

evaluate_post_step_collision_distance `method descriptor`

get_active_collision_pairs `method descriptor`

get_collision_constraint_activation_margin `method descriptor`

get_collision_constraint_activation_multiplier `method descriptor`

get_collision_max_separation_speed_nonpenetrating `method descriptor`

get_collision_min_distance `method descriptor`

get_collision_pair_min_distance_overrides `method descriptor`

get_collision_recovery_scale `method descriptor`

get_collision_refinement_time_budget_us `method descriptor`

get_collision_repulsion_deadband `method descriptor`

get_collision_tuning_mode `method descriptor`

get_com_proximity_threshold `method descriptor`

get_last_collision_debug `method descriptor`

get_last_collision_debug_list `method descriptor`

get_linear_velocity_constraint_rows `method descriptor`

get_proximity_gated_collision_activation_enabled `method descriptor`

get_task `method descriptor`

remove_task `method descriptor`

saturation_exit_behavior_enabled `method descriptor`

set_base_orientation_bounds `method descriptor`

set_base_position_bounds `method descriptor`

set_collision_constraint_activation_multiplier `method descriptor`

set_collision_max_separation_speed_nonpenetrating `method descriptor`

set_collision_min_distance `method descriptor`

set_collision_pair_min_distance `method descriptor`

set_collision_recovery_scale `method descriptor`

set_collision_refinement_time_budget_us `method descriptor`

set_collision_repulsion_deadband `method descriptor`

set_collision_tuning_mode `method descriptor`