Every realistic robot controller evaluates mathematical models numerically rather than symbolically. Forward and inverse kinematics, Jacobians, and eventually dynamics yield equations that must be solved approximately using floating-point arithmetic. Poor numerical practice leads to:

• Inaccurate end-effector poses due to accumulated rounding errors.
• Unstable simulations from inappropriate integrator step sizes.
• Ill-conditioned linear solves when Jacobians or inertia matrices are nearly singular.

A typical computation pipeline in robotics (FK, IK, Jacobians, then dynamics) can be abstracted as:

flowchart TD
  A["Continuous model: geometry + ODEs"] --> B["Discretization: steps, grids, finite differences"]
  B --> C["Linear algebra kernels: solve A x = b, eigen, SVD"]
  C --> D["Implementation: Python / C++ / Java / MATLAB / Mathematica"]
  D --> E["Robot tasks: pose, velocity, simulation, control input"]
  E --> F["Monitoring: error, conditioning, step-size adaptation"]
        

Our goal in this lesson is to understand the numerical core of this pipeline and learn how to implement it robustly across several programming environments.

On all platforms we will use standard IEEE 754 double precision. A real number \( x \) is rounded to a floating-point number \( \operatorname{fl}(x) \) satisfying the standard model:

\[ \operatorname{fl}(x) = x(1 + \delta), \quad |\delta| \leq u, \]

where \( u \) is the unit roundoff (for double precision, \( u \approx 10^{-16} \)). The key idea of backward error analysis is:

• We interpret the floating-point result as the exact result for slightly perturbed data.
• We compare the perturbation size to the original problem size.

2.1 Conditioning of a Problem

Let \( f: \mathbb{R}^n \to \mathbb{R}^m \) be a differentiable mapping. The local condition number of \( f \) at \( x \) with respect to a vector norm \( \|\cdot\| \) can be defined as

\[ \kappa_f(x) = \lim_{\varepsilon \to 0} \sup_{\|\Delta x\| \leq \varepsilon \|x\|} \frac{\|f(x + \Delta x) - f(x)\| / \|f(x)\|} {\|\Delta x\| / \|x\|}. \]

When \( f \) is differentiable and \( f(x) \neq 0 \), this becomes

\[ \kappa_f(x) \approx \frac{\|Df(x)\|\ \|x\|}{\|f(x)\|}, \]

where \( Df(x) \) is the Jacobian of \( f \). In robotics, a common special case is the solution of a linear system \( A x = b \), where \( A \in \mathbb{R}^{n\times n} \) is a Jacobian or an inertia matrix. Here the problem is to compute \( x = A^{-1} b \). The condition number of \( A \) in a matrix norm \( \|\cdot\| \) is

\[ \kappa(A) = \|A\| \, \|A^{-1}\|. \]

2.2 Error Bound for Linear Systems

Consider the exact system \( A x = b \) with nonsingular \( A \), and a perturbed right-hand side \( b + \Delta b \). Let \( x \) solve \( A x = b \) and \( \hat{x} = x + \Delta x \) solve \( A \hat{x} = b + \Delta b \). Then

\[ A(x + \Delta x) = b + \Delta b \quad \Rightarrow \quad A \Delta x = \Delta b \quad \Rightarrow \quad \Delta x = A^{-1} \Delta b. \]

Taking norms:

\[ \|\Delta x\| \leq \|A^{-1}\| \, \|\Delta b\|. \]

Since \( x = A^{-1} b \), we also have \( \|x\| \leq \|A^{-1}\|\,\|b\| \), hence

\[ \frac{\|\Delta x\|}{\|x\|} \leq \|A^{-1}\| \frac{\|\Delta b\|}{\|x\|} \leq \|A^{-1}\|\frac{\|\Delta b\|}{\|b\|}\,\|A\| = \kappa(A)\, \frac{\|\Delta b\|}{\|b\|}. \]

Thus, relative output error is amplified by at most \( \kappa(A) \) times the relative input error. Near-singular Jacobians in robotics lead to large \( \kappa(A) \), and therefore ill-conditioned solves.

The main linear algebra operations in kinematics and dynamics include:

• Solving \( A x = b \) for joint velocities or accelerations.
• Computing condition numbers and norms of Jacobians.
• Computing eigenvalues (e.g., for stiffness matrices or stability analyses later in the course).

3.1 Python: NumPy/SciPy and Robotics Toolboxes

