Inverse Kinematics

RoboPlan provides two inverse kinematics solvers: a simple Jacobian-based solver and an optimal solver based on quadratic programming.

SimpleIK: Jacobian-Based Solver

SimpleIK is a lightweight inverse kinematics solver using the damped least squares (DLS) method, also known as the Levenberg-Marquardt algorithm.

Algorithm

At each iteration, SimpleIK computes joint velocities that minimize the Cartesian error:

\[\dot{q} = -J^T (J J^T + \lambda I)^{-1} e\]

Where:

\(e = \log_6(T_{\text{goal}}^{-1} T_{\text{current}})\) — 6D Cartesian error (position + orientation)
\(J\) — Frame Jacobian in LOCAL reference frame
\(\lambda\) — Damping factor for regularization

The joint configuration is updated via integration:

\[q \leftarrow q \oplus (\dot{q} \cdot \text{step\_size})\]

Properties:

Simple and efficient — minimal computational overhead
Supports multiple simultaneous goal frames
Collision checking with random restarts on failure
Convergence monitoring based on separate linear and angular error thresholds
Optionally attempt to find a nearest solution to the seed until the timeout is reached

Configuration

Parameter	Description	Default
`group_name`	Joint group to control	“”
`max_iters`	Maximum iterations per attempt	100
`max_time`	Maximum computation time (seconds)	0.005
`max_restarts`	Number of random restarts on failure	2
`step_size`	Integration step size	0.25
`damping`	Damping factor \(\lambda\)	0.001
`max_linear_error_norm`	Convergence threshold (meters)	0.001
`max_angular_error_norm`	Convergence threshold (radians)	0.001
`check_collisions`	Enable collision checking	true
`fast_return`	Return the first solution found.	true

Usage Example

import numpy as np
from roboplan.core import Scene, JointConfiguration, CartesianConfiguration
from roboplan.simple_ik import SimpleIkOptions, SimpleIk

# Setup
scene = Scene("robot", urdf_path, srdf_path, package_paths)

options = SimpleIkOptions(
    group_name="arm",
    step_size=0.25,
    max_linear_error_norm=0.001,
    max_angular_error_norm=0.001,
    check_collisions=True
)
ik_solver = SimpleIk(scene, options)

# Define goal
goal = CartesianConfiguration()
goal.base_frame = "base"
goal.tip_frame = "tool0"
goal.tform = target_transform  # 4x4 SE(3) matrix

# Solve
start = JointConfiguration()
start.positions = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])  # initial configuration
solution = JointConfiguration()

success = ik_solver.solveIk(goal, start, solution)

OInK: Optimal Inverse Kinematics

The OInK solver uses Quadratic Programming (QP) to compute joint displacements that achieve multiple objectives while respecting constraints and safety barriers.

QP Problem Formulation

OInK solves the following QP at each control step:

\[\min_{\Delta q} \quad \underbrace{\frac{1}{2} \sum_{k} \| W_k (J_k N_k \Delta q + \alpha_k e_k) \|^2}_{\text{Tasks}} + \underbrace{\frac{\lambda}{2} \|\Delta q\|^2}_{\text{Regularization}} + \underbrace{\sum_{b} \frac{r_b}{2\|J_b\|^2} \|\Delta q - \Delta q_{\text{safe}}\|^2}_{\text{Barrier Regularization}}\]

Subject to:

\[\underbrace{l \leq G_c \Delta q \leq u}_{\text{Hard Constraints}} \quad \text{and} \quad \underbrace{G_b \Delta q \leq h_b}_{\text{Barrier Constraints}}\]

Reformulated as:

\[\min_{\Delta q} \quad \frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q\]

Where:

\[H = \lambda I + \sum_k (N_k^T J_k^T W_k^T W_k J_k N_k + \mu_k I) + \sum_b \frac{r_b}{\|J_b\|^2} I\]

\[c = \sum_k (-\alpha_k N_k^T J_k^T W_k^T W_k e_k) + \sum_b \frac{-r_b}{\|J_b\|^2} \Delta q_{\text{safe}}\]

Task Priorities and Nullspace Projection

Each task carries an integer priority (default 1 = highest). Tasks at a lower priority level (higher priority number) are projected into the nullspace of all higher-priority tasks, so they cannot fight tasks above them. Their contribution is structurally zero in the higher-priority directions.

