After completing this lesson, students should be able to:

• formulate the trajectory-tracking problem for an \(n\)-joint rigid manipulator with unknown constant dynamic parameters;
• construct the reference velocity, reference acceleration, and filtered tracking error used in adaptive robot control;
• derive a regressor matrix \(\mathbf{Y}(\mathbf{q},\dot{\mathbf{q} }, \dot{\mathbf{q} }_r,\ddot{\mathbf{q} }_r)\);
• prove boundedness of all closed-loop signals and asymptotic convergence of joint position and velocity errors;
• distinguish trajectory convergence from physical-parameter convergence; and
• implement the controller in continuous-time simulation and in a sampled real-time loop.

Lesson 1 introduced adaptive control for robot manipulators with uncertain inertia and other mechanical parameters. The present lesson advances from regulation or isolated uncertainty compensation to time-varying multi-joint trajectory tracking. Students are assumed to know linear control, state-space models, Lyapunov stability, persistent excitation, projection, normalization, robust modifications, MIMO adaptive control, and the rigid-manipulator structural properties introduced previously in this course.

For a rigid robot with \(n\) revolute or prismatic joints, the joint-space dynamics are

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

where \(\mathbf{q}\in\mathbb{R}^{n}\) is the joint coordinate vector, \(\mathbf{M}(\mathbf{q})\) is the inertia matrix, \(\mathbf{C}(\mathbf{q},\dot{\mathbf{q} })\) generates Coriolis and centrifugal forces, \(\mathbf{g}(\mathbf{q})\) is gravity, \(\mathbf{f}(\dot{\mathbf{q} })\) is friction, and \(\boldsymbol{\tau}\) is the actuator torque vector. The nominal derivation below omits friction for clarity; known linearly-parameterized friction terms may be appended to the regressor.

2.1 Positive-definite inertia

For every physically valid configuration, the inertia matrix is symmetric and uniformly positive definite on the robot operating set:

\[ m_{\min}\mathbf{I} \preceq \mathbf{M}(\mathbf{q}) \preceq m_{\max}\mathbf{I}, \qquad m_{\min} > 0. \]

2.2 Skew-symmetry identity

A standard choice of the Coriolis matrix satisfies

\[ \dot{\mathbf{M} }(\mathbf{q}) -2\mathbf{C}(\mathbf{q},\dot{\mathbf{q} }) = -\left( \dot{\mathbf{M} }(\mathbf{q}) -2\mathbf{C}(\mathbf{q},\dot{\mathbf{q} }) \right)^{T}. \]

Therefore, for every vector \(\mathbf{x}\),

\[ \mathbf{x}^{T} \left( \dot{\mathbf{M} }-2\mathbf{C} \right)\mathbf{x}=0, \qquad \frac{1}{2}\mathbf{x}^{T}\dot{\mathbf{M} }\mathbf{x} = \mathbf{x}^{T}\mathbf{C}\mathbf{x}. \]

This identity is the cancellation mechanism that makes the kinetic energy term usable in a Lyapunov proof.

2.3 Linear parameterization

Although the robot dynamics are nonlinear in \(\mathbf{q}\) and \(\dot{\mathbf{q} }\), they are linear in a vector of constant base parameters. For any differentiable auxiliary velocity \(\mathbf{v}\),

\[ \mathbf{M}(\mathbf{q})\dot{\mathbf{v} } +\mathbf{C}(\mathbf{q},\dot{\mathbf{q} })\mathbf{v} +\mathbf{g}(\mathbf{q}) = \mathbf{Y}(\mathbf{q},\dot{\mathbf{q} }, \mathbf{v},\dot{\mathbf{v} })\boldsymbol{\theta}. \]

The vector \(\boldsymbol{\theta}\in\mathbb{R}^{p}\) contains inertial, mass-distribution, gravity, payload, and possibly friction combinations. Base parameters need not correspond one-to-one with individual masses or inertias; they are the identifiable linear combinations appearing in the equations of motion.

Let the desired trajectory \(\mathbf{q}_{d}(t)\) be twice continuously differentiable, with bounded \(\mathbf{q}_{d}\), \(\dot{\mathbf{q} }_{d}\), and \(\ddot{\mathbf{q} }_{d}\). Define

\[ \mathbf{e} = \mathbf{q}-\mathbf{q}_{d}, \qquad \dot{\mathbf{e} } = \dot{\mathbf{q} }-\dot{\mathbf{q} }_{d}. \]

Choose a symmetric positive-definite matrix \(\boldsymbol{\Lambda}\) and define the reference velocity and acceleration:

\[ \dot{\mathbf{q} }_{r} = \dot{\mathbf{q} }_{d} -\boldsymbol{\Lambda}\mathbf{e}, \qquad \ddot{\mathbf{q} }_{r} = \ddot{\mathbf{q} }_{d} -\boldsymbol{\Lambda}\dot{\mathbf{e} }. \]

The filtered tracking error is

\[ \mathbf{s} = \dot{\mathbf{q} } -\dot{\mathbf{q} }_{r} = \dot{\mathbf{e} } +\boldsymbol{\Lambda}\mathbf{e}. \]

Hence the position-error subsystem can be written as the stable linear filter

\[ \dot{\mathbf{e} } = -\boldsymbol{\Lambda}\mathbf{e} +\mathbf{s}. \]

Consequently, forcing \(\mathbf{s}(t)\) to zero forces both \(\mathbf{e}(t)\) and \(\dot{\mathbf{e} }(t)\) to zero. The filtered error avoids direct use of measured joint acceleration in the controller.

Evaluate the robot regressor at \(\dot{\mathbf{q} }_{r}\) and \(\ddot{\mathbf{q} }_{r}\):

\[ \mathbf{Y}_{r} = \mathbf{Y} \left( \mathbf{q}, \dot{\mathbf{q} }, \dot{\mathbf{q} }_{r}, \ddot{\mathbf{q} }_{r} \right). \]

The direct adaptive trajectory-tracking controller and parameter update law are

\[ \boxed{ \boldsymbol{\tau} = \mathbf{Y}_{r}\hat{\boldsymbol{\theta} } -\mathbf{K}_{D}\mathbf{s} } \]

\[ \boxed{ \dot{\hat{\boldsymbol{\theta} } } = -\boldsymbol{\Gamma} \mathbf{Y}_{r}^{T}\mathbf{s} } \]

Here \(\mathbf{K}_{D}\) and \(\boldsymbol{\Gamma}\) are symmetric positive-definite matrices. The term \(\mathbf{Y}_{r}\hat{\boldsymbol{\theta} }\) supplies adaptive model compensation, whereas \(-\mathbf{K}_{D}\mathbf{s}\) injects dissipation.

flowchart TD
  QD["Desired trajectory: qd, dqd, ddqd"] --> RF["Reference filter: dqr and ddqr"]
  Q["Measured joints: q and dq"] --> ER["Compute e and s"]
  QD --> ER
  ER --> RF
  Q --> REG["Robot regressor Y"]
  RF --> REG
  EST["Parameter estimate thetaHat"] --> REG
  REG --> TOR["Torque: Y thetaHat - Kd s"]
  ER --> TOR
  TOR --> ROBOT["Multi-joint robot"]
  ROBOT --> Q
  REG --> ADAPT["Update: thetaHatDot = -Gamma Y_T s"]
  ER --> ADAPT
  ADAPT --> EST
        

4.1 Closed-loop filtered-error dynamics

Using \(\dot{\mathbf{q} }=\mathbf{s}+\dot{\mathbf{q} }_{r}\) and \(\ddot{\mathbf{q} }=\dot{\mathbf{s} }+ \ddot{\mathbf{q} }_{r}\), substitute the control law into the robot dynamics:

\[ \mathbf{M}\dot{\mathbf{s} } +\mathbf{C}\mathbf{s} = -\mathbf{K}_{D}\mathbf{s} +\mathbf{Y}_{r} \left( \hat{\boldsymbol{\theta} }-\boldsymbol{\theta} \right). \]

Define the parameter error using the convention \(\tilde{\boldsymbol{\theta} } =\hat{\boldsymbol{\theta} }-\boldsymbol{\theta}\). Then

\[ \mathbf{M}\dot{\mathbf{s} } +\mathbf{C}\mathbf{s} +\mathbf{K}_{D}\mathbf{s} = \mathbf{Y}_{r}\tilde{\boldsymbol{\theta} }. \]

Consider the composite kinetic-energy and parameter-error function

\[ V = \frac{1}{2} \mathbf{s}^{T}\mathbf{M}(\mathbf{q})\mathbf{s} + \frac{1}{2} \tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1} \tilde{\boldsymbol{\theta} }. \]

Because \(\mathbf{M}\) and \(\boldsymbol{\Gamma}^{-1}\) are positive definite, \(V\) is positive definite in \((\mathbf{s},\tilde{\boldsymbol{\theta} })\). Since the true parameter vector is constant, \(\dot{\tilde{\boldsymbol{\theta} } } =\dot{\hat{\boldsymbol{\theta} } }\). Differentiation gives

\[ \dot{V} = \mathbf{s}^{T}\mathbf{M}\dot{\mathbf{s} } + \frac{1}{2}\mathbf{s}^{T}\dot{\mathbf{M} }\mathbf{s} + \tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1} \dot{\hat{\boldsymbol{\theta} } }. \]

