In previous lessons, inverse kinematics (IK) was formulated as: given a desired end-effector pose \( \mathbf{x}_d \) (in task space), find joint coordinates \( \mathbf{q} \in \mathbb{R}^n \) such that the forward kinematics \( \mathbf{x} = f(\mathbf{q}) \) satisfies \( f(\mathbf{q}) = \mathbf{x}_d \), or approximately \( f(\mathbf{q}) \approx \mathbf{x}_d \).

Robustness asks: How sensitive is the IK solution to perturbations? Typical perturbations include:

• Model errors in link lengths, offsets, or joint zero positions.
• Sensor noise in measured end-effector pose \( \mathbf{x}_d \).
• Finite-precision arithmetic, step-size issues, and ill-conditioned Jacobians.

Let the task-space error be \( \mathbf{e}(\mathbf{q}) = \mathbf{x}_d - f(\mathbf{q}) \in \mathbb{R}^m \). A common optimization viewpoint is:

\[ \min_{\mathbf{q} \in \mathbb{R}^n} \; \phi(\mathbf{q}) := \tfrac{1}{2} \left\| \mathbf{e}(\mathbf{q}) \right\|^2 = \tfrac{1}{2} \left\| \mathbf{x}_d - f(\mathbf{q}) \right\|^2 . \]

Numerical IK (e.g., Newton–Raphson or Jacobian pseudoinverse methods) iteratively updates \( \mathbf{q}_{k+1} = \mathbf{q}_k + \Delta\mathbf{q}_k \) to reduce \( \phi(\mathbf{q}_k) \). Robust algorithms must:

• Converge reliably for a range of initial guesses.
• Behave gracefully when exact solutions do not exist.
• Limit joint excursions and respect joint limits.
• Remain stable near poor conditioning of the Jacobian.

The Jacobian \( \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{m \times n} \), already introduced in numerical IK, linearly relates small joint and task variations:

\[ \delta \mathbf{x} \approx \mathbf{J}(\mathbf{q}) \, \delta \mathbf{q}. \]

When solving IK locally, we often invert this relation (in a least-squares sense) to obtain:

\[ \delta \mathbf{q} \approx \mathbf{J}(\mathbf{q})^{+} \, \delta \mathbf{x}, \]

where \( \mathbf{J}^{+} \) is the Moore–Penrose pseudoinverse. Sensitivity analysis uses the singular value decomposition (SVD) \( \mathbf{J} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top \) with singular values \( \sigma_1 \ge \dots \ge \sigma_r > 0 \).

The condition number of the Jacobian is defined as

\[ \kappa(\mathbf{J}) = \frac{\sigma_{\max}}{\sigma_{\min}} = \frac{\sigma_1}{\sigma_r} \ge 1, \]

and measures the amplification of relative errors. A standard inequality from matrix perturbation theory states that for small perturbations \( \delta \mathbf{x} \) and the corresponding IK step \( \delta \mathbf{q} \),

\[ \frac{\left\| \delta \mathbf{q} \right\|}{\left\| \mathbf{q} \right\|} \lesssim \kappa(\mathbf{J}) \frac{\left\| \delta \mathbf{x} \right\|}{\left\| \mathbf{x} \right\|}. \]

Thus, when \( \kappa(\mathbf{J}) \) is large (poorly conditioned), even small task-space perturbations can induce large joint motions. This is a key motivation for regularized or damped IK methods.

Classical Jacobian-based IK uses the least-squares subproblem

\[ \min_{\Delta \mathbf{q}} \; \left\| \mathbf{J}(\mathbf{q}_k)\,\Delta \mathbf{q} - \mathbf{e}_k \right\|^2, \quad \mathbf{e}_k = \mathbf{x}_d - f(\mathbf{q}_k), \]

whose minimum-norm solution is \( \Delta \mathbf{q} = \mathbf{J}(\mathbf{q}_k)^{+} \mathbf{e}_k \) when the pseudoinverse exists. Near poor conditioning, this step may be extremely large and numerically unstable.

The damped least-squares (DLS) IK augments the cost with a quadratic regularizer:

\[ \phi_{\lambda}(\Delta \mathbf{q}) = \left\| \mathbf{J} \Delta \mathbf{q} - \mathbf{e}_k \right\|^2 + \lambda^2 \left\| \Delta \mathbf{q} \right\|^2 , \quad \lambda > 0. \]

Derivation (normal equations). Expanding and differentiating,

