From Lesson 2 you know how to write mechanical systems in state-space form. For a robot, we typically choose a state vector \( \mathbf{x} \in \mathbb{R}^{n} \) stacking joint positions and velocities, and a control input \( \mathbf{u} \in \mathbb{R}^{m} \) representing joint torques or actuator commands. The general nonlinear state-space model is

\[ \dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u}), \qquad \mathbf{y} = h(\mathbf{x},\mathbf{u}), \]

where \( \mathbf{y} \) could be joint positions, end-effector pose, or other measured outputs. The mapping \( f \) encapsulates the manipulator dynamics (inertia, Coriolis/centrifugal terms, gravity, etc.), and \( h \) comes from forward kinematics and sensor models.

Most classical control design techniques (pole placement, LQR, H∞, frequency-domain methods) are built for linear systems. Linearization is the mathematical bridge between the nonlinear robot and these linear design tools. In this lesson we treat linearization as a local approximation derived via multivariate Taylor expansion.

An operating point specifies where in the state and input space the robot is intended to operate. In control, we care about small perturbations around such points.

2.1 Equilibrium Operating Points

A pair \( (\mathbf{x}^{\ast},\mathbf{u}^{\ast}) \) is an equilibrium if the state does not change when the system is initialized at that point:

\[ f(\mathbf{x}^{\ast},\mathbf{u}^{\ast}) = \mathbf{0}. \]

For example, for an \( n \)-DOF manipulator with dynamics

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

a static configuration \( (\mathbf{q}^{\ast},\dot{\mathbf{q}}^{\ast}) \) with zero velocity \( \dot{\mathbf{q}}^{\ast} = \mathbf{0} \) is an equilibrium if

\[ \boldsymbol{\tau}^{\ast} = \mathbf{g}(\mathbf{q}^{\ast}). \]

This corresponds to holding the robot motionless by exactly compensating gravity.

2.2 Trajectory (Time-Varying) Operating Points

In tracking problems, the operating point is a time-varying trajectory \( \mathbf{x}^{\ast}(t),\mathbf{u}^{\ast}(t) \) that satisfies the dynamics:

\[ \dot{\mathbf{x}}^{\ast}(t) = f(\mathbf{x}^{\ast}(t),\mathbf{u}^{\ast}(t)). \]

Linearization can be carried out around such trajectories, yielding a linear time-varying (LTV) system. In this lesson we mainly focus on equilibria but will briefly mention the LTV case in later sections.

Consider small deviations from an operating point:

\[ \delta\mathbf{x} = \mathbf{x} - \mathbf{x}^{\ast}, \qquad \delta\mathbf{u} = \mathbf{u} - \mathbf{u}^{\ast}. \]

Assuming \( f \) and \( h \) are continuously differentiable, we expand them via multivariate Taylor series around \( (\mathbf{x}^{\ast},\mathbf{u}^{\ast}) \):

\[ f(\mathbf{x}^{\ast} + \delta\mathbf{x},\mathbf{u}^{\ast} + \delta\mathbf{u}) = f(\mathbf{x}^{\ast},\mathbf{u}^{\ast}) + \mathbf{A}\,\delta\mathbf{x} + \mathbf{B}\,\delta\mathbf{u} + \mathbf{r}_f(\delta\mathbf{x},\delta\mathbf{u}), \]

\[ h(\mathbf{x}^{\ast} + \delta\mathbf{x},\mathbf{u}^{\ast} + \delta\mathbf{u}) = h(\mathbf{x}^{\ast},\mathbf{u}^{\ast}) + \mathbf{C}\,\delta\mathbf{x} + \mathbf{D}\,\delta\mathbf{u} + \mathbf{r}_h(\delta\mathbf{x},\delta\mathbf{u}), \]

where the Jacobian matrices are

