Consider an n-DOF rigid robot manipulator with joint coordinates \( \mathbf{q}(t)\in\mathbb{R}^n \). You already know from robot dynamics that its motion can be modeled as

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

where \( \mathbf{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis and centrifugal terms, \( \mathbf{g}(\mathbf{q}) \) is gravity, and \( \tau \) is the vector of joint torques.

In joint-space tracking we are given a smooth desired trajectory \( \mathbf{q}_d(t) \) with derivatives \( \dot{\mathbf{q}}_d(t) \), \( \ddot{\mathbf{q}}_d(t) \) and we want \( \mathbf{q}(t) \) to follow it. A PD controller alone must fight against the nonlinear dynamics. The idea of computed-torque control is:

• Use the dynamics model to cancel nonlinearities.
• Inject a linear error feedback law into the “virtual acceleration” command.
• Under perfect modeling, the closed-loop error dynamics are linear and decoupled.

flowchart TD
  QD["Desired trajectory q_d(t), qd_d(t), qdd_d(t)"]
  MEAS["Measured state q(t), qd(t)"]
  ERR["Error: e = q - q_d, ed = qd - qd_d"]
  V["Auxiliary input v = qdd_d - Kd * ed - Kp * e"]
  IDYN["Inverse dynamics: tau = M(q) * v + C(q,qd) * qd + g(q)"]
  PLANT["Robot dynamics M(q) qdd + C(q,qd) qd + g(q) = tau"]

  QD --> ERR
  MEAS --> ERR
  ERR --> V
  V --> IDYN
  IDYN --> PLANT
  PLANT --> MEAS
        

This structure is often described as feedback linearization in joint space: the nonlinear robot looks like a set of decoupled linear second-order systems in the tracking error coordinates.

We assume the standard rigid-body dynamics (no flexible links, no joint elasticity) with generalized coordinates \( \mathbf{q} \). The dynamic model is

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

From your robotics dynamics course you know the following key properties:

• (P1) Inertia matrix: \( \mathbf{M}(\mathbf{q}) \) is symmetric positive definite for all \( \mathbf{q} \); there exist constants \( m_1,m_2 \gt 0 \) such that \( m_1\|\mathbf{x}\|^2 \leq \mathbf{x}^\top\mathbf{M}(\mathbf{q})\mathbf{x} \leq m_2\|\mathbf{x}\|^2 \) for all \( \mathbf{x} \).
• (P2) Skew-symmetry: the matrix \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is skew-symmetric, i.e. \( \mathbf{x}^\top\left(\dot{\mathbf{M}} - 2\mathbf{C}\right)\mathbf{x} = 0 \) for all \( \mathbf{x} \).
• (P3) Boundedness: for bounded \( \mathbf{q}, \dot{\mathbf{q}} \), the matrices \( \mathbf{M}(\mathbf{q}), \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) and vector \( \mathbf{g}(\mathbf{q}) \) remain bounded.

Property (P2) is crucial in Lyapunov analysis because it lets us cancel terms involving \( \dot{\mathbf{M}} \) and \( \mathbf{C} \) in the derivative of an energy-like Lyapunov function.

Define the joint-space tracking errors

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

Let \( \mathbf{K}_p, \mathbf{K}_d \) be symmetric positive definite gain matrices (often chosen diagonal). We introduce a “virtual acceleration” command

\[ \mathbf{v} = \ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}. \]

If we could directly command joint accelerations, the law \( \ddot{\mathbf{q}} = \mathbf{v} \) would produce the error dynamics

\[ \ddot{\mathbf{e}} = -\mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}, \]

which is a stable linear second-order system. However, we do not directly command accelerations; we command torques \( \tau \). Using the dynamics model, we impose

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

Substituting the definition of \( \mathbf{v} \), the computed-torque law becomes

\[ \tau = \mathbf{M}(\mathbf{q}) \left(\ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}\right) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}). \]

Under perfect knowledge of \( \mathbf{M},\mathbf{C},\mathbf{g} \), this choice forces the robot to behave as if the complicated nonlinear dynamics have been cancelled, leaving the desired linear error dynamics.

Plug the computed-torque law into the robot dynamics:

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{M}(\mathbf{q}) \left(\ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}\right) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}). \]

After cancellation of the common terms \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) and \( \mathbf{g}(\mathbf{q}) \), we get

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q}) \left(\ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}\right). \]

Since \( \mathbf{M}(\mathbf{q}) \) is nonsingular, we multiply by \( \mathbf{M}^{-1}(\mathbf{q}) \):

\[ \ddot{\mathbf{q}} = \ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}. \]

Using \( \ddot{\mathbf{e}} = \ddot{\mathbf{q}} - \ddot{\mathbf{q}}_d \) gives

