---
title: "Solving Inverse Kinematics with Dual Quaternions and Polynomial Regression"
description: "Using dual quaternions and JAX-accelerated polynomial regression to solve inverse kinematics for robotic manipulators with low-overhead edge inference."
pubDate: 2025-01-06T00:00:00.000Z
tags: ["robotics","inverse-kinematics","dual-quaternions","jax","machine-learning"]
---

## Introduction

Inverse Kinematics determines the joint angles required to instruct robotic manipulators effectively to achieve a desired end-effector position and orientation. Current methods for solving Inverse Kinematics—geometric, numeric, and differential—each present severe trade-offs:

- **Geometric methods** offer closed-form solutions but become analytically intractable as degrees of freedom increase.
- **Numeric methods** are computationally expensive and iterative, making real-time guarantees difficult on low-power hardware.
- **Differential methods** are plagued by kinematic singularities and rely on Taylor Series linearizations.

These issues pose significant challenges for embedded robotics and IoT edge systems where power consumption, mass, and compute budget are constrained. In this project, I explore using machine learning with polynomial regression and dual quaternions to achieve high accuracy with minimal inference overhead.

---

## Forward Kinematics

Forward Kinematics solves for the position and orientation of a manipulator's end-effector given known joint angles. Because forward kinematics is deterministic and computationally well-behaved, we use dual quaternions to model forward kinematics and generate training data for our machine learning model.

### Quaternions

Quaternions represent 3D rotations without gimbal lock. Extending complex numbers, a quaternion is expressed as:

$$q = a + bi + cj + dk$$

where:
$$i^2 = j^2 = k^2 = ijk = -1, \quad a,b,c,d \in \mathbb{R}, \quad i,j,k \in \mathbb{I}$$

A quaternion can be broken down into a scalar component ($a$) and a vector component ($bi + cj + dk$).

### Quaternion Multiplication

Quaternion multiplication is non-commutative. Given two quaternions:

$$\begin{aligned}
q_1 &= a + bi + cj + dk \\
q_2 &= e + fi + gj + hk
\end{aligned}$$

Their Hamilton product $q_1 \otimes q_2$ is:

$$\begin{aligned}
q_1 \otimes q_2 &= (ae - bf - cg - dh) \\
&\quad + (af + be + ch - dg)\mathbf{i} \\
&\quad + (ag - bh + ce + df)\mathbf{j} \\
&\quad + (ah + bg - cf + de)\mathbf{k}
\end{aligned}$$

Chaining these multiplications models successive rotational joint transformations from the robot's base to the end-effector.

### Axis-Angle to Quaternion Conversion

We represent individual joint rotations as an axis unit vector $\mathbf{u} = u_x \mathbf{i} + u_y \mathbf{j} + u_z \mathbf{k}$ and rotation angle $\theta$. Using the exponential map:

$$\exp(\mathbf{v}) = \cos |\mathbf{v}| + \frac{\mathbf{v}}{|\mathbf{v}|} \sin |\mathbf{v}|, \quad \text{where } \mathbf{v} = \frac{\theta}{2} (u_x \mathbf{i} + u_y \mathbf{j} + u_z \mathbf{k})$$

### Dual Quaternions

Dual quaternions extend quaternions to represent rigid spatial transforms (both rotation and translation) in a single unified algebraic framework. Introducing the dual operator $\epsilon$ where $\epsilon^2 = 0$ ($\epsilon \neq 0$):

$$Q = r + \frac{\epsilon}{2} t \otimes r$$

where:
- $r$ is the unit rotation quaternion
- $t$ is the pure translation quaternion $(0, x, y, z)$

### Dual Quaternion Multiplication

Given two dual quaternions $Q_1 = p_1 + \epsilon q_1$ and $Q_2 = p_2 + \epsilon q_2$:

$$Q_{12} = (p_1 \otimes p_2) + \epsilon [(p_1 \otimes q_2) + (q_1 \otimes p_2)]$$

#### Advantages:
1. **Singularity-free**: Avoids representation singularities and gimbal lock entirely.
2. **Compact & Unified**: Rigid transformations (rotation + translation) are combined in an 8-parameter entity.
3. **Efficient Composition**: Successive coordinate transformations are chained simply through dual multiplication.

---

## Kinematic Chain Implementation

Using SymPy for symbolic formulation, we construct the kinematic chain for a 5-DOF bio-inspired manipulator modeled after a human leg:

```python
import sympy
import numpy as np

def dualq_mul(args):
    """Multiplies a sequence of dual quaternions represented as SymPy matrices [real, dual]."""
    if len(args) < 2:
        raise ValueError("Need at least 2 dual quaternions to multiply")

    result = args[0]
    for i in range(1, len(args)):
        dq_i = args[i]
        r1, d1 = result[0], result[1]
        r2, d2 = dq_i[0], dq_i[1]

        real_part = r1.mul(r2)
        dual_part = (r1.mul(d2)).add(d1.mul(r2))

        result = sympy.Matrix([real_part, dual_part])
        result = dualq_piecewise_expand(result)
        result = dualq_piecewise_trigsimp(result)
    return result
```

### Joint Definitions & Range of Motion

