Consider an n-DOF robot manipulator with generalized coordinates \( \mathbf{q}\in\mathbb{R}^n \) and joint torques \( \boldsymbol{\tau}\in\mathbb{R}^n \). The standard Euler–Lagrange rigid-body dynamics are

\[ \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 symmetric positive definite, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is the Coriolis/centrifugal matrix, and \( \mathbf{g}(\mathbf{q}) \) is the gravity vector. Model-based nonlinear controllers (computed-torque / feedback linearization) use an explicit model of these terms to transform the nonlinear dynamics into a (nominally) linear error system.

In this lesson we answer three questions:

• Under a perfect model, can we guarantee global asymptotic or exponential tracking of a smooth reference trajectory?
• When the model is imperfect (parametric uncertainty, unmodeled friction, disturbances), what type of robust stability (e.g., ISS, ultimate boundedness) can we prove?
• How do these properties manifest in practice through simulation code in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica?

flowchart TD
  A["Robot dynamics: M(q) qdd + C(q,qd) qd + g(q) = tau"] --> B["Choose model-based law: computed torque / feedback lin."]
  B --> C["Closed-loop error dynamics for e = q - q_d"]
  C --> D["Lyapunov analysis: V(e, edot, q)"]
  D --> E["Stability under perfect model"]
  E --> F["Add modeling errors and disturbances"]
  F --> G["ISS / ultimate boundedness, robustness margins"]
        

Let \( \mathbf{q}_d(t) \) be a desired joint trajectory of class \( C^2 \) with derivatives \( \dot{\mathbf{q}}_d, \ddot{\mathbf{q}}_d \). Define the tracking error

\[ \mathbf{e} := \mathbf{q} - \mathbf{q}_d, \qquad \dot{\mathbf{e}} := \dot{\mathbf{q}} - \dot{\mathbf{q}}_d. \]

The standard computed-torque (inverse-dynamics) law designed in Lesson 1 is

\[ \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}), \]

with symmetric positive definite gain matrices \( \mathbf{K}_p \succ 0, \mathbf{K}_d \succ 0 \). Substituting this law into the manipulator dynamics and using the definitions of \( \mathbf{e}, \dot{\mathbf{e}} \), we obtain the error dynamics

\[ \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}. \]

This is a nonlinear second-order system in \( (\mathbf{e}, \dot{\mathbf{e}}) \) with state-dependent inertia and Coriolis matrices. Its stability can be studied using Lyapunov methods that exploit the structural properties of \( \mathbf{M} \) and \( \mathbf{C} \).

A key robot property (from kinematics/dynamics course) is

\[ \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \;\text{is skew-symmetric, i.e.,}\; \mathbf{x}^\top\left(\dot{\mathbf{M}} - 2\mathbf{C}\right)\mathbf{x} = 0 \;\text{for all } \mathbf{x}. \]

We will use this property heavily in the Lyapunov proof.

Define 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}. \]

Since \( \mathbf{M}(\mathbf{q}) \succ 0 \) and \( \mathbf{K}_p \succ 0 \), there exist constants \( m_{\min}, m_{\max}, k_{\min}, k_{\max} > 0 \) such that

\[ \tfrac{1}{2} m_{\min}\|\dot{\mathbf{e}}\|^2 + \tfrac{1}{2}k_{\min}\|\mathbf{e}\|^2 \;\le\; V \;\le\; \tfrac{1}{2} m_{\max}\|\dot{\mathbf{e}}\|^2 + \tfrac{1}{2}k_{\max}\|\mathbf{e}\|^2. \]

Thus \( V \) is radially unbounded and positive definite in \( (\mathbf{e},\dot{\mathbf{e}}) \). Differentiating along trajectories:

\[ \begin{aligned} \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}} + \dot{\mathbf{e}}^\top \mathbf{K}_p \mathbf{e} \\ &= \tfrac{1}{2}\dot{\mathbf{e}}^\top \dot{\mathbf{M}}\dot{\mathbf{e}} + \dot{\mathbf{e}}^\top \left[-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p \mathbf{e}\right] + \dot{\mathbf{e}}^\top \mathbf{K}_p \mathbf{e} \\ &= \tfrac{1}{2}\dot{\mathbf{e}}^\top \dot{\mathbf{M}}\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}}. \end{aligned} \]