\[ \ddot{\mathbf{e}} = -\mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}. \]

This is an exactly linear, decoupled (if gains are diagonal) second-order system. For each joint \( i \), if \( k_{p,i} > 0, k_{d,i} > 0 \), the characteristic polynomial \( s^2 + k_{d,i}s + k_{p,i} \) is Hurwitz, so the error \( e_i(t) \) converges to zero exponentially for the nominal model.

The linear error dynamics already guarantee exponential convergence under the model used for design. However, it is instructive (and important for robustness extensions) to use a Lyapunov argument based on the full nonlinear dynamics and properties (P1)–(P3).

Consider the Lyapunov candidate

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

By (P1) and positive definiteness of \( \mathbf{K}_p \), there exist positive constants \( \alpha_1, \alpha_2 \) such that

\[ \alpha_1\left(\|\mathbf{e}\|^2 + \|\dot{\mathbf{e}}\|^2\right) \leq V(\mathbf{e},\dot{\mathbf{e}},\mathbf{q}) \leq \alpha_2\left(\|\mathbf{e}\|^2 + \|\dot{\mathbf{e}}\|^2\right). \]

Differentiate \( V \) along trajectories:

\[ \dot{V} = \tfrac{1}{2}\dot{\mathbf{e}}^\top\dot{\mathbf{M}}(\mathbf{q})\dot{\mathbf{e}} + \dot{\mathbf{e}}^\top\mathbf{M}(\mathbf{q})\ddot{\mathbf{e}} + \mathbf{e}^\top\mathbf{K}_p\dot{\mathbf{e}}. \]

From the closed-loop error dynamics derived directly from the nonlinear model,

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{e}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e} = \mathbf{0}, \]

we obtain

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{e}} = -\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} -\mathbf{K}_d\dot{\mathbf{e}} -\mathbf{K}_p\mathbf{e}. \]

Substitute into \( \dot{V} \):

\[ \dot{V} = \tfrac{1}{2}\dot{\mathbf{e}}^\top\dot{\mathbf{M}}(\mathbf{q})\dot{\mathbf{e}} - \dot{\mathbf{e}}^\top\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} - \dot{\mathbf{e}}^\top\mathbf{K}_d\dot{\mathbf{e}} - \dot{\mathbf{e}}^\top\mathbf{K}_p\mathbf{e} + \mathbf{e}^\top\mathbf{K}_p\dot{\mathbf{e}}. \]

The last two terms cancel because they are scalars and \( \mathbf{e}^\top\mathbf{K}_p\dot{\mathbf{e}} = \dot{\mathbf{e}}^\top\mathbf{K}_p\mathbf{e} \). For the first two terms, use skew-symmetry property (P2):

\[ \dot{\mathbf{e}}^\top \left(\tfrac{1}{2}\dot{\mathbf{M}}(\mathbf{q}) - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\right) \dot{\mathbf{e}} = 0. \]

Hence

\[ \dot{V} = -\dot{\mathbf{e}}^\top\mathbf{K}_d\dot{\mathbf{e}} \leq 0. \]

Since \( \mathbf{K}_d \) is positive definite, \( \dot{V} \) is negative semidefinite and \( \dot{\mathbf{e}} \) is square-integrable and bounded. Combined with boundedness of \( \mathbf{M},\mathbf{C},\mathbf{g} \) and standard arguments (Barbalat's lemma or LaSalle's invariance principle), we conclude \( \mathbf{e}(t)\to\mathbf{0}, \dot{\mathbf{e}}(t)\to\mathbf{0} \), i.e., global asymptotic tracking for the nominal model.

In practice, one only has estimated dynamics \( \hat{\mathbf{M}},\hat{\mathbf{C}},\hat{\mathbf{g}} \). The implemented control law becomes

\[ \tau = \hat{\mathbf{M}}(\mathbf{q}) \left(\ddot{\mathbf{q}}_d - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}\right) + \hat{\mathbf{C}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \hat{\mathbf{g}}(\mathbf{q}). \]

Subtracting from the true dynamics introduces a lumped uncertainty \( \mathbf{d}(\mathbf{q},\dot{\mathbf{q}},t) \):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{e}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e} = \mathbf{d}(\mathbf{q},\dot{\mathbf{q}},t). \]

If \( \mathbf{d} \) is bounded, sufficiently large \( \mathbf{K}_d, \mathbf{K}_p \) can guarantee small steady-state tracking errors; this motivates robust and adaptive extensions (sliding-mode, adaptive computed-torque), which will be studied in later chapters.

A typical real-time implementation runs at a fixed sampling period \( T_s \) (e.g. 1 ms). Each cycle executes:

