Let \( f : \mathbb{R}^n \to \mathbb{R}^m \) be the kinematic mapping from joint coordinates \( \mathbf{q} \in \mathbb{R}^n \) to some task-space representation \( \mathbf{x} = f(\mathbf{q}) \in \mathbb{R}^m \). The associated analytic Jacobian is

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

In practice, \( \mathbf{J}(\mathbf{q}) \) is implemented using PoE formulas or DH-based recursions. Mistakes in this implementation are subtle yet catastrophic: they break velocity mapping, inverse kinematics, manipulability computations, and dynamic models (which all depend on the Jacobian).

Numerical Jacobian checking constructs a finite-difference Jacobian \( \mathbf{J}^{\text{num}}(\mathbf{q};h) \) using only forward kinematics, and compares it to the analytic Jacobian implementation \( \mathbf{J}^{\text{ana}}(\mathbf{q}) \). A typical error metric is

\[ E(\mathbf{q};h) = \frac{\left\| \mathbf{J}^{\text{num}}(\mathbf{q};h) - \mathbf{J}^{\text{ana}}(\mathbf{q}) \right\|_F} {\left\| \mathbf{J}^{\text{ana}}(\mathbf{q}) \right\|_F + \varepsilon}, \]

where \( \|\cdot\|_F \) is the Frobenius norm and \( \varepsilon \) is a small regularizing constant (e.g. \( \varepsilon = 10^{-8} \)).

flowchart TD
  A["Analytic Jacobian code J_ana(q)"] --> B["Select test configurations q_k"]
  B --> C["For each q_k: build numeric J_num(q_k; h) using finite differences"]
  C --> D["Compute error E(q_k; h) via Frobenius norm"]
  D --> E{"Is E(q_k; h) < tolerance ?"}
  E -->|yes| F["Mark configuration as passed"]
  E -->|no| G["Report mismatch, inspect columns \nassociated with suspect joints"]
  F --> H["Aggregate statistics over all q_k (max, mean, histograms)"]
  G --> H
        

Our goal in this lesson is to (i) derive the finite-difference scheme rigorously, (ii) understand how to choose the stepsize \( h \), and (iii) implement reusable checking routines that wrap around any forward-kinematics function available from previous chapters.

Consider a scalar differentiable function \( \phi : \mathbb{R} \to \mathbb{R} \). The derivative at \( x \) is

\[ \phi'(x) = \lim_{h \to 0} \frac{\phi(x+h) - \phi(x)}{h}. \]

In floating-point arithmetic, we must use a finite stepsize \( h \neq 0 \). Common approximations are:

\[ \begin{aligned} \text{Forward difference:} \quad D^{\text{fwd}}_h\phi(x) &= \frac{\phi(x+h) - \phi(x)}{h}, \\ \text{Backward difference:} \quad D^{\text{bwd}}_h\phi(x) &= \frac{\phi(x) - \phi(x-h)}{h}, \\ \text{Central difference:} \quad D^{\text{cen}}_h\phi(x) &= \frac{\phi(x+h) - \phi(x-h)}{2h}. \end{aligned} \]

Using Taylor expansions around \( x \):

\[ \begin{aligned} \phi(x+h) &= \phi(x) + h\phi'(x) + \frac{h^2}{2}\phi''(x) + \frac{h^3}{6}\phi^{(3)}(x) + \mathcal{O}(h^4), \\ \phi(x-h) &= \phi(x) - h\phi'(x) + \frac{h^2}{2}\phi''(x) - \frac{h^3}{6}\phi^{(3)}(x) + \mathcal{O}(h^4), \end{aligned} \]

we obtain truncation error orders:

\[ \begin{aligned} D^{\text{fwd}}_h\phi(x) &= \phi'(x) + \frac{h}{2}\phi''(x) + \mathcal{O}(h^2) \quad \Rightarrow \quad \text{error } \mathcal{O}(h), \\ D^{\text{cen}}_h\phi(x) &= \phi'(x) + \frac{h^2}{6}\phi^{(3)}(x) + \mathcal{O}(h^4) \quad \Rightarrow \quad \text{error } \mathcal{O}(h^2). \end{aligned} \]

Central differences are therefore preferred for Jacobian checking, because the truncation error decays quadratically in \( h \). However, in finite precision arithmetic, subtractive cancellation makes \( h \) too small problematic: the difference \( \phi(x+h) - \phi(x-h) \) is dominated by rounding errors.

A standard model for the total error combines truncation and roundoff:

\[ \text{err}(h) \approx C_1 h^2 + C_2 \frac{\varepsilon}{h}, \]

where \( \varepsilon \) is machine precision and \( C_1, C_2 \) are problem-dependent constants. Minimizing this with respect to \( h \) yields an optimal stepsize scale

\[ h_{\ast} \propto \varepsilon^{1/3}, \]

which in double precision is typically around \( h_{\ast} \approx 10^{-5} \) to \( 10^{-4} \) in joint units (radians or meters). In robotics, we usually choose \( h \) in this range and verify robustness by sweeping over several values.

For a general vector-valued mapping \( f : \mathbb{R}^n \to \mathbb{R}^m \), write \( \mathbf{x} = f(\mathbf{q}) \). The analytic Jacobian is

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} \frac{\partial f_1}{\partial q_1}(\mathbf{q}) & \cdots & \frac{\partial f_1}{\partial q_n}(\mathbf{q}) \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial q_1}(\mathbf{q}) & \cdots & \frac{\partial f_m}{\partial q_n}(\mathbf{q}) \end{bmatrix}. \]