\[ \mathbf{A} = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{(\mathbf{x}^{\ast},\mathbf{u}^{\ast})}, \quad \mathbf{B} = \left.\frac{\partial f}{\partial \mathbf{u}}\right|_{(\mathbf{x}^{\ast},\mathbf{u}^{\ast})}, \quad \mathbf{C} = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{(\mathbf{x}^{\ast},\mathbf{u}^{\ast})}, \quad \mathbf{D} = \left.\frac{\partial h}{\partial \mathbf{u}}\right|_{(\mathbf{x}^{\ast},\mathbf{u}^{\ast})}. \]

3.1 Linearized State and Output Equations

If \( (\mathbf{x}^{\ast},\mathbf{u}^{\ast}) \) is an equilibrium, \( f(\mathbf{x}^{\ast},\mathbf{u}^{\ast}) = \mathbf{0} \). Subtracting the equilibrium dynamics from the perturbed dynamics yields

\[ \delta\dot{\mathbf{x}} = \dot{\mathbf{x}} - \dot{\mathbf{x}}^{\ast} = f(\mathbf{x}^{\ast} + \delta\mathbf{x},\mathbf{u}^{\ast} + \delta\mathbf{u}) - f(\mathbf{x}^{\ast},\mathbf{u}^{\ast}) = \mathbf{A}\,\delta\mathbf{x} + \mathbf{B}\,\delta\mathbf{u} + \mathbf{r}_f(\delta\mathbf{x},\delta\mathbf{u}). \]

Neglecting higher-order remainders \( \mathbf{r}_f \) and \( \mathbf{r}_h \), we obtain the linearized model:

\[ \delta\dot{\mathbf{x}} = \mathbf{A}\,\delta\mathbf{x} + \mathbf{B}\,\delta\mathbf{u}, \qquad \delta\mathbf{y} = \mathbf{C}\,\delta\mathbf{x} + \mathbf{D}\,\delta\mathbf{u}. \]

3.2 Remainder Bounds and Local Validity

Under mild smoothness assumptions (e.g. Lipschitz continuous Jacobians) the remainder term satisfies a quadratic bound. There exist constants \( L > 0 \) and \( r > 0 \) such that whenever \( \lVert \delta\mathbf{x} \rVert + \lVert \delta\mathbf{u} \rVert < r \),

\[ \lVert \mathbf{r}_f(\delta\mathbf{x},\delta\mathbf{u}) \rVert \leq L \left(\lVert \delta\mathbf{x} \rVert^2 + \lVert \delta\mathbf{u} \rVert^2\right). \]

Thus, the linear approximation error is of second order in the perturbation magnitude. This justifies using the linear model for small deviations around the operating point, but not for large excursions where nonlinear effects (e.g. joint limits, large rotations) dominate.

For an \( n \)-DOF manipulator, recall the standard dynamic model

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}. \]

Define the state and input as

\[ \mathbf{x} = \begin{bmatrix} \mathbf{q} \\ \dot{\mathbf{q}} \end{bmatrix}, \qquad \mathbf{u} = \boldsymbol{\tau}. \]

Solving for \( \ddot{\mathbf{q}} \) gives

\[ \ddot{\mathbf{q}} = \mathbf{M}^{-1}(\mathbf{q}) \left( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \right). \]

Therefore the state-space dynamics can be written as

\[ \dot{\mathbf{x}} = \begin{bmatrix} \dot{\mathbf{q}} \\ \mathbf{M}^{-1}(\mathbf{q}) \big( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \big) \end{bmatrix} = f(\mathbf{x},\boldsymbol{\tau}). \]

4.1 Structure of the Linearized Matrices

For any operating point \( \mathbf{x}^{\ast} = [\mathbf{q}^{\ast};\dot{\mathbf{q}}^{\ast}] \) and \( \boldsymbol{\tau}^{\ast} \), the Jacobians take a block structure

