API Reference

Complete API documentation for EmbodiK. For conceptual guides (robustness, collision, GPU), start with the Guides overview.

Core Classes

RobotModel

The RobotModel class represents a robot kinematic model loaded from URDF.

RobotModel

Attributes

collision_data property

collision_data

Collision geometry data (may be None)

collision_model property

collision_model

Collision geometry model (may be None)

controlled_joint_indices property writable

controlled_joint_indices

Dictionary mapping joint names to indices

controlled_joint_names property writable

controlled_joint_names

List of controlled joint names

is_floating_base property

is_floating_base

Whether robot has floating base

nq property

nq

Number of configuration variables

nv property

nv

Number of velocity variables

urdf_path property

urdf_path

Path to URDF file

visual_data property

visual_data

Visual geometry data (may be None)

visual_model property

visual_model

Visual geometry model (may be None)

Functions

apply_collision_exclusions

apply_collision_exclusions(collision_pairs)

Disable the provided collision pairs using an SRDF-style specification

check_collision

check_collision(min_distance=0.0)

Check whether any collision pair has distance below a threshold.

Returns True if any pair has distance <= min_distance. With default min_distance=0.0, checks for actual contact/overlap.

Parameters:

Name	Type	Description	Default
min_distance	float	Distance threshold (default 0.0)	0.0

Returns:

Type	Description
bool	True if collision detected, False otherwise

compute_collision_distances

compute_collision_distances()

Compute collision distances for all pairs at current configuration.

Returns:

Type	Description
list[float]	List of distances for each collision pair

compute_coriolis

compute_coriolis(q, v)

Compute the Coriolis + centrifugal torque vector C(q,v)*v.

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint configuration vector (size nq)	required
v	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint velocity vector (size nv)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Coriolis torque vector (size nv)

compute_generalized_gravity

compute_generalized_gravity(q)

Compute the generalized gravity torque vector g(q).

Returns the joint torques required to compensate gravity at the given configuration (equivalent to RNEA with zero velocity and acceleration).

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint configuration vector (size nq)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Gravity torque vector (size nv)

compute_mass_matrix

compute_mass_matrix(q)

Compute the joint-space mass/inertia matrix M(q).

Uses the Composite Rigid Body Algorithm (CRBA). The returned matrix is symmetric positive-definite.

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint configuration vector (size nq)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None, None), order=F)]	Mass matrix M(q) (size nv x nv)

compute_min_collision_distance

compute_min_collision_distance()

Compute minimum collision distance at current configuration.

Returns:

Type	Description
float	Minimum distance across all collision pairs, or inf if no collision geometry

difference

difference(q0, q1)

Compute the tangent-vector difference between two configurations.

Returns the velocity v such that q1 = integrate(q0, v). For Euclidean joints this is simply q1 - q0, but for quaternion / floating-base joints the result lives in the tangent space (size nv).

Parameters:

Name	Type	Description	Default
q0	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Start configuration (size nq)	required
q1	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	End configuration (size nq)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Tangent vector v (size nv)

from_xacro staticmethod

from_xacro(xacro_path, floating_base=False)

Create robot model from XACRO file

get_acceleration_limits

get_acceleration_limits()

Get joint acceleration limits (returns defaults if not specified in URDF)

get_collision_geometries

get_collision_geometries()

Get collision geometries as list of dicts with name, parent_frame, placement, geometry_type, and params

get_collision_geometry_names

get_collision_geometry_names()

Return the list of collision geometry object names

get_collision_pair_names

get_collision_pair_names()

Return the list of collision pairs as name tuples

get_com_jacobian

get_com_jacobian()

Get 3xN center of mass Jacobian

get_com_position

get_com_position()

Get center of mass position

get_com_velocity

get_com_velocity()

Get center of mass velocity

get_current_configuration

get_current_configuration()

Get current joint configuration

get_current_velocity

get_current_velocity()

Get current joint velocities

get_effort_limits

get_effort_limits()

Get joint effort/torque limits

get_frame_jacobian

get_frame_jacobian(frame_name, reference_frame=ReferenceFrame.LOCAL_WORLD_ALIGNED)

Get 6xN Jacobian of specified frame

get_frame_names

get_frame_names()

Get list of all frame names

get_frame_pose

get_frame_pose(frame_name)

Get pose of specified frame as SE3 transformation