1. Read sensor data: joint positions and velocities \( \mathbf{q},\dot{\mathbf{q}} \).
2. Evaluate desired trajectory \( \mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d \) at current time.
3. Compute errors \( \mathbf{e},\dot{\mathbf{e}} \).
4. Compute \( \mathbf{M},\mathbf{C},\mathbf{g} \) using model (or library).
5. Compute \( \tau \) by the computed-torque formula.
6. Send \( \tau \) to the actuators via the drive interface.

flowchart TD
  T["Time index k"] --> SENS["Read q(k), qd(k)"]
  SENS --> TRAJ["Evaluate q_d(k), qd_d(k), qdd_d(k)"]
  TRAJ --> ERRK["Compute e(k), ed(k)"]
  ERRK --> DYN["Evaluate M(q(k)), C(q(k),qd(k)), g(q(k))"]
  DYN --> CT["Compute tau(k) with computed-torque law"]
  CT --> ACT["Send tau(k) to drives"]
  ACT --> T
        

The main computational load comes from evaluating \( \mathbf{M},\mathbf{C},\mathbf{g} \). Efficient libraries (symbolically generated or optimized C/C++ implementations) are usually employed.

In Python, one can either:

• Use a robotics library such as roboticstoolbox or pin (Pinocchio bindings) to compute M, C, g.
• Implement dynamics from scratch for a specific manipulator.

The following example implements computed-torque control for a simple 2-link planar arm using numpy. Dynamics formulas are standard from robotics texts.

import numpy as np

# Physical parameters (example values)
g = 9.81
m1, m2 = 1.0, 1.0
l1, l2 = 1.0, 1.0
lc1, lc2 = 0.5, 0.5
I1, I2 = 0.1, 0.1

def M_mat(q):
    q1, q2 = q
    c2 = np.cos(q2)
    M11 = I1 + I2 + m1*lc1**2 + m2*(l1**2 + lc2**2 + 2*l1*lc2*c2)
    M22 = I2 + m2*lc2**2
    M12 = I2 + m2*(lc2**2 + l1*lc2*c2)
    M21 = M12
    return np.array([[M11, M12],
                     [M21, M22]])

def C_mat(q, qd):
    q1, q2 = q
    q1d, q2d = qd
    s2 = np.sin(q2)
    h = -m2*l1*lc2*s2
    C11 = h*q2d
    C12 = h*(q1d + q2d)
    C21 = -h*q1d
    C22 = 0.0
    return np.array([[C11, C12],
                     [C21, C22]])

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

def computed_torque(q, qd, q_d, qd_d, qdd_d, Kp, Kd):
    e = q - q_d
    ed = qd - qd_d
    v = qdd_d - Kd @ ed - Kp @ e
    M = M_mat(q)
    C = C_mat(q, qd)
    g_vec_q = g_vec(q)
    tau = M @ v + C @ qd + g_vec_q
    return tau

# Example of one control step
Kp = np.diag([50.0, 50.0])
Kd = np.diag([10.0, 10.0])

q    = np.array([0.1, 0.1])
qd   = np.array([0.0, 0.0])
q_d  = np.array([0.5, 0.3])
qd_d = np.array([0.0, 0.0])
qdd_d = np.array([0.0, 0.0])

tau = computed_torque(q, qd, q_d, qd_d, qdd_d, Kp, Kd)
print("tau =", tau)
      

Replacing the hand-coded dynamics with calls to roboticstoolbox or pinocchio is straightforward: those libraries provide APIs like robot.inertia(q), robot.coriolis(q, qd), and robot.gravload(q).

9.1 C++ with Eigen (and RBDL / Pinocchio)

In C++, rigid-body dynamics libraries such as RBDL or Pinocchio are commonly used. They provide fast routines to compute M, C, g. Below is a minimal sketch using Eigen-style vectors for a generic manipulator:

#include <Eigen/Dense>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// Suppose we have functions (from RBDL or your own code):
//   MatrixXd M_mat(const VectorXd& q);
//   MatrixXd C_mat(const VectorXd& q, const VectorXd& qd);
//   VectorXd g_vec(const VectorXd& q);

VectorXd computedTorque(
    const VectorXd& q,
    const VectorXd& qd,
    const VectorXd& q_d,
    const VectorXd& qd_d,
    const VectorXd& qdd_d,
    const MatrixXd& Kp,
    const MatrixXd& Kd)
{
    VectorXd e  = q  - q_d;
    VectorXd ed = qd - qd_d;
    VectorXd v  = qdd_d - Kd * ed - Kp * e;

    MatrixXd M = M_mat(q);
    MatrixXd C = C_mat(q, qd);
    VectorXd g = g_vec(q);

    VectorXd tau = M * v + C * qd + g;
    return tau;
}
      