\[ \mathbf{A} = \begin{bmatrix} \mathbf{0} & \mathbf{I} \\ \mathbf{A}_{21} & \mathbf{A}_{22} \end{bmatrix}, \qquad \mathbf{B} = \begin{bmatrix} \mathbf{0} \\ \mathbf{B}_2 \end{bmatrix}. \]

The lower blocks are given by

\[ \mathbf{A}_{21} = \left. \frac{\partial}{\partial \mathbf{q}} \Big[ \mathbf{M}^{-1}(\mathbf{q}) \big( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \big) \Big] \right|_{(\mathbf{x}^{\ast},\boldsymbol{\tau}^{\ast})}, \]

\[ \mathbf{A}_{22} = \left. \frac{\partial}{\partial \dot{\mathbf{q}}} \Big[ \mathbf{M}^{-1}(\mathbf{q}) \big( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \big) \Big] \right|_{(\mathbf{x}^{\ast},\boldsymbol{\tau}^{\ast})}, \]

\[ \mathbf{B}_2 = \left. \frac{\partial}{\partial \boldsymbol{\tau}} \Big[ \mathbf{M}^{-1}(\mathbf{q}) \big( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \big) \Big] \right|_{(\mathbf{x}^{\ast},\boldsymbol{\tau}^{\ast})} = \mathbf{M}^{-1}(\mathbf{q}^{\ast}). \]

The explicit expressions of \( \mathbf{A}_{21} \) and \( \mathbf{A}_{22} \) are tedious for high-DOF robots, which motivates symbolic or automatic differentiation and numerical linearization algorithms, covered later.

4.2 Example: 1-DOF Pendulum with Torque Input

Consider a simple actuated pendulum with angle \( q \), moment of inertia \( I \), mass \( m \), and length \( \ell \). The equation of motion is

\[ I \ddot{q} + m g \ell \sin(q) = \tau. \]

Define \( x_1 = q \), \( x_2 = \dot{q} \), \( u = \tau \). Then

\[ \dot{x}_1 = x_2, \qquad \dot{x}_2 = -\frac{m g \ell}{I}\sin(x_1) + \frac{1}{I} u. \]

We compute the Jacobians

\[ \mathbf{A} = \begin{bmatrix} \dfrac{\partial \dot{x}_1}{\partial x_1} & \dfrac{\partial \dot{x}_1}{\partial x_2} \\ \dfrac{\partial \dot{x}_2}{\partial x_1} & \dfrac{\partial \dot{x}_2}{\partial x_2} \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ -\dfrac{m g \ell}{I}\cos(x_1) & 0 \end{bmatrix}, \qquad \mathbf{B} = \begin{bmatrix} 0 \\ \dfrac{1}{I} \end{bmatrix}. \]

At the downward equilibrium \( x_1^{\ast} = 0, x_2^{\ast} = 0, u^{\ast} = 0 \), \( \cos(x_1^{\ast}) = 1 \), so the linearized matrices are

\[ \mathbf{A} = \begin{bmatrix} 0 & 1 \\ -\dfrac{m g \ell}{I} & 0 \end{bmatrix}, \qquad \mathbf{B} = \begin{bmatrix} 0 \\ \dfrac{1}{I} \end{bmatrix}. \]

At the upright equilibrium \( x_1^{\ast} = \pi \) with appropriate constant torque, \( \cos(\pi) = -1 \), which flips the sign of the lower-left entry of \( \mathbf{A} \), a key difference for stability analysis in the next lesson.

For complex robots, analytic Jacobians can be cumbersome. A generic alternative is to approximate partial derivatives numerically using finite differences. Let \( f:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{n} \), and consider an operating point \( (\mathbf{x}^{\ast},\mathbf{u}^{\ast}) \). For a small scalar step \( \varepsilon > 0 \), the \( i \)-th column of \( \mathbf{A} \) can be approximated as