For each priority level \(k\), the QP uses a projected Jacobian \(J_k N_k\), where \(N_k\) is the cumulative nullspace projector built from the row-stacked Jacobians of all priority levels \(1, \ldots, k-1\). \(N_1 = I\) (no projection at the top level), so a single-priority problem reduces to the standard weighted-sum QP.

The projector is computed via a damped pseudoinverse:

\[N_k = I - J_{\text{stack}}^T \left( J_{\text{stack}} J_{\text{stack}}^T + \lambda I \right)^{-1} J_{\text{stack}}\]

where \(J_{\text{stack}}\) is the vertical stack of all priority-level Jacobians strictly above \(k\), and the same Tikhonov regularization \(\lambda\) from the QP is reused as the damping. The damping keeps \((J J^T + \lambda I)\) SPD even at singular configurations; at well-conditioned configurations (singular values \(\gg \sqrt{\lambda}\)), the expression reduces to the standard nullspace projector \(I - J^+ J\).

Tasks at the same priority level are combined linearly through their weights (no projection between them). The decision variable remains \(\Delta q\); only the per-task Jacobian is projected.

Tasks

Tasks define optimization objectives through Jacobians and errors.

FrameTask

Tracks a target 6-DOF pose (position + orientation).

Error computation:

\[e_{\text{pos}} = p_{\text{target}} - p_{\text{current}}\]

\[e_{\text{rot}} = R_{\text{current}} \cdot \log_3(R_{\text{current}}^T R_{\text{target}})\]

Error saturation (prevents large jumps that invalidate CBF linearization):

\[e_{\text{saturated}} = e_{\max} \cdot \tanh\left(\frac{\|e\|}{e_{\max}}\right) \cdot \frac{e}{\|e\|}\]

Jacobian: Frame Jacobian in LOCAL_WORLD_ALIGNED coordinates, negated so QP moves toward target.

Weight matrix:

\[W = \text{diag}(\sqrt{w_{\text{pos}}} \cdot I_3, \sqrt{w_{\text{rot}}} \cdot I_3)\]

Parameter	Description	Default
`position_cost`	Position error weight	1.0
`orientation_cost`	Orientation error weight	1.0
`task_gain`	Low-pass filter gain \(\alpha\)	1.0
`lm_damping`	Levenberg-Marquardt damping	0.0
`max_position_error`	Saturation limit (meters)	∞
`max_rotation_error`	Saturation limit (radians)	∞
`priority`	Priority level (1 = highest; see Task Priorities)	1

ConfigurationTask

Drives toward a target joint configuration (null-space regularization).

Error: Manifold-aware difference: \(e = \text{difference}(q, q_{\text{target}})\)

Jacobian: \(J = -I\) (negative identity)

Weight matrix: \(W = \text{diag}(\sqrt{w_1}, \ldots, \sqrt{w_{n_v}})\)

Also accepts task_gain, lm_damping, and priority (defaults match FrameTaskOptions). A common pattern is to use a ConfigurationTask at a lower priority level (e.g. priority=2) as a posture / null-space regularizer that will not interfere with a higher-priority FrameTask.

Constraints vs Barriers

Hard Constraints

Hard constraints enforce strict bounds that the QP solver cannot violate:

\[G \cdot \Delta q \leq h\]

Properties:

Exact enforcement — the solution always satisfies the constraint
State-independent bounds — same restriction regardless of distance to limit
No Jacobian needed for joint-space constraints
QP becomes infeasible if constraints conflict

Use for: Physical actuator limits, joint position/velocity bounds

Control Barrier Functions (Barriers)

Barriers enforce forward invariance of a safe set through a differential condition:

\[\dot{h}(q) + \alpha(h(q)) \geq 0\]

In discrete time:

\[\frac{J_h \cdot \Delta q}{\Delta t} + \alpha(h(q)) \geq 0 \quad \Rightarrow \quad -J_h \cdot \Delta q \leq \Delta t \cdot \alpha(h(q))\]

Properties:

State-dependent bounds — more freedom far from boundary, tighter near it
Smooth behavior — graceful slowdown instead of abrupt stop
Requires Jacobian computation
Always feasible (soft constraint via class-K function)
Subject to linearization error in discrete time

Use for: Cartesian position bounds, collision avoidance, workspace constraints

Comparison

Aspect	Hard Constraint	Barrier (CBF)
Formulation	\(\Delta q \leq\) constant	\(J \cdot \Delta q \leq\) \(f(\text{distance})\)
Near boundary	Same restriction	Tighter (slows down)
Far from boundary	Same restriction	Looser (more freedom)
Enforcement	Exact	Approximate (linearization)
Feasibility	Can fail	Always feasible
Behavior	Abrupt at limit	Smooth approach
Computation	Simple	Requires Jacobian