The gravity term disappears because it is exactly canceled by the controller; the \( \mathbf{K}_p \) terms cancel algebraically. Using the skew-symmetry property,

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

Therefore

\[ \dot{V} = -\dot{\mathbf{e}}^\top \mathbf{K}_d \dot{\mathbf{e}} \;\le\; -k_{d,\min}\|\dot{\mathbf{e}}\|^2 \]

where \( k_{d,\min} > 0 \) is the smallest eigenvalue of \( \mathbf{K}_d \). Hence \( \dot{V} \le 0 \) and \( V \) is nonincreasing.

The set where \( \dot{V} = 0 \) is \( \{(\mathbf{e},\dot{\mathbf{e}}) : \dot{\mathbf{e}} = \mathbf{0}\} \). On this set, the error dynamics reduce to \( \mathbf{K}_p \mathbf{e} = \mathbf{0} \), so \( \mathbf{e} = \mathbf{0} \) as well. Therefore the largest invariant set inside \( \{\dot{V}=0\} \) is just the origin, and by LaSalle's invariance principle (from Chapter 1) we obtain:

Theorem. Under perfect model knowledge and constant positive definite gains \( \mathbf{K}_p, \mathbf{K}_d \), the origin \( (\mathbf{e},\dot{\mathbf{e}})=(\mathbf{0},\mathbf{0}) \) of the error dynamics is globally asymptotically stable. Moreover, the convergence is globally uniform for bounded desired trajectories.

The convergence can be strengthened to exponential under additional assumptions (e.g., bounded \( \mathbf{q}(t) \), \( \dot{\mathbf{q}}(t) \), and uniform bounds on \( \mathbf{M}, \mathbf{M}^{-1} \)), by comparison with a linear time-varying system with bounded coefficients.

Real robots never match the nominal model exactly. Let the true dynamics be

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

where \( \mathbf{d} \) aggregates modeling errors, friction, and disturbances (e.g., payload changes). Using the same computed-torque law as before (built from the nominal model), the error dynamics become

\[ \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}(t,\mathbf{q},\dot{\mathbf{q}}). \]

With the same Lyapunov function \( V \) as before, we obtain

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

Using Cauchy–Schwarz and Young inequalities for any \( \varepsilon > 0 \),

\[ \dot{\mathbf{e}}^\top \mathbf{d} \le \|\dot{\mathbf{e}}\|\,\|\mathbf{d}\| \le \tfrac{\varepsilon}{2}\|\dot{\mathbf{e}}\|^2 + \tfrac{1}{2\varepsilon}\|\mathbf{d}\|^2. \]

Choosing \( \varepsilon = k_{d,\min} \), we get

\[ \dot{V} \le -\left(k_{d,\min} - \tfrac{k_{d,\min}}{2}\right)\|\dot{\mathbf{e}}\|^2 + \tfrac{1}{2k_{d,\min}}\|\mathbf{d}\|^2 = -\tfrac{k_{d,\min}}{2}\|\dot{\mathbf{e}}\|^2 + \tfrac{1}{2k_{d,\min}}\|\mathbf{d}\|^2. \]

Since \( \|\dot{\mathbf{e}}\|^2 \) is bounded above and below by \( V \) (up to constants), we can express this as a standard input-to-state stability (ISS) inequality:

\[ \dot{V} \le -\alpha(V) + \gamma(\|\mathbf{d}\|) \]

for suitable class-\(\mathcal{K}_\infty\) functions \( \alpha, \gamma \). From the ISS theory reviewed in Chapter 1, we obtain:

Proposition (ISS). If \( \mathbf{d}(t,\mathbf{q},\dot{\mathbf{q}}) \) is bounded, then the origin of the error dynamics is input-to-state stable. In particular, if \( \sup_t\|\mathbf{d}(t,\cdot)\|\le d_{\max} \), then \( (\mathbf{e},\dot{\mathbf{e}}) \) is ultimately bounded inside a ball whose radius is \( O(d_{\max}) \), and the bound shrinks when \( \mathbf{K}_d, \mathbf{K}_p \) are increased.

This formalizes a key robustness intuition: large feedback gains increase disturbance rejection but may excite unmodeled high-frequency dynamics (to be discussed later in implementation lessons).

Now suppose the controller uses nominal parameters \( \mathbf{M}_n(\mathbf{q}), \mathbf{C}_n(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}_n(\mathbf{q}) \), while the true robot has \( \mathbf{M}_r, \mathbf{C}_r, \mathbf{g}_r \). The true dynamics are

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

The controller, however, computes

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

Subtracting the nominal and real models, we can write the error dynamics in the compact form

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

where \( \boldsymbol{\Delta} \) collects all mismatch terms such as \( (\mathbf{M}_n - \mathbf{M}_r)(\ddot{\mathbf{q}}_d - \mathbf{K}_d \dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e}) \), and differences between \( \mathbf{C}_n, \mathbf{C}_r \), \( \mathbf{g}_n, \mathbf{g}_r \). Under smoothness and bounded desired trajectories, there exist constants \( k_1, k_2, k_3 \ge 0 \) such that

\[ \|\boldsymbol{\Delta}\| \le k_1\|\mathbf{e}\| + k_2\|\dot{\mathbf{e}}\| + k_3. \]

Substituting this into the \( \dot{V} \) expression and using standard inequalities, one can show that for sufficiently large \( \mathbf{K}_p, \mathbf{K}_d \) the origin of the error dynamics is locally asymptotically stable when the parametric mismatch is small, and ultimately bounded when the mismatch is finite.

A simple and practically important special case is gravity mismatch: assume the inertia and Coriolis terms are modeled correctly (\( \mathbf{M}_n=\mathbf{M}_r, \mathbf{C}_n=\mathbf{C}_r \)), but the gravity model is approximate \( \mathbf{g}_n = \mathbf{g}_r + \boldsymbol{\delta}_g(\mathbf{q}) \). Then the steady-state error for a constant desired position \( \mathbf{q}_d \) satisfies

\[ \mathbf{K}_p \mathbf{e}_\infty = -\boldsymbol{\delta}_g(\mathbf{q}_d + \mathbf{e}_\infty). \]

If \( \boldsymbol{\delta}_g \) is small and Lipschitz around \( \mathbf{q}_d \), then \( \|\mathbf{e}_\infty\| = O(\|\boldsymbol{\delta}_g(\mathbf{q}_d)\|) \); i.e., the position error at equilibrium is proportional to the gravity modeling error, and can be reduced by increasing \( \mathbf{K}_p \) or improving the gravity model (e.g., via identification or adaptive control in later chapters).

A common practical procedure is:

• Choose desired linear error dynamics \( \ddot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e} = \mathbf{0} \) by analogy with decoupled second-order systems.
• Map scalar design parameters (e.g., natural frequency and damping ratio) to diagonal gains.
• Verify robustness margins under estimated modeling errors using ISS-type bounds.

For instance, for decoupled joints with scalar gains \( k_{p,i}, k_{d,i} \), one can take

\[ k_{p,i} = \omega_{n,i}^2, \qquad k_{d,i} = 2\zeta_i\omega_{n,i}, \]

where \( \omega_{n,i} \) is a desired natural frequency and \( \zeta_i \in (0,1) \) a damping ratio. The robustness analysis then uses bounds of the form

\[ \|\mathbf{e}(t)\| \le \beta(\|(\mathbf{e}(0),\dot{\mathbf{e}}(0))\|,t) + \gamma(d_{\max}), \]

where \( \beta \) describes the nominal exponential decay and \( \gamma(d_{\max}) \) is an increasing function of the disturbance bound.

flowchart TD
  S["Specify tracking specs (omega_n, zeta)"] --> G["Compute Kp, Kd from specs"]
  G --> B["Estimate modeling error bounds"]
  B --> R["Check ISS / ultimate bound on (e, edot)"]
  R --> A["Adjust gains or refine model"]
        