\[ \mathbf{A}_{(:,i)} \approx \frac{ f(\mathbf{x}^{\ast} + \varepsilon \mathbf{e}_i,\mathbf{u}^{\ast}) - f(\mathbf{x}^{\ast} - \varepsilon \mathbf{e}_i,\mathbf{u}^{\ast}) }{2\varepsilon}, \]

where \( \mathbf{e}_i \) is the \( i \)-th standard basis vector. Similarly, the \( j \)-th column of \( \mathbf{B} \) is

\[ \mathbf{B}_{(:,j)} \approx \frac{ f(\mathbf{x}^{\ast},\mathbf{u}^{\ast} + \varepsilon \mathbf{e}_j) - f(\mathbf{x}^{\ast},\mathbf{u}^{\ast} - \varepsilon \mathbf{e}_j) }{2\varepsilon}. \]

This central difference scheme is \( \mathcal{O}(\varepsilon^2) \) accurate in the step size, but sensitive to numerical round-off for too small \( \varepsilon \).

flowchart TD
  S["Nonlinear model f(x,u)"] --> O["Choose operating point (x0,u0)"]
  O --> EPS["Select small steps dx, du"]
  EPS --> AX["For each state index i, perturb x_i by +/- dx"]
  AX --> AEST["Estimate columns of A using finite differences"]
  AEST --> BU["For each input index j, perturb u_j by +/- du"]
  BU --> BEST["Estimate columns of B using finite differences"]
  BEST --> LIN["Assemble linear model: dx_dot = A dx + B du"]
        

Python offers both symbolic (sympy) and numeric (numpy) tools for linearization. Below we symbolically linearize the actuated pendulum and then evaluate the matrices at an operating point.

import sympy as sp

# Symbolic variables
q, qdot, tau = sp.symbols("q qdot tau")
m, g, l, I = sp.symbols("m g l I", positive=True)

# State and input vectors
x = sp.Matrix([q, qdot])
u = sp.Matrix([tau])

# Nonlinear dynamics: xdot = f(x,u)
f1 = qdot
f2 = -(m*g*l/I) * sp.sin(q) + (1/I) * tau
f = sp.Matrix([f1, f2])

# Jacobians
A = f.jacobian(x)
B = f.jacobian(u)

print("A(q,qdot) =")
sp.pprint(A)
print("B(q,qdot) =")
sp.pprint(B)

# Evaluate at downward equilibrium: q* = 0, qdot* = 0, tau* = 0
eq_point = {q: 0.0, qdot: 0.0, tau: 0.0}
A_eq = A.subs(eq_point)
B_eq = B.subs(eq_point)

print("A at equilibrium:")
sp.pprint(A_eq)
print("B at equilibrium:")
sp.pprint(B_eq)

# Numeric evaluation for specific parameters
params = {m: 1.0, g: 9.81, l: 1.0, I: 1.0}
A_num = A_eq.subs(params).evalf()
B_num = B_eq.subs(params).evalf()

print("Numeric A:")
sp.pprint(A_num)
print("Numeric B:")
sp.pprint(B_num)
      

For a more general robot model, you can write a Python function f(x,u) using NumPy (or a robotics library) and implement the finite-difference scheme from Section 5 to compute A and B numerically.

In C++ we can use Eigen for matrix computations. The following code numerically linearizes the pendulum dynamics around a given operating point.

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

using Vector2 = Eigen::Matrix<double,2,1>;
using Matrix2 = Eigen::Matrix<double,2,2>;

// Nonlinear dynamics: xdot = f(x,u)
Vector2 pendulumDynamics(const Vector2& x,
                         double u,
                         double m, double g, double l, double I)
{
    Vector2 xdot;
    double q     = x(0);
    double qdot  = x(1);

    xdot(0) = qdot;
    xdot(1) = -(m*g*l/I) * std::sin(q) + (1.0/I) * u;
    return xdot;
}