Constraint Details

VelocityLimit

Enforces maximum joint velocities as hard bounds:

\[-\Delta t \cdot v_{\max} \leq \Delta q \leq \Delta t \cdot v_{\max}\]

PositionLimit

Restricts motion based on distance to joint limits:

\[-\gamma (q - q_{\min}) \leq \Delta q \leq \gamma (q_{\max} - q)\]

The gain \(\gamma \in (0, 1]\) controls aggressiveness. As \(q \to q_{\max}\), the upper bound \(\to 0\).

Barrier Details

PositionBarrier

Keeps a frame within an axis-aligned bounding box using CBF constraints.

Barrier function (for lower bound on axis \(i\)):

\[h_i(q) = p_i(q) - p_{\min,i}\]

Barrier Jacobian:

\[J_{h_i} = J_{\text{frame},i}(q) \quad \text{(row } i \text{ of frame Jacobian)}\]

QP constraint (using saturating class-K function):

\[-J_{h_i} \cdot \Delta q \leq \Delta t \cdot \gamma \cdot \frac{h_i}{1 + |h_i|} - m\]

Where:

\(\gamma\) — barrier gain (aggressiveness)
\(m\) — safety margin (conservative buffer for linearization error)

Safe displacement regularization adds to the objective:

\[\frac{r}{2\|J_h\|^2} \|\Delta q - \Delta q_{\text{safe}}\|^2\]

This encourages motion toward a safe configuration when near boundaries.

Parameter	Description	Default
`gain`	Class-K function gain \(\gamma\)	1.0
`dt`	Control timestep	required
`safe_displacement_gain`	Regularization weight \(r\)	1.0
`safety_margin`	Conservative buffer \(m\)	0.0
`axis_selection`	Enable/disable per-axis constraints	all

SelfCollisionBarrier

Keeps the closest pairs of bodies in the robot’s collision model from interpenetrating, by enforcing a minimum signed distance on each tracked pair. Distances come from the narrow-phase collision check on the scene’s collision model.

Barrier function (for the \(i\)-th closest collision pair):

\[h_i(q) = d_i(q) - d_{\min}\]

where \(d_i(q)\) is the signed distance between the two geometries in pair \(i\). Pairs are re-selected at every call: at each step the \(n_{\text{pairs}}\) smallest distances across the full collision model become the active constraints, so the barrier always tracks whichever pairs are most at risk.

Barrier Jacobian (built from witness points and parent-joint Jacobians):

\[J_{h_i} = n^T J^{(1)}_p + (r_1 \times n)^T J^{(1)}_w - n^T J^{(2)}_p - (r_2 \times n)^T J^{(2)}_w\]

Where:

\(n\) — unit vector from witness point 1 to witness point 2 (world frame)
\(r_k\) — vector from joint \(k\)’s origin to its witness point (lever arm)
\(J^{(k)}_p, J^{(k)}_w\) — linear and angular parts of joint \(k\)’s LOCAL_WORLD_ALIGNED Jacobian

If the witness points coincide (\(d_i \approx 0\)) the contact normal is undefined; that Jacobian row is zeroed so the barrier degrades gracefully instead of producing NaNs.

The same safe-displacement regularization described for PositionBarrier applies.

Parameter	Description	Default
`n_collision_pairs`	Number of closest pairs to constrain (must be ≤ total pairs in the model)	required
`d_min`	Minimum allowed distance \(d_{\min}\) (meters)	0.02
`gain`	Class-K function gain \(\gamma\)	1.0
`dt`	Control timestep	required
`safe_displacement_gain`	Regularization weight \(r\)	1.0
`safety_margin`	Conservative buffer \(m\)	0.0

Note

Per-pair narrow-phase distance dominates the per-solve cost when many pairs are tracked. Pick the smallest n_collision_pairs that still covers the pairs you expect to be active. The post-solve enforceBarriers() check only re-evaluates this active set, so over-sizing n_collision_pairs makes both the QP assembly and the FK validation slower.

Additionally, you should consider using robot models that have optimized collision meshes (e.g., simplified convex hulls or simple geometric primitives). If your collision meshes are too high-quality, this will dramatically increase solve time.

Linearization Error and `enforceBarriers()`

The CBF constraint is based on a first-order Taylor expansion:

\[h(q + \Delta q) \approx h(q) + J_h \cdot \Delta q\]

