optimal_ik

Submodules

optimal_ik._optimal_ik_ext

Classes

`Task`	Abstract base class for IK tasks.
`FrameTaskOptions`	Parameters for FrameTask.
`FrameTask`	Task to reach a target pose for a specified frame.
`ConfigurationTaskOptions`	Parameters for ConfigurationTask.
`ConfigurationTask`	Task to reach a target joint configuration.
`Constraints`	Abstract base class for IK constraints.
`PositionLimit`	Constraint to enforce joint position limits.
`VelocityLimit`	Constraint to enforce joint velocity limits.
`Barrier`	Abstract base class for Control Barrier Functions.
`ConstraintAxisSelection`	Axis selection for position barrier constraints.
`PositionBarrier`	Position barrier constraint that keeps a frame within an axis-aligned bounding box.
`SelfCollisionBarrier`	Self-collision avoidance barrier based on hpp-fcl / coal collision pair distances.
`Oink`	Optimal Inverse Kinematics solver.

Package Contents

class optimal_ik.Task

Abstract base class for IK tasks.

property gain: float

Task gain for low-pass filtering.

Return type:: float

property weight: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None, None, order='F')]

Weight matrix for cost normalization.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, None), order=’F’)]

property lm_damping: float

Levenberg-Marquardt damping.

Return type:: float

property priority: int

Priority level (1 = highest; lower priorities are projected into the nullspace of higher priorities).

Return type:: int

property num_variables: int

Number of optimization variables.

Return type:: int

class optimal_ik.FrameTaskOptions(position_cost=1.0, orientation_cost=1.0, task_gain=1.0, lm_damping=0.0, max_position_error=float('inf'), max_rotation_error=float('inf'), priority=1)

Parameters for FrameTask.

Parameters:

position_cost (float)
orientation_cost (float)
task_gain (float)
lm_damping (float)
max_position_error (float)
max_rotation_error (float)
priority (int)

property position_cost: float

Position cost weight.

Return type:: float

property orientation_cost: float

Orientation cost weight.

Return type:: float

property task_gain: float

Task gain for low-pass filtering.

Return type:: float

property lm_damping: float

Levenberg-Marquardt damping.

Return type:: float

property max_position_error: float

Maximum position error magnitude (meters). Infinite = no limit.

Return type:: float

property max_rotation_error: float

Maximum rotation error magnitude (radians). Infinite = no limit.

Return type:: float

property priority: int

Priority level (1 = highest). Tasks at higher priority numbers are projected into the nullspace of lower priority numbers.

Return type:: int

class optimal_ik.FrameTask(oink, scene, target_pose, options=...)

Bases: Task

Task to reach a target pose for a specified frame.

Parameters:

oink (Oink)
scene (roboplan.core._core_ext.Scene)
target_pose (roboplan.core._core_ext.CartesianConfiguration)
options (FrameTaskOptions)

property frame_name: str

Name of the frame to control.

Return type:: str

property frame_id: int

Index of the frame in the scene’s Pinocchio model.

Return type:: int

property v_indices: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=None, order='C')]

Velocity vector indices for the joint group.

Return type:: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order=’C’)]

property target_pose: roboplan.core._core_ext.CartesianConfiguration

Target pose for the frame.

Return type:: roboplan.core._core_ext.CartesianConfiguration

property max_position_error: float

Maximum position error magnitude (meters).

Return type:: float

property max_rotation_error: float

Maximum rotation error magnitude (radians).

Return type:: float

setTargetFrameTransform(tform)

Sets the target transform for this frame task.

Parameters:: tform (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(4, 4), order='F')])
Return type:: None

class optimal_ik.ConfigurationTaskOptions(task_gain=1.0, lm_damping=0.0, priority=1)

Parameters for ConfigurationTask.

Parameters:

task_gain (float)
lm_damping (float)
priority (int)

property task_gain: float

Task gain for low-pass filtering.

Return type:: float

property lm_damping: float

Levenberg-Marquardt damping.

Return type:: float

property priority: int

Priority level (1 = highest). Tasks at higher priority numbers are projected into the nullspace of lower priority numbers.

Return type:: int

class optimal_ik.ConfigurationTask(oink, target_q, joint_weights, options=...)

Bases: Task

Task to reach a target joint configuration.

Parameters:

oink (Oink)
target_q (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')])
joint_weights (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')])
options (ConfigurationTaskOptions)

property target_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None, order='C')]

Target joint configuration.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order=’C’)]

property joint_weights: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None, order='C')]

Weights for each joint in the configuration task.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order=’C’)]

class optimal_ik.Constraints: Abstract base class for IK constraints.

class optimal_ik.PositionLimit(oink, gain=1.0)

Bases: Constraints

Constraint to enforce joint position limits.

Parameters:

oink (Oink)
gain (float)

property config_limit_gain: float

Gain for position limit enforcement.

Return type:: float

class optimal_ik.VelocityLimit(oink, dt, v_max)

Bases: Constraints

Constraint to enforce joint velocity limits.

Parameters:

oink (Oink)
dt (float)
v_max (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')])

property dt: float

Time step for velocity calculation.

Return type:: float

property v_max: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None, order='C')]

Maximum joint velocities.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order=’C’)]

