API Reference

Complete API documentation for EmbodiK.

Core Classes

RobotModel

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

RobotModel

Attributes

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'.

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`

controlled_joint_indices

Dictionary mapping joint names to indices

controlled_joint_names `property`

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

init `method descriptor`

__init__(urdf_path: str, floating_base: bool = False) -> None

__init__(urdf_path: str, actuated_joint_names: Sequence[str], floating_base: bool = False) -> None

__init__()

init(self, urdf_path: str, floating_base: bool = False) -> None init(self, urdf_path: str, actuated_joint_names: collections.abc.Sequence[str], floating_base: bool = False) -> None

Overloaded function.

__init__(self, urdf_path: str, floating_base: bool = False) -> None

Load robot model from URDF file

__init__(self, urdf_path: str, actuated_joint_names: collections.abc.Sequence[str], floating_base: bool = False) -> None

Load reduced robot model with only actuated joints.

Builds a reduced model by locking all joints not in actuated_joint_names at their neutral configuration. model.nq/nv match the actuated joint count, eliminating index mapping when integrating with external systems.

new `builtin`

__new__(*args, **kwargs)

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

repr `method descriptor`

__repr__()

repr(self) -> str

apply_collision_exclusions `method descriptor`

apply_collision_exclusions()

apply_collision_exclusions(self, collision_pairs: collections.abc.Sequence[tuple[str, str]]) -> None

Disable the provided collision pairs using an SRDF-style specification

check_collision `method descriptor`

check_collision()

check_collision(self, min_distance: float = 0.0) -> bool

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`		Distance threshold (default 0.0)	required

Returns:

Type	Description
`bool`	True if collision detected, False otherwise

compute_collision_distances `method descriptor`

compute_collision_distances()

compute_collision_distances(self) -> list[float]

Compute collision distances for all pairs at current configuration.

Returns:

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

compute_coriolis `method descriptor`

compute_coriolis()

compute_coriolis(self, q: numpy.ndarray[dtype=float64, shape=(), order='C'], v: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(*), order='C']

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

Parameters:

Name	Type	Description	Default
`q`		Joint configuration vector (size nq)	required
`v`		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 `method descriptor`

compute_generalized_gravity()

compute_generalized_gravity(self, q: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(), order='C']

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

compute_mass_matrix()

compute_mass_matrix(self, q: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(, *), order='F']

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

compute_min_collision_distance()

compute_min_collision_distance(self) -> float

Compute minimum collision distance at current configuration.

Returns:

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

difference `method descriptor`

difference()

