Week 03 | SKILL-020

Numerical IK: Newton-Raphson and Cyclic Coordinate Descent

Published: April 10, 2026 | Author: Smartotics Learning Journey | Reading Time: 12 min

The Jacobian pseudoinverse gets us started, but real-world IK solvers need more robust iterative strategies. Today we’ll look at two powerful approaches: Newton-Raphson (the gold standard for precision) and Cyclic Coordinate Descent (the go-to for real-time applications like character animation).

Newton-Raphson Method

Newton-Raphson is a root-finding method applied to our IK problem. We define an error function:

$$\mathbf{f}(\mathbf{q}) = \text{FK}(\mathbf{q}) - \mathbf{x}_{target} = \mathbf{0}$$

The update rule is:

$$\mathbf{q}_{k+1} = \mathbf{q}_k - \mathbf{J}^{-1}(\mathbf{q}_k) \cdot \mathbf{f}(\mathbf{q}_k)$$

This is essentially what we did yesterday with the pseudoinverse, but Newton-Raphson has important nuances:

Step Size Control

Pure Newton-Raphson can overshoot. A line search finds the optimal step size α:

$$\mathbf{q}_{k+1} = \mathbf{q}_k - \alpha \cdot \mathbf{J}^+ \cdot \mathbf{f}(\mathbf{q}_k)$$

Joint Limits

Real robots have physical limits. We can project the solution back into the feasible range after each update:

def clamp_joints(q, q_min, q_max):
    return np.clip(q, q_min, q_max)

Full Newton-Raphson IK Solver

import numpy as np

def forward_kinematics_nlink(q, lengths):
    """FK for an n-link planar arm. Returns (px, py)."""
    x, y = 0.0, 0.0
    angle_sum = 0.0
    for i in range(len(q)):
        angle_sum += q[i]
        x += lengths[i] * np.cos(angle_sum)
        y += lengths[i] * np.sin(angle_sum)
    return np.array([x, y])

def jacobian_nlink(q, lengths):
    """Geometric Jacobian for n-link planar arm."""
    n = len(q)
    J = np.zeros((2, n))
    
    for i in range(n):
        # z-axis is always [0, 0, 1] for planar
        # Origin of joint i
        ox, oy = 0.0, 0.0
        angle_sum = 0.0
        for j in range(i):
            angle_sum += q[j]
            ox += lengths[j] * np.cos(angle_sum)
            oy += lengths[j] * np.sin(angle_sum)
        
        # End-effector position
        ex, ey = 0.0, 0.0
        angle_sum_all = angle_sum
        for j in range(i, n):
            angle_sum_all += q[j]
            ex = ox + sum(lengths[k] * np.cos(sum(q[:k+1])) for k in range(i+1, n+1)) if False else 0
            # Simpler: compute full EE position
        ee = forward_kinematics_nlink(q, lengths)
        
        # Cross product: z × (ee - oi) = [-(ey-oy), (ex-ox)]
        J[0, i] = -(ee[1] - oy)
        J[1, i] =  (ee[0] - ox)
    
    return J

def ik_newton_raphson(target, q_init, lengths,
                      q_min=None, q_max=None,
                      max_iter=200, tol=1e-5, alpha=1.0):
    """
    Newton-Raphson IK with line search and joint limits.
    """
    n = len(q_init)
    q = np.array(q_init, dtype=float)
    
    if q_min is None:
        q_min = np.full(n, -np.pi)
    if q_max is None:
        q_max = np.full(n, np.pi)
    
    history = []
    
    for iteration in range(max_iter):
        ee = forward_kinematics_nlink(q, lengths)
        error = target - ee
        err_norm = np.linalg.norm(error)
        history.append(err_norm)
        
        if err_norm < tol:
            return q, True, history
        
        J = jacobian_nlink(q, lengths)
        
        # Damped pseudoinverse for stability
        damping = 0.01
        JJT = J @ J.T + damping * np.eye(2)
        dq = J.T @ np.linalg.solve(JJT, error)
        
        # Line search: try full step, halve if error increases
        step = alpha
        for _ in range(10):
            q_new = clamp_joints(q + step * dq, q_min, q_max)
            ee_new = forward_kinematics_nlink(q_new, lengths)
            if np.linalg.norm(target - ee_new) < err_norm:
                break
            step *= 0.5
        else:
            q_new = clamp_joints(q + step * dq, q_min, q_max)
        
        q = q_new
    
    return q, False, history