class optimal_ik.Barrier

Abstract base class for Control Barrier Functions.

get_num_barriers(scene)

Get the number of barrier constraints.

Parameters:: scene (roboplan.core._core_ext.Scene)
Return type:: int

property gain: float

Barrier gain (gamma).

Return type:: float

property dt: float

Timestep.

Return type:: float

property safe_displacement_gain: float

Gain for safe displacement regularization.

Return type:: float

property safety_margin: float

Conservative margin for hard constraints.

Return type:: float

class optimal_ik.ConstraintAxisSelection(x=True, y=True, z=True)

Axis selection for position barrier constraints.

Parameters:

x (bool)
y (bool)
z (bool)

property x: bool

Constrain X axis.

Return type:: bool

property y: bool

Constrain Y axis.

Return type:: bool

property z: bool

Constrain Z axis.

Return type:: bool

class optimal_ik.PositionBarrier(oink, scene, frame_name, p_min, p_max, dt, axis_selection=..., gain=1.0, safe_displacement_gain=1.0, safety_margin=0.0)

Bases: Barrier

Position barrier constraint that keeps a frame within an axis-aligned bounding box.

Parameters:

oink (Oink)
scene (roboplan.core._core_ext.Scene)
frame_name (str)
p_min (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')])
p_max (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')])
dt (float)
axis_selection (ConstraintAxisSelection)
gain (float)
safe_displacement_gain (float)
safety_margin (float)

get_frame_position(scene)

Get the current frame position in world coordinates.

Parameters:: scene (roboplan.core._core_ext.Scene)
Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order=’C’)]

property frame_name: str

Name of the constrained frame.

Return type:: str

property axis_selection: ConstraintAxisSelection

Axis selection for constraints.

Return type:: ConstraintAxisSelection

property p_min: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')]

Minimum position bounds.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order=’C’)]

property p_max: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')]

Maximum position bounds.

Return type:: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order=’C’)]

class optimal_ik.SelfCollisionBarrier(oink, scene, n_collision_pairs, dt, gain=1.0, safe_displacement_gain=1.0, d_min=0.02, safety_margin=0.0)

Bases: Barrier

Self-collision avoidance barrier based on hpp-fcl / coal collision pair distances.

Constrains the closest n_collision_pairs collision pairs in the scene to remain at least d_min apart. Inspired by pink.barriers.SelfCollisionBarrier.

Parameters:

oink (Oink)
scene (roboplan.core._core_ext.Scene)
n_collision_pairs (int)
dt (float)
gain (float)
safe_displacement_gain (float)
d_min (float)
safety_margin (float)

property n_collision_pairs: int

Number of closest collision pairs to constrain.

Return type:: int

property d_min: float

Minimum allowed distance between any pair of bodies.

Return type:: float

class optimal_ik.Oink(scene: roboplan.core._core_ext.Scene, group_name: str)

class optimal_ik.Oink(scene)

Optimal Inverse Kinematics solver.

property num_variables: int

Number of optimization variables.

Return type:: int

property q_indices: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=None, order='C')]

Position indices of the joint group.

Return type:: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order=’C’)]

property v_indices: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=None, order='C')]

Velocity indices of the joint group.

Return type:: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order=’C’)]

solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], constraints: collections.abc.Sequence[Constraints], barriers: collections.abc.Sequence[Barrier], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None)], regularization: float = 1e-12) → None

solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None)], regularization: float = 1e-12) → None

solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], constraints: collections.abc.Sequence[Constraints], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None)], regularization: float = 1e-12) → None

solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], barriers: collections.abc.Sequence[Barrier], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=None)], regularization: float = 1e-12) → None

Solve inverse kinematics for tasks with barriers (no constraints).

Parameters:

tasks – List of weighted tasks to optimize for.
barriers – List of barrier functions for safety constraints.
delta_q – Pre-allocated numpy array for output (size = num_variables).
regularization – Tikhonov regularization weight (default: 1e-12).

Example

delta_q = np.zeros(oink.num_variables) oink.solveIk(scene, tasks, barriers, delta_q)

enforceBarriers(scene, barriers, delta_q, tolerance=0.0)

Validate delta_q against barriers using forward kinematics.

This method provides a post-solve safety check by evaluating the actual barrier values at the candidate configuration (q + delta_q). If any barrier would be violated, delta_q is set to zero to prevent unsafe motion.

This is a backup safety mechanism for cases where the linearized CBF constraint in the QP has significant error (e.g., large jumps, near-boundary configurations).

Parameters:

barriers (collections.abc.Sequence[Barrier]) – List of barrier functions to check.
delta_q (Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ))]) – Configuration displacement to validate. Modified in place: set to zero if barrier violation is detected.
tolerance (float) – Tolerance for barrier violation detection. A barrier is considered violated if h(q + delta_q) < -tolerance. Default is 0.0.
scene (roboplan.core._core_ext.Scene)

Raises:

RuntimeError – If barrier evaluation fails (e.g., frame not found).

Return type:

None

Example

delta_q = np.zeros(oink.num_variables) oink.solveIk(scene, tasks, constraints, barriers, delta_q) oink.enforceBarriers(scene, barriers, delta_q)