Consider a general nonlinear control-affine system \( \dot{x} = f(x) + G(x)u \) with output \( y = h(x) \). Feedback linearization seeks a state- and output-dependent transformation of the input \( u \) and possibly of the state such that the input-output map behaves like a linear system. For manipulators, we exploit the structure of the rigid-body dynamics to cancel nonlinearities and obtain closed-loop error dynamics of the form

\[ \ddot{e} + K_v\dot{e} + K_p e = 0, \]

where the joint error is \( e = q - q_d \) and \( K_p, K_v \) are positive-definite gain matrices. This is mathematically equivalent to controlling a set of independent second-order linear systems, even though the physical manipulator is strongly nonlinear and coupled.

At a high level, feedback linearization for manipulators proceeds as:

flowchart TD
  A["Rigid-body model: M(q) qddot + C(q,qdot) qdot + g(q) = tau"] --> B["Choose tracking objective: q(t) approx q_d(t)"]
  B --> C["Define error: e = q - q_d, e_dot = qdot - qdot_d"]
  C --> D["Select desired linear error dynamics: eddot + K_v e_dot + K_p e = 0"]
  D --> E["Solve for virtual input v that yields eddot dynamics"]
  E --> F["Construct torque: tau = M(q) v + C(q,qdot) qdot + g(q)"]
  F --> G["Closed-loop: manipulator behaves like linear 2nd-order system in e"]
        

In this lesson we derive the details of this scheme, show why it works mathematically, and discuss its limitations (exact model needed, saturation issues, etc.), while postponing robustness and adaptation to later chapters.

From the robotics course you know the standard rigid-body dynamics of an \( n \)-DOF manipulator:

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

where:

• \( q \in \mathbb{R}^n \): joint positions,
• \( \dot{q}, \ddot{q} \in \mathbb{R}^n \): joint velocities and accelerations,
• \( M(q) \in \mathbb{R}^{n\times n} \): symmetric positive-definite inertia matrix,
• \( C(q,\dot{q})\dot{q} \): Coriolis/centrifugal vector,
• \( g(q) \): gravity vector,
• \( \tau \in \mathbb{R}^n \): joint torques.

Defining the state \( x = \begin{bmatrix}q \\ \dot{q}\end{bmatrix} \in \mathbb{R}^{2n} \), we can write the dynamics in control-affine form:

\[ \dot{x} = \begin{bmatrix} \dot{q} \\ -M(q)^{-1}\big(C(q,\dot{q})\dot{q} + g(q)\big) \end{bmatrix} + \begin{bmatrix} 0 \\ M(q)^{-1} \end{bmatrix}\tau \;\;=\;\; f(x) + G(x)\tau, \]

with joint position output \( y = h(x) = q \). Feedback linearization will choose a state-dependent torque \( \tau(q,\dot{q},q_d,\dot{q}_d,\ddot{q}_d) \) such that the closed-loop input-output behavior is linear in the tracking error.

Let the desired joint trajectory be \( q_d(t) \) with derivatives \( \dot{q}_d(t), \ddot{q}_d(t) \). Define the tracking error and its derivative:

\[ e = q - q_d,\quad \dot{e} = \dot{q} - \dot{q}_d. \]

We introduce a virtual input \( v \in \mathbb{R}^n \) and choose the torque as

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

Substituting this control law into the dynamics:

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) \;=\; M(q)v + C(q,\dot{q})\dot{q} + g(q). \]

Cancelling identical terms on both sides yields

\[ M(q)\big(\ddot{q} - v\big) = 0. \]

Because \( M(q) \) is invertible for all \( q \), we obtain

\[ \ddot{q} = v. \]

Thus, in joint coordinates the manipulator behaves as \( n \) independent double integrators driven by \( v \). To enforce a second-order linear error dynamics, we choose

\[ v = \ddot{q}_d - K_v\dot{e} - K_p e, \]

where \( K_p, K_v \in \mathbb{R}^{n\times n} \) are symmetric positive-definite gain matrices (often diagonal).

Then the closed-loop acceleration is

\[ \ddot{q} = \ddot{q}_d - K_v\dot{e} - K_p e. \]

Using \( \ddot{e} = \ddot{q} - \ddot{q}_d \), we obtain:

\[ \ddot{e} = -K_v\dot{e} - K_p e \quad\Longrightarrow\quad \ddot{e} + K_v\dot{e} + K_p e = 0. \]

