Let a manipulator have joint coordinates \( \mathbf{q} \in \mathbb{R}^n \) and a forward kinematics map (task-space output) \( \mathbf{x} = \mathbf{f}(\mathbf{q}) \in \mathbb{R}^m \), where typically \( m = 3 \) (position) or \( m = 6 \) (position and orientation). Given a desired task configuration \( \mathbf{x}_d \), inverse kinematics asks for \( \mathbf{q}^\star \) such that

\[ \mathbf{f}(\mathbf{q}^\star) = \mathbf{x}_d. \]

Define the task-space error (residual) as

\[ \mathbf{r}(\mathbf{q}) = \mathbf{f}(\mathbf{q}) - \mathbf{x}_d \in \mathbb{R}^m. \]

IK is equivalent to finding a root of the nonlinear system \( \mathbf{r}(\mathbf{q}) = \mathbf{0} \). For square, non-redundant manipulators (\( m = n \)), classical Newton–Raphson for systems of equations applies. For redundant manipulators \( n > m \), we obtain a nonlinear least-squares problem

\[ \min_{\mathbf{q} \in \mathbb{R}^n} \Phi(\mathbf{q}), \quad \Phi(\mathbf{q}) = \tfrac{1}{2}\,\|\mathbf{r}(\mathbf{q})\|_2^2. \]

Orientation can be incorporated using exponential coordinates from previous lessons. If the end-effector pose is \( \mathbf{g}(\mathbf{q}) \in SE(3) \) and the desired pose is \( \mathbf{g}_d \in SE(3) \), define the pose error transform

\[ \mathbf{g}_{\text{err}}(\mathbf{q}) = \mathbf{g}(\mathbf{q})^{-1} \mathbf{g}_d \in SE(3). \]

Using the matrix logarithm from Chapter 2, we map this to a twist in \( \mathfrak{se}(3) \):

\[ \hat{\boldsymbol{\xi}}(\mathbf{q}) = \log\!\big( \mathbf{g}_{\text{err}}(\mathbf{q}) \big) \in \mathfrak{se}(3), \quad \mathbf{r}(\mathbf{q}) = \boldsymbol{\xi}(\mathbf{q})^\vee \in \mathbb{R}^6, \]

where the \( (\cdot)^\vee \) operator collects the 6 twist coordinates. In this lesson we keep the discussion general in terms of \( \mathbf{r}(\mathbf{q}) \), regardless of the specific pose representation.

For a differentiable residual \( \mathbf{r}:\mathbb{R}^n \to \mathbb{R}^m \), define its Jacobian (a matrix-valued derivative) as

