roboplan.roboplan_ext.optimal_ik

Optimal IK solver module

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.
Oink	Optimal Inverse Kinematics solver.

Module Contents

class roboplan.roboplan_ext.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 num_variables: int

Number of optimization variables.

Return type:

int

class roboplan.roboplan_ext.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'))

Parameters for FrameTask.

Parameters:

• position_cost (float)

• orientation_cost (float)

• task_gain (float)

• lm_damping (float)

• max_position_error (float)

• max_rotation_error (float)

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

class roboplan.roboplan_ext.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_ext.core.Scene)

• target_pose (roboplan_ext.core.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_ext.core.CartesianConfiguration

Target pose for the frame.

Return type:

roboplan_ext.core.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 roboplan.roboplan_ext.optimal_ik.ConfigurationTaskOptions(task_gain=1.0, lm_damping=0.0)

Parameters for ConfigurationTask.

Parameters:

• task_gain (float)

• lm_damping (float)

property task_gain: float

Task gain for low-pass filtering.

Return type:

float

property lm_damping: float

Levenberg-Marquardt damping.

Return type:

float

class roboplan.roboplan_ext.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 roboplan.roboplan_ext.optimal_ik.Constraints

Abstract base class for IK constraints.

class roboplan.roboplan_ext.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 roboplan.roboplan_ext.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 roboplan.roboplan_ext.optimal_ik.Barrier

Abstract base class for Control Barrier Functions.

get_num_barriers(scene)

Get the number of barrier constraints.

Parameters:

scene (roboplan_ext.core.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 roboplan.roboplan_ext.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 roboplan.roboplan_ext.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_ext.core.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_ext.core.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 roboplan.roboplan_ext.optimal_ik.Oink(scene: roboplan_ext.core.Scene, group_name: str)

class roboplan.roboplan_ext.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_ext.core.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_ext.core.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_ext.core.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_ext.core.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_ext.core.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)