This is a linear time-invariant system in \( e \). If \( K_p, K_v \) are chosen such that all eigenvalues of the polynomial \( \lambda^2 I + K_v\lambda + K_p \) have negative real parts, the tracking error converges exponentially to zero.

To rigorously justify stability, we use a Lyapunov function constructed from the error and its derivative. A simple, coordinate-wise Lyapunov function is

\[ V(e,\dot{e}) = \tfrac{1}{2}\dot{e}^\top\dot{e} + \tfrac{1}{2}e^\top K_p e. \]

Because \( K_p \) is positive definite, \( V(e,\dot{e}) \) is positive definite and radially unbounded in \( (e,\dot{e}) \).

Differentiating along trajectories of \( \ddot{e} + K_v\dot{e} + K_p e = 0 \):

\[ \dot{V} = \dot{e}^\top\ddot{e} + \tfrac{1}{2}\dot{e}^\top K_p e + \tfrac{1}{2} e^\top K_p \dot{e} = \dot{e}^\top\ddot{e} + e^\top K_p \dot{e}. \]

Substituting \( \ddot{e} = -K_v\dot{e} - K_p e \) gives:

\[ \dot{V} = \dot{e}^\top(-K_v\dot{e} - K_p e) + e^\top K_p \dot{e} = -\dot{e}^\top K_v\dot{e}. \]

Since \( K_v \) is positive definite, \( \dot{V} \le 0 \), with equality only if \( \dot{e} = 0 \). From LaSalle's invariance principle and the structure of the linear closed-loop system, it follows that \( (e,\dot{e}) \rightarrow 0 \) as time grows, i.e., the origin is globally asymptotically stable in the tracking error coordinates.

A more refined proof, using the full robot dynamics and the well-known property that \( \dot{M}(q) - 2C(q,\dot{q}) \) is skew-symmetric, constructs

\[ V(e,\dot{e}) = \tfrac{1}{2}\dot{e}^\top M(q)\dot{e} + \tfrac{1}{2}e^\top K_p e, \]

leading to the same conclusion but explicitly accounting for the inertia matrix. The essence is unchanged: the feedback linearization law renders the nonlinear closed-loop error dynamics equivalent to a stable linear system.

In Python, libraries such as roboticstoolbox-python and pin (Pinocchio bindings) can compute \( M(q), C(q,\dot{q}), g(q) \) from a URDF model. Here we illustrate a minimal from-scratch example for a 2-link planar manipulator and a feedback-linearizing controller.

import numpy as np

# Simple 2-link planar arm parameters
m1, m2 = 1.0, 1.0
l1, l2 = 1.0, 1.0
lc1, lc2 = 0.5, 0.5
I1, I2 = 0.05, 0.05
g_const = 9.81

def M_matrix(q):
    q1, q2 = q
    c2 = np.cos(q2)
    a = I1 + I2 + m2 * l1**2
    b = m2 * l1 * lc2
    d11 = a + 2.0 * b * c2
    d12 = I2 + b * c2
    d21 = d12
    d22 = I2
    return np.array([[d11, d12],
                     [d21, d22]])

def C_matrix(q, qdot):
    q1, q2 = q
    q1dot, q2dot = qdot
    c2 = np.cos(q2)
    s2 = np.sin(q2)
    b = m2 * l1 * lc2
    h = -b * s2
    # 2x2 Coriolis matrix (one common convention)
    return np.array([[h * q2dot, h * (q1dot + q2dot)],
                     [-h * q1dot, 0.0]])

def g_vector(q):
    q1, q2 = q
    g1 = (m1 * lc1 + m2 * l1) * g_const * np.cos(q1) \
         + m2 * lc2 * g_const * np.cos(q1 + q2)
    g2 = m2 * lc2 * g_const * np.cos(q1 + q2)
    return np.array([g1, g2])

def feedback_linearization_tau(q, qdot,
                               qd, qd_dot, qd_ddot,
                               Kp, Kv):
    """
    q, qdot, qd, qd_dot, qd_ddot: 2-element numpy arrays
    Kp, Kv: 2x2 gain matrices (numpy arrays)
    """
    e = q - qd
    e_dot = qdot - qd_dot
    v = qd_ddot - Kv @ e_dot - Kp @ e
    M = M_matrix(q)
    C = C_matrix(q, qdot)
    g = g_vector(q)
    tau = M @ v + C @ qdot + g
    return tau