get_gravity

get_gravity()

Get the current gravity vector.

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=3, order=C)]	3D gravity vector

get_joint_config_index

get_joint_config_index(joint_name)

Get configuration-space index for a joint

Returns the starting index in the configuration vector q where this joint's configuration variables begin.

Parameters:

Name	Type	Description	Default
joint_name	str	Name of the joint	required

Returns:

Type	Description
int	Starting index in q (idx_q)

Raises:

Type	Description
RuntimeError	If joint not found

get_joint_config_size

get_joint_config_size(joint_name)

Get number of configuration variables for a joint

For revolute joints this is typically 1, for continuous joints it is 2 (cos θ, sin θ representation).

Parameters:

Name	Type	Description	Default
joint_name	str	Name of the joint	required

Returns:

Type	Description
int	Number of configuration variables (nq)

Raises:

Type	Description
RuntimeError	If joint not found

get_joint_id

get_joint_id(joint_name)

Get joint index by name

Returns:

Type	Description
int	Joint index (0-based, excluding universe joint)

Raises:

Type	Description
RuntimeError	If joint not found

get_joint_limits

get_joint_limits()

Get joint position limits as (lower, upper) pair

get_joint_names

get_joint_names()

Get list of all joint names

get_joint_velocity_index

get_joint_velocity_index(joint_name)

Get velocity-space index for a joint

Returns the starting index in the velocity vector v where this joint's velocity variables begin.

Parameters:

Name	Type	Description	Default
joint_name	str	Name of the joint	required

Returns:

Type	Description
int	Starting index in v (idx_v)

Raises:

Type	Description
RuntimeError	If joint not found

get_joint_velocity_size

get_joint_velocity_size(joint_name)

Get number of velocity variables for a joint

For most joints this is 1 (single velocity DOF).

Parameters:

Name	Type	Description	Default
joint_name	str	Name of the joint	required

Returns:

Type	Description
int	Number of velocity variables (nv)

Raises:

Type	Description
RuntimeError	If joint not found

get_velocity_limits

get_velocity_limits()

Get joint velocity limits

has_collision_geometry

has_collision_geometry()

Check if collision geometry is available

has_frame

has_frame(frame_name)

Check if frame exists

has_joint

has_joint(joint_name)

Check if joint exists

integrate

integrate(q, v, dt=1.0)

Integrate velocity into configuration using Lie group operations.

For standard revolute/prismatic joints this is equivalent to q + v*dt, but for floating-base (SE3), spherical (quaternion), and other non-Euclidean joint types it performs the correct manifold integration (e.g. quaternion exponential map).

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Current configuration vector (size nq)	required
v	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Velocity / tangent vector (size nv)	required
dt	float	Time step (default 1.0, i.e. v is already scaled)	1.0

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Integrated configuration vector (size nq)

neutral_configuration

neutral_configuration()

Return the neutral (zero / home) configuration.

For floating-base robots this includes a valid unit quaternion for the base orientation rather than all-zeros.

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Neutral configuration vector (size nq)

normalize

normalize(q)

Normalize a configuration vector.

For joints on a manifold (quaternion components of floating-base or spherical joints) this re-normalizes the quaternion part. For standard revolute/prismatic joints this is a no-op.

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Configuration vector (size nq)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Normalized configuration vector (size nq)

random_configuration

random_configuration()

Generate a random valid configuration within joint limits.

Uses Pinocchio's randomConfiguration which respects the joint topology (e.g. generates valid quaternions for floating-base).

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Random configuration vector (size nq)

rnea

rnea(q, v, a)

Compute inverse dynamics using the Recursive Newton-Euler Algorithm.

Returns tau = M(q)a + C(q,v)v + g(q).

Common usage patterns

• Gravity only: rnea(q, zeros, zeros)
• Coriolis+grav: rnea(q, v, zeros)
• Full dynamics: rnea(q, v, a)

Parameters:

Name	Type	Description	Default
q	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint configuration vector (size nq)	required
v	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint velocity vector (size nv)	required
a	Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint acceleration vector (size nv)	required

Returns:

Type	Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]	Joint torque vector tau (size nv)

set_acceleration_limits

set_acceleration_limits(accel_limits)

Set custom joint acceleration limits

set_gravity

set_gravity(gravity)

Set the gravity vector for the model.