| Joint | Flexion / Dorsiflexion | Hyperextension / Plantarflexion | Abduction | Adduction | Int. Rot | Ext. Rot | Tx (mm) | Ty (mm) | Tz (mm) |
|---|---|---|---|---|---|---|---|---|---|
| Hip (3 DOF) | 100° | 30° | 40° | 20° | 40° | 50° | 0 | 0 | 0 |
| Knee (1 DOF) | 150° | 0° | 0° | 0° | 0° | 0° | 0 | 0 | -400 |
| Ankle (1 DOF)| 40° | 30° | 0° | 0° | 0° | 0° | 0 | 0 | -450 |
| End-Effector | — | — | — | — | — | — | 230 | 0 | 0 |

```python
joints = [
    [[0, 1, 0], [0, 0, 0],   False, (np.deg2rad(-100), np.deg2rad(30))],
    [[1, 0, 0], [0, 0, 0],   False, (np.deg2rad(-40),  np.deg2rad(20))],
    [[0, 0, 1], [0, 0, 0],   False, (np.deg2rad(-50),  np.deg2rad(40))],
    [[0, 1, 0], [0, 0, -400],False, (np.deg2rad(0),    np.deg2rad(150))],
    [[0, 1, 0], [0, 0, -450],False, (np.deg2rad(-30),  np.deg2rad(40))],
    [[0, 1, 0], [230, 0, 0], True,  ()]
]
```

We extract the end-effector position from the translation quaternion vector component and maintain the rotation quaternion to prevent attitude singularities:

```python
foot_pose_dqi = dualq_mul(dq_joints)
foot_position_FK = (2 * foot_pose_dqi[1].mul(foot_pose_dqi[0].conjugate())).to_Matrix(vector_only=True)
foot_orientation_FK = foot_pose_dqi[0].to_Matrix(vector_only=False)

foot_position_FK_lambda = sympy.lambdify(target_symbols, foot_position_FK, modules=['numpy'], cse=True)
foot_orientation_FK_lambda = sympy.lambdify(target_symbols, foot_orientation_FK, modules=['numpy'], cse=True)
```

---

## Data Generation via Latin Hypercube Sampling (LHS)

To ensure maximum state-space coverage across joint limit envelopes with minimum sample count, we use Latin Hypercube Sampling:

```python
from scipy.stats import qmc
import pandas as pd

n = 10000
sampler = qmc.LatinHypercube(d=len(param_ranges))
samples = sampler.random(n=n)

scaled_samples = np.zeros_like(samples)
for i, (param_name, (lower, upper)) in enumerate(param_ranges.items()):
    scaled_samples[:, i] = qmc.scale(samples[:, i].reshape(-1, 1), lower, upper).flatten()
```

Generating $N = 10,000$ points across the 5 joint limits yielded uniform joint-space exploration, mapped directly through the compiled FK lambdas to generate 3D positions ($x, y, z$) and 4D orientation quaternions ($w, x, y, z$).

---

## JAX-Accelerated Polynomial Regression

Rather than running deep neural networks that require heavy matrix runtimes, polynomial feature expansion paired with linear regression provides ultra-fast inference on edge microcontrollers.

### Feature Expansion

For degree $d=5$, the initial 7 input features expand to 792 combination terms. We implement this with Google JAX's Just-In-Time (`jit`) compilation:

```python
import jax.numpy as jnp
from jax import jit
from functools import partial
import itertools

class PolynomialTransformer:
    def __init__(self, degree=5, include_bias=True):
        self.degree = degree
        self.include_bias = include_bias

    @staticmethod
    @partial(jit, static_argnums=[1, 2])
    def _expand(Xp, include_bias, degree):
        n_samples, n_features = Xp.shape
        combinations = []
        for deg in range(1, degree + 1):
            combinations.extend(itertools.combinations_with_replacement(range(n_features), deg))
        
        n_output_features = len(combinations)
        X_new = jnp.empty((n_samples, n_output_features), dtype=jnp.float64)
        for i, index_combi in enumerate(combinations):
            X_new = X_new.at[:, i].set(jnp.prod(Xp[:, index_combi], axis=1, dtype=jnp.float64))
            
        if include_bias:
            X_new = jnp.c_[jnp.ones(n_samples), X_new]
        return X_new

    def transform(self, X):
        return self._expand(X, self.include_bias, self.degree)
```

Without JIT, computing combinations took $\sim 14$ seconds; with `jax.jit`, compilation dropped subsequent evaluations to under 0.8 seconds.

---

## Results & Cross-Validation

Evaluating the 5th-degree polynomial model across training and test splits:

| Metric | Training Set | Test Set | 5-Fold Cross Validation |
|---|---|---|---|
| **$R^2$ Score** | **0.999** | **0.999** | **0.999** ($\pm 0.000$) |
| **Mean Squared Error (MSE)** | 0.0001 | 0.0002 | — |
| **Mean Absolute Error (MAE)** | 0.010 rad | 0.012 rad | — |

Individual 5-fold $R^2$ scores: `[0.99849, 0.99848, 0.99851, 0.99861, 0.99844]`.

The model captured the severe non-linear kinematics with near-zero error without requiring iterative gradient descent at runtime.

---

## Edge Deployment & Next Steps

Because polynomial regression reduces to an explicit dot product:

$$\hat{\theta} = \mathbf{W} \cdot \Phi(x) + \mathbf{b}$$

Inference can be executed in pure C++ on low-cost ARM processors (like the Raspberry Pi 4/5) with zero dependencies. Future work will benchmark power consumption, memory footprint, and cycle counts against standard iterative solvers like Levenberg-Marquardt and KDL.