// Finite-difference linearization
void linearizePendulum(const Vector2& x0,
                       double u0,
                       double m, double g, double l, double I,
                       double eps,
                       Matrix2& A,
                       Vector2& B)
{
    Vector2 f0 = pendulumDynamics(x0, u0, m, g, l, I);

    // Columns of A
    for (int i = 0; i < 2; ++i)
    {
        Vector2 x_plus  = x0;
        Vector2 x_minus = x0;
        x_plus(i)  += eps;
        x_minus(i) -= eps;

        Vector2 f_plus  = pendulumDynamics(x_plus,  u0, m, g, l, I);
        Vector2 f_minus = pendulumDynamics(x_minus, u0, m, g, l, I);

        A.col(i) = (f_plus - f_minus) / (2.0 * eps);
    }

    // Column of B (single input)
    double u_plus  = u0 + eps;
    double u_minus = u0 - eps;

    Vector2 f_plus_u  = pendulumDynamics(x0, u_plus,  m, g, l, I);
    Vector2 f_minus_u = pendulumDynamics(x0, u_minus, m, g, l, I);

    B = (f_plus_u - f_minus_u) / (2.0 * eps);
}

int main()
{
    Vector2 x0;
    x0 << 0.0, 0.0;
    double u0 = 0.0;

    double m = 1.0, g = 9.81, l = 1.0, I = 1.0;
    double eps = 1e-6;

    Matrix2 A;
    Vector2 B;
    linearizePendulum(x0, u0, m, g, l, I, eps, A, B);

    std::cout << "A =\n" << A << std::endl;
    std::cout << "B =\n" << B << std::endl;
    return 0;
}
      

For a multi-DOF robot whose dynamics are provided by a library (e.g., RBDL, Pinocchio, KDL), the same linearization routine can be applied by calling the library-based dynamics function inside pendulumDynamics.

Java has several linear algebra libraries such as EJML. Here we write a minimal example using EJML-like interfaces to linearize the pendulum dynamics.

public class PendulumLinearization {

    // Nonlinear dynamics: xdot = f(x,u)
    // x is length-2 array [q, qdot]
    public static double[] f(double[] x, double u,
                             double m, double g, double l, double I) {
        double[] xdot = new double[2];
        double q    = x[0];
        double qdot = x[1];

        xdot[0] = qdot;
        xdot[1] = -(m*g*l/I) * Math.sin(q) + (1.0/I) * u;
        return xdot;
    }

    // Finite-difference linearization
    public static void linearize(double[] x0, double u0,
                                 double m, double g, double l, double I,
                                 double eps,
                                 double[][] A, double[] B) {

        double[] f0 = f(x0, u0, m, g, l, I);

        // Columns of A
        for (int i = 0; i < 2; ++i) {
            double[] xPlus  = x0.clone();
            double[] xMinus = x0.clone();
            xPlus[i]  += eps;
            xMinus[i] -= eps;

            double[] fPlus  = f(xPlus,  u0, m, g, l, I);
            double[] fMinus = f(xMinus, u0, m, g, l, I);

            for (int k = 0; k < 2; ++k) {
                A[k][i] = (fPlus[k] - fMinus[k]) / (2.0 * eps);
            }
        }

        // Column of B
        double uPlus  = u0 + eps;
        double uMinus = u0 - eps;
        double[] fPlusU  = f(x0, uPlus,  m, g, l, I);
        double[] fMinusU = f(x0, uMinus, m, g, l, I);
        for (int k = 0; k < 2; ++k) {
            B[k] = (fPlusU[k] - fMinusU[k]) / (2.0 * eps);
        }
    }

    public static void main(String[] args) {
        double[] x0 = {0.0, 0.0};
        double u0 = 0.0;
        double m = 1.0, g = 9.81, l = 1.0, I = 1.0;
        double eps = 1e-6;

        double[][] A = new double[2][2];
        double[] B = new double[2];

        linearize(x0, u0, m, g, l, I, eps, A, B);

        System.out.println("A:");
        for (int i = 0; i < 2; ++i) {
            System.out.println(A[i][0] + "  " + A[i][1]);
        }
        System.out.println("B:");
        System.out.println(B[0] + "  " + B[1]);
    }
}
      