We simulate a 2-DOF planar manipulator with a computed-torque controller. The controller uses nominal link masses different from the true ones, and we compare the tracking error. Here we implement the dynamics from scratch using numpy and integrate with scipy.integrate.solve_ivp. You can replace the hand-coded dynamics with roboticstoolbox models for more complex robots.

import numpy as np
from numpy import cos, sin
from dataclasses import dataclass

@dataclass
class TwoLinkParams:
    l1: float
    l2: float
    m1: float
    m2: float
    I1: float
    I2: float
    g: float = 9.81

# True and nominal parameters
true_params = TwoLinkParams(l1=1.0, l2=1.0,
                            m1=3.0, m2=2.0,
                            I1=0.2, I2=0.1)
nom_params  = TwoLinkParams(l1=1.0, l2=1.0,
                            m1=2.5, m2=1.5,   # under-estimated masses
                            I1=0.2, I2=0.1)

def M_matrix(q, p: TwoLinkParams):
    q1, q2 = q
    c2 = cos(q2)
    m11 = p.I1 + p.I2 + p.m1*(p.l1**2)/4.0 + p.m2*(p.l1**2 + (p.l2**2)/4.0 + p.l1*p.l2*c2)
    m12 = p.I2 + p.m2*((p.l2**2)/4.0 + 0.5*p.l1*p.l2*c2)
    m22 = p.I2 + p.m2*(p.l2**2)/4.0
    return np.array([[m11, m12],
                     [m12, m22]])

def C_matrix(q, qd, p: TwoLinkParams):
    q1, q2 = q
    q1d, q2d = qd
    s2 = sin(q2)
    h = -p.m2*p.l1*p.l2*s2*(2*q1d*q2d + q2d**2)
    h1 = p.m2*p.l1*p.l2*s2*(q1d**2)
    # A simple skew-symmetric-like structure
    c11 = -p.m2*p.l1*p.l2*s2*q2d
    c12 = -p.m2*p.l1*p.l2*s2*(q1d + q2d)
    c21 =  p.m2*p.l1*p.l2*s2*q1d
    c22 = 0.0
    return np.array([[c11, c12],
                     [c21, c22]])

def g_vector(q, p: TwoLinkParams):
    q1, q2 = q
    g1 = (p.m1*p.l1/2.0 + p.m2*p.l1)*p.g*cos(q1) + p.m2*p.l2/2.0*p.g*cos(q1 + q2)
    g2 = p.m2*p.l2/2.0*p.g*cos(q1 + q2)
    return np.array([g1, g2])

# Desired trajectory: simple sinusoid
def qd_des(t):
    qd  = np.array([0.5*np.sin(0.5*t), 0.3*np.sin(0.5*t)])
    qd1 = np.array([0.5*0.5*np.cos(0.5*t), 0.3*0.5*np.cos(0.5*t)])
    qd2 = np.array([-0.5*(0.5**2)*np.sin(0.5*t), -0.3*(0.5**2)*np.sin(0.5*t)])
    return qd, qd1, qd2

Kp = np.diag([25.0, 16.0])
Kd = np.diag([10.0, 8.0])

def dynamics(t, x):
    q  = x[0:2]
    qd = x[2:4]
    qd_d, qd1_d, qd2_d = qd_des(t)

    e   = q  - qd_d
    ed  = qd - qd1_d

    # Computed-torque using nominal model
    Mn = M_matrix(q, nom_params)
    Cn = C_matrix(q, qd, nom_params)
    gn = g_vector(q, nom_params)
    v  = qd2_d - Kd @ ed - Kp @ e
    tau = Mn @ v + Cn @ qd + gn

    # True robot dynamics
    Mr = M_matrix(q, true_params)
    Cr = C_matrix(q, qd, true_params)
    gr = g_vector(q, true_params)
    qdd = np.linalg.solve(Mr, tau - Cr @ qd - gr)

    return np.hstack((qd, qdd))

