After completing this lesson, students should be able to:

• state the structural properties of rigid-manipulator dynamics used in adaptive control;
• construct a dynamic regressor that is linear in constant base parameters;
• derive the Slotine–Li filtered tracking error and adaptive torque law;
• prove boundedness and asymptotic tracking with Lyapunov and Barbalat arguments;
• distinguish tracking convergence from physical-parameter convergence;
• implement and test the controller in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

The lesson assumes familiarity with robot joint coordinates, nonlinear state equations, Lyapunov stability, projection operators, and persistent excitation from previous chapters. It does not require measured joint accelerations.

For an \(n\)-joint fully actuated rigid manipulator, 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}+\boldsymbol{\tau}_d . \]

Here \(\mathbf{M}(\mathbf{q})\) is the inertia matrix, \(\mathbf{C}(\mathbf{q},\dot{\mathbf{q} })\dot{\mathbf{q} }\) contains Coriolis and centrifugal torques, \(\mathbf{g}(\mathbf{q})\) is gravity torque, \(\mathbf{f}\) represents friction, \(\boldsymbol{\tau}\) is commanded actuator torque, and \(\boldsymbol{\tau}_d\) denotes unmatched modeling error or disturbance.

In the nominal theoretical development, friction and disturbances are omitted:

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

Uncertain inertia arises from unknown link masses, centers of mass, inertia tensors, tools, grasped payloads, or configuration-dependent composite bodies. The nonlinear dependence on \(\mathbf{q}\) is known structurally, while a vector of constant physical or base parameters is unknown.

flowchart TD
  R["Desired trajectory qd, qd_dot, qd_ddot"] --> F["Reference filter and sliding variable s"]
  M["Measured q and q_dot"] --> F
  F --> Y["Dynamic regressor Y"]
  H["Current parameter estimate theta_hat"] --> T["Adaptive computed torque"]
  Y --> T
  T --> A["Robot actuators and manipulator"]
  A --> M
  F --> U["Parameter update law"]
  Y --> U
  U --> H
        

3.1 Positive-definite inertia

On any compact operating set, there exist constants \(m_1,m_2 > 0\) such that

\[ m_1\|\mathbf{x}\|^2 \leq \mathbf{x}^{T}\mathbf{M}(\mathbf{q})\mathbf{x} \leq m_2\|\mathbf{x}\|^2 \quad \forall\,\mathbf{x}\in\mathbb{R}^{n}. \]

3.2 Skew-symmetry identity

A consistent choice of the Coriolis matrix satisfies

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

Equivalently,

\[ \frac{1}{2}\mathbf{x}^{T}\dot{\mathbf{M} }\mathbf{x} = \mathbf{x}^{T}\mathbf{C}\mathbf{x}. \]

This identity cancels the configuration-dependent kinetic-energy terms in the Lyapunov derivative.

3.3 Linear parameterization

Although the robot dynamics are nonlinear in states, they are linear in a suitable constant parameter vector. For arbitrary differentiable vectors \(\mathbf{v}\) and \(\dot{\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 regressor \(\mathbf{Y}\) is known online, while \(\boldsymbol{\theta}\) collects unknown but constant inertial and gravity parameters.

Consider a two-revolute-joint planar arm with the base-parameter vector

\[ \boldsymbol{\theta} = \begin{bmatrix} \theta_1 & \theta_2 & \theta_3 & \theta_4 & \theta_5 \end{bmatrix}^{T}. \]

A convenient parameterization is

\[ \mathbf{M}(\mathbf{q})= \begin{bmatrix} \theta_1+2\theta_2\cos q_2 & \theta_3+\theta_2\cos q_2\\ \theta_3+\theta_2\cos q_2 & \theta_3 \end{bmatrix}, \]

\[ \mathbf{C}(\mathbf{q},\dot{\mathbf{q} })= \begin{bmatrix} -\theta_2\sin q_2\,\dot q_2 & -\theta_2\sin q_2(\dot q_1+\dot q_2)\\ \theta_2\sin q_2\,\dot q_1 & 0 \end{bmatrix}, \]

\[ \mathbf{g}(\mathbf{q})= \begin{bmatrix} \theta_4\cos q_1+\theta_5\cos(q_1+q_2)\\ \theta_5\cos(q_1+q_2) \end{bmatrix}. \]

Let \(\mathbf{v}=[v_1,v_2]^T\) and \(\dot{\mathbf{v} }=[a_1,a_2]^T\). Expanding \(\mathbf{M}\dot{\mathbf{v} }+\mathbf{C}\mathbf{v}+\mathbf{g}\) and grouping coefficients of each parameter gives

\[ \mathbf{Y}= \begin{bmatrix} a_1 & 2c_2a_1+c_2a_2-s_2\dot q_2v_1-s_2(\dot q_1+\dot q_2)v_2 & a_2 & \cos q_1 & \cos(q_1+q_2) \\ 0 & c_2a_1+s_2\dot q_1v_1 & a_1+a_2 & 0 & \cos(q_1+q_2) \end{bmatrix}, \]

where \(c_2=\cos q_2\) and \(s_2=\sin q_2\). Direct multiplication verifies \(\mathbf{Y}\boldsymbol{\theta} =\mathbf{M}\dot{\mathbf{v} }+\mathbf{C}\mathbf{v}+\mathbf{g}\).

Define the tracking error and a positive-definite filter matrix:

\[ \mathbf{e}=\mathbf{q}-\mathbf{q}_d, \qquad \boldsymbol{\Lambda}=\boldsymbol{\Lambda}^{T} > 0. \]

The reference velocity and acceleration are

\[ \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 error is

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

The adaptive computed-torque law and gradient update are

\[ \boldsymbol{\tau} = \mathbf{Y} (\mathbf{q},\dot{\mathbf{q} },\dot{\mathbf{q} }_r,\ddot{\mathbf{q} }_r) \hat{\boldsymbol{\theta} } - \mathbf{K}_D\mathbf{s}, \qquad \mathbf{K}_D=\mathbf{K}_D^{T} > 0, \]

\[ \dot{\hat{\boldsymbol{\theta} } } = -\boldsymbol{\Gamma}\mathbf{Y}^{T}\mathbf{s}, \qquad \boldsymbol{\Gamma}=\boldsymbol{\Gamma}^{T} > 0. \]

This implementation does not use measured \(\ddot{\mathbf{q} }\). It uses the analytically available desired acceleration and measured position/velocity to compute \(\ddot{\mathbf{q} }_r\).

flowchart TD
  A["Read time, q, and q_dot"] --> B["Evaluate desired qd, \nqd_dot, qd_ddot"]
  B --> C["Compute e, e_dot, \nq_dot_r, q_ddot_r, and s"]
  C --> D["Build dynamic regressor Y"]
  D --> E["Compute torque \ntau = Y theta_hat - Kd s"]
  E --> F["Apply torque and \nintegrate robot dynamics"]
  D --> G["Compute theta_hat_dot = \n-Gamma Y^T s"]
  G --> H["Apply projection bounds"]
  H --> I["Integrate parameter estimates"]
  F --> A
  I --> A
        

Define the estimation error as \(\tilde{\boldsymbol{\theta} } =\hat{\boldsymbol{\theta} }-\boldsymbol{\theta}\). From the plant equation and the regressor identity,

\[ \mathbf{M}\dot{\mathbf{s} }+\mathbf{C}\mathbf{s} = \boldsymbol{\tau} - \mathbf{Y}\boldsymbol{\theta}. \]

Substitution of the adaptive torque gives

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

The nonlinear robot is therefore transformed into a passive filtered-error system driven by parameter mismatch.

Choose the composite Lyapunov 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} > 0\) and \(\boldsymbol{\Gamma} > 0\), the function is positive definite in \((\mathbf{s},\tilde{\boldsymbol{\theta} })\). For constant true parameters, \(\dot{\tilde{\boldsymbol{\theta} } } =\dot{\hat{\boldsymbol{\theta} } }\). Differentiating,

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

Using the closed-loop error equation,

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

The skew-symmetry property cancels the two kinetic-energy terms:

\[ -\mathbf{s}^{T}\mathbf{C}\mathbf{s} + \frac{1}{2}\mathbf{s}^{T}\dot{\mathbf{M} }\mathbf{s} =0. \]

Substituting the update law gives exact cancellation:

\[ \tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1} \dot{\hat{\boldsymbol{\theta} } } = - \tilde{\boldsymbol{\theta} }^{T}\mathbf{Y}^{T}\mathbf{s} = - \mathbf{s}^{T}\mathbf{Y}\tilde{\boldsymbol{\theta} }. \]

Hence

\[ \boxed{ \dot V=-\mathbf{s}^{T}\mathbf{K}_D\mathbf{s}\leq 0 }. \]

Therefore \(\mathbf{s}\) and \(\tilde{\boldsymbol{\theta} }\) are bounded, and \(\mathbf{s}\in L_2\cap L_\infty\). Under bounded desired signals and standard smoothness assumptions, the closed-loop equations imply bounded \(\dot{\mathbf{s} }\). Barbalat’s lemma then gives

\[ \lim_{t\to\infty}\mathbf{s}(t)=\mathbf{0}. \]

Since \(\dot{\mathbf{e} }=-\boldsymbol{\Lambda}\mathbf{e}+\mathbf{s}\) is an exponentially stable linear filter driven by an input converging to zero,

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