Replacing the pendulum dynamics with a more complex robot model only affects the f method; the linearization scaffold remains the same.

MATLAB supports both symbolic Jacobian computation and automatic linearization of Simulink models. The script below uses the Symbolic Math Toolbox to derive \( \mathbf{A} \) and \( \mathbf{B} \) for the pendulum.

syms q qdot tau m g l I real

x = [q; qdot];
u = tau;

% Nonlinear dynamics
f1 = qdot;
f2 = -(m*g*l/I) * sin(q) + (1/I) * tau;
f  = [f1; f2];

% Jacobians
A = jacobian(f, x);
B = jacobian(f, u);

disp('A(q,qdot) =');
disp(A);
disp('B(q,qdot) =');
disp(B);

% Evaluate at equilibrium q* = 0, qdot* = 0, tau* = 0
A_eq = subs(A, {q, qdot, tau}, {0, 0, 0});
B_eq = subs(B, {q, qdot, tau}, {0, 0, 0});

disp('A at equilibrium:');
disp(A_eq);
disp('B at equilibrium:');
disp(B_eq);

% Substitute numeric parameters
params = {m, g, l, I};
vals   = {1.0, 9.81, 1.0, 1.0};
A_num  = double(subs(A_eq, params, vals));
B_num  = double(subs(B_eq, params, vals));

disp('Numeric A:');
disp(A_num);
disp('Numeric B:');
disp(B_num);
      

For a Simulink model of the robot, you can use functions such as linmod or the linearize command (from the Control System Toolbox) to obtain \( \mathbf{A},\mathbf{B},\mathbf{C},\mathbf{D} \) directly from the block diagram at specified operating points.

Mathematica provides straightforward symbolic differentiation for linearization. The following code linearizes the pendulum model.

Clear["Global`*"];

(* State and input symbols *)
q   = Symbol["q"];
qdot = Symbol["qdot"];
tau  = Symbol["tau"];
m    = Symbol["m"];
g    = Symbol["g"];
l    = Symbol["l"];
I    = Symbol["I"];

x = {q, qdot};
u = {tau};

(* Nonlinear dynamics *)
f1 = qdot;
f2 = -(m*g*l/I) * Sin[q] + (1/I) * tau;
f  = {f1, f2};

(* Jacobians *)
A = D[f, {x}];
B = D[f, {u}];

Print["A(q,qdot) = ", A];
Print["B(q,qdot) = ", B];

(* Equilibrium q* = 0, qdot* = 0, tau* = 0 *)
Aeq = A /. {q -> 0, qdot -> 0, tau -> 0};
Beq = B /. {q -> 0, qdot -> 0, tau -> 0};

Print["A at equilibrium: ", Aeq];
Print["B at equilibrium: ", Beq];

(* Numeric substitution *)
A_num = Aeq /. {m -> 1.0, g -> 9.81, l -> 1.0, I -> 1.0};
B_num = Beq /. {m -> 1.0, g -> 9.81, l -> 1.0, I -> 1.0};

Print["Numeric A = ", A_num];
Print["Numeric B = ", B_num];
      

For more complex robots with symbolic dynamics available, the same pattern generalizes to higher-dimensional states and inputs.

In this lesson we introduced the concept of linearization around operating points for nonlinear robotic systems. Starting from the general state-space form \( \dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u}) \), we derived the linearized model via Jacobians and discussed the local validity of the approximation. We applied these ideas to manipulator dynamics, highlighted the structure of the resulting \( \mathbf{A},\mathbf{B} \) matrices, and worked through a pendulum example. We then implemented linearization in Python, C++, Java, MATLAB/Simulink, and Mathematica, and proved a basic error bound for the linearization remainder. These tools will be essential in the next lesson, where we study stability notions using linearized models.