In Python, NumPy and SciPy provide core linear algebra. Robotics-specific toolboxes (e.g., roboticstoolbox-python) internally rely on these kernels. For a Jacobian-like matrix \( J \) we can compute the condition number and solve \( J \Delta q = v \) as:

import numpy as np

# 3x3 Jacobian example (nearly singular)
J = np.array([[0.1, 0.0, 0.0],
              [0.0, 1.0, 0.999],
              [0.0, 1.0, 1.001]])

cond_J = np.linalg.cond(J)
print("cond(J) =", cond_J)

if cond_J > 1e6:
    print("Warning: Jacobian is ill-conditioned")

# Solve J * dq = v
v = np.array([0.0, 0.0, 1.0])
dq = np.linalg.solve(J, v)
print("dq =", dq)
      

The np.linalg.solve call uses LU decomposition with pivoting, which is backward stable for most problems. For very ill-conditioned systems, we might switch to regularization (e.g., damped least squares) in later chapters.

3.2 C++: Eigen Library for Robotics

In C++, the Eigen library is a de facto standard for robotics linear algebra and is used, for example, in ROS and KDL. A similar example:

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

int main() {
    Eigen::Matrix3d J;
    J << 0.1, 0.0, 0.0,
          0.0, 1.0, 0.999,
          0.0, 1.0, 1.001;

    // Approximate 2-norm condition number using SVD
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(J);
    double sigma_max = svd.singularValues()(0);
    double sigma_min = svd.singularValues()(2);
    double condJ = sigma_max / sigma_min;

    std::cout << "cond(J) = " << condJ << std::endl;
    if (condJ > 1e6) {
        std::cout << "Warning: Jacobian is ill-conditioned" << std::endl;
    }

    Eigen::Vector3d v(0.0, 0.0, 1.0);
    Eigen::Vector3d dq = J.colPivHouseholderQr().solve(v);
    std::cout << "dq = " << dq.transpose() << std::endl;

    return 0;
}
      

Note that we rarely form matrix inverses explicitly; instead we use factorization-based solvers (QR, LU, Cholesky) that are more stable and efficient.

In robotics, Jacobians map joint velocities to end-effector twists. Sometimes analytic Jacobians are difficult to derive, especially for complex mechanisms. A straightforward alternative is to use finite differences on the forward kinematics map \( f: \mathbb{R}^n \to \mathbb{R}^m \) that gives end-effector poses from joint variables.

4.1 Forward and Central Differences

For a scalar function \( f: \mathbb{R} \to \mathbb{R} \), the forward-difference approximation of the derivative at \( q \) is

