Forward & Inverse Kinematics: Coordinate Transforms, DH Parameters & Jacobian Inverses

1. The Geometry of Rigid Bodies: Special Euclidean Group SE(3)

In classical robotics and physical AI, every link, sensor, and end-effector is treated as a rigid body inhabiting 3-dimensional Euclidean space. To position and orient these bodies relative to a reference coordinate frame, we utilize the Special Euclidean Group \(SE(3)\).

An element \(T \in SE(3)\) represents a combined rotation and translation, represented as a 4×4 homogeneous transformation matrix:

Given point coordinates \(p^B\) measured in coordinate frame \(B\), its coordinates in frame \(A\) are resolved via standard matrix-vector multiplication:

2. Denavit-Hartenberg (DH) Convention

When dealing with an \(n\)-link serial manipulator, attaching coordinate frames arbitrarily leads to redundant parameters. In 1955, Jacques Denavit and Richard Hartenberg formalized a minimal kinematic representation using only four geometric parameters per link:

The individual homogeneous transformation between consecutive link frames \(i-1\) and \(i\) is factored into four elementary spatial transformations:

3. Forward Kinematics (FK) Pipeline

Forward Kinematics resolves the spatial pose (position and orientation) of the robot's end-effector given the vector of joint variables \(q = [\theta_1, \theta_2, \dots, \theta_n]^T\). This is accomplished by cascading the individual transformation matrices along the kinematic chain:

Forward kinematics is computationally deterministic and has a unique closed-form solution for any serial kinematic chain.

4. The Geometric Jacobian Matrix

To control robot motions dynamically, we must relate joint velocities \(\dot{q}\) to the linear and angular velocities of the end-effector:

For a 6-DoF spatial manipulator, \(J(q)\) is a 6×\(n\) matrix composed of linear components \(J_{v,i}\) and rotational components \(J_{\omega,i}\). For a revolute joint \(i\) with axis of rotation \(z_{i-1}\) and origin \(p_{i-1}\):

5. Numerical Inverse Kinematics & Singularities

Inverse Kinematics (IK) solves the opposite, ill-posed problem: given a desired spatial pose \(x_{des} \in SE(3)\), find the joint configurations \(q\) that achieve it.

Unlike FK, IK may have zero solutions (outside reachable workspace), multiple solutions (e.g. elbow-up vs. elbow-down in a 6-axis industrial arm), or infinite solutions (kinematic redundancy).

The Singularity Problem

A kinematic singularity occurs when \(\det(J(q)) = 0\) (or when \(\text{rank}(J) < 6\)). At a singularity:

• The robot loses one or more degrees of freedom in Cartesian space; it cannot move along the singular direction.
• Inverting the Jacobian directly (\(J^{-1}\)) causes joint velocities \(\dot{q} = J^{-1} \dot{x}\) to diverge toward infinity, tripping motor overcurrent faults or damaging mechanical gearboxes.

Damped Least Squares (Levenberg-Marquardt) Solver

To maintain numerical stability near singular configurations, production robotics frameworks (such as Pinocchio, MoveIt, and Isaac Lab) replace standard pseudoinverses with Damped Least Squares (DLS):

6. Python Reference Implementation: Numerical Jacobian Solver

import numpy as np

def damped_least_squares_ik(current_q, target_pos, fk_function, jacobian_function, 
                            max_iters=50, tolerance=1e-4, damping=0.01):
    """
    Computes numerical Inverse Kinematics using Damped Least Squares (DLS).
    """
    q = np.copy(current_q)
    
    for i in range(max_iters):
        # Evaluate forward kinematics
        current_pos = fk_function(q)
        error = target_pos - current_pos
        
        # Check convergence
        if np.linalg.norm(error) < tolerance:
            return q, True, i
            
        # Compute geometric Jacobian J(q)
        J = jacobian_function(q)
        
        # DLS formulation: delta_q = J^T * (J * J^T + lambda^2 * I)^-1 * error
        JJt = J @ J.T
        damped_inv = np.linalg.inv(JJt + (damping ** 2) * np.eye(len(error)))
        delta_q = J.T @ (damped_inv @ error)
        
        # Joint update with step limiter
        step_norm = np.linalg.norm(delta_q)
        if step_norm > 0.2: # max 0.2 radians per iteration
            delta_q = delta_q * (0.2 / step_norm)
            
        q += delta_q
        
    return q, False, max_iters

7. Architectural Takeaways for Physical AI

Modern Vision-Language-Action (VLA) models and imitation learning policies (such as ACT and Diffusion Policy) frequently predict actions directly in End-Effector Cartesian Space (\(\Delta x, \Delta y, \Delta z, \Delta \text{yaw}\)) rather than raw joint space. This makes learned behaviors invariant to minor changes in base mounting or link proportions.

However, executing these Cartesian trajectories on physical hardware requires deterministic, high-frequency (100–500 Hz) IK solvers capable of handling boundary singularities and kinematic constraints without hesitation.