Default is [0, 0, -9.81]. Affects gravity torque computations.

Parameters:

Name	Type	Description	Default
gravity	Annotated[NDArray[float64], dict(shape=3, order=C)]	3D gravity vector (e.g. [0, 0, -9.81])	required

set_joint_limits

set_joint_limits(lower, upper)

Set joint position limits

update_configuration

update_configuration(q)

Update robot configuration and compute forward kinematics

update_kinematics

update_kinematics(q, v=...)

Update robot configuration and optionally velocity

KinematicsSolver

The KinematicsSolver provides inverse kinematics solving capabilities.

KinematicsSolver

High-level kinematics solver providing simple API for IK problems

Attributes

dt property writable

dt

Time step for velocity integration

robot property

robot

Get the robot model

tasks property

tasks

Get all tasks

Functions

__init__

__init__(robot)

Create a kinematics solver for the given robot model

add_absolute_frame_task

add_absolute_frame_task(name, frame_a, frame_b, alpha=0.5)

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

add_collision_constraint

add_collision_constraint(link_pairs, min_distance=0.05)

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

add_com_task

add_com_task(name)

Add a center of mass tracking task

add_contact_frame

add_contact_frame(frame_name, contact_type=ContactType.RIGID_CONTACT)

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_frame_task(name, frame_name, task_type=TaskType.FRAME_POSE)

Add a frame tracking task

add_joint_task

add_joint_task(name, joint_name, target_value=0.0)

Add a single joint 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_relative_frame_task(name, frame_a, frame_b)

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_tight_point_constraint(frame_name, target_point, position_epsilon=1e-05, axis_mask=...)

Add a tight 3D point epsilon-box constraint.

append_linear_velocity_constraints

append_linear_velocity_constraints(C, lower_bounds, upper_bounds)

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_all_target_velocities()

Clear direct target velocities on all registered tasks.

clear_base_bounds

clear_base_bounds()

Clear floating-base bounds (use unlimited bounds)

clear_collision_constraint

clear_collision_constraint()

Disable collision avoidance constraint.

clear_collision_pair_min_distance

clear_collision_pair_min_distance(link_a, link_b)

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

clear_com_constraint

clear_com_constraint()

Disable CoM support-polygon constraint.

clear_contact_frames

clear_contact_frames()

Disable contact-root Jacobian projection.

clear_joint_metric_weights

clear_joint_metric_weights()

Clear the joint-space metric (restore unweighted solve)

clear_joint_velocity_limit_overrides

clear_joint_velocity_limit_overrides()

Clear all per-joint velocity-limit overrides

clear_linear_velocity_constraints

clear_linear_velocity_constraints()

Clear all user-defined linear velocity constraints.

clear_relative_pose_constraint

clear_relative_pose_constraint()

Disable relative pose constraint

clear_tasks

clear_tasks()

Remove all tasks

clear_tight_frame_pose_constraints

clear_tight_frame_pose_constraints()

Clear all tight frame pose constraints.

clear_tight_point_constraints

clear_tight_point_constraints()

Clear all tight point 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

configure_contact_frames(frame_names, contact_type=ContactType.RIGID_CONTACT)

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_relative_pose_constraint(frame_a, frame_b, lower_bounds, upper_bounds, axis_mask=...)

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

configure_runtime(config)

Apply bundled runtime defaults. Stamps damping immediately and stores position-step defaults for make_position_step_options().

configure_stall_handler

configure_stall_handler(stall_threshold=5, restore_rate=0.005, floor_fraction=0.3)

Configure stall handler tuning parameters.

disable_elastic_band

disable_elastic_band()

Disable elastic band and reset expansion state.

disable_stall_handler

disable_stall_handler()

Disable the stall handler and restore nominal parameters.

elastic_band_deltas

elastic_band_deltas()

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

elastic_band_enabled

elastic_band_enabled()

Return True if elastic band is enabled.

elastic_band_is_expanded

elastic_band_is_expanded()

Return True if any joint has nonzero expansion.

elastic_band_max_delta

elastic_band_max_delta()

Return the maximum delta currently active across all joints.

enable_acceleration_limits

enable_acceleration_limits(enable)

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(delta_max=0.05)

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_position_ik_debug(enable)

Enable verbose logging for position IK iterations

enable_position_limits

enable_position_limits(enable)

Enable or disable position limit constraints