\[ \begin{aligned} \phi_{\lambda}(\Delta \mathbf{q}) &= (\mathbf{J}\Delta \mathbf{q} - \mathbf{e}_k)^\top (\mathbf{J}\Delta \mathbf{q} - \mathbf{e}_k) + \lambda^2 \Delta \mathbf{q}^\top \Delta \mathbf{q} \\ &= \Delta \mathbf{q}^\top \mathbf{J}^\top \mathbf{J} \Delta \mathbf{q} - 2 \mathbf{e}_k^\top \mathbf{J} \Delta \mathbf{q} + \mathbf{e}_k^\top \mathbf{e}_k + \lambda^2 \Delta \mathbf{q}^\top \Delta \mathbf{q}. \end{aligned} \]

Taking the gradient with respect to \( \Delta \mathbf{q} \) and setting it to zero:

\[ \nabla_{\Delta \mathbf{q}} \phi_{\lambda} = 2 \mathbf{J}^\top \mathbf{J} \Delta \mathbf{q} - 2 \mathbf{J}^\top \mathbf{e}_k + 2 \lambda^2 \Delta \mathbf{q} = \mathbf{0}, \]

\[ (\mathbf{J}^\top \mathbf{J} + \lambda^2 \mathbf{I}) \Delta \mathbf{q} = \mathbf{J}^\top \mathbf{e}_k . \]

Since \( \mathbf{J}^\top \mathbf{J} + \lambda^2 \mathbf{I} \) is symmetric positive definite for any \( \lambda > 0 \), the solution exists and is unique:

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

In SVD coordinates, damping replaces each factor \( \sigma_i^{-1} \) by \( \frac{\sigma_i}{\sigma_i^2 + \lambda^2} \), drastically reducing the effect of tiny singular values and improving robustness.

The iterative DLS IK update becomes

\[ \mathbf{q}_{k+1} = \mathbf{q}_k + (\mathbf{J}(\mathbf{q}_k)^\top \mathbf{J}(\mathbf{q}_k) + \lambda^2 \mathbf{I})^{-1} \mathbf{J}(\mathbf{q}_k)^\top (\mathbf{x}_d - f(\mathbf{q}_k)). \]

Real manipulators have finite joint ranges \( q_i^{\min} \le q_i \le q_i^{\max} \). Naively applying DLS may propose steps that violate these limits. A common robustification is to incorporate a quadratic joint-limit cost:

\[ \psi(\mathbf{q}) = \frac{1}{2} \sum_{i=1}^n w_i \left( \frac{q_i - q_i^{\mathrm{mid}}}{q_i^{\mathrm{range}}} \right)^2, \quad q_i^{\mathrm{mid}} = \tfrac{1}{2}(q_i^{\min} + q_i^{\max}), \quad q_i^{\mathrm{range}} = q_i^{\max} - q_i^{\min}, \]

and minimize a combined cost

\[ \Phi(\mathbf{q}) = \phi(\mathbf{q}) + \mu \, \psi(\mathbf{q}), \quad \mu \ge 0. \]

For small steps, one can linearize \( \Phi \) around \( \mathbf{q}_k \) and obtain a modified DLS system

\[ \bigl(\mathbf{J}^\top \mathbf{J} + \lambda^2 \mathbf{I} + \mu \mathbf{H}_{\psi}\bigr) \Delta \mathbf{q} = \mathbf{J}^\top \mathbf{e}_k - \mu \, \nabla \psi(\mathbf{q}_k), \]

where \( \mathbf{H}_{\psi} \) is diagonal in this case. In practice, the following simpler heuristics are often used in software:

• Clip tentative updates so that \( q_i \) stays inside \( [q_i^{\min}, q_i^{\max}] \).
• Increase damping \( \lambda \) when near joint limits.
• Prefer initial guesses \( \mathbf{q}_0 \) near mid-range.

The following flow captures a robust DLS IK routine with joint limits and convergence checks. This structure will be implemented in code in the following lab sections.

flowchart TD
  A["Input: x_des, robot_model, q0"] --> B["k = 0"]
  B --> C["Compute x = f(q_k) via FK"]
  C --> D["Compute error e = x_des - x"]
  D --> E{"norm(e) <= eps_task ?"}
  E -->|yes| Z["Return q_k as IK solution"]
  E -->|no| F["Compute J(q_k) and DLS step delta_q"]
  F --> G["Step or joint limit violation ?"]
  G -->|yes| H["Clip/project delta_q and/or increase lambda"]
  G -->|no| I["q_{k+1} = q_k + delta_q"]
  H --> I
  I --> J{"k >= k_max ?"}
  J -->|yes| K["Report failure / best-so-far q_k"]
  J -->|no| C
        