\[ \mathbf{J}_r(\mathbf{q}) = \frac{\partial \mathbf{r}(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{m \times n}. \]

Newton–Raphson for systems of equations constructs a sequence \( \mathbf{q}^{(k)} \) by linearizing \( \mathbf{r}(\mathbf{q}) \) around the current iterate \( \mathbf{q}^{(k)} \):

\[ \mathbf{r}(\mathbf{q}^{(k)} + \Delta \mathbf{q}) \approx \mathbf{r}(\mathbf{q}^{(k)}) + \mathbf{J}_r(\mathbf{q}^{(k)}) \Delta \mathbf{q}. \]

We want the next iterate to satisfy \( \mathbf{r}(\mathbf{q}^{(k+1)}) \approx \mathbf{0} \). Imposing \( \mathbf{r}(\mathbf{q}^{(k)}) + \mathbf{J}_r(\mathbf{q}^{(k)})\Delta\mathbf{q} = \mathbf{0} \) yields the Newton step:

\[ \mathbf{J}_r(\mathbf{q}^{(k)})\,\Delta \mathbf{q}^{(k)} = -\mathbf{r}(\mathbf{q}^{(k)}). \]

For a square, nonsingular Jacobian \( \mathbf{J}_r(\mathbf{q}^{(k)}) \in \mathbb{R}^{n \times n} \), the update is

\[ \Delta \mathbf{q}^{(k)} = -\mathbf{J}_r(\mathbf{q}^{(k)})^{-1}\mathbf{r}(\mathbf{q}^{(k)}), \quad \mathbf{q}^{(k+1)} = \mathbf{q}^{(k)} + \alpha^{(k)} \Delta \mathbf{q}^{(k)}, \]

where \( 0 < \alpha^{(k)} \le 1 \) is a step-size (also called a damping in configuration space or line search parameter).

For IK, \( \mathbf{r}(\mathbf{q}) = \mathbf{f}(\mathbf{q}) - \mathbf{x}_d \), so \( \mathbf{J}_r(\mathbf{q}) = \mathbf{J}(\mathbf{q}) \) is the task-space Jacobian of the forward kinematics. The Newton step reads

\[ \mathbf{J}(\mathbf{q}^{(k)})\,\Delta \mathbf{q}^{(k)} = \mathbf{x}_d - \mathbf{f}(\mathbf{q}^{(k)}), \quad \mathbf{q}^{(k+1)} = \mathbf{q}^{(k)} + \alpha^{(k)}\Delta \mathbf{q}^{(k)}. \]

Under standard regularity assumptions (non-singular Jacobian at the solution and a good initial guess), Newton iterations converge locally with quadratic rate for the square case. However, near kinematic singularities or with redundancy, the linear system becomes ill-conditioned or underdetermined, motivating least-squares and damped methods.

flowchart TD
  S["Choose initial q(0)"] --> L1["Loop over k = 0,1,..."]
  L1 --> FK["Compute task x = f(q(k)) via FK"]
  FK --> ERR["Compute error r = f(q(k)) - x_d"]
  ERR --> STOP{"Is ||r|| < tol or k > k_max?"}
  STOP -->|yes| DONE["Return q(k) as IK solution"]
  STOP -->|no| JAC["Compute Jacobian J(q(k))"]
  JAC --> SOLVE["Solve J * dq = -r (Newton or LS)"]
  SOLVE --> UPDATE["q(k+1) = q(k) + alpha * dq"]
  UPDATE --> L1
        

For redundant manipulators (\( n > m \)) or when the end-effector task is described with fewer than \( n \) constraints, the linearized Newton step becomes an underdetermined system

\[ \mathbf{J}(\mathbf{q}^{(k)})\,\Delta \mathbf{q}^{(k)} = -\mathbf{r}(\mathbf{q}^{(k)}), \quad \mathbf{J} \in \mathbb{R}^{m \times n}, \; m < n. \]

A natural choice is the minimum-norm solution that minimizes joint-space motion:

\[ \Delta \mathbf{q}^{(k)} = \arg\min_{\Delta \mathbf{q}} \tfrac{1}{2}\left\| \mathbf{J}(\mathbf{q}^{(k)}) \Delta \mathbf{q} + \mathbf{r}(\mathbf{q}^{(k)}) \right\|_2^2. \]

This is a linear least-squares problem in \( \Delta \mathbf{q} \). The associated normal equations are

\[ \mathbf{J}(\mathbf{q}^{(k)})^\top \mathbf{J}(\mathbf{q}^{(k)}) \Delta \mathbf{q}^{(k)} = -\mathbf{J}(\mathbf{q}^{(k)})^\top \mathbf{r}(\mathbf{q}^{(k)}). \]

If \( \mathbf{J}(\mathbf{q}^{(k)}) \) has full row rank (\( \operatorname{rank}(\mathbf{J}) = m \)), the unique minimum-norm solution is

\[ \Delta \mathbf{q}^{(k)} = -\mathbf{J}^\#(\mathbf{q}^{(k)}) \mathbf{r}(\mathbf{q}^{(k)}), \quad \mathbf{J}^\#(\mathbf{q}) = \mathbf{J}(\mathbf{q})^\top \big( \mathbf{J}(\mathbf{q}) \mathbf{J}(\mathbf{q})^\top \big)^{-1}, \]

where \( \mathbf{J}^\# \) is the (right) Moore–Penrose pseudo-inverse. The IK iteration becomes

\[ \mathbf{q}^{(k+1)} = \mathbf{q}^{(k)} - \alpha^{(k)} \mathbf{J}^\#(\mathbf{q}^{(k)}) \mathbf{r}(\mathbf{q}^{(k)}). \]

While pseudo-inverse IK gives elegant least-squares updates, near singularities \( \mathbf{J} \) becomes ill-conditioned and \( \mathbf{J}^\# \) can produce very large joint changes in response to modest task-space errors. This motivates a regularized formulation: damped least squares.

Damped least squares modifies the incremental least-squares problem by penalizing large joint increments. For a fixed iterate \( \mathbf{q}^{(k)} \), we solve

\[ \Delta \mathbf{q}^{(k)} = \arg\min_{\Delta \mathbf{q}} \tfrac{1}{2} \left\| \mathbf{J}(\mathbf{q}^{(k)}) \Delta \mathbf{q} + \mathbf{r}(\mathbf{q}^{(k)}) \right\|_2^2 + \tfrac{1}{2}\lambda^2 \left\|\Delta \mathbf{q}\right\|_2^2, \]

where \( \lambda > 0 \) is the damping factor. Taking the gradient with respect to \( \Delta \mathbf{q} \) and setting it to zero:

\[ \begin{aligned} \nabla_{\Delta \mathbf{q}} &\left[ \tfrac{1}{2} \left\| \mathbf{J} \Delta \mathbf{q} + \mathbf{r} \right\|_2^2 + \tfrac{1}{2}\lambda^2 \|\Delta \mathbf{q}\|_2^2 \right] = \mathbf{J}^\top (\mathbf{J}\Delta \mathbf{q} + \mathbf{r}) + \lambda^2 \Delta \mathbf{q} = \mathbf{0} \\ &\Longrightarrow \big(\mathbf{J}^\top \mathbf{J} + \lambda^2 \mathbf{I}\big) \Delta \mathbf{q} = -\mathbf{J}^\top \mathbf{r}. \end{aligned} \]

Thus the DLS step is

\[ \Delta \mathbf{q}^{(k)} = -\big( \mathbf{J}(\mathbf{q}^{(k)})^\top \mathbf{J}(\mathbf{q}^{(k)}) + \lambda^2 \mathbf{I} \big)^{-1} \mathbf{J}(\mathbf{q}^{(k)})^\top \mathbf{r}(\mathbf{q}^{(k)}). \]

Compared to the pseudo-inverse, the damping term \( \lambda^2 \mathbf{I} \) prevents extremely large joint velocities when some singular values of the Jacobian are small. In spectral terms, if \( \sigma_i \) are singular values of \( \mathbf{J} \), DLS scales each mode by

\[ \frac{\sigma_i}{\sigma_i^2 + \lambda^2}, \]

which becomes small when \( \sigma_i \ll \lambda \), effectively suppressing motion along ill-conditioned directions.

flowchart TD
  S["Given J, r at q(k)"] --> BR["Is robot far from singularity?"]
  BR -->|yes| UND["Use undamped step: dq = -J# * r"]
  BR -->|no| DLS["Use damped step: dq = -(J^T J + lambda^2 I)^(-1) J^T r"]
  UND --> UPD["Update q(k+1) = q(k) + alpha * dq"]
  DLS --> UPD
  UPD --> CK["Check error ||r|| and joint limits"]
        

Choosing \( \lambda \) is problem-dependent. Small values behave like pseudo-inverse IK (fast but numerically fragile), while larger values improve robustness at the cost of slower convergence and possible steady-state task-space tracking error.

Consider a 2-DOF planar manipulator with link lengths \( \ell_1, \ell_2 > 0 \) and joint variables \( \mathbf{q} = [q_1, q_2]^\top \). From Chapter 5, the end-effector position is

\[ \begin{aligned} x(\mathbf{q}) &= \ell_1 \cos(q_1) + \ell_2 \cos(q_1 + q_2), \\ y(\mathbf{q}) &= \ell_1 \sin(q_1) + \ell_2 \sin(q_1 + q_2). \end{aligned} \]

Thus \( \mathbf{f}(\mathbf{q}) = [x(\mathbf{q}), y(\mathbf{q})]^\top \) and the residual for a desired point \( \mathbf{x}_d = [x_d, y_d]^\top \) is

\[ \mathbf{r}(\mathbf{q}) = \begin{bmatrix} x(\mathbf{q}) - x_d \\ y(\mathbf{q}) - y_d \end{bmatrix}. \]

The Jacobian \( \mathbf{J}(\mathbf{q}) = \partial \mathbf{r} / \partial \mathbf{q} \) has entries

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} \dfrac{\partial x}{\partial q_1} & \dfrac{\partial x}{\partial q_2} \\ \dfrac{\partial y}{\partial q_1} & \dfrac{\partial y}{\partial q_2} \end{bmatrix} = \begin{bmatrix} -\ell_1 \sin(q_1) - \ell_2 \sin(q_1 + q_2) & -\ell_2 \sin(q_1 + q_2) \\ \ell_1 \cos(q_1) + \ell_2 \cos(q_1 + q_2) & \ell_2 \cos(q_1 + q_2) \end{bmatrix}. \]

For this non-redundant case (\( m = n = 2 \)), the Newton step solves \( \mathbf{J}(\mathbf{q}^{(k)})\Delta\mathbf{q}^{(k)} = -\mathbf{r}(\mathbf{q}^{(k)}) \). When the determinant of \( \mathbf{J} \) is small (elbow stretched), a DLS step provides numerical robustness.

A practical numerical IK solver must handle:

• Step-size control: use \( \alpha^{(k)} \in (0,1] \) and backtracking if the error norm increases.
• Stopping criteria: typically combine \( \|\mathbf{r}(\mathbf{q}^{(k)})\|_2 < \varepsilon_r \), \( \|\Delta\mathbf{q}^{(k)}\|_2 < \varepsilon_q \), and \( k \le k_{\max} \).
• Joint limits: after each update, project \( \mathbf{q}^{(k+1)} \) into allowed bounds, or add inequality constraints to the optimization formulation.
• Orientation errors: if poses are represented by rotation matrices, orientation error can be defined via the logarithm of \( \mathbf{R}(\mathbf{q})^\top \mathbf{R}_d \) and concatenated with position error in \( \mathbf{r}(\mathbf{q}) \).
• Multiple solutions: different initial guesses \( \mathbf{q}^{(0)} \) can converge to different IK branches.

These algorithmic details matter as much as the underlying mathematics in real manipulators where noise, model errors, and actuator limits are present.

We implement Newton and DLS IK for the 2R planar arm using NumPy. Although more general toolboxes (e.g., modern_robotics) exist, this minimal example exposes all numerical details.

import numpy as np

def fk_2r(q, l1, l2):
    """
    Forward kinematics for 2R planar arm.
    q: array-like of size 2 [q1, q2]
    returns: np.array([x, y])
    """
    q1, q2 = q
    x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
    y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
    return np.array([x, y])


def jacobian_2r(q, l1, l2):
    """
    Analytical Jacobian of 2R planar arm.
    J is 2x2.
    """
    q1, q2 = q
    s1 = np.sin(q1)
    c1 = np.cos(q1)
    s12 = np.sin(q1 + q2)
    c12 = np.cos(q1 + q2)

    J = np.zeros((2, 2))
    J[0, 0] = -l1 * s1 - l2 * s12
    J[0, 1] = -l2 * s12
    J[1, 0] =  l1 * c1 + l2 * c12
    J[1, 1] =  l2 * c12
    return J


def ik_newton_2r(x_d, l1, l2, q0, tol=1e-6, max_iter=50, alpha=1.0):
    """
    Newton (square) IK for 2R planar arm.
    x_d: desired [x_d, y_d]
    q0: initial guess [q1, q2]
    Returns (q, success, iters)
    """
    q = np.array(q0, dtype=float)
    x_d = np.array(x_d, dtype=float)

    for k in range(max_iter):
        x = fk_2r(q, l1, l2)
        r = x - x_d
        err = np.linalg.norm(r)
        if err < tol:
            return q, True, k

        J = jacobian_2r(q, l1, l2)
        # Solve J * dq = -r
        try:
            dq = np.linalg.solve(J, -r)
        except np.linalg.LinAlgError:
            # singular Jacobian
            return q, False, k

        q = q + alpha * dq

    return q, False, max_iter


def ik_dls_2r(x_d, l1, l2, q0, tol=1e-6, max_iter=50, alpha=1.0, lam=1e-2):
    """
    Damped least-squares IK for 2R planar arm.
    Uses step dq = -(J^T J + lam^2 I)^(-1) J^T r
    """
    q = np.array(q0, dtype=float)
    x_d = np.array(x_d, dtype=float)

    for k in range(max_iter):
        x = fk_2r(q, l1, l2)
        r = x - x_d
        err = np.linalg.norm(r)
        if err < tol:
            return q, True, k

        J = jacobian_2r(q, l1, l2)
        JTJ = J.T @ J
        n = JTJ.shape[0]
        A = JTJ + (lam ** 2) * np.eye(n)
        g = J.T @ r
        dq = -np.linalg.solve(A, g)

        q = q + alpha * dq

    return q, False, max_iter


if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    x_d = np.array([1.2, 0.5])
    q0 = np.array([0.0, 0.0])

    q_newton, ok_n, it_n = ik_newton_2r(x_d, l1, l2, q0)
    print("Newton IK:", q_newton, "success:", ok_n, "iters:", it_n)

    q_dls, ok_d, it_d = ik_dls_2r(x_d, l1, l2, q0, lam=0.05)
    print("DLS IK:", q_dls, "success:", ok_d, "iters:", it_d)
      

For general n-DOF robots, one would replace fk_2r and jacobian_2r with functions based on the PoE formulation or DH parameters from Chapter 5, and extend J to \( m \times n \).

The following C++ snippet uses the Eigen library for matrix computations. It demonstrates Newton and DLS IK for the same 2R example.

#include <iostream>
#include <Eigen/Dense>

using Eigen::Vector2d;
using Eigen::Matrix2d;

Vector2d fk2R(const Vector2d& q, double l1, double l2) {
    double q1 = q(0);
    double q2 = q(1);
    double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
    return Vector2d(x, y);
}

Matrix2d jacobian2R(const Vector2d& q, double l1, double l2) {
    double q1 = q(0);
    double q2 = q(1);
    double s1  = std::sin(q1);
    double c1  = std::cos(q1);
    double s12 = std::sin(q1 + q2);
    double c12 = std::cos(q1 + q2);

    Matrix2d J;
    J(0,0) = -l1 * s1 - l2 * s12;
    J(0,1) = -l2 * s12;
    J(1,0) =  l1 * c1 + l2 * c12;
    J(1,1) =  l2 * c12;
    return J;
}

bool ikNewton2R(const Vector2d& xd,
                double l1, double l2,
                Vector2d& q,
                double tol = 1e-6,
                int maxIter = 50,
                double alpha = 1.0) {
    for (int k = 0; k < maxIter; ++k) {
        Vector2d x = fk2R(q, l1, l2);
        Vector2d r = x - xd;
        double err = r.norm();
        if (err < tol) {
            return true;
        }
        Matrix2d J = jacobian2R(q, l1, l2);
        Eigen::FullPivLU<Matrix2d> lu(J);
        if (!lu.isInvertible()) {
            return false;  // near singular
        }
        Vector2d dq = lu.solve(-r);
        q += alpha * dq;
    }
    return false;
}

bool ikDLS2R(const Vector2d& xd,
             double l1, double l2,
             Vector2d& q,
             double tol = 1e-6,
             int maxIter = 50,
             double alpha = 1.0,
             double lambda = 1e-2) {
    for (int k = 0; k < maxIter; ++k) {
        Vector2d x = fk2R(q, l1, l2);
        Vector2d r = x - xd;
        double err = r.norm();
        if (err < tol) {
            return true;
        }
        Matrix2d J   = jacobian2R(q, l1, l2);
        Matrix2d JTJ = J.transpose() * J;
        Matrix2d A   = JTJ + (lambda * lambda) * Matrix2d::Identity();
        Vector2d g   = J.transpose() * r;
        Vector2d dq  = -A.ldlt().solve(g);
        q += alpha * dq;
    }
    return false;
}

int main() {
    double l1 = 1.0, l2 = 0.8;
    Vector2d xd(1.2, 0.5);
    Vector2d q0(0.0, 0.0);
    Vector2d qN = q0;
    Vector2d qD = q0;

    bool okN = ikNewton2R(xd, l1, l2, qN);
    bool okD = ikDLS2R(xd, l1, l2, qD, 1e-6, 50, 1.0, 0.05);

    std::cout << "Newton IK success: " << okN << ", q = "
              << qN.transpose() << std::endl;
    std::cout << "DLS   IK success: " << okD << ", q = "
              << qD.transpose() << std::endl;
    return 0;
}
      

For higher-DOF arms, one would switch from Matrix2d to general Eigen::MatrixXd and solve the corresponding linear systems using QR, SVD, or LDLT factorizations.

In Java, one can rely on libraries such as EJML, Apache Commons Math, or implement small fixed-size solvers by hand for low-DOF examples. Below is a compact implementation for 2R using a custom 2x2 solver.

public class IK2R {

    static double[] fk2R(double[] q, double l1, double l2) {
        double q1 = q[0];
        double q2 = q[1];
        double x = l1 * Math.cos(q1) + l2 * Math.cos(q1 + q2);
        double y = l1 * Math.sin(q1) + l2 * Math.sin(q1 + q2);
        return new double[] { x, y };
    }

    static double[][] jacobian2R(double[] q, double l1, double l2) {
        double q1 = q[0];
        double q2 = q[1];
        double s1  = Math.sin(q1);
        double c1  = Math.cos(q1);
        double s12 = Math.sin(q1 + q2);
        double c12 = Math.cos(q1 + q2);

        double[][] J = new double[2][2];
        J[0][0] = -l1 * s1 - l2 * s12;
        J[0][1] = -l2 * s12;
        J[1][0] =  l1 * c1 + l2 * c12;
        J[1][1] =  l2 * c12;
        return J;
    }

    // Solve 2x2 system A x = b
    static double[] solve2x2(double[][] A, double[] b) {
        double a11 = A[0][0], a12 = A[0][1];
        double a21 = A[1][0], a22 = A[1][1];
        double det = a11 * a22 - a12 * a21;
        if (Math.abs(det) < 1e-12) {
            throw new RuntimeException("Singular matrix");
        }
        double inv11 =  a22 / det;
        double inv12 = -a12 / det;
        double inv21 = -a21 / det;
        double inv22 =  a11 / det;
        double x0 = inv11 * b[0] + inv12 * b[1];
        double x1 = inv21 * b[0] + inv22 * b[1];
        return new double[] { x0, x1 };
    }

    static boolean ikNewton2R(double[] xd, double l1, double l2,
                              double[] q, double tol, int maxIter, double alpha) {
        for (int k = 0; k < maxIter; ++k) {
            double[] x = fk2R(q, l1, l2);
            double rx = x[0] - xd[0];
            double ry = x[1] - xd[1];
            double err = Math.sqrt(rx * rx + ry * ry);
            if (err < tol) {
                return true;
            }
            double[][] J = jacobian2R(q, l1, l2);
            double[] minusR = new double[] { -rx, -ry };
            double[] dq = solve2x2(J, minusR);
            q[0] += alpha * dq[0];
            q[1] += alpha * dq[1];
        }
        return false;
    }

    public static void main(String[] args) {
        double l1 = 1.0, l2 = 0.8;
        double[] xd = { 1.2, 0.5 };
        double[] q0 = { 0.0, 0.0 };
        double[] q = q0.clone();
        boolean ok = ikNewton2R(xd, l1, l2, q, 1e-6, 50, 1.0);
        System.out.println("IK success: " + ok +
                           " q = [" + q[0] + ", " + q[1] + "]");
    }
}
      

For higher-DOF robots, numerical linear algebra libraries (EJML, jblas, etc.) become essential for robust pseudo-inverse and DLS implementations.

10.1 MATLAB / Simulink

In MATLAB, the 2R Newton and DLS iterations can be coded as functions. These functions can be embedded into a Simulink diagram using a MATLAB Function block for real-time IK computations.

function [q, success, iters] = ik_newton_2r(xd, l1, l2, q0, tol, maxIter, alpha)
%IK_NEWTON_2R Newton IK for 2R planar manipulator.
%   xd       : desired [x_d; y_d]
%   l1, l2   : link lengths
%   q0       : initial guess [q1; q2]
%   tol      : tolerance on position error norm
%   maxIter  : maximum iterations
%   alpha    : step-size

if nargin < 5, tol = 1e-6; end
if nargin < 6, maxIter = 50; end
if nargin < 7, alpha = 1.0; end

q = q0(:);
xd = xd(:);
success = false;

for k = 1:maxIter
    x = fk_2r(q, l1, l2);
    r = x - xd;
    err = norm(r);
    if err < tol
        success = true;
        iters = k - 1;
        return;
    end
    J = jacobian_2r(q, l1, l2);
    dq = -J \ r;   % solve J*dq = -r
    q = q + alpha * dq;
end

iters = maxIter;
end


function [q, success, iters] = ik_dls_2r(xd, l1, l2, q0, tol, maxIter, alpha, lambda)
%IK_DLS_2R Damped least-squares IK for 2R planar manipulator.

if nargin < 5, tol = 1e-6; end
if nargin < 6, maxIter = 50; end
if nargin < 7, alpha = 1.0; end
if nargin < 8, lambda = 1e-2; end

q = q0(:);
xd = xd(:);
success = false;

for k = 1:maxIter
    x = fk_2r(q, l1, l2);
    r = x - xd;
    err = norm(r);
    if err < tol
        success = true;
        iters = k - 1;
        return;
    end
    J = jacobian_2r(q, l1, l2);
    JTJ = J.' * J;
    A = JTJ + (lambda^2) * eye(2);
    g = J.' * r;
    dq = -A \ g;
    q = q + alpha * dq;
end

iters = maxIter;
end


function x = fk_2r(q, l1, l2)
q1 = q(1); q2 = q(2);
x = [ l1*cos(q1) + l2*cos(q1+q2);
      l1*sin(q1) + l2*sin(q1+q2) ];
end


function J = jacobian_2r(q, l1, l2)
q1 = q(1); q2 = q(2);
s1  = sin(q1);     c1  = cos(q1);
s12 = sin(q1+q2);  c12 = cos(q1+q2);

J = [ -l1*s1 - l2*s12,  -l2*s12;
       l1*c1 + l2*c12,   l2*c12 ];
end
      

In Simulink, one could wrap ik_dls_2r in a MATLAB Function block and feed the desired task-space trajectory xd(t) together with current joint states q(t) from the plant model to compute joint references.

10.2 Wolfram Mathematica

Mathematica offers high-level root-finding functions such as FindRoot, but we can also implement explicit Newton or DLS iterations to maintain control over IK behavior.

(* 2R forward kinematics *)
fk2R[q_List, l1_, l2_] := Module[{q1, q2},
  {q1, q2} = q;
  {
    l1 Cos[q1] + l2 Cos[q1 + q2],
    l1 Sin[q1] + l2 Sin[q1 + q2]
  }
];

(* Jacobian of 2R, symbolic differentiation *)
J2R[q_List, l1_, l2_] := Module[{q1, q2, x, y},
  {q1, q2} = q;
  x = l1 Cos[q1] + l2 Cos[q1 + q2];
  y = l1 Sin[q1] + l2 Sin[q1 + q2];
  D[{x, y}, {{q1, q2}}]  (* 2x2 Jacobian matrix *)
];

ikNewton2R[xd_List, l1_, l2_, q0_List,
  tol_: 10^-6, maxIter_: 50, alpha_: 1.0] := Module[
  {q = N[q0], k, x, r, err, J, dq},
  For[k = 0, k < maxIter, k++,
    x = fk2R[q, l1, l2];
    r = x - xd;
    err = Norm[r];
    If[err < tol, Return[{q, True, k}]];
    J = N[J2R[q, l1, l2]];
    dq = LinearSolve[J, -r];
    q = q + alpha dq;
  ];
  {q, False, maxIter}
];

(* Example usage *)
l1 = 1.0; l2 = 0.8;
xd = {1.2, 0.5};
q0 = {0.0, 0.0};
{qsol, success, iters} = ikNewton2R[xd, l1, l2, q0]
      

For more complex arms modeled via PoE or DH parameters, Mathematica can symbolically differentiate the forward kinematics and then generate efficient numeric code for runtime implementation of Newton or DLS IK.

In this lesson we formulated inverse kinematics as a nonlinear root-finding and least-squares problem in joint space. Starting from the residual \( \mathbf{r}(\mathbf{q}) = \mathbf{f}(\mathbf{q}) - \mathbf{x}_d \), we derived the Newton–Raphson update using the Jacobian of the forward kinematics, and extended it to least-squares IK via the pseudo-inverse for redundant manipulators. We then introduced damped least squares, obtained its normal equations, and analyzed its robustness near kinematic singularities through regularization of small singular values. A detailed 2R planar example and multi-language implementations (Python, C++, Java, MATLAB/Simulink, Mathematica) illustrated how these concepts are implemented in practice. These numerical IK techniques form the backbone of many modern robot controllers and motion planning pipelines, and naturally lead into the more refined differential kinematics and Jacobian analysis of the next chapter.