By the skew-symmetry identity, \(\frac{1}{2}\mathbf{s}^{T}\dot{\mathbf{M} }\mathbf{s} =\mathbf{s}^{T}\mathbf{C}\mathbf{s}\). Therefore,

\[ \begin{aligned} \dot{V} &= \mathbf{s}^{T} \left( \mathbf{M}\dot{\mathbf{s} }+\mathbf{C}\mathbf{s} \right) + \tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1} \dot{\hat{\boldsymbol{\theta} } } \\ &= -\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s} + \mathbf{s}^{T}\mathbf{Y}_{r}\tilde{\boldsymbol{\theta} } - \tilde{\boldsymbol{\theta} }^{T} \mathbf{Y}_{r}^{T}\mathbf{s} \\ &= -\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s} \leq 0. \end{aligned} \]

5.1 Immediate consequences

Because \(\dot V\leq 0\), \(V(t)\leq V(0)\). Uniform positive definiteness of \(\mathbf{M}\) implies that \(\mathbf{s}\) is bounded. The second Lyapunov term implies that \(\tilde{\boldsymbol{\theta} }\) and \(\hat{\boldsymbol{\theta} }\) are bounded. Moreover,

\[ \int_{0}^{\infty} \mathbf{s}^{T}(t)\mathbf{K}_{D}\mathbf{s}(t)\,dt \leq V(0), \]

so \(\mathbf{s}\in L_{2}\cap L_{\infty}\). Since the stable error filter \(\dot{\mathbf{e} }=-\boldsymbol{\Lambda}\mathbf{e} +\mathbf{s}\) is driven by a bounded input, \(\mathbf{e}\) is bounded. Bounded desired signals, bounded errors, smooth robot functions on the operating set, and bounded parameter estimates imply bounded \(\dot{\mathbf{s} }\). Thus \(\mathbf{s}\) is uniformly continuous.

5.2 Asymptotic convergence

Barbalat's lemma now yields \(\mathbf{s}(t)\) converging to zero. The solution of the stable filter is

\[ \mathbf{e}(t) = e^{-\boldsymbol{\Lambda}t}\mathbf{e}(0) + \int_{0}^{t} e^{-\boldsymbol{\Lambda}(t-\sigma)} \mathbf{s}(\sigma)\,d\sigma. \]

The homogeneous term decays exponentially and the convolution term vanishes because the stable filter is driven by a bounded signal that converges to zero. Hence

\[ \lim_{t\to\infty}\mathbf{e}(t)=\mathbf{0}, \qquad \lim_{t\to\infty}\dot{\mathbf{e} }(t)=\mathbf{0}. \]

5.3 Formal theorem

Theorem. Assume that the rigid-manipulator model is exactly linearly parameterized by a constant vector \(\boldsymbol{\theta}\); the desired position, velocity, and acceleration are bounded; the inertia matrix is uniformly positive definite; and \(\boldsymbol{\Lambda}\), \(\mathbf{K}_{D}\), and \(\boldsymbol{\Gamma}\) are symmetric positive definite. Then the adaptive laws of Section 4 keep all ideal closed-loop signals bounded and achieve asymptotic joint position and velocity tracking.

The theorem is a trajectory-tracking result. It does not claim that the estimated physical parameters converge to their true values.

The Lyapunov derivative is independent of \(\tilde{\boldsymbol{\theta} }\) after cancellation. Therefore, trajectory error may converge even when the parameter estimate settles at a nonphysical or nonunique value. A sufficient excitation condition has the general form

\[ \exists\,T > 0,\ \alpha > 0 \quad\text{such that}\quad \int_{t}^{t+T} \mathbf{Y}_{r}^{T}(\sigma)\mathbf{Y}_{r}(\sigma)\,d\sigma \succeq \alpha\mathbf{I} \quad \text{for every }t. \]

When the regressor is persistently exciting and the parameterization has no redundant directions, stronger arguments can establish convergence of the identifiable parameter error. A constant setpoint or a single low-frequency sinusoid often fails to excite every inertial and gravity direction. Multi-sine reference trajectories, sufficiently rich joint coordination, or composite adaptation can improve convergence. However, excitation must respect joint, velocity, torque, and safety limits.

An estimate such as an equivalent inertia parameter may become negative in an ideal unconstrained adaptive simulation without invalidating the trajectory proof. In practical controllers, projection onto a physically admissible set can preserve parameter bounds while retaining the Lyapunov inequality developed in earlier chapters.

Consider a two-revolute-joint planar manipulator. Introduce the base parameter vector

\[ \boldsymbol{\theta} = \begin{bmatrix} a & b & d & g_{1} & g_{2} \end{bmatrix}^{T}, \]

with

\[ \begin{aligned} a &= I_{1}+I_{2} +m_{1}l_{c1}^{2} +m_{2}\left(l_{1}^{2}+l_{c2}^{2}\right), \\ b &= m_{2}l_{1}l_{c2}, \\ d &= I_{2}+m_{2}l_{c2}^{2}, \\ g_{1} &= \left(m_{1}l_{c1}+m_{2}l_{1}\right)g, \\ g_{2} &= m_{2}l_{c2}g. \end{aligned} \]

With \(c_{2}=\cos q_{2}\) and \(s_{2}=\sin q_{2}\), choose

\[ \mathbf{M}(\mathbf{q}) = \begin{bmatrix} a+2bc_{2} & d+bc_{2} \\ d+bc_{2} & d \end{bmatrix}, \]

\[ \mathbf{C}(\mathbf{q},\dot{\mathbf{q} }) = \begin{bmatrix} -bs_{2}\dot q_{2} & -bs_{2}\left(\dot q_{1}+\dot q_{2}\right) \\ bs_{2}\dot q_{1} & 0 \end{bmatrix}, \]

\[ \mathbf{g}(\mathbf{q}) = \begin{bmatrix} g_{1}\cos q_{1}+g_{2}\cos(q_{1}+q_{2}) \\ g_{2}\cos(q_{1}+q_{2}) \end{bmatrix}. \]

Let \(\dot{\mathbf{q} }_{r} =[r_{1},r_{2}]^{T}\) and \(\ddot{\mathbf{q} }_{r} =[\alpha_{1},\alpha_{2}]^{T}\). Expanding \(\mathbf{M}\ddot{\mathbf{q} }_{r} +\mathbf{C}\dot{\mathbf{q} }_{r}+\mathbf{g}\) and collecting coefficients of \(a,b,d,g_{1},g_{2}\) gives

\[ \mathbf{Y}_{r} = \begin{bmatrix} \alpha_{1} & 2c_{2}\alpha_{1} +c_{2}\alpha_{2} -s_{2}\dot q_{2}r_{1} -s_{2}(\dot q_{1}+\dot q_{2})r_{2} & \alpha_{2} & \cos q_{1} & \cos(q_{1}+q_{2}) \\ 0 & c_{2}\alpha_{1} +s_{2}\dot q_{1}r_{1} & \alpha_{1}+\alpha_{2} & 0 & \cos(q_{1}+q_{2}) \end{bmatrix}. \]

Direct multiplication verifies \(\mathbf{Y}_{r}\boldsymbol{\theta} =\mathbf{M}\ddot{\mathbf{q} }_{r} +\mathbf{C}\dot{\mathbf{q} }_{r}+\mathbf{g}\). This is the regressor used identically in all five implementations below.

8.1 Role of the three gain matrices

• \(\boldsymbol{\Lambda}\): determines the desired decay rate of the position-error filter. Large entries accelerate nominal error decay but increase reference acceleration and torque demand.
• \(\mathbf{K}_{D}\): determines direct dissipation in \(\dot V=-\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s}\). Higher values improve rejection of parameter mismatch but amplify velocity noise and actuator effort.
• \(\boldsymbol{\Gamma}\): sets adaptation speed. Excessive adaptation gain can turn sensor noise, discretization error, or unmodeled flexibility into parameter oscillation.

8.2 Sampled parameter update

With sample period \(T_{s}\), forward Euler gives

\[ \hat{\boldsymbol{\theta} }[k+1] = \hat{\boldsymbol{\theta} }[k] - T_{s}\boldsymbol{\Gamma} \mathbf{Y}_{r}^{T}[k]\mathbf{s}[k]. \]

A smaller sample period, a discrete-time integrator, or a higher-order numerical update reduces integration error. The real controller should compute all signals from one coherent sensor timestamp. Joint velocity estimates should be filtered without introducing excessive phase lag.

8.3 Torque saturation and robustification

The ideal proof assumes the commanded torque is applied exactly. If saturation produces \(\boldsymbol{\tau}_{a}\neq\boldsymbol{\tau}\), the Lyapunov derivative acquires the additional term \(\mathbf{s}^{T} (\boldsymbol{\tau}_{a}-\boldsymbol{\tau})\). A practical design should combine torque limiting with a saturation-aware update law, dead zone, projection, leakage, or an adaptation freeze as developed in Chapters 8, 9, 19, and 24.

