---
title: Redundant Inverse Kinematics (iKinQP)
url: https://www.emergentmind.com/topics/functionally-redundant-inverse-kinematics-algorithm
type: topic
---

# Redundant Inverse Kinematics (iKinQP)

A functionally redundant inverse kinematics (IK) algorithm addresses the problem of computing joint configurations for robotic manipulators possessing more actuated degrees of freedom (DOF) than task-space constraints require, thereby enabling the exploitation of this redundancy for objectives such as collision avoidance, joint-limit avoidance, and trajectory smoothness. The mathematical and algorithmic complexity of inverse kinematics with redundancy—where the set of joint configurations mapping to a single end-effector pose forms a continuum—renders analytic solutions generally intractable for general articulated robots. State-of-the-art approaches formulate the problem as a constrained optimization solved at each control tick, leveraging numerical solvers to enforce hard constraints and optimize trade-offs among competing objectives.

## 1. Mathematical Formulation and QP Structure

The iKinQP algorithm models functionally redundant IK as a strictly constrained quadratic program (QP) that operates in joint velocity space at each control cycle for an n-DOF manipulator ($n>6$) [2308.15268]. The decision variable is the joint-velocity update vector $\dot{q}_d \in \mathbb{R}^n$. The cost function integrates three orthogonal soft objectives: end-effector velocity tracking, trajectory smoothness, and (n–6)-DOF redundancy resolution via joint-effort minimization. The QP is posed as:

\[
\begin{aligned}
\min_{\dot{q}_d} \quad & \frac{1}{2}\,\dot{q}_d^{T}\,H(q)\,\dot{q}_d + g(q)^{T}\,\dot{q}_d \\
\text{s.t.} \quad      & q_{\min} \leq q + \delta t\,\dot{q}_d \leq q_{\max}, \\
                       & \dot{q}_{\min} \leq \dot{q}_d \leq \dot{q}_{\max}, \\
                       & A(q)\,\dot{q}_d \geq b(q) \quad \text{(collision avoidance)}
\end{aligned}
\]

**Soft cost terms** (expanded in the QP quadratic form):
- **End-effector velocity tracking**: $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$, with $J(q) \in \mathbb{R}^{6 \times n}$ the spatial Jacobian, and $v_d$ the desired end-effector velocity.
- **Smoothness**: $\|\dot{q}_d - \dot{q}_{\text{prev}}\|_{W_s}^2$ (penalizes deviation from nominal joint motion).
- **Redundancy resolution**: $\|\dot{q}_d\|_{W_r}^2$ (minimum-norm velocity solution in the null-space).

The **Hessian** and **linear term** are:
\[
\begin{aligned}
H(q) &= J^T J + \gamma\,I_n + \lambda\,I_n,\\
g(q) &= - J^T v_d - \gamma\,\dot{q}_{\text{prev}}
\end{aligned}
\]
for diagonal weight matrices $W_e = I_6$, $W_s = \gamma I_n$, $W_r = \lambda I_n$.

**Hard constraints** guarantee:
- **Joint position/velocity limits:** projected via forward Euler over $\delta t$, bounds expressed directly in joint-velocity space.
- **Collision avoidance:** pairwise, linearized via first-order Taylor expansion of the signed minimal separation, with buffer $d_{\text{buff}}$:
  \[
  d\left(q + \delta t\,\dot{q}_d\right) \approx d(q) + \nabla_q d(q)^T \delta t\,\dot{q}_d \geq d_{\text{buff}}
  \]
  Matrices $A(q), b(q)$ aggregate all such constraints row-wise.

## 2. Kinematic and Redundancy Operators

- **Spatial Jacobian $J(q)$:** maps joint velocities to end-effector spatial twist; computed via standard rigid-body libraries (e.g. Pinocchio).
- **Redundancy projectors:** Minimum-norm joint motion term implicitly realizes the null-space projector $N(q) = I - J(q)^{\dagger} J(q)$ for redundancy in the primary (6-DOF) velocity task; secondary objectives (e.g., posture, energy) can be integrated by augmenting $H(q)$ with additional null-space terms.
- **Collision gradient:** $\nabla_q d(q)$ computed numerically via symmetric difference for each joint and critical geometry pair.