# Example usage at one time instant
q = np.array([0.1, 0.1])
qdot = np.array([0.0, 0.0])
qd = np.array([0.5, -0.3])
qd_dot = np.array([0.0, 0.0])
qd_ddot = np.array([0.0, 0.0])

Kp = np.diag([25.0, 25.0])
Kv = np.diag([10.0, 10.0])

tau = feedback_linearization_tau(q, qdot, qd, qd_dot, qd_ddot, Kp, Kv)
print("Tau command:", tau)
      

In a full simulation, you would integrate the ODE \( \dot{x} = f(x) + G(x)\tau(x,t) \) using scipy.integrate.solve_ivp and update the desired trajectory \( q_d(t) \) at each step.

In C++, rigid-body dynamics are often computed via libraries such as RBDL, Pinocchio, or KDL, which provide functions for M(q), C(q,qdot), and g(q). Below is a sketch of a feedback-linearizing controller for a generic manipulator using Eigen types:

#include <Eigen/Dense>

// These would typically be provided by RBDL, Pinocchio, or a similar library.
void computeInertia(const Eigen::VectorXd& q, Eigen::MatrixXd& M);
void computeCoriolis(const Eigen::VectorXd& q,
                     const Eigen::VectorXd& qdot,
                     Eigen::MatrixXd& C);
void computeGravity(const Eigen::VectorXd& q, Eigen::VectorXd& g);

Eigen::VectorXd feedbackLinearization(
    const Eigen::VectorXd& q,
    const Eigen::VectorXd& qdot,
    const Eigen::VectorXd& qd,
    const Eigen::VectorXd& qd_dot,
    const Eigen::VectorXd& qd_ddot,
    const Eigen::MatrixXd& Kp,
    const Eigen::MatrixXd& Kv)
{
    Eigen::VectorXd e = q - qd;
    Eigen::VectorXd e_dot = qdot - qd_dot;

    Eigen::VectorXd v = qd_ddot - Kv * e_dot - Kp * e;

    Eigen::MatrixXd M;
    Eigen::MatrixXd C;
    Eigen::VectorXd g;

    computeInertia(q, M);
    computeCoriolis(q, qdot, C);
    computeGravity(q, g);

    Eigen::VectorXd tau = M * v + C * qdot + g;
    return tau;
}
      

This function can be called inside a real-time loop (e.g., using ROS control) where sensor readings provide \( q, \dot{q} \), and a higher layer supplies the desired trajectory \( q_d(t) \).

Java is used in some robotics platforms (e.g., FRC robots via WPILib). While robotics-specific dynamics libraries are less common than in C++/Python, you can still implement feedback linearization using numeric arrays or a linear algebra library such as EJML. Below is a minimal 2-DOF version using plain arrays:

public class FLController2DOF {

    // Simple diagonal gains
    private final double[] kp = {25.0, 25.0};
    private final double[] kv = {10.0, 10.0};

    // Compute M(q), C(q,qdot), g(q) as in the Python example (omitted details)
    private double[][] M(double[] q) {
        double q1 = q[0];
        double q2 = q[1];
        // Placeholder: user should fill actual expressions
        return new double[][]{
            {1.0, 0.0},
            {0.0, 1.0}
        };
    }

    private double[][] C(double[] q, double[] qdot) {
        return new double[][]{
            {0.0, 0.0},
            {0.0, 0.0}
        };
    }

    private double[] g(double[] q) {
        return new double[]{0.0, 0.0};
    }

    private double[] matVec(double[][] A, double[] x) {
        int n = x.length;
        double[] y = new double[n];
        for (int i = 0; i != n; ++i) {
            double sum = 0.0;
            for (int j = 0; j != n; ++j) {
                sum += A[i][j] * x[j];
            }
            y[i] = sum;
        }
        return y;
    }

    public double[] computeTau(double[] q,
                               double[] qdot,
                               double[] qd,
                               double[] qdDot,
                               double[] qdDDot) {
        double[] e = new double[2];
        double[] eDot = new double[2];
        for (int i = 0; i != 2; ++i) {
            e[i] = q[i] - qd[i];
            eDot[i] = qdot[i] - qdDot[i];
        }

        double[] v = new double[2];
        for (int i = 0; i != 2; ++i) {
            v[i] = qdDDot[i]
                   - kv[i] * eDot[i]
                   - kp[i] * e[i];
        }

        double[][] Mq = M(q);
        double[][] Cq = C(q, qdot);
        double[] gq = g(q);

        double[] Cv = matVec(Cq, qdot);
        double[] Mv = matVec(Mq, v);

        double[] tau = new double[2];
        for (int i = 0; i != 2; ++i) {
            tau[i] = Mv[i] + Cv[i] + gq[i];
        }
        return tau;
    }
}
      