flowchart TD
  S0["Read q and dq at one timestamp"] --> S1["Evaluate qd, dqd, ddqd"]
  S1 --> S2["Compute e, de, dqr, ddqr, and s"]
  S2 --> S3["Build robot regressor Y"]
  S3 --> S4["Compute torque command"]
  S4 --> S5["Apply safety and actuator limits"]
  S5 --> S6["Send torque to robot"]
  S3 --> S7["Compute parameter derivative"]
  S7 --> S8["Integrate or discretely update thetaHat"]
  S8 --> S9["Log errors, torque, estimates, and limit flags"]
  S6 --> S9
  S9 --> S0
        

This implementation uses NumPy for matrix operations, Matplotlib for plots, and an explicit fourth-order Runge-Kutta integrator. It simulates both the true nonlinear robot and the adaptive parameter dynamics.

Chapter25_Lesson2.py

"""
Chapter25_Lesson2.py
Adaptive trajectory tracking of a two-joint planar robot using the
Slotine-Li filtered-error controller.

Dependencies:
    pip install numpy matplotlib
"""

from __future__ import annotations

import math
from dataclasses import dataclass
from typing import Tuple

import matplotlib.pyplot as plt
import numpy as np


@dataclass(frozen=True)
class RobotParameters:
    """Physical parameters of a rigid two-link planar manipulator."""

    m1: float = 2.0
    m2: float = 1.5
    l1: float = 1.0
    lc1: float = 0.5
    lc2: float = 0.5
    i1: float = 0.2
    i2: float = 0.1
    gravity: float = 9.81

    def base_parameters(self) -> np.ndarray:
        """Return theta = [a, b, d, g1, g2]."""
        a = (
            self.i1
            + self.i2
            + self.m1 * self.lc1**2
            + self.m2 * (self.l1**2 + self.lc2**2)
        )
        b = self.m2 * self.l1 * self.lc2
        d = self.i2 + self.m2 * self.lc2**2
        g1 = (self.m1 * self.lc1 + self.m2 * self.l1) * self.gravity
        g2 = self.m2 * self.lc2 * self.gravity
        return np.array([a, b, d, g1, g2], dtype=float)


TRUE_PARAMETERS = RobotParameters().base_parameters()
LAMBDA = np.diag([4.0, 4.0])
K_D = np.diag([20.0, 20.0])
GAMMA = np.diag([2.0, 1.0, 2.0, 1.0, 1.0])