# Integrate with solve_ivp
if __name__ == "__main__":
    from scipy.integrate import solve_ivp
    T = 10.0
    x0 = np.zeros(4)  # start at rest
    sol = solve_ivp(dynamics, [0.0, T], x0, max_step=0.01, rtol=1e-6, atol=1e-8)
    t = sol.t
    q  = sol.y[0:2, :]
    qd = np.array([qd_des(tt)[0] for tt in t]).T
    e  = q - qd

    import matplotlib.pyplot as plt
    plt.figure()
    plt.plot(t, e[0, :], label="e1")
    plt.plot(t, e[1, :], label="e2")
    plt.xlabel("time [s]")
    plt.ylabel("tracking error [rad]")
    plt.legend()
    plt.grid(True)
    plt.show()
      

By changing the nominal parameters (e.g., nom_params.m1) you can observe how the ultimate bound on the tracking error changes, confirming the robustness analysis.

In C++, one can implement the same 2-DOF controller using the Eigen library for linear algebra. For more complex robots, libraries like Pinocchio, RBDL, or Orocos KDL can provide efficient functions for computing \( \mathbf{M}, \mathbf{C}, \mathbf{g} \). Below is a minimal sketch.

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

using Eigen::Vector2d;
using Eigen::Matrix2d;

struct TwoLinkParams {
  double l1, l2;
  double m1, m2;
  double I1, I2;
  double g;
};

Matrix2d M_matrix(const Vector2d& q, const TwoLinkParams& p) {
  double q2 = q(1);
  double c2 = std::cos(q2);
  double m11 = p.I1 + p.I2 + p.m1*(p.l1*p.l1)/4.0
               + p.m2*(p.l1*p.l1 + (p.l2*p.l2)/4.0 + p.l1*p.l2*c2);
  double m12 = p.I2 + p.m2*((p.l2*p.l2)/4.0 + 0.5*p.l1*p.l2*c2);
  double m22 = p.I2 + p.m2*(p.l2*p.l2)/4.0;
  Matrix2d M;
  M << m11, m12,
        m12, m22;
  return M;
}

Matrix2d C_matrix(const Vector2d& q, const Vector2d& qd, const TwoLinkParams& p) {
  double q2 = q(1);
  double q1d = qd(0);
  double q2d = qd(1);
  double s2 = std::sin(q2);
  double c11 = -p.m2*p.l1*p.l2*s2*q2d;
  double c12 = -p.m2*p.l1*p.l2*s2*(q1d + q2d);
  double c21 =  p.m2*p.l1*p.l2*s2*q1d;
  double c22 = 0.0;
  Matrix2d C;
  C << c11, c12,
        c21, c22;
  return C;
}

Vector2d g_vector(const Vector2d& q, const TwoLinkParams& p) {
  double q1 = q(0);
  double q2 = q(1);
  double g1 = (p.m1*p.l1/2.0 + p.m2*p.l1)*p.g*std::cos(q1)
              + p.m2*p.l2/2.0*p.g*std::cos(q1 + q2);
  double g2 = p.m2*p.l2/2.0*p.g*std::cos(q1 + q2);
  return Vector2d(g1, g2);
}