This has \(O(\|\Delta q\|^2)\) error. Near boundaries with large commands, the linearized constraint can be satisfied while the actual barrier is violated.

enforceBarriers() provides a post-solve safety check using forward kinematics:

// After solving QP
oink.solveIk(tasks, constraints, barriers, scene, delta_q);

// Validate using FK: if h(q + delta_q) < -tolerance, set delta_q = 0
oink.enforceBarriers(barriers, scene, delta_q, tolerance);

Implementation Notes

Numerical Properties

Positive definiteness: Guaranteed by Tikhonov regularization \(\lambda I\)
Convexity: Quadratic objective + linear constraints → unique global optimum
Weight scaling: Applied as \(\sqrt{w}\) for better conditioning

Solver

OInK uses OSQP with:

Dense accumulation of \(H\) and \(c\)
Sparse conversion for solving
Warm-starting between iterations
Workspace caching for constraints

Usage Example

import numpy as np
from roboplan.core import Scene, CartesianConfiguration
from roboplan.optimal_ik import (
    ConfigurationTask, ConfigurationTaskOptions,
    FrameTask, FrameTaskOptions,
    Oink, PositionLimit, VelocityLimit,
    PositionBarrier, SelfCollisionBarrier,
)

# Scene + solver. urdf/srdf are XML strings (e.g. from xacro.process_file(...).toxml()).
scene = Scene("robot", urdf=urdf_xml, srdf=srdf_xml, package_paths=package_paths)
oink = Oink(scene, group_name="arm")
nv = len(oink.v_indices)                  # joint-group velocity dimension
dt = 0.01

# Primary task: track an SE(3) target with the end-effector frame.
goal = CartesianConfiguration()
goal.base_frame = "base_link"
goal.tip_frame = "tool0"
goal.tform = target_transform             # 4x4 SE(3) matrix

frame_task = FrameTask(oink, scene, goal, FrameTaskOptions(
    position_cost=1.0,
    orientation_cost=0.1,
    task_gain=1.0,
    lm_damping=0.01,
    max_position_error=0.1,               # keeps the CBF linearization valid
))

# Lower-priority posture regularization. priority=2 projects it into the FrameTask
# nullspace, so it only uses the redundant DoF the EE task leaves free.
posture_task = ConfigurationTask(
    oink,
    q_nominal[oink.q_indices],
    np.full(nv, 0.05),
    ConfigurationTaskOptions(priority=2),
)
tasks = [frame_task, posture_task]

# Hard constraints (exact enforcement).
constraints = [
    PositionLimit(oink, gain=1.0),
    VelocityLimit(oink, dt, v_max=np.ones(nv)),
]

# Barriers (smooth safety).
barriers = [
    PositionBarrier(
        oink, scene,
        frame_name="tool0",
        p_min=np.array([-0.5, -0.5, 0.1]),
        p_max=np.array([0.5, 0.5, 1.0]),
        dt=dt,
        gain=5.0,
        safety_margin=0.01,
    ),
    SelfCollisionBarrier(
        oink, scene,
        n_collision_pairs=4,              # track the 4 closest pairs each step
        dt=dt,
        gain=0.01,
        d_min=0.02,
    ),
]

# One control step.
delta_q = np.zeros(nv)
oink.solveIk(scene, tasks, constraints, barriers, delta_q, regularization=1e-6)

# When the joint group is a subset of the model, scatter the group's velocity into
# the full-model nv vector before enforceBarriers / integrate.
delta_q_full = np.zeros(model_nv)         # full model velocity dimension
delta_q_full[oink.v_indices] = delta_q

# Optional FK-based safety check: zeros delta_q_full if any barrier would be violated.
oink.enforceBarriers(scene, barriers, delta_q_full)

q_next = scene.integrate(scene.getCurrentJointPositions(), delta_q_full)
scene.setJointPositions(q_next)

Inverse Kinematics

SimpleIK: Jacobian-Based Solver

Algorithm

Configuration

Usage Example

OInK: Optimal Inverse Kinematics

QP Problem Formulation

Task Priorities and Nullspace Projection

Tasks

FrameTask

ConfigurationTask

Constraints vs Barriers

Hard Constraints

Control Barrier Functions (Barriers)

Comparison

Constraint Details

VelocityLimit

PositionLimit

Barrier Details

PositionBarrier

SelfCollisionBarrier

Linearization Error and enforceBarriers()

Implementation Notes

Numerical Properties

Solver

Usage Example

Linearization Error and `enforceBarriers()`