# Example: 4-link planar arm reaching (3, 2)
lengths = [1.5, 1.2, 1.0, 0.8]
q_init = [0.5, 0.3, -0.2, 0.1]
target = np.array([3.0, 2.0])

q_sol, converged, hist = ik_newton_raphson(target, q_init, lengths)
print(f"Converged: {converged} in {len(hist)} iterations")
print(f"Angles: {[f'{np.degrees(a):.1f}°' for a in q_sol]}")
print(f"EE: {forward_kinematics_nlink(q_sol, lengths)}")

Cyclic Coordinate Descent (CCD)

Newton-Raphson moves all joints simultaneously. CCD takes a different philosophy: optimize one joint at a time, cycling through them.

For each joint in sequence:

  1. Compute the angle that minimizes the distance to the target, holding all other joints fixed
  2. Apply that change
  3. Move to the next joint

This is incredibly simple and guaranteed to decrease (or maintain) the error at each step.

def ik_ccd(target, q_init, lengths, max_iter=100, tol=1e-4):
    """
    Cyclic Coordinate Descent IK solver.
    
    Iterates through each joint, rotating it to minimize
    the distance from end-effector to target.
    """
    n = len(q_init)
    q = np.array(q_init, dtype=float)
    history = []
    
    for iteration in range(max_iter):
        ee = forward_kinematics_nlink(q, lengths)
        err = np.linalg.norm(target - ee)
        history.append(err)
        
        if err < tol:
            return q, True, history
        
        # Cycle through each joint
        for i in range(n):
            # Compute position of joint i
            jx, jy = 0.0, 0.0
            angle_sum = 0.0
            for j in range(i):
                angle_sum += q[j]
                jx += lengths[j] * np.cos(angle_sum)
                jy += lengths[j] * np.sin(angle_sum)
            
            # Current end-effector
            ee = forward_kinematics_nlink(q, lengths)
            
            # Vectors from joint to target and joint to EE
            to_target = target - np.array([jx, jy])
            to_ee = ee - np.array([jx, jy])
            
            # Angle between these vectors
            angle_target = np.arctan2(to_target[1], to_target[0])
            angle_ee = np.arctan2(to_ee[1], to_ee[0])
            
            # Rotation needed
            delta = angle_target - angle_ee
            # Normalize to [-pi, pi]
            delta = (delta + np.pi) % (2 * np.pi) - np.pi
            
            q[i] += delta
    
    return q, False, history

# Compare methods
q_init = [0.5, 0.3, -0.2, 0.1]
lengths = [1.5, 1.2, 1.0, 0.8]
target = np.array([3.0, 2.0])

print("=== Newton-Raphson ===")
q_nr, conv_nr, h_nr = ik_newton_raphson(target, q_init, lengths)
print(f"Converged: {conv_nr}, Iterations: {len(h_nr)}, Final error: {h_nr[-1]:.6f}")

print("\n=== CCD ===")
q_ccd, conv_ccd, h_ccd = ik_ccd(target, q_init, lengths)
print(f"Converged: {conv_ccd}, Iterations: {len(h_ccd)}, Final error: {h_ccd[-1]:.6f}")

CCD vs Newton-Raphson

FeatureNewton-RaphsonCCD
SpeedFewer iterations, more computation per stepMore iterations, trivial per step
SingularitiesNeeds dampingNaturally handles them
Joint limitsNeeds explicit clampingEasy to add per-joint
Local minimaCan get stuckCan get stuck (different traps)
Best forIndustrial robots, precisionCharacter animation, games
ConvergenceQuadratic (near solution)Linear

FABRIK: A Modern Alternative

FABRIK (Forward And Backward Reaching Inverse Kinematics) is a popular method in computer graphics that works by iteratively adjusting joint positions rather than angles:

  1. Forward pass: Start from the end-effector, move toward target
  2. Backward pass: Start from the base, move back to anchor

FABRIK produces very natural-looking poses and is the default IK solver in many animation tools. It’s worth exploring if you’re interested in character animation or game development.

Key Takeaways

Next Steps

Tomorrow we’ll explore redundancy and optimization — what happens when your robot has more DOF than needed, and how to use that extra freedom to optimize for secondary objectives like avoiding obstacles or minimizing energy.