int main() {
  TwoLinkParams trueP{1.0, 1.0, 3.0, 2.0, 0.2, 0.1, 9.81};
  TwoLinkParams nomP {1.0, 1.0, 2.5, 1.5, 0.2, 0.1, 9.81};

  Matrix2d Kp = Matrix2d::Zero();
  Matrix2d Kd = Matrix2d::Zero();
  Kp(0,0) = 25.0; Kp(1,1) = 16.0;
  Kd(0,0) = 10.0; Kd(1,1) =  8.0;

  double dt = 0.001;
  Vector2d q   = Vector2d::Zero();
  Vector2d qd  = Vector2d::Zero();

  for (int k = 0; k < 10000; ++k) {
    double t = k*dt;

    // Example desired trajectory (replace with a function as in Python)
    Vector2d qd_d(0.5*std::sin(0.5*t), 0.3*std::sin(0.5*t));
    Vector2d qd1_d(0.5*0.5*std::cos(0.5*t), 0.3*0.5*std::cos(0.5*t));
    Vector2d qd2_d(-0.5*0.5*0.5*std::sin(0.5*t), -0.3*0.5*0.5*std::sin(0.5*t));

    Vector2d e  = q  - qd_d;
    Vector2d ed = qd - qd1_d;

    Matrix2d Mn = M_matrix(q, nomP);
    Matrix2d Cn = C_matrix(q, qd, nomP);
    Vector2d gn = g_vector(q, nomP);

    Vector2d v   = qd2_d - Kd*ed - Kp*e;
    Vector2d tau = Mn*v + Cn*qd + gn;

    Matrix2d Mr = M_matrix(q, trueP);
    Matrix2d Cr = C_matrix(q, qd, trueP);
    Vector2d gr = g_vector(q, trueP);

    Vector2d qdd = Mr.ldlt().solve(tau - Cr*qd - gr);

    // simple Euler integration
    q  += dt*qd;
    qd += dt*qdd;
  }

  std::cout << "Final q: " << q.transpose() << std::endl;
  return 0;
}
      

When using Pinocchio or RBDL, you would replace M_matrix, C_matrix, and g_vector by calls to their respective routines (e.g., pinocchio::crba, pinocchio::nonLinearEffects).

In Java, we can use EJML for basic linear algebra. Robot-specific libraries are less common, so we typically implement the dynamics formulas ourselves. The structure mirrors the Python and C++ versions.

import org.ejml.simple.SimpleMatrix;

class TwoLinkParams {
    double l1, l2;
    double m1, m2;
    double I1, I2;
    double g;
}

public class TwoLinkComputedTorque {

    static SimpleMatrix M(SimpleMatrix q, TwoLinkParams p) {
        double q2 = q.get(1);
        double c2 = Math.cos(q2);
        double m11 = p.I1 + p.I2 + p.m1*(p.l1*p.l1)/4.0
                     + p.m2*(p.l1*p.l1 + (p.l2*p.l2)/4.0 + p.l1*p.l2*c2);
        double m12 = p.I2 + p.m2*((p.l2*p.l2)/4.0 + 0.5*p.l1*p.l2*c2);
        double m22 = p.I2 + p.m2*(p.l2*p.l2)/4.0;
        double[][] data = { {m11, m12},
                           {m12, m22} };
        return new SimpleMatrix(data);
    }

    static SimpleMatrix C(SimpleMatrix q, SimpleMatrix qd, TwoLinkParams p) {
        double q2  = q.get(1);
        double q1d = qd.get(0);
        double q2d = qd.get(1);
        double s2  = Math.sin(q2);
        double c11 = -p.m2*p.l1*p.l2*s2*q2d;
        double c12 = -p.m2*p.l1*p.l2*s2*(q1d + q2d);
        double c21 =  p.m2*p.l1*p.l2*s2*q1d;
        double c22 = 0.0;
        double[][] data = { {c11, c12},
                           {c21, c22} };
        return new SimpleMatrix(data);
    }

    static SimpleMatrix g(SimpleMatrix q, TwoLinkParams p) {
        double q1 = q.get(0);
        double q2 = q.get(1);
        double g1 = (p.m1*p.l1/2.0 + p.m2*p.l1)*p.g*Math.cos(q1)
                    + p.m2*p.l2/2.0*p.g*Math.cos(q1 + q2);
        double g2 = p.m2*p.l2/2.0*p.g*Math.cos(q1 + q2);
        return new SimpleMatrix(new double[][]{ {g1},{g2} });
    }