Let \( \mathbf{e}_i \) be the i-th standard basis vector in \( \mathbb{R}^n \). The central-difference numerical Jacobian constructs the i-th column as

\[ \mathbf{J}^{\text{num}}_{(:,i)}(\mathbf{q};h) = \frac{ f(\mathbf{q} + h \mathbf{e}_i) - f(\mathbf{q} - h \mathbf{e}_i) }{2h}, \quad i = 1,\dots,n. \]

This is component-wise application of the scalar central difference to each entry of \( f \). Under smoothness assumptions, the truncation error of each column remains \( \mathcal{O}(h^2) \).

In manipulator kinematics, \( f \) is typically:

• Position-only task: \( m = 3 \), \( f(\mathbf{q}) = \mathbf{p}(\mathbf{q}) \in \mathbb{R}^3 \).
• Full pose task: \( m = 6 \), stacked position and orientation parameters (e.g. XYZ position and roll–pitch–yaw, or exponential coordinates of rotation).

Because orientation representations can be singular (e.g. Euler angles), it is often preferable to work directly in SE(3) using the matrix logarithm:

\[ T(\mathbf{q}) \in \mathrm{SE}(3), \quad T(\mathbf{q}) = \begin{bmatrix} R(\mathbf{q}) & \mathbf{p}(\mathbf{q}) \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

The incremental motion from \( \mathbf{q} \) to \( \mathbf{q} + h \mathbf{e}_i \) is encoded by

\[ \Delta T_i(\mathbf{q};h) = T(\mathbf{q})^{-1} T(\mathbf{q} + h \mathbf{e}_i) \in \mathrm{SE}(3), \]

and its body twist representation via the matrix logarithm:

\[ \boldsymbol{\xi}_i^{\text{num}}(\mathbf{q};h) = \frac{1}{h} \, \operatorname{vee} \big( \log\big( \Delta T_i(\mathbf{q};h) \big) \big) \in \mathbb{R}^6, \]

where \( \log : \mathrm{SE}(3) \to \mathfrak{se}(3) \) is the matrix logarithm (covered in Chapter 2), and \( \operatorname{vee} : \mathfrak{se}(3) \to \mathbb{R}^6 \) converts the twist matrix to a 6-vector (angular and linear components). Stacking these columns yields a numerical body Jacobian \( \mathbf{J}_b^{\text{num}}(\mathbf{q};h) \in \mathbb{R}^{6 \times n} \).

The same construction can be applied for the spatial Jacobian by left-multiplying with \( T(\mathbf{q}) \) where appropriate, using the adjoint representation (introduced in earlier chapters).

Given an analytic Jacobian \( \mathbf{J}^{\text{ana}} \) and a numeric Jacobian \( \mathbf{J}^{\text{num}}(\cdot;h) \), we define:

\[ \begin{aligned} \Delta \mathbf{J}(\mathbf{q};h) &= \mathbf{J}^{\text{num}}(\mathbf{q};h) - \mathbf{J}^{\text{ana}}(\mathbf{q}), \\ E_{\text{rel}}(\mathbf{q};h) &= \frac{\|\Delta \mathbf{J}(\mathbf{q};h)\|_F} {\|\mathbf{J}^{\text{ana}}(\mathbf{q})\|_F + \varepsilon}. \end{aligned} \]

Typical tolerances are in the range \( 10^{-6} \) to \( 10^{-3} \), depending on the scale of the task-space variables (meters vs. millimeters) and the quality of the forward kinematics implementation. It is important to:

• Check multiple configurations spread across the workspace.
• Include configurations near singularities to reveal sensitivity, but interpret errors carefully there.
• Inspect errors per column to isolate mistakes associated with a specific joint.

For more detailed diagnostics, one may compute column-wise errors \( \|\Delta \mathbf{J}_{(:,i)}\|_2 \) and normalize each by \( \|\mathbf{J}^{\text{ana}}_{(:,i)}\|_2 + \varepsilon \).

flowchart TD
  S["Select configuration grid {q_k}"] --> H["Choose step sizes h candidates"]
  H --> L["Loop over (q_k, h): build J_num(q_k; h) \nand compare to J_ana(q_k)"]
  L --> M["Accumulate statistics: max relative error, \nmean error, column-wise errors"]
  M --> R{"Errors within tolerance \nfor all tests?"}
  R -->|yes| OK["Accept Jacobian implementation"]
  R -->|no| DBG["Debug specific joints / frames, \nre-check kinematics and adjoint usage"]
        

We assume you have already implemented forward kinematics fk(q) that returns either:

• a 6D pose vector x (e.g. position + Euler angles), or
• an SE(3) matrix T via PoE using a library like modern_robotics or roboticstoolbox-python.

First, a generic vector-valued central-difference Jacobian:

import numpy as np

def numeric_jacobian_vec(fk, q, h=1e-6):
    """
    Central-difference Jacobian for f: R^n -> R^m.

    fk : callable, fk(q) -> x (1D array of length m)
    q  : 1D array-like of length n
    h  : stepsize
    """
    q = np.asarray(q, dtype=float).reshape(-1)
    n = q.size
    x0 = np.asarray(fk(q), dtype=float).reshape(-1)
    m = x0.size
    J = np.zeros((m, n))

    for i in range(n):
        dq = np.zeros_like(q)
        dq[i] = h
        xp = np.asarray(fk(q + dq), dtype=float).reshape(-1)
        xm = np.asarray(fk(q - dq), dtype=float).reshape(-1)
        J[:, i] = (xp - xm) / (2.0 * h)

    return J
      

For SE(3)-valued forward kinematics using modern_robotics, we can build a 6D twist-based numerical Jacobian. We assume earlier in the course you have used FKinSpace, MatrixLog6, and se3ToVec.

import modern_robotics as mr

def fk_se3(q, M, Slist):
    """
    SE(3) forward kinematics using product of exponentials.
    M     : 4x4 home configuration
    Slist : 6xn screw-axis matrix
    """
    return mr.FKinSpace(M, Slist, q)

def numeric_body_jacobian(fk_se3_fun, q, h=1e-6):
    """
    Numeric body Jacobian via SE(3) finite differences.
    fk_se3_fun : callable, fk_se3_fun(q) -> 4x4 SE(3) matrix T
    q          : 1D array (n,)
    h          : stepsize
    """
    q = np.asarray(q, dtype=float).reshape(-1)
    n = q.size
    T0 = fk_se3_fun(q)
    Jb = np.zeros((6, n))

    for i in range(n):
        dq = np.zeros_like(q)
        dq[i] = h
        Tp = fk_se3_fun(q + dq)

        # Body twist corresponding to T0^{-1} Tp
        Delta = np.linalg.inv(T0) @ Tp
        xi_hat = mr.MatrixLog6(Delta)
        xi = mr.se3ToVec(xi_hat)  # 6D vector
        Jb[:, i] = xi / h

    return Jb

def check_jacobian(fk_fun, jac_fun, q, h=1e-6, atol=1e-8):
    """
    Compare numeric and analytic Jacobians, return relative Frobenius error.
    fk_fun  : pose function for numeric Jacobian (vector or SE(3))
    jac_fun : analytic Jacobian implementation, jac_fun(q) -> m x n
    """
    J_ana = np.asarray(jac_fun(q), dtype=float)
    # Choose appropriate numeric routine
    if J_ana.shape[0] == 6 and J_ana.shape[1] == q.size:
        J_num = numeric_body_jacobian(fk_fun, q, h)
    else:
        J_num = numeric_jacobian_vec(fk_fun, q, h)

    diff = J_num - J_ana
    err = np.linalg.norm(diff, ord="fro")
    ref = np.linalg.norm(J_ana, ord="fro") + atol
    rel_err = err / ref
    return rel_err, J_num, J_ana
      

You can now sweep over random joint vectors q within joint limits and log the distribution of rel_err to gain confidence in the correctness of the analytic Jacobian.

In C++, linear algebra is typically handled with Eigen. Forward kinematics can come from libraries such as orocos_kdl, pinocchio, or custom PoE implementations. Below we implement a vector-valued numeric Jacobian:

#include <Eigen/Dense>
#include <functional>

using Vector = Eigen::VectorXd;
using Matrix = Eigen::MatrixXd;

// Pose function: R^n -> R^m
using PoseFunction = std::function<Vector(const Vector&)>;

Matrix numericJacobianVec(const PoseFunction& fk,
                          const Vector& q,
                          double h = 1e-6)
{
    Vector qv = q;
    const int n = static_cast<int>(qv.size());
    Vector x0 = fk(qv);
    const int m = static_cast<int>(x0.size());

    Matrix J = Matrix::Zero(m, n);

    for (int i = 0; i < n; ++i) {
        Vector dq = Vector::Zero(n);
        dq(i) = h;

        Vector xp = fk(qv + dq);
        Vector xm = fk(qv - dq);

        J.col(i) = (xp - xm) / (2.0 * h);
    }
    return J;
}

// Example: wrapper around some FK implementation returning a 3D position
Vector fk_example(const Vector& q)
{
    // TODO: call your PoE-based or DH-based FK here
    Vector x(3);
    x.setZero();
    // Fill x based on q
    return x;
}

double checkJacobian(const PoseFunction& fk,
                     const std::function<Matrix(const Vector&)>& jacAna,
                     const Vector& q,
                     double h = 1e-6,
                     double atol = 1e-8)
{
    Matrix J_ana = jacAna(q);
    Matrix J_num = numericJacobianVec(fk, q, h);
    Matrix diff  = J_num - J_ana;

    double err = diff.norm();           // Frobenius for MatrixXd
    double ref = J_ana.norm() + atol;
    return err / ref;
}
      

To integrate with pinocchio, for example, you would let fk call pinocchio::forwardKinematics and pinocchio::updateFramePlacements, then extract the frame translation or pose parameters, while jacAna calls pinocchio::computeFrameJacobian. The numeric checker remains the same.

Java does not have a de facto robotics standard library, but matrix libraries such as EJML are widely used. We define a simple functional interface for the pose function and implement a numeric Jacobian returning an org.ejml.simple.SimpleMatrix.

import org.ejml.simple.SimpleMatrix;

public interface PoseFunction {
    double[] apply(double[] q);
}

public class NumericJacobian {

    public static SimpleMatrix numericJacobianVec(PoseFunction fk,
                                                  double[] q,
                                                  double h) {
        int n = q.length;
        double[] x0 = fk.apply(q);
        int m = x0.length;

        SimpleMatrix J = new SimpleMatrix(m, n);

        for (int i = 0; i < n; ++i) {
            double[] qp = q.clone();
            double[] qm = q.clone();
            qp[i] += h;
            qm[i] -= h;

            double[] xp = fk.apply(qp);
            double[] xm = fk.apply(qm);

            for (int r = 0; r < m; ++r) {
                double val = (xp[r] - xm[r]) / (2.0 * h);
                J.set(r, i, val);
            }
        }
        return J;
    }

    public static double checkJacobian(PoseFunction fk,
                                       java.util.function.Function<double[], SimpleMatrix> jacAna,
                                       double[] q,
                                       double h,
                                       double atol) {
        SimpleMatrix J_ana = jacAna.apply(q);
        SimpleMatrix J_num = numericJacobianVec(fk, q, h);
        SimpleMatrix diff  = J_num.minus(J_ana);

        double err = diff.normF();
        double ref = J_ana.normF() + atol;
        return err / ref;
    }
}
      

In a robotics project, fk might wrap your own Java kinematics code or a JNI binding to a C++ library (e.g. pinocchio). The numeric checker remains purely in Java and can be used in unit tests (e.g. JUnit) to assert that the Jacobian implementation is correct.

MATLAB offers rigidBodyTree models and functions such as getTransform and geometricJacobian. We first implement a generic numeric Jacobian for vector-valued mappings:

function Jnum = numericJacobianVec(fkHandle, q, h)
%NUMERICJACOBIANVEC Central-difference Jacobian for f: R^n -> R^m
%
% fkHandle : function handle, fkHandle(q) -> x (m-by-1)
% q        : column vector (n-by-1)
% h        : stepsize

    if nargin < 3
        h = 1e-6;
    end

    q = q(:);
    n = numel(q);
    x0 = fkHandle(q);
    m = numel(x0);

    Jnum = zeros(m, n);

    for i = 1:n
        dq = zeros(n,1);
        dq(i) = h;

        xp = fkHandle(q + dq);
        xm = fkHandle(q - dq);

        Jnum(:, i) = (xp - xm) / (2*h);
    end
end
      

For a rigidBodyTree robot, we can define a pose function returning a 6D pose vector (XYZ position + ZYX Euler angles), and compare to geometricJacobian:

function [relErr, Jnum, Jana] = checkJacobianRigidBodyTree(robot, q, baseName, eeName, h)

    if nargin < 5
        h = 1e-6;
    end

    q = q(:).';
    fkHandle = @(qcol) pose6FromTree(robot, qcol.', baseName, eeName);

    % Numeric Jacobian (6 x n)
    Jnum = numericJacobianVec(fkHandle, q(:), h);

    % Analytic geometric Jacobian (base frame)
    Jana = geometricJacobian(robot, q, eeName);

    diff = Jnum - Jana;
    err  = norm(diff, "fro");
    ref  = norm(Jana, "fro") + 1e-8;
    relErr = err / ref;
end

function x = pose6FromTree(robot, q, baseName, eeName)
    T = getTransform(robot, q.', eeName, baseName); % 4x4
    p = T(1:3, 4);
    rpy = rotm2eul(T(1:3,1:3), "ZYX").';  % 3x1
    x = [p; rpy];
end
      

In Simulink, this logic can be encapsulated in a MATLAB Function block:

1. Create a MATLAB Function block that accepts q as input.
2. Inside the block, call a MATLAB function like checkJacobianRigidBodyTree (with a persistent robot object initialized once).
3. Output the relative error relErr, and use Assertion blocks or Dashboard instruments to visualize Jacobian correctness in simulation.

In Wolfram Mathematica, we can implement a numeric Jacobian using lists and UnitVector. We work with a vector-valued pose function f[q_List] that returns a list of length m.

NumericJacobian[f_, q_List, h_: 10.^-6] :=
 Module[{n, x0, m, J, ei, qp, qm},
  n  = Length[q];
  x0 = f[q];
  m  = Length[x0];
  J  = ConstantArray[0.0, {m, n}];

  Do[
    ei = UnitVector[n, i];
    qp = f[q + h ei];
    qm = f[q - h ei];
    J[[All, i]] = (qp - qm)/(2.0 h);
    ,
    {i, 1, n}
  ];
  J
 ]

CheckJacobian[f_, Jfun_, q_List, h_: 10.^-6] :=
 Module[{Jana, Jnum, diff, err, ref},
  Jana = Jfun[q];
  Jnum = NumericJacobian[f, q, h];
  diff = Jnum - Jana;

  err  = Norm[diff, "Frobenius"];
  ref  = Norm[Jana, "Frobenius"] + 10.^-8;
  {err/ref, Jnum, Jana}
 ]
      

Here, f could internally use SE(3) matrices and the Lie algebra tools available in Mathematica (matrix logarithms and exponentials) to construct a twist-based Jacobian check analogous to the SE(3) Python implementation.

In this lesson we formalized numerical Jacobian checking as a finite-difference approximation problem on the kinematic map \( f(\mathbf{q}) \). Using central differences, we derived \( \mathcal{O}(h^2) \) truncation error and a standard mixed truncation–roundoff model suggesting optimal stepsizes on the order of \( \varepsilon^{1/3} \). We extended finite differences from scalar functions to vector-valued kinematics and to SE(3)-valued forward kinematics using the matrix logarithm and twist representations.

We then implemented reusable Jacobian checkers in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, all following the same conceptual pattern: build a numeric Jacobian around forward kinematics, compare to the analytic Jacobian using a norm-based relative error, and sweep over test configurations. These tools are indispensable for validating analytic Jacobians derived in previous lessons and for debugging complex manipulator models before moving on to singularity analysis, workspace metrics, and dynamics in later chapters.