enable_saturation_exit_behavior

enable_saturation_exit_behavior(enable)

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

enable_sphere_broadphase

enable_sphere_broadphase(enable)

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_stall_handler(nominal_min_distance)

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_timing_breakdown(enable)

Enable/disable detailed timing breakdown fields in VelocitySolverResult

enable_velocity_limits

enable_velocity_limits(enable)

Enable or disable velocity limit constraints

evaluate_collision_debug

evaluate_collision_debug(current_q=...)

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

evaluate_min_collision_distance

evaluate_min_collision_distance(current_q=...)

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

evaluate_post_step_collision_distance

evaluate_post_step_collision_distance(current_q)

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

get_active_collision_pairs()

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

get_collision_constraint_activation_margin

get_collision_constraint_activation_margin()

Get effective collision-row activation margin in meters.

get_collision_constraint_activation_multiplier

get_collision_constraint_activation_multiplier()

Get collision-row activation multiplier.

get_collision_min_distance

get_collision_min_distance()

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

get_collision_pair_min_distance_overrides

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_collision_refinement_time_budget_us()

Get exact collision refinement budget per solve (microseconds).

get_collision_structural_floor

get_collision_structural_floor()

Return the structural collision recovery floor distance (m).

get_collision_tuning_mode

get_collision_tuning_mode()

Get the active high-level collision tuning preset.

get_com_proximity_threshold

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

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

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

get_linear_velocity_constraint_rows()

Return active user-defined linear constraint row count.

get_proximity_gated_collision_activation_enabled

get_proximity_gated_collision_activation_enabled()

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

get_task

get_task(name)

Get a task by name

has_contact_frames

has_contact_frames()

Return true if contact-root projection is active.

make_position_step_options

make_position_step_options()

Return fresh PositionStepOptions populated from runtime defaults.

pose_task_group

pose_task_group(name)

Get a pose task group by name

remove_task

remove_task(name)

Remove a task by name

reset_adaptive_state

reset_adaptive_state()

Reset default-off stateful runtime adapters without changing the stored runtime configuration.

runtime_config

runtime_config()

Return the last-applied runtime defaults.

saturation_exit_behavior_enabled

saturation_exit_behavior_enabled()

Return whether saturation-exit softening is enabled.

set_acceleration_limits

set_acceleration_limits(limits)

Set per-joint acceleration limits (rad/s^2)

set_base_orientation_bounds

set_base_orientation_bounds(lower, upper)

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

set_base_position_bounds

set_base_position_bounds(lower, upper)

Set floating-base position bounds (3D)

set_collision_constraint_activation_multiplier