8.1 Projection

Known physical bounds can be imposed with a projection operator:

\[ \dot{\hat{\boldsymbol{\theta} } } = \operatorname{Proj}_{\Omega} \left( \hat{\boldsymbol{\theta} }, -\boldsymbol{\Gamma}\mathbf{Y}^{T}\mathbf{s} \right). \]

If the true parameter lies in the convex set \(\Omega\) and the projection satisfies its standard inward-pointing inequality, the Lyapunov derivative remains no larger than \(-\mathbf{s}^{T}\mathbf{K}_D\mathbf{s}\).

8.2 Payload changes

The formal proof assumes constant parameters. A payload attached at a discrete time produces a piecewise-constant parameter vector. Between jumps, the same Lyapunov proof applies. At a jump, the state remains continuous but \(\tilde{\boldsymbol{\theta} }\) changes, causing a finite jump in the parameter-energy term. Repeated rapid variation, flexible payload motion, backlash, or unmodeled friction requires robust modifications such as leakage, dead zones, disturbance estimation, or composite adaptation.

8.3 Tracking versus identification

The preceding proof guarantees tracking without guaranteeing \(\hat{\boldsymbol{\theta} }\to\boldsymbol{\theta}\). Exact parameter convergence generally requires persistent excitation:

\[ \exists\,T,\alpha > 0: \quad \int_{t}^{t+T} \mathbf{Y}^{T}(\sigma)\mathbf{Y}(\sigma)\,d\sigma \geq \alpha\mathbf{I} \quad \forall t. \]

A robot can track a low-dimensional or slowly varying trajectory while many inertial parameters remain unidentifiable. Consequently, parameter estimates should not automatically be interpreted as calibrated physical measurements.

• \(\boldsymbol{\Lambda}\) determines the desired first-order decay of position error after \(\mathbf{s}\) becomes small.
• \(\mathbf{K}_D\) supplies damping and sets the direct dissipation rate in \(\dot V\).
• \(\boldsymbol{\Gamma}\) controls adaptation speed. Excessive values amplify noise, discretization error, and torque transients.
• Projection bounds should reflect physically admissible base parameters, not arbitrary numerical clipping.
• Torque saturation breaks the exact cancellation used in the proof. Saturation-aware adaptation or update freezing is required in hardware.
• Joint velocities should be measured or estimated with appropriately filtered differentiators. Raw finite differences can destabilize a high-gain adaptive loop.

The numerical laboratory uses \(\boldsymbol{\Lambda}=\operatorname{diag}(5,5)\), \(\mathbf{K}_D=\operatorname{diag}(20,15)\), and \(\boldsymbol{\Gamma} =\operatorname{diag}(2,1,1.5,0.05,0.05)\). A payload change at \(t=3\,\mathrm{s}\) modifies the true inertia and distal gravity parameters.

The implementation uses NumPy and a from-scratch fourth-order Runge–Kutta integrator. Robotics Toolbox for Python can additionally evaluate inertia, Coriolis, gravity, and inverse dynamics for imported robot models; the explicit equations below keep the adaptive regressor visible.

Chapter25_Lesson1.py

#!/usr/bin/env python3
"""
Chapter25_Lesson1.py
Slotine-Li adaptive tracking control for a two-link planar manipulator.

Dependencies:
    Python 3.10+
    numpy
Optional:
    matplotlib (plots)

Run:
    python Chapter25_Lesson1.py
"""

from __future__ import annotations

import csv
import math
from pathlib import Path
from typing import Tuple

import numpy as np

DT = 1.0e-3
T_FINAL = 8.0
PAYLOAD_TIME = 3.0

LAMBDA = np.diag([5.0, 5.0])
K_D = np.diag([20.0, 15.0])
GAMMA = np.diag([2.0, 1.0, 1.5, 0.05, 0.05])

THETA_MIN = np.array([0.20, 0.05, 0.05, 1.0, 0.20])
THETA_MAX = np.array([8.00, 3.00, 3.00, 40.0, 15.0])


