Redundant Inverse Kinematics (iKinQP)

• Functionally redundant inverse kinematics is a QP-based method for high-DOF manipulators that computes joint configurations while satisfying end-effector and safety constraints.
• The algorithm integrates soft objectives for velocity tracking, trajectory smoothness, and minimal joint effort with hard constraints for collision avoidance and joint limits.
• Real-time performance is achieved with sub-millisecond solver times, making iKinQP suitable for complex multi-arm and high-DOF robotic applications.

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$) (Ashkanazy et al., 2023). 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 (Ashkanazy et al., 2023).

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 (Ashkanazy et al., 2023).