\[ f'(q) \approx \frac{f(q + h) - f(q)}{h}. \]

By Taylor expansion,

\[ f(q + h) = f(q) + h f'(q) + \frac{h^2}{2} f''(\xi_1), \]

for some \( \xi_1 \) between \( q \) and \( q + h \). Subtracting \( f(q) \) and dividing by \( h \) gives

\[ \frac{f(q + h) - f(q)}{h} = f'(q) + \frac{h}{2} f''(\xi_1), \]

so the truncation error is \( \mathcal{O}(h) \). A more accurate alternative is the central-difference approximation:

\[ f'(q) \approx \frac{f(q + h) - f(q - h)}{2h}, \]

which, by symmetric Taylor expansions, has truncation error \( \mathcal{O}(h^2) \).

4.2 Finite-Difference Jacobians in Multiple Dimensions

For \( f: \mathbb{R}^n \to \mathbb{R}^m \), the Jacobian \( J(q) \in \mathbb{R}^{m \times n} \) can be approximated column by column:

\[ J(q) e_j \approx \frac{f(q + h e_j) - f(q - h e_j)}{2h}, \quad j = 1,\dots,n, \]

where \( e_j \) is the \( j \)-th unit vector. In code, we treat each column as a finite-difference directional derivative.

4.3 Python Implementation of a Central-Difference Jacobian

import numpy as np

def fd_jacobian_central(f, q, h=1e-6):
    """
    Central-difference Jacobian of f: R^n -> R^m at point q.
    f(q) must return a 1D array of length m.
    """
    q = np.asarray(q, dtype=float)
    m = len(f(q))
    n = len(q)
    J = np.zeros((m, n))
    for j in range(n):
        dq = np.zeros_like(q)
        dq[j] = h
        f_plus = f(q + dq)
        f_minus = f(q - dq)
        J[:, j] = (f_plus - f_minus) / (2.0 * h)
    return J
      

4.4 C++ Implementation with Eigen

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

// f: R^n -> R^m, implemented as std::function<Eigen::VectorXd(const Eigen::VectorXd&)>
Eigen::MatrixXd fdJacobianCentral(
    const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& f,
    const Eigen::VectorXd& q,
    double h = 1e-6)
{
    Eigen::VectorXd f0 = f(q);
    int m = static_cast<int>(f0.size());
    int n = static_cast<int>(q.size());
    Eigen::MatrixXd J(m, n);

    for (int j = 0; j < n; ++j) {
        Eigen::VectorXd dq = Eigen::VectorXd::Zero(n);
        dq(j) = h;
        Eigen::VectorXd f_plus  = f(q + dq);
        Eigen::VectorXd f_minus = f(q - dq);
        J.col(j) = (f_plus - f_minus) / (2.0 * h);
    }
    return J;
}
      

Later in the course, when analytic Jacobians are available, we will use finite differences primarily as a debugging tool to verify symbolic or automatic differentiation implementations.

Robot joint angles and velocities evolve according to ordinary differential equations (ODEs). Even when we have exact continuous-time models, control and simulation must discretize time. Consider a generic ODE

\[ \dot{y}(t) = f(t, y(t)), \quad y(t_0) = y_0. \]

5.1 Explicit Euler Method

The simplest time-stepping scheme is explicit Euler:

\[ y_{k+1} = y_k + h f(t_k, y_k), \]

where \( h \) is the step size and \( t_k = t_0 + k h \). The local truncation error is \( \mathcal{O}(h^2) \), and the global error is \( \mathcal{O}(h) \).

5.2 Stability for the Test Equation

For the linear test equation \( \dot{y} = \lambda y \), the exact solution is \( y(t) = e^{\lambda t} y_0 \). Applying explicit Euler gives

\[ y_{k+1} = y_k + h \lambda y_k = (1 + h \lambda) y_k \quad \Rightarrow \quad y_k = (1 + h \lambda)^k y_0. \]

For asymptotic stability we need \( |1 + h \lambda| \leq 1 \). When \( \lambda \) is real and negative, this implies

\[ -2 \leq h \lambda \leq 0 \quad \Rightarrow \quad 0 \leq h \leq \frac{2}{|\lambda|}. \]

In robotics, stiff dynamics (widely separated time scales) may produce large negative eigenvalues of the linearized system, which in turn forces very small \( h \) for explicit Euler to remain stable.

5.3 Fourth-Order Runge–Kutta (RK4)

A widely used higher-order explicit method is RK4:

\[ \begin{aligned} k_1 &= f(t_k, y_k),\\ k_2 &= f\Bigl(t_k + \frac{h}{2}, y_k + \frac{h}{2} k_1\Bigr),\\ k_3 &= f\Bigl(t_k + \frac{h}{2}, y_k + \frac{h}{2} k_2\Bigr),\\ k_4 &= f(t_k + h, y_k + h k_3),\\[4pt] y_{k+1} &= y_k + \frac{h}{6} (k_1 + 2 k_2 + 2 k_3 + k_4). \end{aligned} \]

RK4 has global error \( \mathcal{O}(h^4) \) and better stability properties than explicit Euler. In practice, modern libraries use adaptive step-size control, but understanding Euler and RK4 is essential.

5.4 Python Example: Single-Joint Oscillator

As a simple surrogate for a joint with a torsional spring, consider \( \ddot{q} + k q = 0 \), or in first-order form \( \dot{y} = f(y) \) with \( y = (q, \dot{q})^\top \):

\[ \dot{q} = \dot{q}, \quad \ddot{q} = -k q. \]

import numpy as np

def f_oscillator(t, y, k=10.0):
    q, qdot = y
    dqdt = qdot
    dqdotdt = -k * q
    return np.array([dqdt, dqdotdt])

def rk4_step(f, t, y, h, **kwargs):
    k1 = f(t,         y,             **kwargs)
    k2 = f(t + 0.5*h, y + 0.5*h*k1,  **kwargs)
    k3 = f(t + 0.5*h, y + 0.5*h*k2,  **kwargs)
    k4 = f(t + h,     y + h*k3,      **kwargs)
    return y + (h/6.0)*(k1 + 2*k2 + 2*k3 + k4)

t = 0.0
y = np.array([0.1, 0.0])  # small initial displacement
h = 0.001
T = 1.0
while t < T:
    y = rk4_step(f_oscillator, t, y, h, k=10.0)
    t += h
# y now approximates [q(T), qdot(T)]
print("q(T), qdot(T) =", y)
      

6.1 Java with EJML: Linear Solve and Euler Integration

The EJML (Efficient Java Matrix Library) provides dense linear algebra suitable for robotics. A minimal example:

import org.ejml.simple.SimpleMatrix;

public class NumericalRoboticsExample {
    public static void main(String[] args) {
        // Jacobian-like matrix
        double[][] dataJ = {
            {0.1, 0.0, 0.0},
            {0.0, 1.0, 0.999},
            {0.0, 1.0, 1.001}
        };
        SimpleMatrix J = new SimpleMatrix(dataJ);
        double condJ = J.conditionP2();
        System.out.println("cond(J) = " + condJ);
        if (condJ > 1e6) {
            System.out.println("Warning: ill-conditioned Jacobian");
        }

        // Simple joint integration: qdot = omega, omega_dot = -k*q
        double k = 10.0;
        double q = 0.1;
        double qdot = 0.0;
        double h = 0.001;
        double T = 1.0;
        double t = 0.0;
        while (t < T) {
            double dqdt = qdot;
            double dqdotdt = -k * q;
            q += h * dqdt;
            qdot += h * dqdotdt;
            t += h;
        }
        System.out.println("q(T) = " + q + ", qdot(T) = " + qdot);
    }
}
      

6.2 MATLAB/Simulink: Using Built-in ODE Solvers

In MATLAB, we use built-in matrix operations and ODE solvers such as ode45. For the same oscillator:

function numerical_oscillator_demo
    k = 10.0;
    y0 = [0.1; 0.0];
    tspan = [0 1];
    % ODE45 uses adaptive RK methods
    [t, y] = ode45(@(t,y) f_oscillator(t,y,k), tspan, y0);
    q_end = y(end,1);
    qdot_end = y(end,2);
    fprintf('q(T) = %g, qdot(T) = %g\n', q_end, qdot_end);
end

function dydt = f_oscillator(~, y, k)
    q = y(1);
    qdot = y(2);
    dqdt = qdot;
    dqdotdt = -k * q;
    dydt = [dqdt; dqdotdt];
end
      

A corresponding Simulink diagram would contain an integrator chain (two cascaded integrators) and a gain block implementing -k on the position signal feeding back to the acceleration.

6.3 Wolfram Mathematica: LinearSolve and NDSolve

Mathematica provides high-level numerical solvers very useful for symbolic-to-numeric workflows:

(* Linear system for Jacobian-like matrix *)
J = { {0.1, 0.0, 0.0},
     {0.0, 1.0, 0.999},
     {0.0, 1.0, 1.001} };
v = {0.0, 0.0, 1.0};
dq = LinearSolve[J, v];

(* Condition number (2-norm) *)
condJ = ConditionNumber[J];

(* Oscillator ODE: q''[t] + k q[t] == 0 *)
k = 10.0;
sol = NDSolve[
   {q''[t] + k q[t] == 0, q[0] == 0.1, q'[0] == 0.0},
   q, {t, 0, 1}];

qEnd = q[1] /. sol[[1]];
      

Such environments are excellent for prototyping algorithms and validating numerical properties before porting implementations to embedded robotics code.

• Always check conditioning. Before solving \( A x = b \), estimate \( \kappa(A) \). Near singularities (e.g., kinematic singularities) consider regularization.
• Avoid explicit matrix inverses. Use factorization-based solvers (solve, QR, SVD).
• Use consistent units. Mixing meters with millimeters or radians with degrees can create huge scaling differences and ill-conditioning.
• Scale variables when necessary. For optimization or IK problems, nondimensionalize or rescale joint variables to comparable magnitudes.
• Choose step sizes carefully. For integration, too large \( h \) causes instability; too small causes excessive floating-point error and computation time.
• Debug with finite differences. Compare analytic Jacobians against central-difference approximations to catch implementation bugs.

In this lesson we introduced the numerical foundations of robot modeling. We saw how floating-point errors are modeled, how conditioning governs sensitivity of linear solves, and why ill-conditioned Jacobians correspond to problematic robot configurations. We studied finite-difference methods for approximating Jacobians, basic ODE integrators (explicit Euler and RK4), and elementary stability analysis for the test equation. Finally, we implemented these concepts in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, establishing a multi-language toolkit for the subsequent kinematics and dynamics chapters.