def desired_trajectory(t: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Return q_d, qdot_d, qddot_d."""
    q_d = np.array([
        0.6 * math.sin(0.7 * t),
        0.5 * math.cos(0.5 * t),
    ])
    qdot_d = np.array([
        0.42 * math.cos(0.7 * t),
        -0.25 * math.sin(0.5 * t),
    ])
    qddot_d = np.array([
        -0.294 * math.sin(0.7 * t),
        -0.125 * math.cos(0.5 * t),
    ])
    return q_d, qdot_d, qddot_d


def true_parameters(t: float) -> np.ndarray:
    """
    Base parameters theta = [theta1, theta2, theta3, theta4, theta5].
    At PAYLOAD_TIME, inertia and distal gravity terms change.
    """
    theta = np.array([3.0, 0.8, 0.7, 19.0, 5.0])
    if t >= PAYLOAD_TIME:
        theta += np.array([0.8, 0.2, 0.3, 0.0, 2.0])
    return theta


def mass_matrix(q: np.ndarray, theta: np.ndarray) -> np.ndarray:
    c2 = math.cos(q[1])
    return np.array([
        [theta[0] + 2.0 * theta[1] * c2, theta[2] + theta[1] * c2],
        [theta[2] + theta[1] * c2, theta[2]],
    ])


def coriolis_matrix(q: np.ndarray, qdot: np.ndarray, theta: np.ndarray) -> np.ndarray:
    h = theta[1] * math.sin(q[1])
    return np.array([
        [-h * qdot[1], -h * (qdot[0] + qdot[1])],
        [h * qdot[0], 0.0],
    ])


def gravity_vector(q: np.ndarray, theta: np.ndarray) -> np.ndarray:
    return np.array([
        theta[3] * math.cos(q[0]) + theta[4] * math.cos(q[0] + q[1]),
        theta[4] * math.cos(q[0] + q[1]),
    ])


def regressor(
    q: np.ndarray,
    qdot: np.ndarray,
    qdot_r: np.ndarray,
    qddot_r: np.ndarray,
) -> np.ndarray:
    """Y(q, qdot, qdot_r, qddot_r) such that Y theta = M qddot_r + C qdot_r + g."""
    c2 = math.cos(q[1])
    s2 = math.sin(q[1])

    return np.array([
        [
            qddot_r[0],
            2.0 * c2 * qddot_r[0]
            + c2 * qddot_r[1]
            - s2 * qdot[1] * qdot_r[0]
            - s2 * (qdot[0] + qdot[1]) * qdot_r[1],
            qddot_r[1],
            math.cos(q[0]),
            math.cos(q[0] + q[1]),
        ],
        [
            0.0,
            c2 * qddot_r[0] + s2 * qdot[0] * qdot_r[0],
            qddot_r[0] + qddot_r[1],
            0.0,
            math.cos(q[0] + q[1]),
        ],
    ])


def box_projection(theta_hat: np.ndarray, raw_update: np.ndarray) -> np.ndarray:
    """Componentwise projection: suppress only updates pointing outside known bounds."""
    update = raw_update.copy()
    update[(theta_hat <= THETA_MIN) & (raw_update < 0.0)] = 0.0
    update[(theta_hat >= THETA_MAX) & (raw_update > 0.0)] = 0.0
    return update


def closed_loop_rhs(t: float, state: np.ndarray) -> np.ndarray:
    q = state[0:2]
    qdot = state[2:4]
    theta_hat = state[4:9]

    q_d, qdot_d, qddot_d = desired_trajectory(t)
    e = q - q_d
    edot = qdot - qdot_d

    qdot_r = qdot_d - LAMBDA @ e
    qddot_r = qddot_d - LAMBDA @ edot
    s = qdot - qdot_r

    y = regressor(q, qdot, qdot_r, qddot_r)
    tau = y @ theta_hat - K_D @ s

    theta = true_parameters(t)
    qddot = np.linalg.solve(
        mass_matrix(q, theta),
        tau - coriolis_matrix(q, qdot, theta) @ qdot - gravity_vector(q, theta),
    )

    raw_theta_dot = -GAMMA @ y.T @ s
    theta_hat_dot = box_projection(theta_hat, raw_theta_dot)

    return np.concatenate((qdot, qddot, theta_hat_dot))


def rk4_step(t: float, state: np.ndarray, dt: float) -> np.ndarray:
    k1 = closed_loop_rhs(t, state)
    k2 = closed_loop_rhs(t + 0.5 * dt, state + 0.5 * dt * k1)
    k3 = closed_loop_rhs(t + 0.5 * dt, state + 0.5 * dt * k2)
    k4 = closed_loop_rhs(t + dt, state + dt * k3)
    next_state = state + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
    next_state[4:9] = np.clip(next_state[4:9], THETA_MIN, THETA_MAX)
    return next_state


def simulate() -> np.ndarray:
    state = np.array([0.0, 0.0, 0.0, 0.0, 2.0, 0.5, 0.5, 12.0, 3.0])
    steps = int(round(T_FINAL / DT))
    data = np.zeros((steps + 1, 21))

    for k in range(steps + 1):
        t = k * DT
        q_d, qdot_d, _ = desired_trajectory(t)
        q = state[0:2]
        qdot = state[2:4]
        theta_hat = state[4:9]
        theta = true_parameters(t)

        e = q - q_d
        qdot_r = qdot_d - LAMBDA @ e
        qddot_r = desired_trajectory(t)[2] - LAMBDA @ (qdot - qdot_d)
        s = qdot - qdot_r
        tau = regressor(q, qdot, qdot_r, qddot_r) @ theta_hat - K_D @ s

        data[k, :] = np.concatenate((
            [t],
            q,
            q_d,
            e,
            qdot,
            theta_hat,
            theta,
            tau,
        ))

        if k < steps:
            state = rk4_step(t, state, DT)

    return data


def save_csv(data: np.ndarray, path: Path) -> None:
    header = [
        "t", "q1", "q2", "qd1", "qd2", "e1", "e2", "qdot1", "qdot2",
        "thetaHat1", "thetaHat2", "thetaHat3", "thetaHat4", "thetaHat5",
        "thetaTrue1", "thetaTrue2", "thetaTrue3", "thetaTrue4", "thetaTrue5",
        "tau1", "tau2",
    ]
    # data has 20 columns before torques? Rebuild explicitly to avoid a silent mismatch.
    if data.shape[1] != len(header):
        raise RuntimeError(f"Expected {len(header)} columns, received {data.shape[1]}")

    with path.open("w", newline="", encoding="utf-8") as file:
        writer = csv.writer(file)
        writer.writerow(header)
        writer.writerows(data)


def report(data: np.ndarray) -> None:
    t = data[:, 0]
    error = data[:, 5:7]
    before = t < PAYLOAD_TIME
    after = t >= PAYLOAD_TIME

    rms_before = np.sqrt(np.mean(error[before] ** 2, axis=0))
    rms_after = np.sqrt(np.mean(error[after] ** 2, axis=0))
    final_norm = np.linalg.norm(error[-1])

    print("RMS tracking error before payload:", rms_before)
    print("RMS tracking error after payload: ", rms_after)
    print("Final tracking-error norm:        ", final_norm)
    print("Final parameter estimate:          ", data[-1, 9:14])


def plot_results(data: np.ndarray) -> None:
    try:
        import matplotlib.pyplot as plt
    except ImportError:
        print("matplotlib is not installed; CSV output was still generated.")
        return

    t = data[:, 0]

    plt.figure()
    plt.plot(t, data[:, 1], label="q1")
    plt.plot(t, data[:, 3], "--", label="qd1")
    plt.plot(t, data[:, 2], label="q2")
    plt.plot(t, data[:, 4], "--", label="qd2")
    plt.axvline(PAYLOAD_TIME, linestyle=":", label="payload change")
    plt.xlabel("time [s]")
    plt.ylabel("joint angle [rad]")
    plt.grid(True)
    plt.legend()
    plt.tight_layout()
    plt.savefig("Chapter25_Lesson1_tracking.png", dpi=180)

    plt.figure()
    for index in range(5):
        plt.plot(t, data[:, 9 + index], label=f"thetaHat{index + 1}")
    plt.axvline(PAYLOAD_TIME, linestyle=":")
    plt.xlabel("time [s]")
    plt.ylabel("parameter estimate")
    plt.grid(True)
    plt.legend()
    plt.tight_layout()
    plt.savefig("Chapter25_Lesson1_parameters.png", dpi=180)


def main() -> None:
    data = simulate()
    output = Path("Chapter25_Lesson1_results.csv")
    save_csv(data, output)
    report(data)
    plot_results(data)
    print(f"Saved {output.resolve()}")


if __name__ == "__main__":
    main()

This version is self-contained C++17. In production systems, fixed-size matrices are commonly implemented with Eigen, while rigid-body dynamics libraries can supply nominal dynamics and model parsing.

Chapter25_Lesson1.cpp

/*
Chapter25_Lesson1.cpp
Self-contained C++17 implementation of Slotine-Li adaptive control
for a two-link planar robot. It writes Chapter25_Lesson1_results_cpp.csv.

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

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

using Vec2 = std::array<double, 2>;
using Vec5 = std::array<double, 5>;
using State = std::array<double, 9>;
using Mat2 = std::array<std::array<double, 2>, 2>;
using Reg = std::array<std::array<double, 5>, 2>;

constexpr double DT = 1.0e-3;
constexpr double T_FINAL = 8.0;
constexpr double PAYLOAD_TIME = 3.0;

const Vec2 LAMBDA{5.0, 5.0};
const Vec2 KD{20.0, 15.0};
const Vec5 GAMMA{2.0, 1.0, 1.5, 0.05, 0.05};
const Vec5 THETA_MIN{0.20, 0.05, 0.05, 1.0, 0.20};
const Vec5 THETA_MAX{8.00, 3.00, 3.00, 40.0, 15.0};

struct Desired {
    Vec2 q;
    Vec2 qdot;
    Vec2 qddot;
};

Desired desired(double t) {
    return {
        {0.6 * std::sin(0.7 * t), 0.5 * std::cos(0.5 * t)},
        {0.42 * std::cos(0.7 * t), -0.25 * std::sin(0.5 * t)},
        {-0.294 * std::sin(0.7 * t), -0.125 * std::cos(0.5 * t)}
    };
}

Vec5 trueParameters(double t) {
    Vec5 p{3.0, 0.8, 0.7, 19.0, 5.0};
    if (t >= PAYLOAD_TIME) {
        p[0] += 0.8; p[1] += 0.2; p[2] += 0.3; p[4] += 2.0;
    }
    return p;
}

Mat2 massMatrix(const Vec2& q, const Vec5& p) {
    const double c2 = std::cos(q[1]);
    return { {
        { {p[0] + 2.0 * p[1] * c2, p[2] + p[1] * c2} },
        { {p[2] + p[1] * c2, p[2]} }
    } };
}

Mat2 coriolisMatrix(const Vec2& q, const Vec2& qdot, const Vec5& p) {
    const double h = p[1] * std::sin(q[1]);
    return { {
        { {-h * qdot[1], -h * (qdot[0] + qdot[1])} },
        { { h * qdot[0], 0.0} }
    } };
}

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

Vec2 matVec(const Mat2& a, const Vec2& x) {
    return {
        a[0][0] * x[0] + a[0][1] * x[1],
        a[1][0] * x[0] + a[1][1] * x[1]
    };
}

Vec2 solve2x2(const Mat2& a, const Vec2& b) {
    const double det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
    if (std::abs(det) < 1.0e-10) {
        throw std::runtime_error("Near-singular inertia matrix");
    }
    return {
        ( b[0] * a[1][1] - a[0][1] * b[1]) / det,
        (-b[0] * a[1][0] + a[0][0] * b[1]) / det
    };
}

Reg regressor(const Vec2& q, const Vec2& qdot,
              const Vec2& qdotR, const Vec2& qddotR) {
    const double c2 = std::cos(q[1]);
    const double s2 = std::sin(q[1]);
    return { {
        { {
            qddotR[0],
            2.0 * c2 * qddotR[0] + c2 * qddotR[1]
              - s2 * qdot[1] * qdotR[0]
              - s2 * (qdot[0] + qdot[1]) * qdotR[1],
            qddotR[1],
            std::cos(q[0]),
            std::cos(q[0] + q[1])
        } },
        { {
            0.0,
            c2 * qddotR[0] + s2 * qdot[0] * qdotR[0],
            qddotR[0] + qddotR[1],
            0.0,
            std::cos(q[0] + q[1])
        } }
    } };
}

Vec2 regTimesTheta(const Reg& y, const Vec5& theta) {
    Vec2 out{0.0, 0.0};
    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j < 5; ++j) out[i] += y[i][j] * theta[j];
    }
    return out;
}

State rhs(double t, const State& x) {
    const Vec2 q{x[0], x[1]};
    const Vec2 qdot{x[2], x[3]};
    const Vec5 thetaHat{x[4], x[5], x[6], x[7], x[8]};
    const Desired d = desired(t);

    Vec2 e{}, edot{}, qdotR{}, qddotR{}, s{};
    for (int i = 0; i < 2; ++i) {
        e[i] = q[i] - d.q[i];
        edot[i] = qdot[i] - d.qdot[i];
        qdotR[i] = d.qdot[i] - LAMBDA[i] * e[i];
        qddotR[i] = d.qddot[i] - LAMBDA[i] * edot[i];
        s[i] = qdot[i] - qdotR[i];
    }

    const Reg y = regressor(q, qdot, qdotR, qddotR);
    Vec2 tau = regTimesTheta(y, thetaHat);
    for (int i = 0; i < 2; ++i) tau[i] -= KD[i] * s[i];

    const Vec5 theta = trueParameters(t);
    const Vec2 cqd = matVec(coriolisMatrix(q, qdot, theta), qdot);
    const Vec2 g = gravityVector(q, theta);
    const Vec2 net{tau[0] - cqd[0] - g[0], tau[1] - cqd[1] - g[1]};
    const Vec2 qddot = solve2x2(massMatrix(q, theta), net);

    Vec5 thetaDot{};
    for (int j = 0; j < 5; ++j) {
        double grad = y[0][j] * s[0] + y[1][j] * s[1];
        double raw = -GAMMA[j] * grad;
        if ((thetaHat[j] <= THETA_MIN[j] && raw < 0.0) ||
            (thetaHat[j] >= THETA_MAX[j] && raw > 0.0)) {
            raw = 0.0;
        }
        thetaDot[j] = raw;
    }

    return {qdot[0], qdot[1], qddot[0], qddot[1],
            thetaDot[0], thetaDot[1], thetaDot[2], thetaDot[3], thetaDot[4]};
}

State addScaled(const State& a, const State& b, double scale) {
    State out{};
    for (int i = 0; i < 9; ++i) out[i] = a[i] + scale * b[i];
    return out;
}

State rk4(double t, const State& x) {
    const State k1 = rhs(t, x);
    const State k2 = rhs(t + 0.5 * DT, addScaled(x, k1, 0.5 * DT));
    const State k3 = rhs(t + 0.5 * DT, addScaled(x, k2, 0.5 * DT));
    const State k4 = rhs(t + DT, addScaled(x, k3, DT));

    State next{};
    for (int i = 0; i < 9; ++i) {
        next[i] = x[i] + DT * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) / 6.0;
    }
    for (int j = 0; j < 5; ++j) {
        if (next[4 + j] < THETA_MIN[j]) next[4 + j] = THETA_MIN[j];
        if (next[4 + j] > THETA_MAX[j]) next[4 + j] = THETA_MAX[j];
    }
    return next;
}

int main() {
    State x{0.0, 0.0, 0.0, 0.0, 2.0, 0.5, 0.5, 12.0, 3.0};
    std::ofstream file("Chapter25_Lesson1_results_cpp.csv");
    if (!file) {
        std::cerr << "Cannot create output CSV\n";
        return 1;
    }

    file << "t,q1,q2,qd1,qd2,e1,e2,qdot1,qdot2,"
            "thetaHat1,thetaHat2,thetaHat3,thetaHat4,thetaHat5,"
            "thetaTrue1,thetaTrue2,thetaTrue3,thetaTrue4,thetaTrue5,tau1,tau2\n";
    file << std::setprecision(12);

    double sumBefore1 = 0.0, sumBefore2 = 0.0;
    double sumAfter1 = 0.0, sumAfter2 = 0.0;
    long nBefore = 0, nAfter = 0;

    const int steps = static_cast<int>(std::lround(T_FINAL / DT));
    for (int k = 0; k <= steps; ++k) {
        const double t = k * DT;
        const Desired d = desired(t);
        const Vec2 q{x[0], x[1]};
        const Vec2 qdot{x[2], x[3]};
        const Vec5 thetaHat{x[4], x[5], x[6], x[7], x[8]};
        const Vec5 theta = trueParameters(t);

        Vec2 e{}, edot{}, qdotR{}, qddotR{}, s{};
        for (int i = 0; i < 2; ++i) {
            e[i] = q[i] - d.q[i];
            edot[i] = qdot[i] - d.qdot[i];
            qdotR[i] = d.qdot[i] - LAMBDA[i] * e[i];
            qddotR[i] = d.qddot[i] - LAMBDA[i] * edot[i];
            s[i] = qdot[i] - qdotR[i];
        }
        Vec2 tau = regTimesTheta(regressor(q, qdot, qdotR, qddotR), thetaHat);
        for (int i = 0; i < 2; ++i) tau[i] -= KD[i] * s[i];

        file << t << ',' << q[0] << ',' << q[1] << ','
             << d.q[0] << ',' << d.q[1] << ','
             << e[0] << ',' << e[1] << ','
             << qdot[0] << ',' << qdot[1];
        for (double v : thetaHat) file << ',' << v;
        for (double v : theta) file << ',' << v;
        file << ',' << tau[0] << ',' << tau[1] << '\n';

        if (t < PAYLOAD_TIME) {
            sumBefore1 += e[0] * e[0]; sumBefore2 += e[1] * e[1]; ++nBefore;
        } else {
            sumAfter1 += e[0] * e[0]; sumAfter2 += e[1] * e[1]; ++nAfter;
        }

        if (k < steps) x = rk4(t, x);
    }

    std::cout << "RMS before payload: ["
              << std::sqrt(sumBefore1 / nBefore) << ", "
              << std::sqrt(sumBefore2 / nBefore) << "]\n";
    std::cout << "RMS after payload:  ["
              << std::sqrt(sumAfter1 / nAfter) << ", "
              << std::sqrt(sumAfter2 / nAfter) << "]\n";
    std::cout << "Final theta estimate: [";
    for (int j = 0; j < 5; ++j) {
        if (j) std::cout << ", ";
        std::cout << x[4 + j];
    }
    std::cout << "]\n";
    return 0;
}

The Java 17 implementation uses primitive arrays so that it runs without external dependencies. EJML is a suitable replacement for the manual matrix operations in larger manipulators.

Chapter25_Lesson1.java

/*
Chapter25_Lesson1.java
Self-contained Java 17 implementation of Slotine-Li adaptive control
for a two-link planar robot.

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

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

public final class Chapter25_Lesson1 {
    private static final double DT = 1.0e-3;
    private static final double T_FINAL = 8.0;
    private static final double PAYLOAD_TIME = 3.0;

    private static final double[] LAMBDA = {5.0, 5.0};
    private static final double[] KD = {20.0, 15.0};
    private static final double[] GAMMA = {2.0, 1.0, 1.5, 0.05, 0.05};
    private static final double[] THETA_MIN = {0.20, 0.05, 0.05, 1.0, 0.20};
    private static final double[] THETA_MAX = {8.00, 3.00, 3.00, 40.0, 15.0};

    private record Desired(double[] q, double[] qdot, double[] qddot) {}

    private static Desired desired(double t) {
        return new Desired(
            new double[] {
                0.6 * Math.sin(0.7 * t),
                0.5 * Math.cos(0.5 * t)
            },
            new double[] {
                0.42 * Math.cos(0.7 * t),
                -0.25 * Math.sin(0.5 * t)
            },
            new double[] {
                -0.294 * Math.sin(0.7 * t),
                -0.125 * Math.cos(0.5 * t)
            }
        );
    }

    private static double[] trueParameters(double t) {
        double[] p = {3.0, 0.8, 0.7, 19.0, 5.0};
        if (t >= PAYLOAD_TIME) {
            p[0] += 0.8;
            p[1] += 0.2;
            p[2] += 0.3;
            p[4] += 2.0;
        }
        return p;
    }

    private static double[][] massMatrix(double[] q, double[] p) {
        double c2 = Math.cos(q[1]);
        return new double[][] {
            {p[0] + 2.0 * p[1] * c2, p[2] + p[1] * c2},
            {p[2] + p[1] * c2, p[2]}
        };
    }

    private static double[][] coriolisMatrix(double[] q, double[] qdot, double[] p) {
        double h = p[1] * Math.sin(q[1]);
        return new double[][] {
            {-h * qdot[1], -h * (qdot[0] + qdot[1])},
            {h * qdot[0], 0.0}
        };
    }

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

    private static double[] matVec(double[][] a, double[] x) {
        return new double[] {
            a[0][0] * x[0] + a[0][1] * x[1],
            a[1][0] * x[0] + a[1][1] * x[1]
        };
    }

    private static double[] solve2x2(double[][] a, double[] b) {
        double det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
        if (Math.abs(det) < 1.0e-10) {
            throw new IllegalStateException("Near-singular inertia matrix");
        }
        return new double[] {
            (b[0] * a[1][1] - a[0][1] * b[1]) / det,
            (-b[0] * a[1][0] + a[0][0] * b[1]) / det
        };
    }

    private static double[][] regressor(
        double[] q,
        double[] qdot,
        double[] qdotR,
        double[] qddotR
    ) {
        double c2 = Math.cos(q[1]);
        double s2 = Math.sin(q[1]);
        return new double[][] {
            {
                qddotR[0],
                2.0 * c2 * qddotR[0] + c2 * qddotR[1]
                    - s2 * qdot[1] * qdotR[0]
                    - s2 * (qdot[0] + qdot[1]) * qdotR[1],
                qddotR[1],
                Math.cos(q[0]),
                Math.cos(q[0] + q[1])
            },
            {
                0.0,
                c2 * qddotR[0] + s2 * qdot[0] * qdotR[0],
                qddotR[0] + qddotR[1],
                0.0,
                Math.cos(q[0] + q[1])
            }
        };
    }

    private static double[] regTimesTheta(double[][] y, double[] theta) {
        double[] out = {0.0, 0.0};
        for (int i = 0; i < 2; i++) {
            for (int j = 0; j < 5; j++) {
                out[i] += y[i][j] * theta[j];
            }
        }
        return out;
    }

    private static double[] rhs(double t, double[] x) {
        double[] q = {x[0], x[1]};
        double[] qdot = {x[2], x[3]};
        double[] thetaHat = {x[4], x[5], x[6], x[7], x[8]};
        Desired d = desired(t);

        double[] e = new double[2];
        double[] edot = new double[2];
        double[] qdotR = new double[2];
        double[] qddotR = new double[2];
        double[] s = new double[2];

        for (int i = 0; i < 2; i++) {
            e[i] = q[i] - d.q()[i];
            edot[i] = qdot[i] - d.qdot()[i];
            qdotR[i] = d.qdot()[i] - LAMBDA[i] * e[i];
            qddotR[i] = d.qddot()[i] - LAMBDA[i] * edot[i];
            s[i] = qdot[i] - qdotR[i];
        }

        double[][] y = regressor(q, qdot, qdotR, qddotR);
        double[] tau = regTimesTheta(y, thetaHat);
        for (int i = 0; i < 2; i++) {
            tau[i] -= KD[i] * s[i];
        }

        double[] theta = trueParameters(t);
        double[] cqd = matVec(coriolisMatrix(q, qdot, theta), qdot);
        double[] g = gravityVector(q, theta);
        double[] qddot = solve2x2(
            massMatrix(q, theta),
            new double[] {tau[0] - cqd[0] - g[0], tau[1] - cqd[1] - g[1]}
        );

        double[] thetaDot = new double[5];
        for (int j = 0; j < 5; j++) {
            double grad = y[0][j] * s[0] + y[1][j] * s[1];
            double raw = -GAMMA[j] * grad;
            if ((thetaHat[j] <= THETA_MIN[j] && raw < 0.0)
                || (thetaHat[j] >= THETA_MAX[j] && raw > 0.0)) {
                raw = 0.0;
            }
            thetaDot[j] = raw;
        }

        return new double[] {
            qdot[0], qdot[1], qddot[0], qddot[1],
            thetaDot[0], thetaDot[1], thetaDot[2], thetaDot[3], thetaDot[4]
        };
    }

    private static double[] addScaled(double[] a, double[] b, double scale) {
        double[] out = new double[a.length];
        for (int i = 0; i < a.length; i++) {
            out[i] = a[i] + scale * b[i];
        }
        return out;
    }

    private static double[] rk4(double t, double[] x) {
        double[] k1 = rhs(t, x);
        double[] k2 = rhs(t + 0.5 * DT, addScaled(x, k1, 0.5 * DT));
        double[] k3 = rhs(t + 0.5 * DT, addScaled(x, k2, 0.5 * DT));
        double[] k4 = rhs(t + DT, addScaled(x, k3, DT));

        double[] next = new double[9];
        for (int i = 0; i < 9; i++) {
            next[i] = x[i] + DT * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) / 6.0;
        }
        for (int j = 0; j < 5; j++) {
            next[4 + j] = Math.max(THETA_MIN[j], Math.min(THETA_MAX[j], next[4 + j]));
        }
        return next;
    }

    public static void main(String[] args) throws IOException {
        Locale.setDefault(Locale.US);
        double[] x = {0.0, 0.0, 0.0, 0.0, 2.0, 0.5, 0.5, 12.0, 3.0};

        double sumBefore1 = 0.0;
        double sumBefore2 = 0.0;
        double sumAfter1 = 0.0;
        double sumAfter2 = 0.0;
        long nBefore = 0;
        long nAfter = 0;

        try (BufferedWriter writer = new BufferedWriter(
                new FileWriter("Chapter25_Lesson1_results_java.csv"))) {
            writer.write(
                "t,q1,q2,qd1,qd2,e1,e2,qdot1,qdot2,"
                + "thetaHat1,thetaHat2,thetaHat3,thetaHat4,thetaHat5,"
                + "thetaTrue1,thetaTrue2,thetaTrue3,thetaTrue4,thetaTrue5,tau1,tau2\n"
            );

            int steps = (int) Math.round(T_FINAL / DT);
            for (int k = 0; k <= steps; k++) {
                double t = k * DT;
                Desired d = desired(t);
                double[] q = {x[0], x[1]};
                double[] qdot = {x[2], x[3]};
                double[] thetaHat = {x[4], x[5], x[6], x[7], x[8]};
                double[] theta = trueParameters(t);

                double[] e = new double[2];
                double[] edot = new double[2];
                double[] qdotR = new double[2];
                double[] qddotR = new double[2];
                double[] s = new double[2];
                for (int i = 0; i < 2; i++) {
                    e[i] = q[i] - d.q()[i];
                    edot[i] = qdot[i] - d.qdot()[i];
                    qdotR[i] = d.qdot()[i] - LAMBDA[i] * e[i];
                    qddotR[i] = d.qddot()[i] - LAMBDA[i] * edot[i];
                    s[i] = qdot[i] - qdotR[i];
                }
                double[] tau = regTimesTheta(regressor(q, qdot, qdotR, qddotR), thetaHat);
                for (int i = 0; i < 2; i++) {
                    tau[i] -= KD[i] * s[i];
                }

                writer.write(String.format(Locale.US,
                    "%.12g,%.12g,%.12g,%.12g,%.12g,%.12g,%.12g,%.12g,%.12g",
                    t, q[0], q[1], d.q()[0], d.q()[1], e[0], e[1], qdot[0], qdot[1]));
                for (double value : thetaHat) {
                    writer.write(String.format(Locale.US, ",%.12g", value));
                }
                for (double value : theta) {
                    writer.write(String.format(Locale.US, ",%.12g", value));
                }
                writer.write(String.format(Locale.US, ",%.12g,%.12g%n", tau[0], tau[1]));

                if (t < PAYLOAD_TIME) {
                    sumBefore1 += e[0] * e[0];
                    sumBefore2 += e[1] * e[1];
                    nBefore++;
                } else {
                    sumAfter1 += e[0] * e[0];
                    sumAfter2 += e[1] * e[1];
                    nAfter++;
                }

                if (k < steps) {
                    x = rk4(t, x);
                }
            }
        }

        System.out.printf(Locale.US, "RMS before payload: [%.7f, %.7f]%n",
            Math.sqrt(sumBefore1 / nBefore), Math.sqrt(sumBefore2 / nBefore));
        System.out.printf(Locale.US, "RMS after payload:  [%.7f, %.7f]%n",
            Math.sqrt(sumAfter1 / nAfter), Math.sqrt(sumAfter2 / nAfter));
        System.out.printf(Locale.US,
            "Final theta estimate: [%.6f, %.6f, %.6f, %.6f, %.6f]%n",
            x[4], x[5], x[6], x[7], x[8]);
    }
}

The first file is a directly executable MATLAB simulation. The second file creates an equivalent Simulink model with MATLAB Function blocks and continuous integrators. Robotics System Toolbox can replace the explicit plant equations with rigidBodyTree, massMatrix, velocityProduct, gravityTorque, and inverseDynamics.

Chapter25_Lesson1.m

% Chapter25_Lesson1.m
% Slotine-Li adaptive tracking control for a two-link planar manipulator.
% The script uses a fixed-step RK4 integrator and writes a CSV file.
%
% Run:
%   Chapter25_Lesson1

clear; clc; close all;

dt = 1.0e-3;
tFinal = 8.0;
payloadTime = 3.0;
steps = round(tFinal / dt);

lambda = diag([5.0, 5.0]);
Kd = diag([20.0, 15.0]);
Gamma = diag([2.0, 1.0, 1.5, 0.05, 0.05]);
thetaMin = [0.20; 0.05; 0.05; 1.0; 0.20];
thetaMax = [8.00; 3.00; 3.00; 40.0; 15.0];

% x = [q1;q2;qdot1;qdot2;thetaHat1;...;thetaHat5]
x = [0; 0; 0; 0; 2.0; 0.5; 0.5; 12.0; 3.0];
data = zeros(steps + 1, 21);

for k = 1:(steps + 1)
    t = (k - 1) * dt;
    [qd, qdDot, qdDDot] = desiredTrajectory(t);
    q = x(1:2);
    qDot = x(3:4);
    thetaHat = x(5:9);
    theta = trueParameters(t, payloadTime);

    e = q - qd;
    eDot = qDot - qdDot;
    qDotR = qdDot - lambda * e;
    qDDotR = qdDDot - lambda * eDot;
    s = qDot - qDotR;

    Y = robotRegressor(q, qDot, qDotR, qDDotR);
    tau = Y * thetaHat - Kd * s;

    data(k, :) = [t; q; qd; e; qDot; thetaHat; theta; tau].';

    if k <= steps
        k1 = closedLoopRhs(t, x, lambda, Kd, Gamma, thetaMin, thetaMax, payloadTime);
        k2 = closedLoopRhs(t + dt/2, x + dt*k1/2, lambda, Kd, Gamma, thetaMin, thetaMax, payloadTime);
        k3 = closedLoopRhs(t + dt/2, x + dt*k2/2, lambda, Kd, Gamma, thetaMin, thetaMax, payloadTime);
        k4 = closedLoopRhs(t + dt, x + dt*k3, lambda, Kd, Gamma, thetaMin, thetaMax, payloadTime);
        x = x + dt * (k1 + 2*k2 + 2*k3 + k4) / 6;
        x(5:9) = min(max(x(5:9), thetaMin), thetaMax);
    end
end

names = {'t','q1','q2','qd1','qd2','e1','e2','qdot1','qdot2', ...
    'thetaHat1','thetaHat2','thetaHat3','thetaHat4','thetaHat5', ...
    'thetaTrue1','thetaTrue2','thetaTrue3','thetaTrue4','thetaTrue5', ...
    'tau1','tau2'};
results = array2table(data, 'VariableNames', names);
writetable(results, 'Chapter25_Lesson1_results_matlab.csv');

before = results.t < payloadTime;
after = ~before;
rmsBefore = sqrt(mean([results.e1(before).^2, results.e2(before).^2], 1));
rmsAfter = sqrt(mean([results.e1(after).^2, results.e2(after).^2], 1));

fprintf('RMS before payload: [%.7f, %.7f]\n', rmsBefore(1), rmsBefore(2));
fprintf('RMS after payload:  [%.7f, %.7f]\n', rmsAfter(1), rmsAfter(2));
fprintf('Final theta estimate: [%.6f %.6f %.6f %.6f %.6f]\n', x(5:9));

figure('Name', 'Adaptive robot tracking');
plot(results.t, results.q1, 'LineWidth', 1.1); hold on;
plot(results.t, results.qd1, '--', 'LineWidth', 1.1);
plot(results.t, results.q2, 'LineWidth', 1.1);
plot(results.t, results.qd2, '--', 'LineWidth', 1.1);
xline(payloadTime, ':', 'Payload change');
grid on;
xlabel('Time [s]');
ylabel('Joint angle [rad]');
legend('q_1','q_{d1}','q_2','q_{d2}', 'Location', 'best');

figure('Name', 'Parameter estimates');
plot(results.t, data(:, 10:14), 'LineWidth', 1.0);
xline(payloadTime, ':', 'Payload change');
grid on;
xlabel('Time [s]');
ylabel('Parameter estimate');
legend('\theta_1','\theta_2','\theta_3','\theta_4','\theta_5', 'Location', 'best');


function dx = closedLoopRhs(t, x, lambda, Kd, Gamma, thetaMin, thetaMax, payloadTime)
    q = x(1:2);
    qDot = x(3:4);
    thetaHat = x(5:9);

    [qd, qdDot, qdDDot] = desiredTrajectory(t);
    e = q - qd;
    eDot = qDot - qdDot;
    qDotR = qdDot - lambda * e;
    qDDotR = qdDDot - lambda * eDot;
    s = qDot - qDotR;

    Y = robotRegressor(q, qDot, qDotR, qDDotR);
    tau = Y * thetaHat - Kd * s;

    theta = trueParameters(t, payloadTime);
    qDDot = massMatrix2R(q, theta) \ ...
        (tau - coriolisMatrix2R(q, qDot, theta) * qDot - gravityVector2R(q, theta));

    rawThetaDot = -Gamma * Y.' * s;
    thetaHatDot = boxProjection(thetaHat, rawThetaDot, thetaMin, thetaMax);

    dx = [qDot; qDDot; thetaHatDot];
end


function [qd, qdDot, qdDDot] = desiredTrajectory(t)
    qd = [0.6*sin(0.7*t); 0.5*cos(0.5*t)];
    qdDot = [0.42*cos(0.7*t); -0.25*sin(0.5*t)];
    qdDDot = [-0.294*sin(0.7*t); -0.125*cos(0.5*t)];
end


function theta = trueParameters(t, payloadTime)
    theta = [3.0; 0.8; 0.7; 19.0; 5.0];
    if t >= payloadTime
        theta = theta + [0.8; 0.2; 0.3; 0.0; 2.0];
    end
end


function M = massMatrix2R(q, theta)
    c2 = cos(q(2));
    M = [theta(1) + 2*theta(2)*c2, theta(3) + theta(2)*c2; ...
         theta(3) + theta(2)*c2, theta(3)];
end


function C = coriolisMatrix2R(q, qDot, theta)
    h = theta(2) * sin(q(2));
    C = [-h*qDot(2), -h*(qDot(1) + qDot(2)); ...
          h*qDot(1), 0];
end


function g = gravityVector2R(q, theta)
    g = [theta(4)*cos(q(1)) + theta(5)*cos(q(1) + q(2)); ...
         theta(5)*cos(q(1) + q(2))];
end


function Y = robotRegressor(q, qDot, qDotR, qDDotR)
    c2 = cos(q(2));
    s2 = sin(q(2));

    Y = [ ...
        qDDotR(1), ...
        2*c2*qDDotR(1) + c2*qDDotR(2) ...
            - s2*qDot(2)*qDotR(1) ...
            - s2*(qDot(1) + qDot(2))*qDotR(2), ...
        qDDotR(2), ...
        cos(q(1)), ...
        cos(q(1) + q(2)); ...
        0, ...
        c2*qDDotR(1) + s2*qDot(1)*qDotR(1), ...
        qDDotR(1) + qDDotR(2), ...
        0, ...
        cos(q(1) + q(2)) ...
    ];
end


function projected = boxProjection(thetaHat, rawUpdate, thetaMin, thetaMax)
    projected = rawUpdate;
    for i = 1:numel(thetaHat)
        if (thetaHat(i) <= thetaMin(i) && rawUpdate(i) < 0) || ...
           (thetaHat(i) >= thetaMax(i) && rawUpdate(i) > 0)
            projected(i) = 0;
        end
    end
end

Chapter25_Lesson1_Simulink.m

% Chapter25_Lesson1_Simulink.m
% Programmatically constructs a Simulink model of the same adaptive
% two-link robot experiment used in Chapter25_Lesson1.m.
%
% Requirements:
%   Simulink
%   Stateflow (used internally by MATLAB Function blocks)
%
% Run:
%   Chapter25_Lesson1_Simulink
% Then press Run in the generated model.

model = 'Chapter25_Lesson1_Simulink_Model';

if bdIsLoaded(model)
    close_system(model, 0);
end
if isfile([model '.slx'])
    delete([model '.slx']);
end

new_system(model);
open_system(model);
set_param(model, ...
    'SolverType', 'Fixed-step', ...
    'Solver', 'ode4', ...
    'FixedStep', '0.001', ...
    'StopTime', '8');

add_block('simulink/Sources/Clock', [model '/Clock'], ...
    'Position', [30 45 60 65]);

add_block('simulink/User-Defined Functions/MATLAB Function', ...
    [model '/Adaptive Controller'], ...
    'Position', [250 40 460 170]);

add_block('simulink/User-Defined Functions/MATLAB Function', ...
    [model '/Robot Plant'], ...
    'Position', [560 210 750 330]);

add_block('simulink/Continuous/Integrator', [model '/Integrate qddot'], ...
    'InitialCondition', '[0;0]', ...
    'Position', [820 230 850 260]);

add_block('simulink/Continuous/Integrator', [model '/Integrate qdot'], ...
    'InitialCondition', '[0;0]', ...
    'Position', [920 230 950 260]);

add_block('simulink/Continuous/Integrator', [model '/Integrate thetaHat'], ...
    'InitialCondition', '[2;0.5;0.5;12;3]', ...
    'LimitOutput', 'on', ...
    'LowerSaturationLimit', '[0.2;0.05;0.05;1;0.2]', ...
    'UpperSaturationLimit', '[8;3;3;40;15]', ...
    'Position', [560 80 590 120]);

add_block('simulink/Sinks/To Workspace', [model '/q_log'], ...
    'VariableName', 'q_sim', 'SaveFormat', 'Structure With Time', ...
    'Position', [1030 210 1120 240]);

add_block('simulink/Sinks/To Workspace', [model '/theta_log'], ...
    'VariableName', 'theta_hat_sim', 'SaveFormat', 'Structure With Time', ...
    'Position', [650 70 755 100]);

controllerCode = sprintf([ ...
'function [tau,thetaDot] = fcn(t,q,qDot,thetaHat)\n' ...
'lambda = diag([5 5]); Kd = diag([20 15]);\n' ...
'Gamma = diag([2 1 1.5 0.05 0.05]);\n' ...
'qd = [0.6*sin(0.7*t);0.5*cos(0.5*t)];\n' ...
'qdDot = [0.42*cos(0.7*t);-0.25*sin(0.5*t)];\n' ...
'qdDDot = [-0.294*sin(0.7*t);-0.125*cos(0.5*t)];\n' ...
'e = q-qd; eDot = qDot-qdDot;\n' ...
'qDotR = qdDot-lambda*e; qDDotR = qdDDot-lambda*eDot;\n' ...
's = qDot-qDotR; c2=cos(q(2)); s2=sin(q(2));\n' ...
'Y=[qDDotR(1),2*c2*qDDotR(1)+c2*qDDotR(2)-s2*qDot(2)*qDotR(1)-s2*(qDot(1)+qDot(2))*qDotR(2),qDDotR(2),cos(q(1)),cos(q(1)+q(2));0,c2*qDDotR(1)+s2*qDot(1)*qDotR(1),qDDotR(1)+qDDotR(2),0,cos(q(1)+q(2))];\n' ...
'tau=Y*thetaHat-Kd*s;\n' ...
'thetaDot=-Gamma*Y''*s;\n' ...
'lo=[0.2;0.05;0.05;1;0.2]; hi=[8;3;3;40;15];\n' ...
'for i=1:5\n' ...
' if (thetaHat(i)<=lo(i) && thetaDot(i)<0) || (thetaHat(i)>=hi(i) && thetaDot(i)>0), thetaDot(i)=0; end\n' ...
'end\n' ...
'end\n']);

plantCode = sprintf([ ...
'function qDDot = fcn(t,q,qDot,tau)\n' ...
'theta=[3;0.8;0.7;19;5];\n' ...
'if t>=3, theta=theta+[0.8;0.2;0.3;0;2]; end\n' ...
'c2=cos(q(2)); h=theta(2)*sin(q(2));\n' ...
'M=[theta(1)+2*theta(2)*c2,theta(3)+theta(2)*c2;theta(3)+theta(2)*c2,theta(3)];\n' ...
'C=[-h*qDot(2),-h*(qDot(1)+qDot(2));h*qDot(1),0];\n' ...
'g=[theta(4)*cos(q(1))+theta(5)*cos(q(1)+q(2));theta(5)*cos(q(1)+q(2))];\n' ...
'qDDot=M\\(tau-C*qDot-g);\n' ...
'end\n']);

root = sfroot;
controllerChart = root.find('-isa', 'Stateflow.EMChart', ...
    'Path', [model '/Adaptive Controller']);
plantChart = root.find('-isa', 'Stateflow.EMChart', ...
    'Path', [model '/Robot Plant']);
controllerChart.Script = controllerCode;
plantChart.Script = plantCode;

% Update once so MATLAB Function block ports match the function signatures.
set_param(model, 'SimulationCommand', 'update');

add_line(model, 'Clock/1', 'Adaptive Controller/1', 'autorouting', 'on');
add_line(model, 'Clock/1', 'Robot Plant/1', 'autorouting', 'on');

add_line(model, 'Integrate qdot/1', 'Adaptive Controller/2', 'autorouting', 'on');
add_line(model, 'Integrate qddot/1', 'Adaptive Controller/3', 'autorouting', 'on');
add_line(model, 'Integrate thetaHat/1', 'Adaptive Controller/4', 'autorouting', 'on');

add_line(model, 'Adaptive Controller/1', 'Robot Plant/4', 'autorouting', 'on');
add_line(model, 'Adaptive Controller/2', 'Integrate thetaHat/1', 'autorouting', 'on');

add_line(model, 'Integrate qdot/1', 'Robot Plant/2', 'autorouting', 'on');
add_line(model, 'Integrate qddot/1', 'Robot Plant/3', 'autorouting', 'on');

add_line(model, 'Robot Plant/1', 'Integrate qddot/1', 'autorouting', 'on');
add_line(model, 'Integrate qddot/1', 'Integrate qdot/1', 'autorouting', 'on');

add_line(model, 'Integrate qdot/1', 'q_log/1', 'autorouting', 'on');
add_line(model, 'Integrate thetaHat/1', 'theta_log/1', 'autorouting', 'on');

save_system(model);
open_system(model);
disp(['Created ' model '.slx']);

The notebook loads the accompanying Wolfram Language source file. The source performs the same RK4 experiment, exports results, and creates tracking and parameter plots.

Chapter25_Lesson1.nb

Notebook[{
 Cell["Chapter 25, Lesson 1: Adaptive Control for Robot Manipulators with Uncertain Inertia", "Title"],
 Cell["This notebook loads and executes the accompanying Chapter25_Lesson1.wl implementation. Keep both files in the same directory.", "Text"],
 Cell[BoxData[
  RowBox[{
   "Get", "[",
   RowBox[{
    "FileNameJoin", "[",
    RowBox[{"{",
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}],
      ",",
      "\"Chapter25_Lesson1.wl\""
     }],
     "}"
    }],
    "]"
   }],
   "]"
  }]
 ], "Input"]
},
WindowSize->{1100, 800},
WindowMargins->{ {Automatic, 30}, {Automatic, 30} },
FrontEndVersion->"13.3 for Microsoft Windows (64-bit)",
StyleDefinitions->"Default.nb"
]

Chapter25_Lesson1.wl

(* Chapter25_Lesson1.wl
   Slotine-Li adaptive tracking control for a two-link planar manipulator.
   Run with Wolfram Mathematica or:
       wolframscript -file Chapter25_Lesson1.wl
*)

ClearAll["Global`*"];

dt = 1.*^-3;
tFinal = 8.0;
payloadTime = 3.0;

lambda = DiagonalMatrix[{5.0, 5.0}];
kd = DiagonalMatrix[{20.0, 15.0}];
gamma = DiagonalMatrix[{2.0, 1.0, 1.5, 0.05, 0.05}];

thetaMin = {0.20, 0.05, 0.05, 1.0, 0.20};
thetaMax = {8.00, 3.00, 3.00, 40.0, 15.0};

desired[t_?NumericQ] := {
  {0.6 Sin[0.7 t], 0.5 Cos[0.5 t]},
  {0.42 Cos[0.7 t], -0.25 Sin[0.5 t]},
  {-0.294 Sin[0.7 t], -0.125 Cos[0.5 t]}
};

trueParameters[t_?NumericQ] := Module[{theta = {3.0, 0.8, 0.7, 19.0, 5.0} },
  If[t >= payloadTime, theta += {0.8, 0.2, 0.3, 0.0, 2.0}];
  theta
];

massMatrix2R[q_, theta_] := Module[{c2 = Cos[q[[2]]]},
  {
    {theta[[1]] + 2 theta[[2]] c2, theta[[3]] + theta[[2]] c2},
    {theta[[3]] + theta[[2]] c2, theta[[3]]}
  }
];

coriolisMatrix2R[q_, qDot_, theta_] := Module[
  {h = theta[[2]] Sin[q[[2]]]},
  {
    {-h qDot[[2]], -h (qDot[[1]] + qDot[[2]])},
    {h qDot[[1]], 0.0}
  }
];

gravityVector2R[q_, theta_] := {
  theta[[4]] Cos[q[[1]]] + theta[[5]] Cos[q[[1]] + q[[2]]],
  theta[[5]] Cos[q[[1]] + q[[2]]]
};

robotRegressor[q_, qDot_, qDotR_, qDDotR_] := Module[
  {c2 = Cos[q[[2]]], s2 = Sin[q[[2]]]},
  {
    {
      qDDotR[[1]],
      2 c2 qDDotR[[1]] + c2 qDDotR[[2]]
        - s2 qDot[[2]] qDotR[[1]]
        - s2 (qDot[[1]] + qDot[[2]]) qDotR[[2]],
      qDDotR[[2]],
      Cos[q[[1]]],
      Cos[q[[1]] + q[[2]]]
    },
    {
      0.0,
      c2 qDDotR[[1]] + s2 qDot[[1]] qDotR[[1]],
      qDDotR[[1]] + qDDotR[[2]],
      0.0,
      Cos[q[[1]] + q[[2]]]
    }
  }
];

boxProjection[thetaHat_, rawUpdate_] := MapThread[
  If[(#1 <= #3 && #2 < 0) || (#1 >= #4 && #2 > 0), 0.0, #2] &,
  {thetaHat, rawUpdate, thetaMin, thetaMax}
];

closedLoopRhs[t_?NumericQ, x_?VectorQ] := Module[
  {
    q = x[[1 ;; 2]], qDot = x[[3 ;; 4]],
    thetaHat = x[[5 ;; 9]], qd, qdDot, qdDDot,
    e, eDot, qDotR, qDDotR, s, y, tau, theta,
    qDDot, rawThetaDot, thetaHatDot
  },
  {qd, qdDot, qdDDot} = desired[t];
  e = q - qd;
  eDot = qDot - qdDot;
  qDotR = qdDot - lambda.e;
  qDDotR = qdDDot - lambda.eDot;
  s = qDot - qDotR;

  y = robotRegressor[q, qDot, qDotR, qDDotR];
  tau = y.thetaHat - kd.s;

  theta = trueParameters[t];
  qDDot = LinearSolve[
    massMatrix2R[q, theta],
    tau - coriolisMatrix2R[q, qDot, theta].qDot - gravityVector2R[q, theta]
  ];

  rawThetaDot = -gamma.Transpose[y].s;
  thetaHatDot = boxProjection[thetaHat, rawThetaDot];

  Join[qDot, qDDot, thetaHatDot]
];

rk4Step[t_?NumericQ, x_?VectorQ] := Module[{k1, k2, k3, k4, next},
  k1 = closedLoopRhs[t, x];
  k2 = closedLoopRhs[t + dt/2, x + dt k1/2];
  k3 = closedLoopRhs[t + dt/2, x + dt k2/2];
  k4 = closedLoopRhs[t + dt, x + dt k3];
  next = x + dt (k1 + 2 k2 + 2 k3 + k4)/6;
  next[[5 ;; 9]] = Clip[next[[5 ;; 9]], {thetaMin, thetaMax}];
  next
];

state = {0.0, 0.0, 0.0, 0.0, 2.0, 0.5, 0.5, 12.0, 3.0};
steps = Round[tFinal/dt];
rows = Reap[
  Do[
    t = k dt;
    {qd, qdDot, qdDDot} = desired[t];
    q = state[[1 ;; 2]];
    qDot = state[[3 ;; 4]];
    thetaHat = state[[5 ;; 9]];
    theta = trueParameters[t];
    e = q - qd;
    eDot = qDot - qdDot;
    qDotR = qdDot - lambda.e;
    qDDotR = qdDDot - lambda.eDot;
    s = qDot - qDotR;
    tau = robotRegressor[q, qDot, qDotR, qDDotR].thetaHat - kd.s;

    Sow[Join[{t}, q, qd, e, qDot, thetaHat, theta, tau]];
    If[k < steps, state = rk4Step[t, state]],
    {k, 0, steps}
  ]
][[2, 1]];

header = {
  "t", "q1", "q2", "qd1", "qd2", "e1", "e2", "qdot1", "qdot2",
  "thetaHat1", "thetaHat2", "thetaHat3", "thetaHat4", "thetaHat5",
  "thetaTrue1", "thetaTrue2", "thetaTrue3", "thetaTrue4", "thetaTrue5",
  "tau1", "tau2"
};

Export["Chapter25_Lesson1_results_wolfram.csv", Prepend[rows, header]];

before = Select[rows, #[[1]] < payloadTime &];
after = Select[rows, #[[1]] >= payloadTime &];
rmsBefore = Sqrt[Mean[#[[6 ;; 7]]^2 & /@ before]];
rmsAfter = Sqrt[Mean[#[[6 ;; 7]]^2 & /@ after]];

Print["RMS before payload: ", rmsBefore];
Print["RMS after payload:  ", rmsAfter];
Print["Final theta estimate: ", state[[5 ;; 9]]];

trackingPlot = ListLinePlot[
  {
    rows[[All, {1, 2}]],
    rows[[All, {1, 4}]],
    rows[[All, {1, 3}]],
    rows[[All, {1, 5}]]
  },
  PlotLegends -> {"q1", "qd1", "q2", "qd2"},
  Frame -> True,
  FrameLabel -> {"time [s]", "joint angle [rad]"},
  GridLines -> { {payloadTime}, None},
  ImageSize -> Large
];

parameterPlot = ListLinePlot[
  Table[rows[[All, {1, 9 + i}]], {i, 1, 5}],
  PlotLegends -> Table["thetaHat" <> ToString[i], {i, 1, 5}],
  Frame -> True,
  FrameLabel -> {"time [s]", "parameter estimate"},
  GridLines -> { {payloadTime}, None},
  ImageSize -> Large
];

Export["Chapter25_Lesson1_tracking_wolfram.png", trackingPlot];
Export["Chapter25_Lesson1_parameters_wolfram.png", parameterPlot];

trackingPlot
parameterPlot

Problem 1: Derive the Two-Link Regressor

Starting from the matrices in Section 4, compute \(\mathbf{M}\dot{\mathbf{v} }+\mathbf{C}\mathbf{v}+\mathbf{g}\) and collect the coefficients of \(\theta_1,\ldots,\theta_5\).

Solution:

The first torque component is

\[ \begin{aligned} \tau_1^{*} ={}& (\theta_1+2\theta_2c_2)a_1 +(\theta_3+\theta_2c_2)a_2\\ &-\theta_2s_2\dot q_2v_1 -\theta_2s_2(\dot q_1+\dot q_2)v_2\\ &+\theta_4\cos q_1+\theta_5\cos(q_1+q_2). \end{aligned} \]

Grouping each parameter gives

\[ \begin{aligned} \tau_1^{*} ={}& a_1\theta_1\\ &+ \left[ 2c_2a_1+c_2a_2-s_2\dot q_2v_1 -s_2(\dot q_1+\dot q_2)v_2 \right]\theta_2\\ &+a_2\theta_3+\cos q_1\theta_4 +\cos(q_1+q_2)\theta_5. \end{aligned} \]

The second component is

\[ \tau_2^{*} = \left(c_2a_1+s_2\dot q_1v_1\right)\theta_2 +(a_1+a_2)\theta_3 +\cos(q_1+q_2)\theta_5. \]

Placing these coefficients row by row produces exactly the matrix \(\mathbf{Y}\) given in Section 4.

Problem 2: Complete the Lyapunov Derivative

For \(V=\frac12\mathbf{s}^{T}\mathbf{M}\mathbf{s} +\frac12\tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1}\tilde{\boldsymbol{\theta} }\), prove that the adaptive law yields \(\dot V=-\mathbf{s}^{T}\mathbf{K}_D\mathbf{s}\).

Solution:

Differentiate and substitute the error dynamics:

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

The identity \(\frac12\mathbf{s}^{T}\dot{\mathbf{M} }\mathbf{s} =\mathbf{s}^{T}\mathbf{C}\mathbf{s}\) cancels the Coriolis term. The update law gives

\[ \tilde{\boldsymbol{\theta} }^{T} \boldsymbol{\Gamma}^{-1} (-\boldsymbol{\Gamma}\mathbf{Y}^{T}\mathbf{s}) = -\mathbf{s}^{T}\mathbf{Y}\tilde{\boldsymbol{\theta} }. \]

Only the dissipative term remains:

\[ \dot V=-\mathbf{s}^{T}\mathbf{K}_D\mathbf{s}\leq 0. \]

Problem 3: Tracking Without Parameter Convergence

Suppose the desired trajectory holds both joints at a constant configuration after a transient. Can the adaptive controller still achieve tracking? Must all five parameter estimates converge?

Solution:

Tracking can still converge because the Lyapunov proof only requires the closed-loop structural properties, not persistent excitation. However, after the transient, the regressor may contain only constant gravity columns or may lose rank. Therefore the integral \(\int_t^{t+T}\mathbf{Y}^{T}\mathbf{Y}\,d\sigma\) need not be uniformly positive definite. Multiple parameter vectors can produce the same torque along that restricted motion. Thus \(\mathbf{e}\to\mathbf{0}\) does not imply \(\hat{\boldsymbol{\theta} }\to\boldsymbol{\theta}\).

Problem 4: Initial Torque Calculation

Use the numerical laboratory values at \(t=0\): \(\mathbf{q}=\dot{\mathbf{q} }=\mathbf{0}\), \(\hat{\boldsymbol{\theta} } =[2,0.5,0.5,12,3]^T\), \(\boldsymbol{\Lambda}=\operatorname{diag}(5,5)\), and \(\mathbf{K}_D=\operatorname{diag}(20,15)\). Compute the initial adaptive torque.

Solution:

The desired signals at zero are

\[ \mathbf{q}_d(0)= \begin{bmatrix}0\\0.5\end{bmatrix}, \quad \dot{\mathbf{q} }_d(0)= \begin{bmatrix}0.42\\0\end{bmatrix}, \quad \ddot{\mathbf{q} }_d(0)= \begin{bmatrix}0\\-0.125\end{bmatrix}. \]

Therefore

\[ \mathbf{e}= \begin{bmatrix}0\\-0.5\end{bmatrix}, \quad \dot{\mathbf{q} }_r= \begin{bmatrix}0.42\\2.5\end{bmatrix}, \quad \ddot{\mathbf{q} }_r= \begin{bmatrix}2.1\\-0.125\end{bmatrix}, \quad \mathbf{s}= \begin{bmatrix}-0.42\\-2.5\end{bmatrix}. \]

At \(\mathbf{q}=0\) and \(\dot{\mathbf{q} }=0\),

\[ \mathbf{Y}(0)= \begin{bmatrix} 2.1 & 4.075 & -0.125 & 1 & 1\\ 0 & 2.1 & 1.975 & 0 & 1 \end{bmatrix}. \]

Hence

\[ \boldsymbol{\tau}(0) = \mathbf{Y}(0)\hat{\boldsymbol{\theta} }(0) -\mathbf{K}_D\mathbf{s}(0) = \begin{bmatrix} 29.575\\42.5375 \end{bmatrix}. \]

Problem 5: Effect of a Payload Jump

A payload is attached instantaneously at \(t=t_p\), changing the true parameter from \(\boldsymbol{\theta}^{-}\) to \(\boldsymbol{\theta}^{+}\). What changes in the Lyapunov function at that instant?

Solution:

Mechanical position and velocity are continuous in the absence of an impulse, and the numerical parameter estimate is also continuous. Thus the kinetic term in \(V\) is continuous. The estimation error changes from \(\tilde{\boldsymbol{\theta} }^{-} =\hat{\boldsymbol{\theta} }-\boldsymbol{\theta}^{-}\) to \(\tilde{\boldsymbol{\theta} }^{+} =\hat{\boldsymbol{\theta} }-\boldsymbol{\theta}^{+}\). The jump is

\[ \Delta V = \frac12 (\tilde{\boldsymbol{\theta} }^{+})^{T} \boldsymbol{\Gamma}^{-1} \tilde{\boldsymbol{\theta} }^{+} - \frac12 (\tilde{\boldsymbol{\theta} }^{-})^{T} \boldsymbol{\Gamma}^{-1} \tilde{\boldsymbol{\theta} }^{-}. \]

This jump is finite for a finite payload change. After the jump, the standard dissipation proof resumes for the new constant parameter.

Problem 6: Discretization and Adaptation Gain

Explain why increasing \(\boldsymbol{\Gamma}\) without reducing the sampling interval can worsen a digital implementation even though the continuous-time Lyapunov derivative is nonpositive.

Solution:

The continuous proof assumes exact integration of the differential update law. A digital implementation uses an approximation such as

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

The effective discrete update magnitude is proportional to \(h\boldsymbol{\Gamma}\). Large values can overshoot projection boundaries, amplify measurement noise, generate rapidly varying torques, and invalidate the small-step approximation. The remedy is joint selection of sampling period, adaptation gain, filtering, projection, and actuator limits.

Rigid-robot dynamics combine nonlinear state dependence with linear dependence on constant inertial parameters. The adaptive controller uses this linear parameterization to replace exact model knowledge with an online estimate. A filtered tracking error creates an error model whose energy derivative is simplified by the skew-symmetry property of \(\dot{\mathbf{M} }-2\mathbf{C}\). The gradient update cancels the state–parameter cross term, yielding \(\dot V=-\mathbf{s}^{T}\mathbf{K}_D\mathbf{s}\) and asymptotic trajectory tracking. Parameter convergence is a separate identification question requiring sufficiently exciting motion.