In a Java-based robot stack, such a controller class can be invoked in each control cycle using encoder readings and desired trajectories from higher-level planning modules.

MATLAB with the Robotics System Toolbox and Simscape Multibody is a standard environment for robot control. Dynamics can be computed via rigidBodyTree and inverseDynamics, or symbolically using the Robotics System Toolbox functions. A simple script-level implementation of joint feedback linearization is:

function tau = fl_controller(q, qdot, qd, qd_dot, qd_ddot, Kp, Kv, robot)
% q, qdot, qd, qd_dot, qd_ddot: column vectors
% Kp, Kv: diagonal or full gain matrices
% robot: rigidBodyTree from Robotics System Toolbox

e = q - qd;
e_dot = qdot - qd_dot;

v = qd_ddot - Kv * e_dot - Kp * e;

% Compute dynamics using Robotics System Toolbox:
M = massMatrix(robot, q.');
% velocityProduct(robot,q,qdot) returns C(q,qdot)qdot
cq = velocityProduct(robot, q.', qdot.');
gq = gravityTorque(robot, q.');

tau = M * v + cq.' + gq.';
end
      

In Simulink, a typical implementation uses a block diagram where q, qdot, and desired trajectories are inputs to a MATLAB Function block implementing the above law, and the output tau feeds a plant model (e.g., Simscape Multibody or a custom dynamics S-function).

Wolfram Mathematica is well-suited to derive manipulator dynamics symbolically and then generate feedback-linearizing torque expressions. For a simple 2-link planar arm, one can proceed as follows:

(* Define symbols *)
ClearAll[q1, q2, q1d, q2d, q1dd, q2dd, m1, m2, l1, l2, lc1, lc2, I1, I2, g];
q = {q1, q2};
qd = {q1d, q2d};
qdd = {q1dd, q2dd};

(* Kinetic and potential energy (schematic) *)
T = 1/2 I1 q1d^2 + 1/2 I2 (q1d + q2d)^2;
V = m1 g lc1 Cos[q1] + m2 g (l1 Cos[q1] + lc2 Cos[q1 + q2]);

(* Lagrange equations to obtain M(q), C(q,qdot), g(q) *)
lag = T - V;
eqs = Table[
   D[D[lag, qd[[i]]], t] - D[lag, q[[i]]] == 0,
   {i, 1, 2}
];

(* From eqs, extract M(q) and remaining terms symbolically *)
(* In practice use: M = CoefficientArrays[...] etc. *)

(* Feedback linearization (vector form) *)
Kp = DiagonalMatrix[{kp1, kp2}];
Kv = DiagonalMatrix[{kv1, kv2}];

e = {q1 - q1d_ref[t], q2 - q2d_ref[t]};
ed = D[e, t];

v = D[qd, {t, 2}] - Kv . ed - Kp . e;

(* Torque law *)
tauFL[q_, qd_, v_] := M[q].v + C[q, qd].qd + gVec[q];
      

Mathematica can further generate C or C++ code for the symbolic expressions using CCodeGenerate or Export, enabling verified model-based controllers in embedded targets.

In this lesson, we:

• Expressed manipulator dynamics in control-affine form and identified joint positions as the controlled outputs.
• Introduced a virtual input \( v \) and derived the feedback linearization torque law \( \tau = M(q)v + C(q,\dot{q})\dot{q} + g(q) \).
• Chose \( v = \ddot{q}_d - K_v\dot{e} - K_p e \) to obtain the linear error dynamics \( \ddot{e} + K_v\dot{e} + K_p e = 0 \).
• Proved, via Lyapunov analysis, that suitable positive-definite gains \( K_p, K_v \) guarantee global asymptotic tracking in joint space.
• Illustrated concrete implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, and showed how standard robotics libraries can provide \( M, C, g \) for real robots.

The next lessons will study the robustness of this approach to modeling errors and unmodeled dynamics, and will refine the implementation details needed for real-time execution on physical manipulators.