Here eps_task is the task-space tolerance, k_max a maximum iteration count, and lambda the damping parameter, which can be adapted during iterations.

To make the IK solver reusable, we separate kinematic modeling, numerical solver, and experiment harness. The architecture below applies across Python, C++, Java, MATLAB, and Mathematica implementations.

flowchart TD
  M["RobotModel (link lengths, FK, J)"] --> S["IKSolver (DLS + limits)"]
  P["Experiment / Main Script"] --> M
  P --> S
  S --> O["Solution q_star, status, stats"]
  O --> L["Logging: errors, iterations"]
  L --> V["Visualization: end-effector path, joint trajectories"]
        

Each language-specific lab will implement RobotModel and IKSolver in its own idiom, while preserving the same mathematical behavior.

We consider a planar 2R manipulator previously used in FK examples, with link lengths \( L_1, L_2 > 0 \) and joint angles \( \mathbf{q} = (q_1, q_2)^\top \). FK and Jacobian are:

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

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} -L_1 \sin q_1 - L_2 \sin(q_1 + q_2) & -L_2 \sin(q_1 + q_2) \\ L_1 \cos q_1 + L_2 \cos(q_1 + q_2) & L_2 \cos(q_1 + q_2) \end{bmatrix}. \]

The Python code below implements DLS IK with step clipping and joint limits. We use numpy for linear algebra.

import numpy as np

# Robot model: 2R planar arm
class Planar2RModel:
    def __init__(self, L1, L2,
                 q_min=np.array([-np.pi, -np.pi]),
                 q_max=np.array([ np.pi,  np.pi])):
        self.L1 = L1
        self.L2 = L2
        self.q_min = q_min
        self.q_max = q_max

    def fk(self, q):
        q1, q2 = q
        x = self.L1 * np.cos(q1) + self.L2 * np.cos(q1 + q2)
        y = self.L1 * np.sin(q1) + self.L2 * np.sin(q1 + q2)
        return np.array([x, y])

    def jacobian(self, q):
        q1, q2 = q
        s1 = np.sin(q1)
        c1 = np.cos(q1)
        s12 = np.sin(q1 + q2)
        c12 = np.cos(q1 + q2)
        J = np.array([
            [-self.L1 * s1 - self.L2 * s12, -self.L2 * s12],
            [ self.L1 * c1 + self.L2 * c12,  self.L2 * c12],
        ])
        return J

    def clip_to_limits(self, q):
        return np.minimum(np.maximum(q, self.q_min), self.q_max)


def ik_dls(model, x_des, q0,
           lambda_init=1e-2,
           eps_task=1e-4,
           max_iters=100,
           step_max=0.2):
    """
    Damped least-squares IK with simple joint limits and step-size control.
    Returns (q_star, success, info_dict).
    """
    q = model.clip_to_limits(np.array(q0, dtype=float))
    lam = lambda_init
    history = []

    for k in range(max_iters):
        x = model.fk(q)
        e = x_des - x
        err = np.linalg.norm(e)
        history.append({"iter": k, "err": err, "q": q.copy(), "lambda": lam})

        if err <= eps_task:
            return q, True, {"history": history}

        J = model.jacobian(q)
        JTJ = J.T @ J
        H = JTJ + (lam ** 2) * np.eye(J.shape[1])
        g = J.T @ e

        # Solve H delta_q = g
        try:
            delta_q = np.linalg.solve(H, g)
        except np.linalg.LinAlgError:
            # Increase damping if numerical issues
            lam *= 10.0
            continue

        # Step-size control
        step_norm = np.linalg.norm(delta_q)
        if step_norm > step_max:
            delta_q *= step_max / step_norm

        q_new = model.clip_to_limits(q + delta_q)

        # If clipping was severe, increase damping
        if np.linalg.norm(q_new - (q + delta_q)) > 1e-6:
            lam *= 2.0
        else:
            # If progress is good, we can try reducing damping
            if err > 1e-6 and np.linalg.norm(model.fk(q_new) - x_des) < err:
                lam = max(lam * 0.7, 1e-4)

        q = q_new

    return q, False, {"history": history}