def desired_trajectory(t: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Return desired joint position, velocity, and acceleration."""
    q_d = np.array(
        [0.60 * math.sin(0.50 * t), 0.45 * math.cos(0.70 * t)],
        dtype=float,
    )
    dq_d = np.array(
        [0.30 * math.cos(0.50 * t), -0.315 * math.sin(0.70 * t)],
        dtype=float,
    )
    ddq_d = np.array(
        [-0.15 * math.sin(0.50 * t), -0.2205 * math.cos(0.70 * t)],
        dtype=float,
    )
    return q_d, dq_d, ddq_d


def inertia_matrix(q: np.ndarray, theta: np.ndarray) -> np.ndarray:
    """M(q) for theta = [a, b, d, g1, g2]."""
    a, b, d, _, _ = theta
    c2 = math.cos(float(q[1]))
    return np.array(
        [[a + 2.0 * b * c2, d + b * c2], [d + b * c2, d]],
        dtype=float,
    )


def coriolis_matrix(q: np.ndarray, dq: np.ndarray, theta: np.ndarray) -> np.ndarray:
    """A C(q,dq) satisfying dot(M)-2C skew-symmetric."""
    _, b, _, _, _ = theta
    s2 = math.sin(float(q[1]))
    dq1, dq2 = float(dq[0]), float(dq[1])
    return np.array(
        [
            [-b * s2 * dq2, -b * s2 * (dq1 + dq2)],
            [b * s2 * dq1, 0.0],
        ],
        dtype=float,
    )


def gravity_vector(q: np.ndarray, theta: np.ndarray) -> np.ndarray:
    """g(q) for theta = [a, b, d, g1, g2]."""
    _, _, _, g1, g2 = theta
    q1, q2 = float(q[0]), float(q[1])
    return np.array(
        [g1 * math.cos(q1) + g2 * math.cos(q1 + q2), g2 * math.cos(q1 + q2)],
        dtype=float,
    )


def regressor(
    q: np.ndarray,
    dq: np.ndarray,
    dq_r: np.ndarray,
    ddq_r: np.ndarray,
) -> np.ndarray:
    """Y(q,dq,dq_r,ddq_r) such that Y theta = M ddq_r + C dq_r + g."""
    q1, q2 = float(q[0]), float(q[1])
    dq1, dq2 = float(dq[0]), float(dq[1])
    r1, r2 = float(dq_r[0]), float(dq_r[1])
    ar1, ar2 = float(ddq_r[0]), float(ddq_r[1])
    c2, s2 = math.cos(q2), math.sin(q2)

    return np.array(
        [
            [
                ar1,
                2.0 * c2 * ar1
                + c2 * ar2
                - s2 * dq2 * r1
                - s2 * (dq1 + dq2) * r2,
                ar2,
                math.cos(q1),
                math.cos(q1 + q2),
            ],
            [
                0.0,
                c2 * ar1 + s2 * dq1 * r1,
                ar1 + ar2,
                0.0,
                math.cos(q1 + q2),
            ],
        ],
        dtype=float,
    )


def closed_loop_rhs(t: float, state: np.ndarray) -> np.ndarray:
    """Continuous-time plant and parameter-update dynamics."""
    q = state[0:2]
    dq = state[2:4]
    theta_hat = state[4:9]

    q_d, dq_d, ddq_d = desired_trajectory(t)
    e = q - q_d
    de = dq - dq_d

    dq_r = dq_d - LAMBDA @ e
    ddq_r = ddq_d - LAMBDA @ de
    s = dq - dq_r

    y_matrix = regressor(q, dq, dq_r, ddq_r)
    torque = y_matrix @ theta_hat - K_D @ s
    dtheta_hat = -GAMMA @ y_matrix.T @ s

    m_matrix = inertia_matrix(q, TRUE_PARAMETERS)
    c_matrix = coriolis_matrix(q, dq, TRUE_PARAMETERS)
    g_vector = gravity_vector(q, TRUE_PARAMETERS)
    ddq = np.linalg.solve(m_matrix, torque - c_matrix @ dq - g_vector)

    return np.concatenate((dq, ddq, dtheta_hat))


def rk4_step(t: float, state: np.ndarray, step: float) -> np.ndarray:
    """One explicit fourth-order Runge-Kutta step."""
    k1 = closed_loop_rhs(t, state)
    k2 = closed_loop_rhs(t + 0.5 * step, state + 0.5 * step * k1)
    k3 = closed_loop_rhs(t + 0.5 * step, state + 0.5 * step * k2)
    k4 = closed_loop_rhs(t + step, state + step * k3)
    return state + (step / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)


def simulate(final_time: float = 15.0, step: float = 0.002) -> dict[str, np.ndarray]:
    """Run the adaptive tracking experiment."""
    if final_time <= 0.0 or step <= 0.0:
        raise ValueError("final_time and step must be positive")

    count = int(round(final_time / step)) + 1
    times = np.linspace(0.0, final_time, count)
    state = np.array(
        [0.40, -0.20, 0.0, 0.0, 1.80, 0.35, 0.30, 16.0, 4.50],
        dtype=float,
    )

    states = np.zeros((count, state.size), dtype=float)
    desired = np.zeros((count, 2), dtype=float)
    filtered_error = np.zeros((count, 2), dtype=float)
    torques = np.zeros((count, 2), dtype=float)

    for index, current_time in enumerate(times):
        states[index] = state
        q_d, dq_d, ddq_d = desired_trajectory(float(current_time))
        desired[index] = q_d

        q, dq, theta_hat = state[0:2], state[2:4], state[4:9]
        e, de = q - q_d, dq - dq_d
        dq_r = dq_d - LAMBDA @ e
        ddq_r = ddq_d - LAMBDA @ de
        s = dq - dq_r
        filtered_error[index] = s
        torques[index] = regressor(q, dq, dq_r, ddq_r) @ theta_hat - K_D @ s

        if index + 1 < count:
            state = rk4_step(float(current_time), state, step)
            if not np.all(np.isfinite(state)):
                raise FloatingPointError("Simulation became non-finite")

    return {
        "time": times,
        "state": states,
        "desired": desired,
        "filtered_error": filtered_error,
        "torque": torques,
    }


def plot_results(data: dict[str, np.ndarray]) -> None:
    """Plot joint tracking, tracking errors, and parameter estimates."""
    time = data["time"]
    state = data["state"]
    desired = data["desired"]

    plt.figure()
    plt.plot(time, state[:, 0], label="q1")
    plt.plot(time, desired[:, 0], "--", label="q1 desired")
    plt.plot(time, state[:, 1], label="q2")
    plt.plot(time, desired[:, 1], "--", label="q2 desired")
    plt.xlabel("Time (s)")
    plt.ylabel("Joint angle (rad)")
    plt.title("Adaptive trajectory tracking")
    plt.grid(True)
    plt.legend()

    plt.figure()
    errors = state[:, 0:2] - desired
    plt.plot(time, errors[:, 0], label="e1")
    plt.plot(time, errors[:, 1], label="e2")
    plt.xlabel("Time (s)")
    plt.ylabel("Position error (rad)")
    plt.title("Joint tracking errors")
    plt.grid(True)
    plt.legend()

    plt.figure()
    for parameter_index in range(5):
        plt.plot(
            time,
            state[:, 4 + parameter_index],
            label=f"theta_hat[{parameter_index + 1}]",
        )
    plt.xlabel("Time (s)")
    plt.ylabel("Parameter estimate")
    plt.title("Adaptive parameter estimates")
    plt.grid(True)
    plt.legend()

    plt.show()


def main() -> None:
    data = simulate()
    final_error = data["state"][-1, 0:2] - data["desired"][-1]
    print("True base parameters:", TRUE_PARAMETERS)
    print("Final estimates:", data["state"][-1, 4:9])
    print("Final position error:", final_error)
    print(
        "Note: trajectory convergence does not imply parameter convergence "
        "without persistent excitation."
    )
    plot_results(data)


if __name__ == "__main__":
    main()

The C++17 version uses only the standard library. Fixed-size arrays make the dimensions explicit, and the program writes a CSV file for plotting in Python, MATLAB, a spreadsheet, or another analysis tool.

Chapter25_Lesson2.cpp

/*
Chapter25_Lesson2.cpp
Adaptive trajectory tracking of a two-joint planar robot using the
Slotine-Li filtered-error controller.

Build:
    g++ -std=c++17 -O2 Chapter25_Lesson2.cpp -o Chapter25_Lesson2
Run:
    ./Chapter25_Lesson2
*/

#include <array>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <string>

namespace {

constexpr std::size_t STATE_SIZE = 9;
using Vec2 = std::array<double, 2>;
using Vec5 = std::array<double, 5>;
using State = std::array<double, STATE_SIZE>;
using Mat2 = std::array<std::array<double, 2>, 2>;
using Regressor = std::array<std::array<double, 5>, 2>;

constexpr Vec5 TRUE_THETA{2.675, 0.75, 0.475, 24.525, 7.3575};
constexpr Vec2 LAMBDA_GAIN{4.0, 4.0};
constexpr Vec2 KD_GAIN{20.0, 20.0};
constexpr Vec5 GAMMA_GAIN{2.0, 1.0, 2.0, 1.0, 1.0};

struct Desired {
    Vec2 q;
    Vec2 dq;
    Vec2 ddq;
};

Desired desiredTrajectory(double t) {
    return {
        {0.60 * std::sin(0.50 * t), 0.45 * std::cos(0.70 * t)},
        {0.30 * std::cos(0.50 * t), -0.315 * std::sin(0.70 * t)},
        {-0.15 * std::sin(0.50 * t), -0.2205 * std::cos(0.70 * t)}
    };
}

Mat2 inertiaMatrix(const Vec2& q, const Vec5& theta) {
    const double a = theta[0];
    const double b = theta[1];
    const double d = theta[2];
    const double c2 = std::cos(q[1]);
    return { { {a + 2.0 * b * c2, d + b * c2},
             {d + b * c2, d} } };
}

Mat2 coriolisMatrix(const Vec2& q, const Vec2& dq, const Vec5& theta) {
    const double b = theta[1];
    const double s2 = std::sin(q[1]);
    return { { {-b * s2 * dq[1], -b * s2 * (dq[0] + dq[1])},
             { b * s2 * dq[0], 0.0} } };
}

Vec2 gravityVector(const Vec2& q, const Vec5& theta) {
    const double g1 = theta[3];
    const double g2 = theta[4];
    return {
        g1 * std::cos(q[0]) + g2 * std::cos(q[0] + q[1]),
        g2 * std::cos(q[0] + q[1])
    };
}

Regressor regressor(
    const Vec2& q,
    const Vec2& dq,
    const Vec2& dqR,
    const Vec2& ddqR
) {
    const double q1 = q[0];
    const double q2 = q[1];
    const double dq1 = dq[0];
    const double dq2 = dq[1];
    const double r1 = dqR[0];
    const double r2 = dqR[1];
    const double ar1 = ddqR[0];
    const double ar2 = ddqR[1];
    const double c2 = std::cos(q2);
    const double s2 = std::sin(q2);

    return { {
        { {
            ar1,
            2.0 * c2 * ar1 + c2 * ar2
                - s2 * dq2 * r1
                - s2 * (dq1 + dq2) * r2,
            ar2,
            std::cos(q1),
            std::cos(q1 + q2)
        } },
        { {
            0.0,
            c2 * ar1 + s2 * dq1 * r1,
            ar1 + ar2,
            0.0,
            std::cos(q1 + q2)
        } }
    } };
}

Vec2 multiply(const Mat2& matrix, const Vec2& vector) {
    return {
        matrix[0][0] * vector[0] + matrix[0][1] * vector[1],
        matrix[1][0] * vector[0] + matrix[1][1] * vector[1]
    };
}

Vec2 multiply(const Regressor& matrix, const Vec5& vector) {
    Vec2 result{0.0, 0.0};
    for (std::size_t row = 0; row < 2; ++row) {
        for (std::size_t column = 0; column < 5; ++column) {
            result[row] += matrix[row][column] * vector[column];
        }
    }
    return result;
}

Vec5 transposeMultiply(const Regressor& matrix, const Vec2& vector) {
    Vec5 result{0.0, 0.0, 0.0, 0.0, 0.0};
    for (std::size_t column = 0; column < 5; ++column) {
        result[column] =
            matrix[0][column] * vector[0] + matrix[1][column] * vector[1];
    }
    return result;
}

Vec2 solve2x2(const Mat2& matrix, const Vec2& vector) {
    const double determinant =
        matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
    if (std::abs(determinant) < 1.0e-12) {
        throw std::runtime_error("Singular inertia matrix");
    }

    return {
        ( matrix[1][1] * vector[0] - matrix[0][1] * vector[1]) / determinant,
        (-matrix[1][0] * vector[0] + matrix[0][0] * vector[1]) / determinant
    };
}

State derivative(double t, const State& state) {
    const Vec2 q{state[0], state[1]};
    const Vec2 dq{state[2], state[3]};
    const Vec5 thetaHat{
        state[4], state[5], state[6], state[7], state[8]
    };

    const Desired desired = desiredTrajectory(t);
    Vec2 error{};
    Vec2 errorDot{};
    Vec2 dqR{};
    Vec2 ddqR{};
    Vec2 filteredError{};

    for (std::size_t index = 0; index < 2; ++index) {
        error[index] = q[index] - desired.q[index];
        errorDot[index] = dq[index] - desired.dq[index];
        dqR[index] = desired.dq[index] - LAMBDA_GAIN[index] * error[index];
        ddqR[index] =
            desired.ddq[index] - LAMBDA_GAIN[index] * errorDot[index];
        filteredError[index] = dq[index] - dqR[index];
    }

    const Regressor y = regressor(q, dq, dqR, ddqR);
    Vec2 torque = multiply(y, thetaHat);
    for (std::size_t index = 0; index < 2; ++index) {
        torque[index] -= KD_GAIN[index] * filteredError[index];
    }

    const Mat2 mass = inertiaMatrix(q, TRUE_THETA);
    const Mat2 coriolis = coriolisMatrix(q, dq, TRUE_THETA);
    const Vec2 gravity = gravityVector(q, TRUE_THETA);
    const Vec2 coriolisForce = multiply(coriolis, dq);

    const Vec2 rightHandSide{
        torque[0] - coriolisForce[0] - gravity[0],
        torque[1] - coriolisForce[1] - gravity[1]
    };
    const Vec2 ddq = solve2x2(mass, rightHandSide);

    const Vec5 yTransposeS = transposeMultiply(y, filteredError);

    State result{};
    result[0] = dq[0];
    result[1] = dq[1];
    result[2] = ddq[0];
    result[3] = ddq[1];
    for (std::size_t index = 0; index < 5; ++index) {
        result[4 + index] = -GAMMA_GAIN[index] * yTransposeS[index];
    }
    return result;
}

State addScaled(const State& left, const State& right, double scale) {
    State result{};
    for (std::size_t index = 0; index < STATE_SIZE; ++index) {
        result[index] = left[index] + scale * right[index];
    }
    return result;
}

State rk4Step(double t, const State& state, double step) {
    const State k1 = derivative(t, state);
    const State k2 = derivative(
        t + 0.5 * step,
        addScaled(state, k1, 0.5 * step)
    );
    const State k3 = derivative(
        t + 0.5 * step,
        addScaled(state, k2, 0.5 * step)
    );
    const State k4 = derivative(t + step, addScaled(state, k3, step));

    State result{};
    for (std::size_t index = 0; index < STATE_SIZE; ++index) {
        result[index] = state[index]
            + (step / 6.0)
                * (k1[index] + 2.0 * k2[index] + 2.0 * k3[index] + k4[index]);
        if (!std::isfinite(result[index])) {
            throw std::runtime_error("Simulation became non-finite");
        }
    }
    return result;
}

}  // namespace

int main() {
    constexpr double step = 0.002;
    constexpr double finalTime = 15.0;

    State state{
        0.40, -0.20, 0.0, 0.0,
        1.80, 0.35, 0.30, 16.0, 4.50
    };

    std::ofstream output("Chapter25_Lesson2_cpp.csv");
    if (!output) {
        std::cerr << "Could not create output CSV.\n";
        return 1;
    }

    output << "time,q1,q1_desired,q2,q2_desired,e1,e2,"
              "theta1,theta2,theta3,theta4,theta5\n";
    output << std::setprecision(12);

    const int steps = static_cast<int>(std::round(finalTime / step));
    for (int sample = 0; sample <= steps; ++sample) {
        const double t = sample * step;
        const Desired desired = desiredTrajectory(t);
        output
            << t << ','
            << state[0] << ',' << desired.q[0] << ','
            << state[1] << ',' << desired.q[1] << ','
            << state[0] - desired.q[0] << ','
            << state[1] - desired.q[1];

        for (std::size_t index = 4; index < STATE_SIZE; ++index) {
            output << ',' << state[index];
        }
        output << '\n';

        if (sample < steps) {
            state = rk4Step(t, state, step);
        }
    }

    const Desired finalDesired = desiredTrajectory(finalTime);
    std::cout << "Final position error: ["
              << state[0] - finalDesired.q[0] << ", "
              << state[1] - finalDesired.q[1] << "]\n";
    std::cout << "Final parameter estimates: [";
    for (std::size_t index = 4; index < STATE_SIZE; ++index) {
        std::cout << state[index];
        if (index + 1 < STATE_SIZE) {
            std::cout << ", ";
        }
    }
    std::cout << "]\n";
    std::cout
        << "CSV written to Chapter25_Lesson2_cpp.csv\n"
        << "Trajectory convergence does not require parameter convergence.\n";

    return 0;
}

The Java implementation uses standard arrays, a small collection of matrix helper functions, fourth-order Runge-Kutta integration, and CSV output. Java 17 or later is recommended because the trajectory container is declared as a record.

Chapter25_Lesson2.java

/*
Chapter25_Lesson2.java
Adaptive trajectory tracking of a two-joint planar robot using the
Slotine-Li filtered-error controller.

Build:
    javac Chapter25_Lesson2.java
Run:
    java Chapter25_Lesson2
*/

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Locale;

public final class Chapter25_Lesson2 {
    private static final int STATE_SIZE = 9;

    private static final double[] TRUE_THETA =
        {2.675, 0.75, 0.475, 24.525, 7.3575};
    private static final double[] LAMBDA_GAIN = {4.0, 4.0};
    private static final double[] KD_GAIN = {20.0, 20.0};
    private static final double[] GAMMA_GAIN = {2.0, 1.0, 2.0, 1.0, 1.0};

    private Chapter25_Lesson2() {
        // Utility class.
    }

    private record Desired(double[] q, double[] dq, double[] ddq) {}

    private static Desired desiredTrajectory(double time) {
        return new Desired(
            new double[] {
                0.60 * Math.sin(0.50 * time),
                0.45 * Math.cos(0.70 * time)
            },
            new double[] {
                0.30 * Math.cos(0.50 * time),
                -0.315 * Math.sin(0.70 * time)
            },
            new double[] {
                -0.15 * Math.sin(0.50 * time),
                -0.2205 * Math.cos(0.70 * time)
            }
        );
    }

    private static double[][] inertiaMatrix(double[] q, double[] theta) {
        double a = theta[0];
        double b = theta[1];
        double d = theta[2];
        double c2 = Math.cos(q[1]);
        return new double[][] {
            {a + 2.0 * b * c2, d + b * c2},
            {d + b * c2, d}
        };
    }

    private static double[][] coriolisMatrix(
        double[] q,
        double[] dq,
        double[] theta
    ) {
        double b = theta[1];
        double s2 = Math.sin(q[1]);
        return new double[][] {
            {
                -b * s2 * dq[1],
                -b * s2 * (dq[0] + dq[1])
            },
            {
                b * s2 * dq[0],
                0.0
            }
        };
    }

    private static double[] gravityVector(double[] q, double[] theta) {
        double g1 = theta[3];
        double g2 = theta[4];
        return new double[] {
            g1 * Math.cos(q[0]) + g2 * Math.cos(q[0] + q[1]),
            g2 * Math.cos(q[0] + q[1])
        };
    }

    private static double[][] regressor(
        double[] q,
        double[] dq,
        double[] dqR,
        double[] ddqR
    ) {
        double q1 = q[0];
        double q2 = q[1];
        double dq1 = dq[0];
        double dq2 = dq[1];
        double r1 = dqR[0];
        double r2 = dqR[1];
        double ar1 = ddqR[0];
        double ar2 = ddqR[1];
        double c2 = Math.cos(q2);
        double s2 = Math.sin(q2);

        return new double[][] {
            {
                ar1,
                2.0 * c2 * ar1
                    + c2 * ar2
                    - s2 * dq2 * r1
                    - s2 * (dq1 + dq2) * r2,
                ar2,
                Math.cos(q1),
                Math.cos(q1 + q2)
            },
            {
                0.0,
                c2 * ar1 + s2 * dq1 * r1,
                ar1 + ar2,
                0.0,
                Math.cos(q1 + q2)
            }
        };
    }

    private static double[] multiply(double[][] matrix, double[] vector) {
        double[] result = new double[matrix.length];
        for (int row = 0; row < matrix.length; row++) {
            for (int column = 0; column < vector.length; column++) {
                result[row] += matrix[row][column] * vector[column];
            }
        }
        return result;
    }

    private static double[] transposeMultiply(
        double[][] matrix,
        double[] vector
    ) {
        double[] result = new double[matrix[0].length];
        for (int column = 0; column < matrix[0].length; column++) {
            result[column] =
                matrix[0][column] * vector[0]
                + matrix[1][column] * vector[1];
        }
        return result;
    }

    private static double[] solve2x2(double[][] matrix, double[] vector) {
        double determinant =
            matrix[0][0] * matrix[1][1]
            - matrix[0][1] * matrix[1][0];

        if (Math.abs(determinant) < 1.0e-12) {
            throw new IllegalStateException("Singular inertia matrix");
        }

        return new double[] {
            (
                matrix[1][1] * vector[0]
                - matrix[0][1] * vector[1]
            ) / determinant,
            (
                -matrix[1][0] * vector[0]
                + matrix[0][0] * vector[1]
            ) / determinant
        };
    }

    private static double[] derivative(double time, double[] state) {
        double[] q = {state[0], state[1]};
        double[] dq = {state[2], state[3]};
        double[] thetaHat = {
            state[4], state[5], state[6], state[7], state[8]
        };

        Desired desired = desiredTrajectory(time);
        double[] error = new double[2];
        double[] errorDot = new double[2];
        double[] dqR = new double[2];
        double[] ddqR = new double[2];
        double[] filteredError = new double[2];

        for (int index = 0; index < 2; index++) {
            error[index] = q[index] - desired.q()[index];
            errorDot[index] = dq[index] - desired.dq()[index];
            dqR[index] =
                desired.dq()[index] - LAMBDA_GAIN[index] * error[index];
            ddqR[index] =
                desired.ddq()[index]
                - LAMBDA_GAIN[index] * errorDot[index];
            filteredError[index] = dq[index] - dqR[index];
        }

        double[][] y = regressor(q, dq, dqR, ddqR);
        double[] torque = multiply(y, thetaHat);
        for (int index = 0; index < 2; index++) {
            torque[index] -= KD_GAIN[index] * filteredError[index];
        }

        double[][] mass = inertiaMatrix(q, TRUE_THETA);
        double[][] coriolis = coriolisMatrix(q, dq, TRUE_THETA);
        double[] gravity = gravityVector(q, TRUE_THETA);
        double[] coriolisForce = multiply(coriolis, dq);

        double[] rightHandSide = {
            torque[0] - coriolisForce[0] - gravity[0],
            torque[1] - coriolisForce[1] - gravity[1]
        };
        double[] ddq = solve2x2(mass, rightHandSide);
        double[] yTransposeS = transposeMultiply(y, filteredError);

        double[] result = new double[STATE_SIZE];
        result[0] = dq[0];
        result[1] = dq[1];
        result[2] = ddq[0];
        result[3] = ddq[1];
        for (int index = 0; index < 5; index++) {
            result[4 + index] =
                -GAMMA_GAIN[index] * yTransposeS[index];
        }

        return result;
    }

    private static double[] addScaled(
        double[] left,
        double[] right,
        double scale
    ) {
        double[] result = new double[STATE_SIZE];
        for (int index = 0; index < STATE_SIZE; index++) {
            result[index] = left[index] + scale * right[index];
        }
        return result;
    }

    private static double[] rk4Step(
        double time,
        double[] state,
        double step
    ) {
        double[] k1 = derivative(time, state);
        double[] k2 = derivative(
            time + 0.5 * step,
            addScaled(state, k1, 0.5 * step)
        );
        double[] k3 = derivative(
            time + 0.5 * step,
            addScaled(state, k2, 0.5 * step)
        );
        double[] k4 = derivative(
            time + step,
            addScaled(state, k3, step)
        );

        double[] result = new double[STATE_SIZE];
        for (int index = 0; index < STATE_SIZE; index++) {
            result[index] =
                state[index]
                + (step / 6.0)
                    * (
                        k1[index]
                        + 2.0 * k2[index]
                        + 2.0 * k3[index]
                        + k4[index]
                    );

            if (!Double.isFinite(result[index])) {
                throw new IllegalStateException(
                    "Simulation became non-finite"
                );
            }
        }
        return result;
    }

    public static void main(String[] args) {
        Locale.setDefault(Locale.US);

        final double step = 0.002;
        final double finalTime = 15.0;
        final int steps = (int) Math.round(finalTime / step);

        double[] state = {
            0.40, -0.20, 0.0, 0.0,
            1.80, 0.35, 0.30, 16.0, 4.50
        };

        try (
            BufferedWriter output = new BufferedWriter(
                new FileWriter("Chapter25_Lesson2_java.csv")
            )
        ) {
            output.write(
                "time,q1,q1_desired,q2,q2_desired,e1,e2,"
                + "theta1,theta2,theta3,theta4,theta5\n"
            );

            for (int sample = 0; sample <= steps; sample++) {
                double time = sample * step;
                Desired desired = desiredTrajectory(time);

                output.write(
                    String.format(
                        Locale.US,
                        "%.12f,%.12f,%.12f,%.12f,%.12f,"
                            + "%.12f,%.12f",
                        time,
                        state[0],
                        desired.q()[0],
                        state[1],
                        desired.q()[1],
                        state[0] - desired.q()[0],
                        state[1] - desired.q()[1]
                    )
                );

                for (int index = 4; index < STATE_SIZE; index++) {
                    output.write(
                        String.format(Locale.US, ",%.12f", state[index])
                    );
                }
                output.write("\n");

                if (sample < steps) {
                    state = rk4Step(time, state, step);
                }
            }
        } catch (IOException exception) {
            System.err.println("CSV error: " + exception.getMessage());
            System.exit(1);
        }

        Desired finalDesired = desiredTrajectory(finalTime);
        System.out.printf(
            Locale.US,
            "Final position error: [%.8f, %.8f]%n",
            state[0] - finalDesired.q()[0],
            state[1] - finalDesired.q()[1]
        );
        System.out.print("Final parameter estimates: [");
        for (int index = 4; index < STATE_SIZE; index++) {
            System.out.printf(Locale.US, "%.8f", state[index]);
            if (index + 1 < STATE_SIZE) {
                System.out.print(", ");
            }
        }
        System.out.println("]");
        System.out.println(
            "CSV written to Chapter25_Lesson2_java.csv"
        );
        System.out.println(
            "Trajectory convergence does not require parameter convergence."
        );
    }
}

MATLAB uses ode45 for continuous-time simulation. The local function adaptiveControllerBlock exposes the exact signals needed by a Simulink MATLAB Function block. In continuous Simulink, connect dthetaHat to an Integrator block. In discrete Simulink, use a Discrete-Time Integrator or Unit Delay implementing the sampled update from Section 8.

Chapter25_Lesson2.m

% Chapter25_Lesson2.m
% Adaptive trajectory tracking of a two-joint planar robot using the
% Slotine-Li filtered-error controller.
%
% Run:
%   Chapter25_Lesson2
%
% The local function adaptiveControllerBlock can also be copied into a
% MATLAB Function block in Simulink. Use q, dq, thetaHat, qd, dqd, ddqd as
% inputs and torque, dthetaHat, filteredError as outputs.

clear;
clc;
close all;

trueTheta = [2.675; 0.75; 0.475; 24.525; 7.3575];
lambda = diag([4.0, 4.0]);
kd = diag([20.0, 20.0]);
gamma = diag([2.0, 1.0, 2.0, 1.0, 1.0]);

initialState = [
    0.40;
   -0.20;
    0.00;
    0.00;
    1.80;
    0.35;
    0.30;
   16.00;
    4.50
];

timeSpan = [0.0, 15.0];
options = odeset( ...
    'RelTol', 1.0e-8, ...
    'AbsTol', 1.0e-10, ...
    'MaxStep', 0.01 ...
);

dynamics = @(time, state) closedLoopDynamics( ...
    time, ...
    state, ...
    trueTheta, ...
    lambda, ...
    kd, ...
    gamma ...
);

[time, state] = ode45(dynamics, timeSpan, initialState, options);

sampleCount = numel(time);
desiredPosition = zeros(sampleCount, 2);
filteredError = zeros(sampleCount, 2);
torque = zeros(sampleCount, 2);

for sample = 1:sampleCount
    [qd, dqd, ddqd] = desiredTrajectory(time(sample));
    q = state(sample, 1:2).';
    dq = state(sample, 3:4).';
    thetaHat = state(sample, 5:9).';

    [tau, ~, s] = adaptiveControllerBlock( ...
        q, ...
        dq, ...
        thetaHat, ...
        qd, ...
        dqd, ...
        ddqd, ...
        lambda, ...
        kd, ...
        gamma ...
    );

    desiredPosition(sample, :) = qd.';
    filteredError(sample, :) = s.';
    torque(sample, :) = tau.';
end

positionError = state(:, 1:2) - desiredPosition;

figure;
plot(time, state(:, 1), 'LineWidth', 1.2);
hold on;
plot(time, desiredPosition(:, 1), '--', 'LineWidth', 1.2);
plot(time, state(:, 2), 'LineWidth', 1.2);
plot(time, desiredPosition(:, 2), '--', 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Joint angle (rad)');
title('Adaptive trajectory tracking');
legend('q_1', 'q_{1d}', 'q_2', 'q_{2d}', 'Location', 'best');

figure;
plot(time, positionError(:, 1), 'LineWidth', 1.2);
hold on;
plot(time, positionError(:, 2), 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Position error (rad)');
title('Joint tracking errors');
legend('e_1', 'e_2', 'Location', 'best');

figure;
plot(time, state(:, 5:9), 'LineWidth', 1.1);
grid on;
xlabel('Time (s)');
ylabel('Parameter estimate');
title('Adaptive parameter estimates');
legend( ...
    '\hat{\theta}_1', ...
    '\hat{\theta}_2', ...
    '\hat{\theta}_3', ...
    '\hat{\theta}_4', ...
    '\hat{\theta}_5', ...
    'Location', ...
    'best' ...
);

figure;
plot(time, torque(:, 1), 'LineWidth', 1.1);
hold on;
plot(time, torque(:, 2), 'LineWidth', 1.1);
grid on;
xlabel('Time (s)');
ylabel('Torque (N m)');
title('Control torques');
legend('\tau_1', '\tau_2', 'Location', 'best');

fprintf('True base parameters:\n');
disp(trueTheta.');
fprintf('Final parameter estimates:\n');
disp(state(end, 5:9));
fprintf('Final position error:\n');
disp(positionError(end, :));
fprintf([ ...
    'Note: trajectory convergence does not imply parameter convergence ', ...
    'without persistent excitation.\n' ...
]);

results = table( ...
    time, ...
    state(:, 1), ...
    desiredPosition(:, 1), ...
    state(:, 2), ...
    desiredPosition(:, 2), ...
    positionError(:, 1), ...
    positionError(:, 2), ...
    state(:, 5), ...
    state(:, 6), ...
    state(:, 7), ...
    state(:, 8), ...
    state(:, 9), ...
    'VariableNames', { ...
        'time', ...
        'q1', ...
        'q1Desired', ...
        'q2', ...
        'q2Desired', ...
        'e1', ...
        'e2', ...
        'theta1', ...
        'theta2', ...
        'theta3', ...
        'theta4', ...
        'theta5' ...
    } ...
);
writetable(results, 'Chapter25_Lesson2_matlab.csv');


function derivative = closedLoopDynamics( ...
    time, ...
    state, ...
    trueTheta, ...
    lambda, ...
    kd, ...
    gamma ...
)
    q = state(1:2);
    dq = state(3:4);
    thetaHat = state(5:9);

    [qd, dqd, ddqd] = desiredTrajectory(time);
    [torque, dthetaHat, ~] = adaptiveControllerBlock( ...
        q, ...
        dq, ...
        thetaHat, ...
        qd, ...
        dqd, ...
        ddqd, ...
        lambda, ...
        kd, ...
        gamma ...
    );

    mass = inertiaMatrix(q, trueTheta);
    coriolis = coriolisMatrix(q, dq, trueTheta);
    gravity = gravityVector(q, trueTheta);
    ddq = mass \ (torque - coriolis * dq - gravity);

    derivative = [dq; ddq; dthetaHat];
end


function [torque, dthetaHat, filteredError] = adaptiveControllerBlock( ...
    q, ...
    dq, ...
    thetaHat, ...
    qd, ...
    dqd, ...
    ddqd, ...
    lambda, ...
    kd, ...
    gamma ...
)
% adaptiveControllerBlock
% Suitable equations for a MATLAB Function block in Simulink.
%
% Discrete implementation:
%   thetaHat(k+1) = thetaHat(k) + Ts * dthetaHat(k)
%
% Store thetaHat with an Integrator block for continuous simulation or a
% Unit Delay / Discrete-Time Integrator block for sampled simulation.

    error = q - qd;
    errorDot = dq - dqd;

    dqReference = dqd - lambda * error;
    ddqReference = ddqd - lambda * errorDot;
    filteredError = dq - dqReference;

    y = robotRegressor(q, dq, dqReference, ddqReference);
    torque = y * thetaHat - kd * filteredError;
    dthetaHat = -gamma * y.' * filteredError;
end


function [qd, dqd, ddqd] = desiredTrajectory(time)
    qd = [
        0.60 * sin(0.50 * time);
        0.45 * cos(0.70 * time)
    ];

    dqd = [
        0.30 * cos(0.50 * time);
       -0.315 * sin(0.70 * time)
    ];

    ddqd = [
       -0.15 * sin(0.50 * time);
       -0.2205 * cos(0.70 * time)
    ];
end


function mass = inertiaMatrix(q, theta)
    a = theta(1);
    b = theta(2);
    d = theta(3);
    c2 = cos(q(2));

    mass = [
        a + 2.0 * b * c2, d + b * c2;
        d + b * c2,       d
    ];
end


function coriolis = coriolisMatrix(q, dq, theta)
    b = theta(2);
    s2 = sin(q(2));

    coriolis = [
        -b * s2 * dq(2), -b * s2 * (dq(1) + dq(2));
         b * s2 * dq(1),  0.0
    ];
end


function gravity = gravityVector(q, theta)
    g1 = theta(4);
    g2 = theta(5);

    gravity = [
        g1 * cos(q(1)) + g2 * cos(q(1) + q(2));
        g2 * cos(q(1) + q(2))
    ];
end


function y = robotRegressor(q, dq, dqReference, ddqReference)
    q1 = q(1);
    q2 = q(2);
    dq1 = dq(1);
    dq2 = dq(2);
    r1 = dqReference(1);
    r2 = dqReference(2);
    ar1 = ddqReference(1);
    ar2 = ddqReference(2);

    c2 = cos(q2);
    s2 = sin(q2);

    y = [
        ar1, ...
        2.0 * c2 * ar1 + c2 * ar2 ...
            - s2 * dq2 * r1 ...
            - s2 * (dq1 + dq2) * r2, ...
        ar2, ...
        cos(q1), ...
        cos(q1 + q2);
        0.0, ...
        c2 * ar1 + s2 * dq1 * r1, ...
        ar1 + ar2, ...
        0.0, ...
        cos(q1 + q2)
    ];
end

The Mathematica notebook uses list-based matrix operations, LinearSolve, a programmed Runge-Kutta integrator, ListLinePlot, and CSV export. The downloadable notebook contains the following source in its input cell.

Chapter25_Lesson2.nb

ClearAll["Global`*"];

trueTheta = {2.675, 0.75, 0.475, 24.525, 7.3575};
lambdaGain = DiagonalMatrix[{4.0, 4.0}];
kdGain = DiagonalMatrix[{20.0, 20.0}];
gammaGain = DiagonalMatrix[{2.0, 1.0, 2.0, 1.0, 1.0}];

desiredTrajectory[t_?NumericQ] := {
    {0.60 Sin[0.50 t], 0.45 Cos[0.70 t]},
    {0.30 Cos[0.50 t], -0.315 Sin[0.70 t]},
    {-0.15 Sin[0.50 t], -0.2205 Cos[0.70 t]}
};

inertiaMatrix[q_List, theta_List] := Module[
    {a, b, d, c2},
    {a, b, d} = theta[[1 ;; 3]];
    c2 = Cos[q[[2]]];
    {
        {a + 2.0 b c2, d + b c2},
        {d + b c2, d}
    }
];

coriolisMatrix[q_List, dq_List, theta_List] := Module[
    {b, s2},
    b = theta[[2]];
    s2 = Sin[q[[2]]];
    {
        {
            -b s2 dq[[2]],
            -b s2 (dq[[1]] + dq[[2]])
        },
        {
            b s2 dq[[1]],
            0.0
        }
    }
];

gravityVector[q_List, theta_List] := Module[
    {g1, g2},
    {g1, g2} = theta[[4 ;; 5]];
    {
        g1 Cos[q[[1]]] + g2 Cos[q[[1]] + q[[2]]],
        g2 Cos[q[[1]] + q[[2]]]
    }
];

robotRegressor[q_List, dq_List, dqR_List, ddqR_List] := Module[
    {q1, q2, dq1, dq2, r1, r2, ar1, ar2, c2, s2},
    {q1, q2} = q;
    {dq1, dq2} = dq;
    {r1, r2} = dqR;
    {ar1, ar2} = ddqR;
    c2 = Cos[q2];
    s2 = Sin[q2];

    {
        {
            ar1,
            2.0 c2 ar1 + c2 ar2
                - s2 dq2 r1
                - s2 (dq1 + dq2) r2,
            ar2,
            Cos[q1],
            Cos[q1 + q2]
        },
        {
            0.0,
            c2 ar1 + s2 dq1 r1,
            ar1 + ar2,
            0.0,
            Cos[q1 + q2]
        }
    }
];

closedLoopRhs[t_?NumericQ, state_List] := Module[
    {
        q, dq, thetaHat, desired, qd, dqd, ddqd,
        error, errorDot, dqR, ddqR, filteredError,
        y, torque, mass, coriolis, gravity, ddq, dthetaHat
    },

    q = state[[1 ;; 2]];
    dq = state[[3 ;; 4]];
    thetaHat = state[[5 ;; 9]];

    desired = desiredTrajectory[t];
    qd = desired[[1]];
    dqd = desired[[2]];
    ddqd = desired[[3]];

    error = q - qd;
    errorDot = dq - dqd;
    dqR = dqd - lambdaGain.error;
    ddqR = ddqd - lambdaGain.errorDot;
    filteredError = dq - dqR;

    y = robotRegressor[q, dq, dqR, ddqR];
    torque = y.thetaHat - kdGain.filteredError;
    dthetaHat = -gammaGain.Transpose[y].filteredError;

    mass = inertiaMatrix[q, trueTheta];
    coriolis = coriolisMatrix[q, dq, trueTheta];
    gravity = gravityVector[q, trueTheta];
    ddq = LinearSolve[mass, torque - coriolis.dq - gravity];

    Join[dq, ddq, dthetaHat]
];

rk4Step[t_?NumericQ, state_List, h_?NumericQ] := Module[
    {k1, k2, k3, k4},
    k1 = closedLoopRhs[t, state];
    k2 = closedLoopRhs[t + h/2.0, state + h k1/2.0];
    k3 = closedLoopRhs[t + h/2.0, state + h k2/2.0];
    k4 = closedLoopRhs[t + h, state + h k3];
    state + h (k1 + 2.0 k2 + 2.0 k3 + k4)/6.0
];

step = 0.002;
finalTime = 15.0;
times = N[Range[0.0, finalTime, step]];
state = {0.40, -0.20, 0.0, 0.0, 1.80, 0.35, 0.30, 16.0, 4.50};
states = ConstantArray[0.0, {Length[times], Length[state]}];

Do[
    states[[sample]] = state;
    If[
        sample < Length[times],
        state = rk4Step[times[[sample]], state, step]
    ],
    {sample, 1, Length[times]}
];

desiredPositions = desiredTrajectory[#][[1]] & /@ times;
positionErrors = states[[All, 1 ;; 2]] - desiredPositions;

trackingPlot = ListLinePlot[
    {
        Transpose[{times, states[[All, 1]]}],
        Transpose[{times, desiredPositions[[All, 1]]}],
        Transpose[{times, states[[All, 2]]}],
        Transpose[{times, desiredPositions[[All, 2]]}]
    },
    PlotLegends -> {"q1", "q1 desired", "q2", "q2 desired"},
    PlotLabel -> "Adaptive trajectory tracking",
    AxesLabel -> {"Time (s)", "Joint angle (rad)"},
    GridLines -> Automatic,
    ImageSize -> Large
];

errorPlot = ListLinePlot[
    {
        Transpose[{times, positionErrors[[All, 1]]}],
        Transpose[{times, positionErrors[[All, 2]]}]
    },
    PlotLegends -> {"e1", "e2"},
    PlotLabel -> "Joint tracking errors",
    AxesLabel -> {"Time (s)", "Position error (rad)"},
    GridLines -> Automatic,
    ImageSize -> Large
];

parameterPlot = ListLinePlot[
    Table[
        Transpose[{times, states[[All, 4 + index]]}],
        {index, 1, 5}
    ],
    PlotLegends -> Table[
        "thetaHat" <> ToString[index],
        {index, 1, 5}
    ],
    PlotLabel -> "Adaptive parameter estimates",
    AxesLabel -> {"Time (s)", "Parameter estimate"},
    GridLines -> Automatic,
    ImageSize -> Large
];

Print["True base parameters: ", trueTheta];
Print["Final parameter estimates: ", states[[-1, 5 ;; 9]]];
Print["Final position error: ", positionErrors[[-1]]];
Print[
    "Trajectory convergence does not imply parameter convergence ",
    "without persistent excitation."
];

csvRows = MapThread[
    Join[
        { #1 },
        #2[[1 ;; 2]],
        #3,
        #4,
        #2[[5 ;; 9]]
    ] &,
    {times, states, desiredPositions, positionErrors}
];

Export[
    "Chapter25_Lesson2_mathematica.csv",
    Prepend[
        csvRows,
        {
            "time", "q1", "q2", "q1Desired", "q2Desired",
            "e1", "e2", "theta1", "theta2", "theta3", "theta4", "theta5"
        }
    ],
    "CSV"
];

Column[{trackingPlot, errorPlot, parameterPlot}]

The supplied simulations use the true base-parameter vector

\[ \boldsymbol{\theta} = \begin{bmatrix} 2.675 & 0.75 & 0.475 & 24.525 & 7.3575 \end{bmatrix}^{T}, \]

while the controller begins from a deliberately inaccurate estimate. The desired motions are sinusoids with different frequencies for the two joints. The filtered error and position errors decay while the parameter vector moves to values that reproduce the required dynamics along the executed trajectory.

The final estimate generally differs from the true vector because the finite experiment is not guaranteed to satisfy persistent excitation for every base-parameter direction. This is not a contradiction: Lyapunov stability requires the adaptive feedforward model to become sufficiently accurate along the trajectory, not necessarily that every physical parameter be identified uniquely.

The Python, C++, and Java implementations use the same fixed-step Runge-Kutta method, gains, initial conditions, and regressor. Their numerical outputs should therefore agree to floating-point tolerance. MATLAB's variable-step solver may produce slightly different transient samples while preserving the same qualitative behavior.

Problem 1: Derivation of the Filtered-Error Dynamics

Starting from the robot dynamics and the definitions of \(\dot{\mathbf{q} }_{r}\) and \(\mathbf{s}\), derive \(\mathbf{M}\dot{\mathbf{s} }+\mathbf{C}\mathbf{s} +\mathbf{K}_{D}\mathbf{s} =\mathbf{Y}_{r}\tilde{\boldsymbol{\theta} }\).

Solution:

Since \(\dot{\mathbf{q} }=\mathbf{s} +\dot{\mathbf{q} }_{r}\), differentiation gives \(\ddot{\mathbf{q} }=\dot{\mathbf{s} } +\ddot{\mathbf{q} }_{r}\). Substitute into the plant:

\[ \mathbf{M}\dot{\mathbf{s} } +\mathbf{C}\mathbf{s} + \left( \mathbf{M}\ddot{\mathbf{q} }_{r} +\mathbf{C}\dot{\mathbf{q} }_{r} +\mathbf{g} \right) = \boldsymbol{\tau}. \]

The parenthesized term is \(\mathbf{Y}_{r}\boldsymbol{\theta}\). Insert \(\boldsymbol{\tau} =\mathbf{Y}_{r}\hat{\boldsymbol{\theta} } -\mathbf{K}_{D}\mathbf{s}\) and rearrange:

\[ \mathbf{M}\dot{\mathbf{s} } +\mathbf{C}\mathbf{s} +\mathbf{K}_{D}\mathbf{s} = \mathbf{Y}_{r} \left( \hat{\boldsymbol{\theta} }-\boldsymbol{\theta} \right) = \mathbf{Y}_{r}\tilde{\boldsymbol{\theta} }. \]

Problem 2: Verify the Skew-Symmetry Property for the Two-Link Model

For the matrices in Section 7, compute \(\dot{\mathbf{M} }-2\mathbf{C}\) and show that it is skew-symmetric.

Solution:

Only \(q_{2}\) appears in \(\mathbf{M}\). Therefore,

\[ \dot{\mathbf{M} } = \begin{bmatrix} -2bs_{2}\dot q_{2} & -bs_{2}\dot q_{2} \\ -bs_{2}\dot q_{2} & 0 \end{bmatrix}. \]

Subtracting twice the selected Coriolis matrix gives

\[ \dot{\mathbf{M} }-2\mathbf{C} = \begin{bmatrix} 0 & bs_{2}\left(2\dot q_{1}+\dot q_{2}\right) \\ -bs_{2}\left(2\dot q_{1}+\dot q_{2}\right) & 0 \end{bmatrix}. \]

Its transpose is its negative, so \(\mathbf{x}^{T}(\dot{\mathbf{M} }-2\mathbf{C})\mathbf{x}=0\) for every \(\mathbf{x}\).

Problem 3: Consequence of an Incorrect Adaptation Sign

Suppose the update law is mistakenly implemented as \(\dot{\hat{\boldsymbol{\theta} } } =+\boldsymbol{\Gamma}\mathbf{Y}_{r}^{T}\mathbf{s}\). Compute the resulting Lyapunov derivative using the same definition \(\tilde{\boldsymbol{\theta} } =\hat{\boldsymbol{\theta} }-\boldsymbol{\theta}\).

Solution:

The parameter term now has the same sign as the cross term:

\[ \begin{aligned} \dot V &= -\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s} + \mathbf{s}^{T}\mathbf{Y}_{r}\tilde{\boldsymbol{\theta} } + \tilde{\boldsymbol{\theta} }^{T} \mathbf{Y}_{r}^{T}\mathbf{s} \\ &= -\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s} + 2\mathbf{s}^{T}\mathbf{Y}_{r}\tilde{\boldsymbol{\theta} }. \end{aligned} \]

This expression is not negative semidefinite. The sign error destroys the cancellation on which the stability proof depends and may cause parameter and tracking divergence.

Problem 4: Tracking Without Parameter Convergence

A robot tracks a constant joint setpoint and the measured tracking error converges to zero, but several parameter estimates remain far from their physical values. Is this behavior inconsistent with the theorem?

Solution:

No. At a constant setpoint, \(\dot{\mathbf{q} }_{d}\) and \(\ddot{\mathbf{q} }_{d}\) vanish. After the transient, much of the inertial and Coriolis regressor disappears, leaving mainly gravity information. The regressor is therefore not persistently exciting in all parameter directions. The controller can find one of many parameter vectors that generates the required equilibrium torque. The trajectory theorem requires bounded estimates and vanishing tracking error, not full identification.

Problem 5: Sampled Update Calculation

Let \(T_{s}=0.002\), \(\boldsymbol{\Gamma}=\operatorname{diag}(2,1)\), \(\mathbf{Y}_{r} =\begin{bmatrix}1&2\\-1&1\end{bmatrix}\), \(\mathbf{s}=[0.1,-0.2]^{T}\), and \(\hat{\boldsymbol{\theta} }[k]=[3,4]^{T}\). Compute one forward-Euler update.

Solution:

\[ \mathbf{Y}_{r}^{T}\mathbf{s} = \begin{bmatrix} 1&-1\\2&1 \end{bmatrix} \begin{bmatrix} 0.1\\-0.2 \end{bmatrix} = \begin{bmatrix} 0.3\\0 \end{bmatrix}. \]

\[ \hat{\boldsymbol{\theta} }[k+1] = \begin{bmatrix} 3\\4 \end{bmatrix} - 0.002 \begin{bmatrix} 2&0\\0&1 \end{bmatrix} \begin{bmatrix} 0.3\\0 \end{bmatrix} = \begin{bmatrix} 2.9988\\4 \end{bmatrix}. \]

Problem 6: Effect of Torque Saturation on the Proof

Let the actuator apply \(\boldsymbol{\tau}_{a} =\boldsymbol{\tau}+\boldsymbol{\Delta}_{\tau}\). Derive the modified Lyapunov derivative and state why the ideal conclusion no longer follows automatically.

Solution:

The filtered-error dynamics acquire the additive torque error:

\[ \mathbf{M}\dot{\mathbf{s} } +\mathbf{C}\mathbf{s} +\mathbf{K}_{D}\mathbf{s} = \mathbf{Y}_{r}\tilde{\boldsymbol{\theta} } +\boldsymbol{\Delta}_{\tau}. \]

Repeating the Lyapunov calculation yields

\[ \dot V = -\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s} + \mathbf{s}^{T}\boldsymbol{\Delta}_{\tau}. \]

The second term can be positive and can dominate dissipation. Therefore, exact asymptotic convergence is not guaranteed by the nominal proof. Saturation-aware adaptation, robust leakage, projection, a dead zone, or an ultimate-boundedness analysis is needed.

Adaptive trajectory tracking for rigid multi-joint robots is enabled by three structural facts: positive-definite inertia, the skew-symmetry identity, and linearity of the dynamics in constant base parameters. The reference filter converts position tracking into stabilization of \(\mathbf{s}=\dot{\mathbf{e} } +\boldsymbol{\Lambda}\mathbf{e}\). The control law \(\boldsymbol{\tau} =\mathbf{Y}_{r}\hat{\boldsymbol{\theta} } -\mathbf{K}_{D}\mathbf{s}\) and update law \(\dot{\hat{\boldsymbol{\theta} } } =-\boldsymbol{\Gamma}\mathbf{Y}_{r}^{T}\mathbf{s}\) cancel the state-parameter cross term in the Lyapunov derivative, producing \(\dot V=-\mathbf{s}^{T}\mathbf{K}_{D}\mathbf{s}\). Under ideal-model assumptions, joint position and velocity errors converge asymptotically to zero. Parameter convergence is a separate question requiring sufficient excitation. Real implementations must additionally address sampling, noise, torque saturation, unmodeled dynamics, and parameter constraints.