set_collision_constraint_activation_multiplier(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

set_collision_max_separation_speed_nonpenetrating(mps)

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

set_collision_min_distance

set_collision_min_distance(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_collision_pair_min_distance(link_a, link_b, min_distance, activate_when_clear=True)

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

set_collision_recovery_scale(scale)

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

set_collision_refinement_time_budget_us

set_collision_refinement_time_budget_us(budget_us)

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

set_collision_repulsion_deadband

set_collision_repulsion_deadband(metres)

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

set_collision_structural_floor(metres)

Minimum clearance (m) maintained for structurally-close pairs when the non-worsening floor is enabled. Default 0.005 m.

set_collision_tuning_mode

set_collision_tuning_mode(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_tolerance(epsilon)

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

set_damping

set_damping(damping)

Set singularity robust damping factor

set_joint_metric_weights

set_joint_metric_weights(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

set_joint_velocity_limit(nv_idx, 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_limit_exit_release_margin(margin)

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

set_limit_recovery_gain

set_limit_recovery_gain(gain)

Set joint limit recovery gain in [0, 1]

set_limit_recovery_hysteresis

set_limit_recovery_hysteresis(enter_epsilon, exit_epsilon)

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

set_linear_velocity_constraints

set_linear_velocity_constraints(C, lower_bounds, upper_bounds)

Replace user-defined linear velocity constraints.

Enforces lower_bounds <= C @ dq <= upper_bounds.

set_max_iterations

set_max_iterations(max_iter)

Set maximum solver iterations

set_non_worsening_collision_floor_enabled

set_non_worsening_collision_floor_enabled(enable)

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

set_proximity_gated_collision_activation_enabled(enabled)

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

set_regularization_epsilon

set_regularization_epsilon(epsilon)

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

set_solver_recovery_enabled

set_solver_recovery_enabled(enable)

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

set_tolerance

set_tolerance(tolerance)

Set singular-value damping threshold for the regularized pseudoinverse (default 0.1).

solve_position

solve_position(seed_q, target_pose, frame_name, options=...)

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_in_tcp(seed_q, relative_target, frame_name, options=...)

Solve position-level IK with target relative to TCP frame

solve_velocity

solve_velocity(current_q=..., apply_limits=True, stall_recovery=False)

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_velocity_dq(current_q=..., apply_limits=True, stall_recovery=False)

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

solver_recovery_enabled

solver_recovery_enabled()

Deprecated no-op. Always returns False.

sphere_broadphase_enabled

sphere_broadphase_enabled()

Whether sphere broadphase culling is enabled.

stall_handler_consecutive_stall_steps

stall_handler_consecutive_stall_steps()

Return the number of consecutive stall steps.

stall_handler_current_min_distance

stall_handler_current_min_distance()

Return the current effective collision min_distance.

stall_handler_enabled

stall_handler_enabled()

Return True if the stall handler is enabled.

stall_handler_floor_fraction

stall_handler_floor_fraction()

Return the current stall-handler floor fraction.

stall_handler_is_relaxed

stall_handler_is_relaxed()

Return True if collision margin is currently relaxed.

stall_handler_restore_rate

stall_handler_restore_rate()

Return the current stall-handler restore rate.

stall_handler_threshold

stall_handler_threshold()

Return the current stall threshold.

Task Types

EmbodiK supports various task types for multi-task IK:

• FrameTask: Control end-effector pose (position + orientation)
• PostureTask: Maintain desired joint configuration
• COMTask: Control center of mass position
• JointTask: Control individual joint positions
• MultiJointTask: Control multiple joints simultaneously

See the Tasks page for detailed documentation.

Transforms

Native spatial transform helpers (no SciPy dependency):

• Rotation / SO3: SO(3) rotations with spatialmath-style shorthands (Rx, Ry, Rz, RPY, etc.)
• SE3: Rigid-body transforms with composition (T1 * T2), point transforms (act, actInv), and property aliases (R, t, A)

See the Transforms page for full API.

Utilities

utils

Utility functions for embodiK.

Classes

PoseData

Lightweight holder for rotation/translation pairs.

Source code in python/embodik/utils.py

class PoseData:
    """Lightweight holder for rotation/translation pairs."""

    __slots__ = ("R", "t")

    def __init__(self, rotation: np.ndarray, translation: np.ndarray):
        self.R = np.asarray(rotation, dtype=float).reshape(3, 3)
        self.t = np.asarray(translation, dtype=float).reshape(3)

    @classmethod
    def wrap(cls, pose: object) -> "PoseData":
        if isinstance(pose, cls):
            return pose

        rotation = getattr(pose, "R", getattr(pose, "rotation", None))
        translation = getattr(pose, "t", getattr(pose, "translation", None))
        if rotation is None or translation is None:
            raise TypeError(
                "Expected pose-like object with rotation/translation attributes; got "
                f"{type(pose)!r}"
            )
        return cls(rotation, translation)

Functions

Rt

Rt(R=None, t=None)

Create SE3 transform from rotation matrix and translation (spatialmath-python compatible).

Equivalent to spatialmath-python's SE3.Rt(R, t).

Parameters:

Name	Type	Description	Default
R	Optional[ndarray]	3x3 rotation matrix (default: identity)	None
t	Optional[ndarray]	3D translation vector (default: zero)	None

Returns:

Type	Description
Any	SE3 transform

Examples:

>>> R = np.eye(3)
>>> t = np.array([1, 2, 3])
>>> T = Rt(R=R, t=t)  # Create SE3 from R and t
>>> T = Rt(t=t)  # Create SE3 with identity rotation
>>> T = Rt(R=R)  # Create SE3 with zero translation
>>> T = Rt()  # Create identity transform

Source code in python/embodik/utils.py

def Rt(R: Optional[np.ndarray] = None, t: Optional[np.ndarray] = None) -> Any:
    """
    Create SE3 transform from rotation matrix and translation (spatialmath-python compatible).

    Equivalent to spatialmath-python's SE3.Rt(R, t).

    Args:
        R: 3x3 rotation matrix (default: identity)
        t: 3D translation vector (default: zero)

    Returns:
        SE3 transform

    Examples:
        >>> R = np.eye(3)
        >>> t = np.array([1, 2, 3])
        >>> T = Rt(R=R, t=t)  # Create SE3 from R and t
        >>> T = Rt(t=t)  # Create SE3 with identity rotation
        >>> T = Rt(R=R)  # Create SE3 with zero translation
        >>> T = Rt()  # Create identity transform
    """
    if R is None:
        R = np.eye(3)
    if t is None:
        t = np.zeros(3)

    R = np.asarray(R, dtype=float)
    t = np.asarray(t, dtype=float)

    if R.shape != (3, 3):
        raise ValueError(f"Expected 3x3 rotation matrix, got shape {R.shape}")
    if t.shape != (3,):
        raise ValueError(f"Expected 3D translation vector, got shape {t.shape}")

    return _native.SE3(R, t)

clamp_configuration

clamp_configuration(configuration, lower, upper)

Clip joint configuration to provided limits.

Source code in python/embodik/utils.py

def clamp_configuration(
    configuration: np.ndarray, lower: np.ndarray, upper: np.ndarray
) -> np.ndarray:
    """Clip joint configuration to provided limits."""

    return np.clip(configuration, lower, upper)

compute_pose_error

compute_pose_error(pose_current, pose_goal)

Compute 6D pose error (goal - current) using native :func:log3.

Optimized to work directly with SE3 objects without wrapping when possible. Extracts rotation/translation once to minimize Python binding overhead.

Source code in python/embodik/utils.py

def compute_pose_error(pose_current: PoseData | object, pose_goal: PoseData | object) -> np.ndarray:
    """Compute 6D pose error (goal - current) using native :func:`log3`.

    Optimized to work directly with SE3 objects without wrapping when possible.
    Extracts rotation/translation once to minimize Python binding overhead.
    """
    # Fast path for native SE3 objects (most common case)
    if isinstance(pose_current, _native.SE3) and isinstance(pose_goal, _native.SE3):
        # Extract rotation and translation once to avoid repeated attribute access overhead
        t_current = pose_current.translation
        t_goal = pose_goal.translation
        R_current = pose_current.rotation
        R_goal = pose_goal.rotation

        error = np.empty(6, dtype=float)
        error[:3] = t_goal - t_current
        error[3:] = _native.log3(R_goal @ R_current.T)
        return error

    # Fallback to PoseData.wrap for other types
    current = PoseData.wrap(pose_current)
    goal = PoseData.wrap(pose_goal)
    error = np.empty(6, dtype=float)
    error[:3] = goal.t - current.t
    error[3:] = _native.log3(goal.R @ current.R.T)
    return error

get_pose_error_vector

get_pose_error_vector(pose_current, pose_goal)

Pose-error helper function.

Uses Pinocchio's log3 for rotation error computation.

Supports both PoseData objects and Pinocchio SE3 objects.

Source code in python/embodik/utils.py

def get_pose_error_vector(pose_current, pose_goal):
    """Pose-error helper function.

    Uses Pinocchio's log3 for rotation error computation.

    Supports both PoseData objects and Pinocchio SE3 objects.
    """

    pose_error = np.zeros(6, dtype=np.float64)

    # Handle both PoseData (has .t) and Pinocchio SE3 (has .translation)
    if hasattr(pose_goal, "translation"):
        # Pinocchio SE3 object
        t_goal = pose_goal.translation
        t_current = pose_current.translation
        R_goal = pose_goal.rotation
        R_current = pose_current.rotation
    elif hasattr(pose_goal, "t"):
        # PoseData object
        t_goal = pose_goal.t
        t_current = pose_current.t
        R_goal = pose_goal.R
        R_current = pose_current.R
    else:
        raise TypeError(f"Unsupported pose type: {type(pose_goal)}")

    pose_error[:3] = t_goal - t_current
    # Use native log3 for rotation error (equivalent to SO3.log(twist=True))
    # Compute relative rotation: R_error = R_goal * R_current^T
    R_error = R_goal @ R_current.T
    pose_error[3:] = _native.log3(R_error)
    return pose_error

limit_task_velocity

limit_task_velocity(
    velocity_error,
    max_linear_step=0.1,
    max_angular_step=0.1,
    *,
    enable_debug=False,
    debug_logger=None
)

Clamp linear and angular components of a 6D velocity vector.

Source code in python/embodik/utils.py

def limit_task_velocity(
    velocity_error: np.ndarray,
    max_linear_step: float = 0.1,
    max_angular_step: float = 0.1,
    *,
    enable_debug: bool = False,
    debug_logger=None,
) -> np.ndarray:
    """Clamp linear and angular components of a 6D velocity vector."""

    limited_error = velocity_error.copy()

    linear_norm = float(np.linalg.norm(limited_error[:3]))
    if linear_norm > max_linear_step and linear_norm > 1e-9:
        scale = max_linear_step / linear_norm
        limited_error[:3] *= scale
        if enable_debug and debug_logger is not None:
            debug_logger.info(
                "Linear velocity limited: %.3f -> %.3f m/s", linear_norm, max_linear_step
            )

    angular_norm = float(np.linalg.norm(limited_error[3:]))
    if angular_norm > max_angular_step and angular_norm > 1e-9:
        scale = max_angular_step / angular_norm
        limited_error[3:] *= scale
        if enable_debug and debug_logger is not None:
            debug_logger.info(
                "Angular velocity limited: %.3f -> %.3f rad/s",
                angular_norm,
                max_angular_step,
            )

    return limited_error

normalize_quaternion

normalize_quaternion(quaternion)

Return a unit quaternion, defaulting to [0,0,0,1] if norm is tiny.

Source code in python/embodik/utils.py

def normalize_quaternion(quaternion: np.ndarray) -> np.ndarray:
    """Return a unit quaternion, defaulting to ``[0,0,0,1]`` if norm is tiny."""

    quat = np.asarray(quaternion, dtype=float).reshape(4)
    norm = float(np.linalg.norm(quat))
    if norm < 1e-12:
        return np.array([0.0, 0.0, 0.0, 1.0], dtype=float)
    return quat / norm

q2r

q2r(quaternion, order='sxyz')

Convert quaternion to rotation matrix (spatialmath-python compatible).

Uses native embodiK/Pinocchio conversion (no SciPy dependency).

Parameters:

Name	Type	Description	Default
quaternion	ndarray	Quaternion as array	required
order	str	Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])	'sxyz'

Returns:

Type	Description
ndarray	3x3 rotation matrix

Examples:

>>> q = np.array([1, 0, 0, 0])  # Identity quaternion (wxyz)
>>> R = q2r(q)  # Returns 3x3 identity matrix
>>> q = np.array([0, 0, 0, 1])  # Identity quaternion (xyzw)
>>> R = q2r(q, order='xyzs')  # Returns 3x3 identity matrix

Source code in python/embodik/utils.py

def q2r(quaternion: np.ndarray, order: str = "sxyz") -> np.ndarray:
    """
    Convert quaternion to rotation matrix (spatialmath-python compatible).

    Uses native embodiK/Pinocchio conversion (no SciPy dependency).

    Args:
        quaternion: Quaternion as array
        order: Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])

    Returns:
        3x3 rotation matrix

    Examples:
        >>> q = np.array([1, 0, 0, 0])  # Identity quaternion (wxyz)
        >>> R = q2r(q)  # Returns 3x3 identity matrix
        >>> q = np.array([0, 0, 0, 1])  # Identity quaternion (xyzw)
        >>> R = q2r(q, order='xyzs')  # Returns 3x3 identity matrix
    """
    quaternion = np.asarray(quaternion, dtype=float)
    if quaternion.shape != (4,):
        raise ValueError(f"Expected 4-element quaternion, got shape {quaternion.shape}")

    # Normalize quaternion
    norm = float(np.linalg.norm(quaternion))
    if norm < 1e-12:
        return np.eye(3, dtype=float)
    qn = quaternion / norm

    if order == "sxyz" or order == "wxyz":
        # Scalar first: [w, x, y, z]
        R = _native.quaternion_wxyz_to_matrix(
            float(qn[0]), float(qn[1]), float(qn[2]), float(qn[3])
        )
    elif order == "xyzs" or order == "xyzw":
        # Scalar last: [x, y, z, w]
        R = _native.quaternion_xyzw_to_matrix(
            float(qn[0]), float(qn[1]), float(qn[2]), float(qn[3])
        )
    else:
        raise ValueError(f"Unknown quaternion order: {order}. Use 'sxyz' or 'xyzs'")

    return np.array(R)

r2q

r2q(rotation, order='sxyz')

Convert rotation matrix to quaternion (spatialmath-python compatible).

Uses native embodiK/Pinocchio conversion (no SciPy dependency).

Parameters:

Name	Type	Description	Default
rotation	ndarray	3x3 rotation matrix	required
order	str	Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])	'sxyz'

Returns:

Type	Description
ndarray	Quaternion as numpy array:
ndarray	If order='sxyz': [w, x, y, z] (scalar first, default)
ndarray	If order='xyzs': [x, y, z, w] (scalar last)

Examples:

>>> R = np.eye(3)
>>> q = r2q(R)  # Returns [1, 0, 0, 0] (wxyz format)
>>> q = r2q(R, order='xyzs')  # Returns [0, 0, 0, 1] (xyzw format)

Source code in python/embodik/utils.py

def r2q(rotation: np.ndarray, order: str = "sxyz") -> np.ndarray:
    """
    Convert rotation matrix to quaternion (spatialmath-python compatible).

    Uses native embodiK/Pinocchio conversion (no SciPy dependency).

    Args:
        rotation: 3x3 rotation matrix
        order: Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])

    Returns:
        Quaternion as numpy array:
        - If order='sxyz': [w, x, y, z] (scalar first, default)
        - If order='xyzs': [x, y, z, w] (scalar last)

    Examples:
        >>> R = np.eye(3)
        >>> q = r2q(R)  # Returns [1, 0, 0, 0] (wxyz format)
        >>> q = r2q(R, order='xyzs')  # Returns [0, 0, 0, 1] (xyzw format)
    """
    rotation = np.asarray(rotation, dtype=float)
    if rotation.shape != (3, 3):
        raise ValueError(f"Expected 3x3 rotation matrix, got shape {rotation.shape}")

    if order == "sxyz" or order == "wxyz":
        # Scalar first: [w, x, y, z] via native wxyz converter
        quat = _native.matrix_to_quaternion_wxyz(rotation)
        return np.array(quat, dtype=float)
    elif order == "xyzs" or order == "xyzw":
        # Scalar last: [x, y, z, w] via native xyzw converter
        quat = _native.matrix_to_quaternion_xyzw(rotation)
        return np.array(quat, dtype=float)
    else:
        raise ValueError(f"Unknown quaternion order: {order}. Use 'sxyz' or 'xyzs'")

Visualization

Optional visualization tools are documented on the Visualization page.

GPU Solvers

For GPU-accelerated batched velocity IK, see the GPU Solvers documentation. The embodik.gpu module provides:

• build_fi_pesns_single_task — FI-PeSNS solver (primary)
• build_pph_sns_single_task — PPH-SNS solver (alternative)

Enumerations

SolverStatus

Status codes returned by IK solvers:

• SUCCESS: Solver converged successfully
• MAX_ITERATIONS: Maximum iterations reached
• INVALID_INPUT: Invalid input parameters
• SINGULARITY: Singular configuration encountered
• CONSTRAINT_VIOLATION: Joint limits or constraints violated

Result Types

All solver result types expose shared diagnostics:

• status: Solver status code
• computation_time_ms: Computation time in milliseconds
• task_scales: Task scaling factors for multi-task problems, when applicable
• task_errors: Per-task error magnitudes, when applicable
• status_message: Optional solver status detail
• condition_number: Worst Jacobian condition number observed during the solve. Values near 1.0 are well-conditioned and larger values indicate increasing sensitivity near singular or rank-deficient configurations. This is a diagnostic field for logging and tuning; it is not a manipulability score and does not by itself change solver behavior.

PositionIKResult

Result from position IK solving:

• solution: Final joint configuration (numpy array)
• status: Solver status code
• iterations: Number of iterations performed
• final_error: Final pose error magnitude
• computation_time_ms: Computation time in milliseconds
• condition_number: Worst Jacobian condition number observed during the solve

VelocitySolverResult

Result from velocity IK solving:

• solution: Joint velocities (numpy array)
• status: Solver status code
• task_scales: Task scaling factors for multi-task problems
• computation_time_ms: Computation time in milliseconds
• condition_number: Worst Jacobian condition number observed during the solve