In a real project, M_mat, C_mat, and g_vec would wrap calls to RBDL::CompositeRigidBodyAlgorithm, RBDL::NonlinearEffects, etc., or their Pinocchio equivalents.

9.2 Java with EJML

Java does not have as many mature open-source robotics dynamics libraries, but you can combine an auto-generated dynamics model with a linear algebra library such as EJML.

import org.ejml.simple.SimpleMatrix;

public class ComputedTorqueController {

    // Example placeholders - implement your own dynamics or call a generator
    public SimpleMatrix M(SimpleMatrix q) { /* ... */ return null; }
    public SimpleMatrix C(SimpleMatrix q, SimpleMatrix qd) { /* ... */ return null; }
    public SimpleMatrix g(SimpleMatrix q) { /* ... */ return null; }

    public SimpleMatrix computeTau(
            SimpleMatrix q,
            SimpleMatrix qd,
            SimpleMatrix q_d,
            SimpleMatrix qd_d,
            SimpleMatrix qdd_d,
            SimpleMatrix Kp,
            SimpleMatrix Kd) {

        SimpleMatrix e  = q.minus(q_d);
        SimpleMatrix ed = qd.minus(qd_d);
        SimpleMatrix v  = qdd_d.minus(Kd.mult(ed)).minus(Kp.mult(e));

        SimpleMatrix Mq = M(q);
        SimpleMatrix Cq = C(q, qd);
        SimpleMatrix gq = g(q);

        SimpleMatrix tau = Mq.mult(v).plus(Cq.mult(qd)).plus(gq);
        return tau;
    }
}
      

The structure mirrors the Python and C++ versions. The only difference is the matrix API and the way you supply the dynamics routines.

10.1 MATLAB Function and Simulink Structure

MATLAB provides the Robotics System Toolbox with functions such as massMatrix, coriolis, and gravityTorque for rigidBodyTree models. A generic computed-torque function can be written as:

function tau = computed_torque(q, qd, q_d, qd_d, qdd_d, Kp, Kd, robot)
    % q, qd, ... are column vectors
    e  = q  - q_d;
    ed = qd - qd_d;

    v = qdd_d - Kd * ed - Kp * e;

    M = massMatrix(robot, q');
    C = coriolis(robot, q', qd');
    g = gravityTorque(robot, q');

    tau = M * v + C * qd + g';
end
      

In Simulink, a common implementation uses:

• Blocks that integrate \( \ddot{\mathbf{q}} \) to obtain \( \dot{\mathbf{q}},\mathbf{q} \).
• A block computing trajectory \( \mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d \).
• A MATLAB Function block that calls computed_torque and outputs \( \tau \).
• A plant model block implementing the robot dynamics (either a Simscape model or custom S-function).

10.2 Wolfram Mathematica (Symbolic Derivation)

Mathematica is particularly convenient for symbolic derivation of the inverse-dynamics law. For a given Lagrangian, one can derive \( \mathbf{M},\mathbf{C},\mathbf{g} \) symbolically and then generate code.

(* Example skeleton for n-DOF symbolic computed-torque *)
Clear[q, qd, qdd, qdD, qddD, e, ed, v, Kp, Kd, M, C, g];

n = 2;
q    = Array[q, n];
qd   = Array[qd, n];
qdd  = Array[qdd, n];
qD   = Array[qD, n];   (* desired q_d *)
qdD  = Array[qdD, n];  (* desired qd_d *)
qddD = Array[qddD, n]; (* desired qdd_d *)

Kp = DiagonalMatrix[Array[Subscript[kp, #] &, n]];
Kd = DiagonalMatrix[Array[Subscript[kd, #] &, n]];

e  = q - qD;
ed = qd - qdD;
v  = qddD - Kd.ed - Kp.e;

(* Assume M[q], C[q,qd], g[q] have been symbolically derived *)
tau = M[q].v + C[q, qd].qd + g[q] // Simplify;
      

From here, Mathematica can export optimized C or C++ code for real-time execution using CCodeGenerate or the newer Export-based workflows.

In this lesson we:

• Formulated the joint-space tracking problem for rigid manipulators.
• Reviewed structural properties of robot dynamics crucial for control design.
• Derived the computed-torque (inverse-dynamics) control law and showed that, under perfect modeling, it yields exactly linear error dynamics.
• Proved global asymptotic tracking using a Lyapunov function based on kinetic and potential energy of the error coordinates.
• Discussed robustness issues when only approximate models are available.
• Presented implementation sketches in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

In the next lesson we will generalize this idea to more abstract nonlinear feedback linearization, including input–output decoupling and relative degree analysis for robot manipulators.