if __name__ == "__main__":
    # Example usage
    model = Planar2RModel(L1=0.5, L2=0.4)
    x_des = np.array([0.6, 0.3])

    # Add synthetic sensor noise to target
    noise = 0.005 * np.random.randn(2)
    x_noisy = x_des + noise

    q0 = np.array([0.0, 0.0])
    q_star, success, info = ik_dls(model, x_noisy, q0)

    print("Success:", success)
    print("q_star:", q_star)
    print("End-effector (noisy target):", x_noisy)
    print("End-effector (achieved):   ", model.fk(q_star))
      

You can examine info["history"] to plot convergence curves for \( \left\|\mathbf{e}_k\right\| \) and visualize robustness under varying noise and damping settings.

In C++, the Eigen library provides convenient dense linear algebra. The code below implements the same 2R DLS IK scheme.

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

struct Planar2RModel {
    double L1, L2;
    Eigen::Vector2d q_min;
    Eigen::Vector2d q_max;

    Planar2RModel(double L1_, double L2_)
        : L1(L1_), L2(L2_) {
        q_min = Eigen::Vector2d(-M_PI, -M_PI);
        q_max = Eigen::Vector2d( M_PI,  M_PI);
    }

    Eigen::Vector2d fk(const Eigen::Vector2d &q) const {
        double q1 = q(0), 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 Eigen::Vector2d(x, y);
    }

    Eigen::Matrix2d jacobian(const Eigen::Vector2d &q) const {
        double q1 = q(0), 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);

        Eigen::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;
    }

    Eigen::Vector2d clip(const Eigen::Vector2d &q) const {
        Eigen::Vector2d qc;
        for (int i = 0; i < 2; ++i) {
            qc(i) = std::min(std::max(q(i), q_min(i)), q_max(i));
        }
        return qc;
    }
};

struct IKResult {
    Eigen::Vector2d q_star;
    bool success;
    int iters;
};

IKResult ik_dls(const Planar2RModel &model,
                const Eigen::Vector2d &x_des,
                const Eigen::Vector2d &q0,
                double lambda_init = 1e-2,
                double eps_task = 1e-4,
                int max_iters = 100,
                double step_max = 0.2)
{
    Eigen::Vector2d q = model.clip(q0);
    double lambda = lambda_init;

    for (int k = 0; k < max_iters; ++k) {
        Eigen::Vector2d x = model.fk(q);
        Eigen::Vector2d e = x_des - x;
        double err = e.norm();

        if (err <= eps_task) {
            return {q, true, k};
        }

        Eigen::Matrix2d J = model.jacobian(q);
        Eigen::Matrix2d H = J.transpose() * J
                            + lambda * lambda * Eigen::Matrix2d::Identity();
        Eigen::Vector2d g = J.transpose() * e;

        Eigen::Vector2d delta_q = H.ldlt().solve(g);
        double step_norm = delta_q.norm();
        if (step_norm > step_max) {
            delta_q *= (step_max / step_norm);
        }

        Eigen::Vector2d q_new = model.clip(q + delta_q);
        if ((q_new - (q + delta_q)).norm() > 1e-6) {
            lambda *= 2.0;
        } else {
            Eigen::Vector2d x_new = model.fk(q_new);
            if (x_new - x_des).norm() < err {
                lambda = std::max(lambda * 0.7, 1e-4);
            }
        }
        q = q_new;
    }
    return {q, false, max_iters};
}

int main() {
    Planar2RModel model(0.5, 0.4);
    Eigen::Vector2d x_des(0.6, 0.3);

    // Noisy target
    Eigen::Vector2d noise;
    noise.setRandom();
    noise *= 0.005;
    Eigen::Vector2d x_noisy = x_des + noise;

    Eigen::Vector2d q0(0.0, 0.0);
    IKResult res = ik_dls(model, x_noisy, q0);

    std::cout << "Success: " << std::boolalpha << res.success << "\n";
    std::cout << "q_star: " << res.q_star.transpose() << "\n";
    std::cout << "achieved x: " << model.fk(res.q_star).transpose() << "\n";
    return 0;
}
      

You can easily extend this skeleton to generic \( n \)-DOF arms by switching to dynamic-size Eigen matrices and connecting to a generic FK/Jacobian routine from your earlier kinematics code.