## 3. Construction of the QP System

The QP system matrices $H(q)$ and $g(q)$ are constructed at each tick using the current joint state, previous velocity, and desired Cartesian tracking commands. The linear constraint matrices $A(q)$ and $b(q)$ are assembled from all joint and collision-buffer constraints.

**Collision constraints per critical pair:**
\[
A_{ij}(q) = \delta t\,\frac{\partial d(q)}{\partial q_j},\quad
b_i(q) = d_{\text{buff}} - d(q)
\]
The full constraint set remains efficiently sparse, as only proximal geometry pairs contribute.

## 4. Real-Time Implementation Details

- **Solver:** iKinQP uses qpOASES in single-threaded C++ with warm-starts from previous solutions, achieving solver times of $0.26 \pm 0.004$ ms (single-arm) and $1.47$ ms (dual-arm) on a 10th-gen i9 CPU.
- **Integration:** Upon solving for $\dot{q}_d$, joint states are forward-integrated ($q \leftarrow q + \delta t\,\dot{q}_d$) and passed to the low-level torque controller. The QP loop remains kinematic-only; robot dynamics are handled downstream at the torque-control level.
- **Scheduling:** Control steps at 2 ms ($\delta t$), compatible with high-frequency torque control loops.

## 5. Experimental Metrics

Evaluations on a 7-DOF Kinova Gen3 (MuJoCo simulation) cover:
- **Scenarios:** single arm avoiding floor/sphere, two arms avoiding each other, with waypoints on a 0.9 m sphere and task durations of 5 and 15 s per waypoint.
- **Computation time:** $0.26$–$0.28$ ms (single arm), $1.47$–$1.64$ ms (two arms).
- **Active collision constraints (NAC):** typically $< 1$ (single-arm), $\approx 2$ (dual-arm) per tick.
- **Optimal working set iterations (nWSR):** $\sim 10$–$25$ per tick.
- **Mean tracking error:** $<6$ mrad/joint (single arm), $<12$ mrad/joint (dual arm).
- **Trajectory smoothness:** Jerk distributions indicate 90% of joint jerk within $[-1,1]$ rad/s³ (cf. $[-4,4]$ for random profile); frequency spectra show main power $<0.5$ Hz.
  
## 6. Functional Redundancy and Applications

iKinQP’s structure is fundamentally designed to exploit and resolve functional redundancy:
- **Redundancy utilization:** The quadratic penalty on joint velocities resolves (n–6)-DOF redundancy via minimal joint-motion effort.
- **Secondary objectives:** The algorithm can be extended by augmenting the QP with additional null-space projection terms, allowing explicit integration of secondary costs (e.g., posture, manipulability).
- **Collision avoidance and joint limit compliance:** All geometric and actuation limits are imposed as hard QP constraints, ensuring system-level safety guarantees not achievable with unconstrained or heuristic redundancy-resolving methods.
- **Trajectory smoothness and real-time control:** The penalty on deviation from previous velocity ensures spectrally smooth and trackable profiles suitable for direct low-level execution.

iKinQP is applicable to any serial redundant manipulator and is readily extensible for dual-arm, high-DOF, or complex spatial environments [2308.15268].

## 7. Comparative Summary and Implementation Guidance

The iKinQP approach provides a unified, real-time framework for functionally redundant inverse kinematics:
- **QP formulation natively incorporates all constraints and objectives.**
- **Collision avoidance is enforced as a hard constraint, independent of the collision detection engine.**
- **Redundancy is resolved optimally at every control tick, with optional extensibility toward secondary objectives via null-space projection.**
- **All required elements—Jacobian, nullspace operator, collision gradient, QP system construction, and solver settings—are specified for direct implementation or extension to other manipulator platforms.**

In summary, iKinQP offers a practical and computationally efficient method for resolving functional redundancy in inverse kinematics, demonstrated to deliver real-time, collision-free, joint-limit-safe, and smooth kinematic trajectories at sub-millisecond rates [2308.15268].

Source: https://www.emergentmind.com/topics/functionally-redundant-inverse-kinematics-algorithm