    public static void main(String[] args) {
        TwoLinkParams trueP = new TwoLinkParams();
        trueP.l1 = 1.0; trueP.l2 = 1.0;
        trueP.m1 = 3.0; trueP.m2 = 2.0;
        trueP.I1 = 0.2; trueP.I2 = 0.1;
        trueP.g  = 9.81;

        TwoLinkParams nomP = new TwoLinkParams();
        nomP.l1 = 1.0; nomP.l2 = 1.0;
        nomP.m1 = 2.5; nomP.m2 = 1.5;
        nomP.I1 = 0.2; nomP.I2 = 0.1;
        nomP.g  = 9.81;

        SimpleMatrix Kp = SimpleMatrix.diag(25.0, 16.0);
        SimpleMatrix Kd = SimpleMatrix.diag(10.0, 8.0);

        double dt = 0.001;
        SimpleMatrix q  = new SimpleMatrix(2,1);
        SimpleMatrix qd = new SimpleMatrix(2,1);

        for (int k = 0; k < 10000; ++k) {
            double t = k*dt;
            double qd1 = 0.5*Math.sin(0.5*t);
            double qd2 = 0.3*Math.sin(0.5*t);
            double qd1d = 0.5*0.5*Math.cos(0.5*t);
            double qd2d = 0.3*0.5*Math.cos(0.5*t);
            double qd1dd = -0.5*0.5*0.5*Math.sin(0.5*t);
            double qd2dd = -0.3*0.5*0.5*Math.sin(0.5*t);

            SimpleMatrix qd_d   = new SimpleMatrix(new double[][]{ {qd1},{qd2} });
            SimpleMatrix qd1_d  = new SimpleMatrix(new double[][]{ {qd1d},{qd2d} });
            SimpleMatrix qd2_d  = new SimpleMatrix(new double[][]{ {qd1dd},{qd2dd} });

            SimpleMatrix e  = q.minus(qd_d);
            SimpleMatrix ed = qd.minus(qd1_d);

            SimpleMatrix Mn = M(q, nomP);
            SimpleMatrix Cn = C(q, qd, nomP);
            SimpleMatrix gn = g(q, nomP);

            SimpleMatrix v   = qd2_d.minus(Kd.mult(ed)).minus(Kp.mult(e));
            SimpleMatrix tau = Mn.mult(v).plus(Cn.mult(qd)).plus(gn);

            SimpleMatrix Mr = M(q, trueP);
            SimpleMatrix Cr = C(q, qd, trueP);
            SimpleMatrix gr = g(q, trueP);

            SimpleMatrix qdd = Mr.solve(tau.minus(Cr.mult(qd)).minus(gr));
            q  = q.plus(qd.scale(dt));
            qd = qd.plus(qdd.scale(dt));
        }

        q.print("Final q:");
    }
}
      

This structure is typical for real-time robot controllers implemented on the JVM (e.g., for simulation or middleware), with dynamics functions factored out and gains configurable at runtime.

10.1 MATLAB / Simulink

Using MATLAB, we can implement the same dynamics and computed-torque controller and then connect it to a Simulink model for more realistic discrete-time simulation and saturation modeling.

function dx = twolink_ctc(t, x)
    % x = [q1; q2; q1d; q2d]
    q  = x(1:2);
    qd = x(3:4);

    % True parameters
    p.l1 = 1.0; p.l2 = 1.0;
    p.m1 = 3.0; p.m2 = 2.0;
    p.I1 = 0.2; p.I2 = 0.1;
    p.g  = 9.81;

    % Nominal parameters (controller)
    pn = p;
    pn.m1 = 2.5; pn.m2 = 1.5;

    % Desired trajectory
    w = 0.5;
    qd_d  = [0.5*sin(w*t); 0.3*sin(w*t)];
    qd1_d = [0.5*w*cos(w*t); 0.3*w*cos(w*t)];
    qd2_d = [-0.5*w^2*sin(w*t); -0.3*w^2*sin(w*t)];

    e  = q  - qd_d;
    ed = qd - qd1_d;

    Kp = diag([25, 16]);
    Kd = diag([10, 8]);

    Mn = Mmat(q, pn);
    Cn = Cmat(q, qd, pn);
    gn = gvec(q, pn);

    v   = qd2_d - Kd*ed - Kp*e;
    tau = Mn*v + Cn*qd + gn;

    Mr = Mmat(q, p);
    Cr = Cmat(q, qd, p);
    gr = gvec(q, p);

    qdd = Mr \ (tau - Cr*qd - gr);
    dx = [qd; qdd];