difference(self, q0: numpy.ndarray[dtype=float64, shape=(), order='C'], q1: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(*), order='C']

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`		Start configuration (size nq)	required
`q1`		End configuration (size nq)	required

Returns:

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

from_xacro

from_xacro(*args, **kwargs)

from_xacro(xacro_path: str, floating_base: bool = False) -> std::unique_ptr >

Create robot model from XACRO file

get_acceleration_limits `method descriptor`

get_acceleration_limits()

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

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

get_collision_geometries `method descriptor`

get_collision_geometries()

get_collision_geometries(self) -> list[dict]

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

get_collision_geometry_names `method descriptor`

get_collision_geometry_names()

get_collision_geometry_names(self) -> list[str]

Return the list of collision geometry object names

get_collision_pair_names `method descriptor`

get_collision_pair_names()

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

Return the list of collision pairs as name tuples

get_com_jacobian `method descriptor`

get_com_jacobian()

get_com_jacobian(self) -> numpy.ndarray[dtype=float64, shape=(3, *), order='F']

Get 3xN center of mass Jacobian

get_com_position `method descriptor`

get_com_position()

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

Get center of mass position

get_com_velocity `method descriptor`

get_com_velocity()

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

Get center of mass velocity

get_current_configuration `method descriptor`

get_current_configuration()

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

Get current joint configuration

get_current_velocity `method descriptor`

get_current_velocity()

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

Get current joint velocities

get_effort_limits `method descriptor`

get_effort_limits()

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

Get joint effort/torque limits

get_frame_jacobian `method descriptor`

get_frame_jacobian()

get_frame_jacobian(self, frame_name: str, reference_frame: embodik._embodik_impl.ReferenceFrame = ReferenceFrame.LOCAL_WORLD_ALIGNED) -> numpy.ndarray[dtype=float64, shape=(6, *), order='F']

Get 6xN Jacobian of specified frame

get_frame_names `method descriptor`

get_frame_names()

get_frame_names(self) -> list[str]

Get list of all frame names

get_frame_pose `method descriptor`

get_frame_pose()

get_frame_pose(self, frame_name: str) -> embodik._embodik_impl.SE3

Get pose of specified frame as SE3 transformation

get_gravity `method descriptor`

get_gravity()

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

Get the current gravity vector.

Returns:

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

get_joint_config_index `method descriptor`

get_joint_config_index()

get_joint_config_index(self, joint_name: str) -> int

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

get_joint_config_size()

get_joint_config_size(self, joint_name: str) -> int

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

get_joint_id()

get_joint_id(self, joint_name: str) -> int

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

get_joint_limits()

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

Get joint position limits as (lower, upper) pair

get_joint_names `method descriptor`

get_joint_names()

get_joint_names(self) -> list[str]

Get list of all joint names

get_joint_velocity_index `method descriptor`

get_joint_velocity_index()

get_joint_velocity_index(self, joint_name: str) -> int

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

get_joint_velocity_size()

get_joint_velocity_size(self, joint_name: str) -> int

Get number of velocity variables for a joint

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

Parameters:

Name	Type	Description	Default
`joint_name`		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 `method descriptor`

get_velocity_limits()

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

Get joint velocity limits

has_collision_geometry `method descriptor`

has_collision_geometry()

has_collision_geometry(self) -> bool

Check if collision geometry is available

has_frame `method descriptor`

has_frame()

has_frame(self, frame_name: str) -> bool

Check if frame exists

has_joint `method descriptor`

has_joint()

has_joint(self, joint_name: str) -> bool

Check if joint exists

integrate `method descriptor`

integrate()

integrate(self, q: numpy.ndarray[dtype=float64, shape=(), order='C'], v: numpy.ndarray[dtype=float64, shape=(), order='C'], dt: float = 1.0) -> numpy.ndarray[dtype=float64, shape=(*), order='C']

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	Description	Default
`q`	Current configuration vector (size nq)	required
`v`	Velocity / tangent vector (size nv)	required
`dt`	Time step (default 1.0, i.e. v is already scaled)	required

Returns:

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

neutral_configuration `method descriptor`

neutral_configuration()

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

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

normalize()

normalize(self, q: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(), order='C']

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`		Configuration vector (size nq)	required

Returns:

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

random_configuration `method descriptor`

random_configuration()

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

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

rnea()

rnea(self, q: numpy.ndarray[dtype=float64, shape=(), order='C'], v: numpy.ndarray[dtype=float64, shape=(), order='C'], a: numpy.ndarray[dtype=float64, shape=(), order='C']) -> numpy.ndarray[dtype=float64, shape=(), order='C']

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	Description	Default
`q`	Joint configuration vector (size nq)	required
`v`	Joint velocity vector (size nv)	required
`a`	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 `method descriptor`

set_acceleration_limits()

set_acceleration_limits(self, accel_limits: numpy.ndarray[dtype=float64, shape=(*), order='C']) -> None

Set custom joint acceleration limits

set_gravity `method descriptor`

set_gravity()

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

Set the gravity vector for the model.

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

Parameters:

Name	Type	Description	Default
`gravity`		3D gravity vector (e.g. [0, 0, -9.81])	required

set_joint_limits `method descriptor`

set_joint_limits()

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

Set joint position limits

update_configuration `method descriptor`

update_configuration()

update_configuration(self, q: numpy.ndarray[dtype=float64, shape=(*), order='C']) -> None

Update robot configuration and compute forward kinematics

