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$, $\dot{q}_d \in \mathbb{R}^n$0, $\dot{q}_d \in \mathbb{R}^n$1.

Hard constraints guarantee:

• Joint position/velocity limits: projected via forward Euler over $\dot{q}_d \in \mathbb{R}^n$2, bounds expressed directly in joint-velocity space.
• Collision avoidance: pairwise, linearized via first-order Taylor expansion of the signed minimal separation, with buffer $\dot{q}_d \in \mathbb{R}^n$3:

$\dot{q}_d \in \mathbb{R}^n$4

Matrices $\dot{q}_d \in \mathbb{R}^n$5 aggregate all such constraints row-wise.

2. Kinematic and Redundancy Operators

• Spatial Jacobian $\dot{q}_d \in \mathbb{R}^n$6: 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 $\dot{q}_d \in \mathbb{R}^n$7 for redundancy in the primary (6-DOF) velocity task; secondary objectives (e.g., posture, energy) can be integrated by augmenting $\dot{q}_d \in \mathbb{R}^n$8 with additional null-space terms.
• Collision gradient: $\dot{q}_d \in \mathbb{R}^n$9 computed numerically via symmetric difference for each joint and critical geometry pair.

3. Construction of the QP System

The QP system matrices $$\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}$$0 and $$\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}$$1 are constructed at each tick using the current joint state, previous velocity, and desired Cartesian tracking commands. The linear constraint matrices $$\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}$$2 and $$\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}$$3 are assembled from all joint and collision-buffer constraints.

Collision constraints per critical pair:

$$\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}$$4

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 $$\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}$$5 ms (single-arm) and $$\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}$$6 ms (dual-arm) on a 10th-gen i9 CPU.
• Integration: Upon solving for $$\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}$$7, joint states are forward-integrated ($$\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}$$8) 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 ($$\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}$$9), 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: $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$0–$\|v_d - J(q)\dot{q}_d\|_{W_e}^2$1 ms (single arm), $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$2–$\|v_d - J(q)\dot{q}_d\|_{W_e}^2$3 ms (two arms).
• Active collision constraints (NAC): typically $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$4 (single-arm), $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$5 (dual-arm) per tick.
• Optimal working set iterations (nWSR): $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$6–$\|v_d - J(q)\dot{q}_d\|_{W_e}^2$7 per tick.
• Mean tracking error: $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$8 mrad/joint (single arm), $\|v_d - J(q)\dot{q}_d\|_{W_e}^2$9 mrad/joint (dual arm).
• Trajectory smoothness: Jerk distributions indicate 90% of joint jerk within $J(q) \in \mathbb{R}^{6 \times n}$0 rad/s³ (cf. $J(q) \in \mathbb{R}^{6 \times n}$1 for random profile); frequency spectra show main power $J(q) \in \mathbb{R}^{6 \times n}$2 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).