end

function M = Mmat(q, p)
    q2 = q(2);
    c2 = cos(q2);
    m11 = p.I1 + p.I2 + p.m1*(p.l1^2)/4 + p.m2*(p.l1^2 + (p.l2^2)/4 + p.l1*p.l2*c2);
    m12 = p.I2 + p.m2*((p.l2^2)/4 + 0.5*p.l1*p.l2*c2);
    m22 = p.I2 + p.m2*(p.l2^2)/4;
    M = [m11 m12; m12 m22];
end

function C = Cmat(q, qd, p)
    q2  = q(2);
    q1d = qd(1); q2d = qd(2);
    s2 = sin(q2);
    c11 = -p.m2*p.l1*p.l2*s2*q2d;
    c12 = -p.m2*p.l1*p.l2*s2*(q1d + q2d);
    c21 =  p.m2*p.l1*p.l2*s2*q1d;
    c22 = 0;
    C = [c11 c12; c21 c22];
end

function g = gvec(q, p)
    q1 = q(1); q2 = q(2);
    g1 = (p.m1*p.l1/2 + p.m2*p.l1)*p.g*cos(q1) + p.m2*p.l2/2*p.g*cos(q1 + q2);
    g2 = p.m2*p.l2/2*p.g*cos(q1 + q2);
    g = [g1; g2];
end
      

In Simulink, you can create a subsystem that implements the dynamics and controller using MATLAB Function blocks for twolink_ctc, or separate blocks for \( \mathbf{M}, \mathbf{C}, \mathbf{g} \). This allows you to add saturations, sample-and-hold, and sensor noise, which all influence robustness in practice.

10.2 Wolfram Mathematica

Mathematica is well suited for symbolic verification of Lyapunov derivatives and generation of C/C++ code. Below is a sketch for deriving \( \dot{V} \) symbolically for a single-DOF example.

ClearAll[q, qd, e, ed, Mq, Kp, Kd, V, tau, qdDes, qdDesd, qdDesdd];

(* Single-DOF example *)
Mq[q_] := m;  (* constant inertia *)
Kp = kp; Kd = kd;

qdDes[t_]   := qd0*Sin[w*t];
qdDesd[t_]  := D[qdDes[t], t];
qdDesdd[t_] := D[qdDes[t], {t,2}];

e[q_, t_]  := q - qdDes[t];
ed[qd_, t_] := qd - qdDesd[t];

V[q_, qd_, t_] := 1/2*Mq[q]*ed[qd, t]^2 + 1/2*Kp*e[q, t]^2;

(* Error dynamics with computed torque under perfect model *)
qdd[q_, qd_, t_] := qdDesdd[t] - Kd*ed[qd, t] - Kp*e[q, t];

Vdot[q_, qd_, t_] :=
  D[V[q, qd, t], q]*qd
  + D[V[q, qd, t], qd]*qdd[q, qd, t]
  + D[V[q, qd, t], t];

FullSimplify[Vdot[q, qd, t]]
(* Expected result: -Kd * ed^2 *)
      

For multi-DOF robots, you can derive symbolic expressions for \( \mathbf{M}(\mathbf{q}) \), compute \( V \), and verify that \( \dot{V} = -\dot{\mathbf{e}}^\top \mathbf{K}_d\dot{\mathbf{e}} \) using the robot's skew-symmetry property, which can be encoded symbolically or checked numerically.

In this lesson we rigorously analyzed the stability and robustness of model-based nonlinear controllers for robot manipulators. For computed-torque control with a perfect model, we used Lyapunov methods and the skew-symmetry property of Euler–Lagrange systems to prove global asymptotic (and under reasonable assumptions, exponential) tracking of desired trajectories. When modeling errors and disturbances are present, we interpreted their effect as an input in the error dynamics and established input-to-state stability and ultimate boundedness, showing how tracking error depends on disturbance magnitude and feedback gains. Finally, we illustrated these properties via multi-language implementations suitable for simulation and real-time control.