Java has several linear algebra libraries (e.g., EJML, Apache Commons Math). For the 2R case, we can implement a small 2×2 solver directly to avoid dependencies while keeping the mathematics explicit.

public class Planar2RIKLab {

    public static class Vec2 {
        public double x, y;
        public Vec2(double x, double y) { this.x = x; this.y = y; }
        public Vec2 plus(Vec2 other) { return new Vec2(x + other.x, y + other.y); }
        public Vec2 minus(Vec2 other) { return new Vec2(x - other.x, y - other.y); }
        public Vec2 scale(double s) { return new Vec2(s * x, s * y); }
        public double norm() { return Math.sqrt(x * x + y * y); }
    }

    public static class Robot2R {
        public double L1, L2;
        public double[] qMin = {-Math.PI, -Math.PI};
        public double[] qMax = { Math.PI,  Math.PI};

        public Robot2R(double L1, double L2) {
            this.L1 = L1;
            this.L2 = L2;
        }

        public Vec2 fk(double[] q) {
            double q1 = q[0], 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 Vec2(x, y);
        }

        public double[][] jacobian(double[] q) {
            double q1 = q[0], 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;
        }

        public double[] clip(double[] q) {
            double[] out = new double[2];
            for (int i = 0; i < 2; ++i) {
                out[i] = Math.min(Math.max(q[i], qMin[i]), qMax[i]);
            }
            return out;
        }
    }

    // Solve (H) delta = g for 2x2 symmetric positive definite H
    public static double[] solve2x2(double[][] H, double[] g) {
        double a = H[0][0], b = H[0][1], c = H[1][1];
        double det = a * c - b * b;
        double inv00 =  c / det;
        double inv01 = -b / det;
        double inv11 =  a / det;
        double d0 = inv00 * g[0] + inv01 * g[1];
        double d1 = inv01 * g[0] + inv11 * g[1];
        return new double[]{d0, d1};
    }

    public static class IKResult {
        public double[] qStar;
        public boolean success;
        public int iters;
    }

    public static IKResult ikDLS(Robot2R robot,
                                 Vec2 xDes,
                                 double[] q0,
                                 double lambdaInit,
                                 double epsTask,
                                 int maxIters,
                                 double stepMax) {
        double[] q = robot.clip(q0);
        double lambda = lambdaInit;

        for (int k = 0; k < maxIters; ++k) {
            Vec2 x = robot.fk(q);
            Vec2 e = new Vec2(xDes.x - x.x, xDes.y - x.y);
            double err = e.norm();
            if (err <= epsTask) {
                IKResult res = new IKResult();
                res.qStar = q;
                res.success = true;
                res.iters = k;
                return res;
            }

            double[][] J = robot.jacobian(q);
            // H = J^T J + lambda^2 I
            double JTJ00 = J[0][0] * J[0][0] + J[1][0] * J[1][0];
            double JTJ01 = J[0][0] * J[0][1] + J[1][0] * J[1][1];
            double JTJ11 = J[0][1] * J[0][1] + J[1][1] * J[1][1];

            double[][] H = new double[2][2];
            H[0][0] = JTJ00 + lambda * lambda;
            H[0][1] = JTJ01;
            H[1][0] = JTJ01;
            H[1][1] = JTJ11 + lambda * lambda;

            // g = J^T e
            double[] g = new double[2];
            g[0] = J[0][0] * e.x + J[1][0] * e.y;
            g[1] = J[0][1] * e.x + J[1][1] * e.y;

            double[] delta = solve2x2(H, g);
            double stepNorm = Math.sqrt(delta[0] * delta[0] + delta[1] * delta[1]);
            if (stepNorm > stepMax) {
                double scale = stepMax / stepNorm;
                delta[0] *= scale;
                delta[1] *= scale;
            }

            double[] qNew = new double[2];
            qNew[0] = q[0] + delta[0];
            qNew[1] = q[1] + delta[1];
            qNew = robot.clip(qNew);

            // simple damping adaptation
            q = qNew;
        }

        IKResult res = new IKResult();
        res.qStar = q;
        res.success = false;
        res.iters = maxIters;
        return res;
    }

    public static void main(String[] args) {
        Robot2R robot = new Robot2R(0.5, 0.4);
        Vec2 xDes = new Vec2(0.6, 0.3);
        double[] q0 = {0.0, 0.0};

        IKResult res = ikDLS(robot, xDes, q0, 1e-2, 1e-4, 100, 0.2);
        System.out.println("Success: " + res.success);
        System.out.println("q_star: [" + res.qStar[0] + ", " + res.qStar[1] + "]");
        Vec2 xAch = robot.fk(res.qStar);
        System.out.println("achieved x: [" + xAch.x + ", " + xAch.y + "]");
    }
}
      