update_kinematics `method descriptor`

update_kinematics()

update_kinematics(self, q: numpy.ndarray[dtype=float64, shape=(), order='C'], v: numpy.ndarray[dtype=float64, shape=(), order='C'] = array([], dtype=float64)) -> None

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

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.

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

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

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

API Reference

Core Classes

RobotModel

RobotModel

Attributes

__module__ class-attribute

collision_data property

collision_model property

controlled_joint_indices property

controlled_joint_names property

is_floating_base property

nq property

nv property

urdf_path property

visual_data property

visual_model property

Functions

__init__ method descriptor

__new__ builtin

__repr__ method descriptor

apply_collision_exclusions method descriptor

check_collision method descriptor

compute_collision_distances method descriptor

compute_coriolis method descriptor

compute_generalized_gravity method descriptor

compute_mass_matrix method descriptor

compute_min_collision_distance method descriptor

difference method descriptor

from_xacro

get_acceleration_limits method descriptor

get_collision_geometries method descriptor

get_collision_geometry_names method descriptor

get_collision_pair_names method descriptor

get_com_jacobian method descriptor

get_com_position method descriptor

get_com_velocity method descriptor

get_current_configuration method descriptor

get_current_velocity method descriptor

get_effort_limits method descriptor

get_frame_jacobian method descriptor

get_frame_names method descriptor

get_frame_pose method descriptor

get_gravity method descriptor

get_joint_config_index method descriptor

get_joint_config_size method descriptor

get_joint_id method descriptor

get_joint_limits method descriptor

get_joint_names method descriptor

get_joint_velocity_index method descriptor

get_joint_velocity_size method descriptor

get_velocity_limits method descriptor

has_collision_geometry method descriptor

has_frame method descriptor

has_joint method descriptor

integrate method descriptor

neutral_configuration method descriptor

normalize method descriptor

random_configuration method descriptor

rnea method descriptor

set_acceleration_limits method descriptor

set_gravity method descriptor

set_joint_limits method descriptor

update_configuration method descriptor

update_kinematics method descriptor

KinematicsSolver

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

module `class-attribute`

collision_data `property`

collision_model `property`

controlled_joint_indices `property`

controlled_joint_names `property`

is_floating_base `property`

nq `property`

nv `property`

urdf_path `property`

visual_data `property`

visual_model `property`

init `method descriptor`

new `builtin`

repr `method descriptor`

apply_collision_exclusions `method descriptor`

check_collision `method descriptor`

compute_collision_distances `method descriptor`

compute_coriolis `method descriptor`

compute_generalized_gravity `method descriptor`

compute_mass_matrix `method descriptor`

compute_min_collision_distance `method descriptor`

difference `method descriptor`

get_acceleration_limits `method descriptor`

get_collision_geometries `method descriptor`

get_collision_geometry_names `method descriptor`

get_collision_pair_names `method descriptor`

get_com_jacobian `method descriptor`

get_com_position `method descriptor`

get_com_velocity `method descriptor`

get_current_configuration `method descriptor`

get_current_velocity `method descriptor`

get_effort_limits `method descriptor`

get_frame_jacobian `method descriptor`

get_frame_names `method descriptor`

get_frame_pose `method descriptor`

get_gravity `method descriptor`

get_joint_config_index `method descriptor`

get_joint_config_size `method descriptor`

get_joint_id `method descriptor`

get_joint_limits `method descriptor`

get_joint_names `method descriptor`

get_joint_velocity_index `method descriptor`

get_joint_velocity_size `method descriptor`

get_velocity_limits `method descriptor`

has_collision_geometry `method descriptor`

has_frame `method descriptor`

has_joint `method descriptor`

integrate `method descriptor`

neutral_configuration `method descriptor`

normalize `method descriptor`

random_configuration `method descriptor`

rnea `method descriptor`

set_acceleration_limits `method descriptor`

set_gravity `method descriptor`

set_joint_limits `method descriptor`

update_configuration `method descriptor`

update_kinematics `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`