For larger manipulators, it is strongly recommended to switch to a dedicated linear algebra library and generalize the 2×2 solver to \( n \times n \).

MATLAB has strong matrix support and integrates with Simulink. We implement a function ikDLS2R and then wrap it in a MATLAB Function block in Simulink.

function [qStar, success, iters] = ikDLS2R(xDes, q0, L1, L2, varargin)
%IKDLS2R Damped least-squares IK for 2R planar arm.
%   xDes: [2x1] desired end-effector position
%   q0:   [2x1] initial guess
%   L1,L2: link lengths

p = inputParser;
addParameter(p, 'lambda', 1e-2);
addParameter(p, 'epsTask', 1e-4);
addParameter(p, 'maxIters', 100);
addParameter(p, 'stepMax', 0.2);
addParameter(p, 'qMin', [-pi; -pi]);
addParameter(p, 'qMax', [ pi;  pi]);
parse(p, varargin{:});
lambda   = p.Results.lambda;
epsTask  = p.Results.epsTask;
maxIters = p.Results.maxIters;
stepMax  = p.Results.stepMax;
qMin     = p.Results.qMin(:);
qMax     = p.Results.qMax(:);

q = min(max(q0(:), qMin), qMax);
success = false;

for k = 1:maxIters
    [x, J] = fkJac2R(q, L1, L2);
    e = xDes(:) - x;
    err = norm(e);

    if err <= epsTask
        success = true;
        break;
    end

    H = J.' * J + (lambda^2) * eye(2);
    g = J.' * e;
    delta_q = H \ g;

    stepNorm = norm(delta_q);
    if stepNorm > stepMax
        delta_q = delta_q * (stepMax / stepNorm);
    end

    qNew = q + delta_q;
    qNew = min(max(qNew, qMin), qMax);
    q = qNew;
end

qStar = q;
iters = k;
end

function [x, J] = fkJac2R(q, L1, L2)
q1 = q(1); q2 = q(2);
x = [L1*cos(q1) + L2*cos(q1+q2);
     L1*sin(q1) + L2*sin(q1+q2)];
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
      

Simulink integration: create a MATLAB Function block that calls ikDLS2R, with inputs xDes, q0 and outputs qStar, success. The 2R FK block (from earlier lessons) can then be driven by qStar to visualize end-effector trajectories and empirically test robustness to noisy references.

In Mathematica, the DLS step can be realized by directly minimizing the quadratic approximation of the cost. For the 2R arm, we can form a regularized objective and use FindMinimum:

(* Link lengths *)
L1 = 0.5; L2 = 0.4;

(* Desired position (possibly noisy) *)
xDes = {0.6, 0.3};

(* Forward kinematics *)
fk[{q1_, q2_}] := {
  L1 Cos[q1] + L2 Cos[q1 + q2],
  L1 Sin[q1] + L2 Sin[q1 + q2]
};

(* Cost function: task error + Tikhonov regularization on q *)
lambda = 0.05;
phi[q1_, q2_] := Module[{x = fk[{q1, q2}]},
  1/2 Norm[xDes - x]^2 + (lambda^2)/2 (q1^2 + q2^2)
];

(* Initial guess *)
q0 = {0.0, 0.0};

sol = FindMinimum[
  phi[q1, q2],
  { {q1, q0[[1]]}, {q2, q0[[2]]} },
  Method -> "QuasiNewton"
];

qStar = {q1, q2} /. Last[sol];
xAchieved = fk[qStar];
{qStar, xAchieved}
      

While this does not explicitly compute the DLS step, the optimization problem corresponds to a regularized IK objective. For more direct control, one can symbolically derive the Jacobian and implement the DLS update in a loop using Do or FixedPointList.

This lab-focused lesson analyzed the robustness of IK solutions to perturbations by studying the Jacobian and its singular values, and derived damped least-squares IK as a regularized extension of pseudoinverse-based methods. We incorporated joint-limit penalties and numerical safeguards, then implemented a robust IK loop for a 2R planar arm in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These ideas extend directly to higher-DOF manipulators and prepare the ground for a deeper study of singularities and workspace properties in later chapters.