In previous lessons of this capstone chapter you have:

• Defined a control problem and performance metrics (Lesson 1).
• Chosen and designed a controller architecture (Lesson 2).
• Implemented and debugged the controller (Lesson 3).
• Evaluated stability and robustness properties (Lesson 4).

The purpose of the final demonstration is to show, on a consistent and reproducible set of tasks, that your controller meets the specified objectives in joint space and/or task space under realistic disturbances and modeling errors.

Let \( \mathbf{q}_d(t) \in \mathbb{R}^n \) denote the desired joint trajectory and \( \mathbf{q}(t) \) the measured joints. The tracking error is

\[ \mathbf{e}(t) = \mathbf{q}_d(t) - \mathbf{q}(t). \]

In task space, with end-effector pose \( \mathbf{x}(t) \in \mathbb{R}^m \) and desired pose \( \mathbf{x}_d(t) \), the corresponding error is

\[ \mathbf{e}_x(t) = \mathbf{x}_d(t) - \mathbf{x}(t). \]

The technical report must (i) clearly document the closed-loop architecture, (ii) state and justify stability/robustness properties using the tools from earlier chapters (Lyapunov analysis, gain margins, passivity, etc.), and (iii) present quantitative results for chosen metrics with appropriate interpretation and limitations.

The final demonstration should not rely on qualitative judgement alone (“the robot moves nicely”), but on well-defined performance indices computed from \( \mathbf{e}(t) \) and the control input \( \tau(t) \).

2.1 Time-domain tracking indices

Common integral indices for vector errors are:

\[ J_{\mathrm{IAE}} = \int_{0}^{T_f} \|\mathbf{e}(t)\|_1 \,\mathrm{d}t,\quad J_{\mathrm{ISE}} = \int_{0}^{T_f} \|\mathbf{e}(t)\|_2^2 \,\mathrm{d}t, \]

\[ J_{\mathrm{ITAE}} = \int_{0}^{T_f} t \,\|\mathbf{e}(t)\|_1 \,\mathrm{d}t. \]

Here \( \|\cdot\|_1 \) and \( \|\cdot\|_2 \) denote the 1-norm and Euclidean norm, and \( T_f \) is the demonstration horizon. You may also report component-wise RMS errors:

\[ \mathrm{RMS}_i = \sqrt{\frac{1}{T_f}\int_{0}^{T_f} e_i^2(t)\,\mathrm{d}t}, \quad i=1,\dots,n, \]

or task-space RMS errors for Cartesian coordinates or orientation parameters.

2.2 Effort and smoothness

Let \( \tau(t) \in \mathbb{R}^n \) be the commanded joint torques. A quadratic effort index with positive-definite \( \mathbf{R} \in \mathbb{R}^{n\times n} \) is

\[ J_u = \int_{0}^{T_f} \tau(t)^{\top}\mathbf{R}\,\tau(t) \,\mathrm{d}t. \]

To penalize jerkiness, you may include derivatives such as \( \dot{\tau}(t) \) or \( \dddot{\mathbf{q}}(t) \) as long as they are measurable or approximated consistently.

2.3 Multi-objective cost for the project

A standard way to aggregate goals is a quadratic cost functional reminiscent of chapters on optimal control and MPC:

\[ J = \int_{0}^{T_f} \Big( \mathbf{e}(t)^{\top}\mathbf{Q}\,\mathbf{e}(t) + \tau(t)^{\top}\mathbf{R}\,\tau(t) \Big)\,\mathrm{d}t, \]

where \( \mathbf{Q} \succeq 0 \) and \( \mathbf{R} \succ 0 \) encode the relative weights on tracking versus effort. This can be interpreted as a finite-horizon version of the LQR cost considered in the optimal control chapter.

2.4 Connection with Lyapunov analysis

Suppose your closed-loop error dynamics admit a Lyapunov function \( V(\mathbf{e}) \) such that

\[ \dot{V}(\mathbf{e}(t)) \leq -\alpha \,\|\mathbf{e}(t)\|_2^2, \quad \alpha > 0. \]

Integrating from \( 0 \) to \( T_f \) gives

\[ V(\mathbf{e}(T_f)) - V(\mathbf{e}(0)) \leq -\alpha \int_{0}^{T_f} \|\mathbf{e}(t)\|_2^2 \,\mathrm{d}t. \]

Since \( V(\mathbf{e}(T_f)) \geq 0 \), we obtain the bound

\[ J_{\mathrm{ISE}} = \int_{0}^{T_f} \|\mathbf{e}(t)\|_2^2 \,\mathrm{d}t \leq \frac{V(\mathbf{e}(0))}{\alpha}, \]

which links your Lyapunov analysis directly to an experimentally measurable integral performance index.

To make results comparable across teams and runs, define a finite set of scenarios \( \mathcal{S} = \{1,\dots,S\} \). Each scenario \( s \) specifies:

• A reference trajectory \( \mathbf{q}_d^{(s)}(t) \) and/or \( \mathbf{x}_d^{(s)}(t) \).
• Initial state \( \mathbf{x}_0^{(s)} \) (joint positions and velocities).
• Disturbance profile \( \mathbf{d}^{(s)}(t) \) (e.g., payload changes, pushes).
• Constraint set (joint limits, task-space bounds, safety constraints).

For each scenario you compute a set of metrics \( J_k^{(s)} \), \( k=1,\dots,K \) (e.g., ISE, RMS effort, maximum overshoot, constraint violation margin). An aggregated score may be defined as

\[ \bar{J}_k = \frac{1}{S}\sum_{s=1}^{S} J_k^{(s)},\quad J_{\mathrm{tot}} = \sum_{k=1}^{K} w_k\,\bar{J}_k, \]

with weights \( w_k \geq 0 \) chosen during project definition.

3.1 Acceptance inequalities

Typical acceptance requirements are:

• Tracking error: for each joint \( i \), \( \mathrm{RMS}_i \leq \epsilon_i \).
• Effort: \( J_u \leq J_{u,\max} \), or actuator limits respected for all \( t \).
• Safety: task-space constraints and joint limits never violated.

3.2 Demonstration workflow (diagram)

flowchart TD
  A["Define scenarios and metrics"] --> B["Freeze controller parameters"]
  B --> C["Run simulations for all scenarios"]
  C --> D["Run hardware tests (if available)"]
  D --> E["Log states, commands, events"]
  E --> F["Compute metrics (ISE, RMS, effort)"]
  F --> G["Check acceptance conditions"]
  G --> H["Compile plots and tables for report"]
        

In practice, you only observe sampled trajectories. Let the sampling times be \( t_k = kT_s \), \( k=0,\dots,N \), with sampling period \( T_s \). Define discrete-time errors

\[ \mathbf{e}[k] = \mathbf{q}_d(t_k) - \mathbf{q}(t_k),\quad \tau[k] = \tau(t_k). \]

A standard Riemann approximation of the continuous ISE is

\[ \widehat{J}_{\mathrm{ISE}} = T_s \sum_{k=0}^{N-1} \|\mathbf{e}[k]\|_2^2. \]

Under mild regularity (e.g., \( \mathbf{e}(t) \) piecewise Lipschitz), \( \widehat{J}_{\mathrm{ISE}} \) converges to \( J_{\mathrm{ISE}} \) as \( T_s \rightarrow 0 \).

4.1 Multiple runs and confidence intervals

To quantify repeatability, run each scenario \( s \) independently \( M \) times, obtaining metrics \( J^{(s)}_m \), \( m=1,\dots,M \). The sample mean and variance are

\[ \bar{J}^{(s)} = \frac{1}{M}\sum_{m=1}^{M} J^{(s)}_m,\quad s_J^{2\,(s)} = \frac{1}{M-1}\sum_{m=1}^{M} \big(J^{(s)}_m - \bar{J}^{(s)}\big)^2. \]

Assuming approximate normality, a two-sided \( 95\% \) confidence interval for the mean performance on scenario \( s \) is

\[ \bar{J}^{(s)} \pm t_{0.975,M-1} \frac{s_J^{(s)}}{\sqrt{M}}, \]

where \( t_{0.975,M-1} \) is the Student-t quantile. Reporting such intervals in the technical report provides a quantitative sense of variability due to noise and unmodeled effects.

A rigorous technical report should be structured so that another control engineer can reproduce your controller and results. A recommended outline is:

1. Introduction and problem statement.
2. Robot and model description. Kinematics, dynamics, and any simplifications.
3. Control architecture. Block diagrams, control laws, and stability/robustness analysis summaries.
4. Implementation details. Sampling times, saturation, filters, safety layers.
5. Experimental setup. Scenarios, instruments, noise sources, logging.
6. Results. Plots, tables of metrics, discussion.
7. Conclusions and limitations.
8. Appendices. Detailed derivations and code excerpts.

5.1 Logical flow of the report (diagram)

flowchart TD
  A["Problem & objectives"] --> B["Robot model & assumptions"]
  B --> C["Controller design & stability arguments"]
  C --> D["Implementation & safety mechanisms"]
  D --> E["Experiment protocol & metrics"]
  E --> F["Results (plots, tables, statistics)"]
  F --> G["Discussion, limitations, future work"]
        

Crucially, every performance plot in the results section should be tied back to the formal metrics and criteria defined earlier, and any claims of stability or robustness should reference the appropriate proofs or theorems from previous chapters.

The following Python example assumes a CSV log demo_log.csv with columns t, joint position columns prefixed by q_, desired positions prefixed by qd_, and torque commands prefixed by tau_. It computes approximate ISE and RMS errors and a quadratic effort index.

import numpy as np
import pandas as pd

# Load log
df = pd.read_csv("demo_log.csv")

t = df["t"].to_numpy()
Ts = np.mean(np.diff(t))

# Extract joint trajectories as matrices of shape (N, n)
q_cols = [c for c in df.columns if c.startswith("q_")]
qd_cols = [c for c in df.columns if c.startswith("qd_")]
tau_cols = [c for c in df.columns if c.startswith("tau_")]

q = df[q_cols].to_numpy()
qd = df[qd_cols].to_numpy()
tau = df[tau_cols].to_numpy()

e = qd - q

def ise(e, Ts):
    """
    Integral of squared error, approximated by Riemann sum.
    e: array of shape (N, n)
    """
    # Skip last sample to match Ts intervals
    return Ts * np.sum(np.sum(e[:-1, :] ** 2, axis=1))

def iae(e, Ts):
    """
    Integral of absolute error.
    """
    return Ts * np.sum(np.sum(np.abs(e[:-1, :]), axis=1))

def rms_per_joint(e, Ts):
    """
    RMS error per joint using integral approximation.
    """
    T = Ts * (e.shape[0] - 1)
    sq_int = Ts * np.sum(e[:-1, :] ** 2, axis=0)
    return np.sqrt(sq_int / T)

def quadratic_effort(tau, Ts, R=None):
    """
    Quadratic effort J_u = integral tau^T R tau dt.
    If R is None, use identity weighting.
    """
    if R is None:
        R = np.eye(tau.shape[1])
    # Apply R to each sample
    weighted = np.einsum("ij,jk,ik->i", tau[:-1, :], R, tau[:-1, :])
    return Ts * np.sum(weighted)

J_ISE = ise(e, Ts)
J_IAE = iae(e, Ts)
rms_joints = rms_per_joint(e, Ts)
J_u = quadratic_effort(tau, Ts)

print("ISE:", J_ISE)
print("IAE:", J_IAE)
print("RMS per joint:", rms_joints)
print("Quadratic effort J_u:", J_u)
      

These scalar metrics can then be aggregated across scenarios and reported in tables, together with plots of \( \mathbf{q}_d(t) \) and \( \mathbf{q}(t) \) for representative runs.

In C++, it is natural to pair ROS (or another middleware) with a linear-algebra library such as Eigen. The snippet below sketches functions that compute ISE and RMS from vectors of timestamps and errors. They can be integrated into a ROS node that subscribes to joint state topics and stores logs.

#include <vector>
#include <cstddef>
#include <Eigen/Dense>

double computeISE(const std::vector<double>& t,
                  const std::vector<Eigen::VectorXd>& e)
{
    const std::size_t N = t.size();
    if (N < 2 || e.size() != N) {
        throw std::runtime_error("Invalid log lengths");
    }
    double J = 0.0;
    for (std::size_t k = 1; k < N; ++k) {
        const double dt = t[k] - t[k - 1];
        J += e[k - 1].squaredNorm() * dt;
    }
    return J;
}

Eigen::VectorXd computeRMS(const std::vector<double>& t,
                           const std::vector<Eigen::VectorXd>& e)
{
    const std::size_t N = t.size();
    const std::size_t n = static_cast<std::size_t>(e.front().size());
    Eigen::VectorXd sq_int = Eigen::VectorXd::Zero(n);

    double T = 0.0;
    for (std::size_t k = 1; k < N; ++k) {
        const double dt = t[k] - t[k - 1];
        sq_int += dt * e[k - 1].cwiseProduct(e[k - 1]);
        T += dt;
    }
    Eigen::VectorXd rms = sq_int;
    for (std::size_t i = 0; i < n; ++i) {
        rms[i] = std::sqrt(rms[i] / T);
    }
    return rms;
}
      

In a ROS-based system, you would typically fill t and e from a playback of bag files or from an online logging node before calling these functions.

Java is less common in low-level robot control, but it is frequently used in higher-level logic or simulation environments. The following snippet illustrates how to compute ISE and RMS per joint from arrays of samples. A numerical library such as EJML can be used for more advanced operations.

public final class Metrics {

    public static double computeISE(double[] t, double[][] e) {
        int N = t.length;
        if (N < 2 || e.length != N) {
            throw new IllegalArgumentException("Invalid log lengths");
        }
        double J = 0.0;
        for (int k = 1; k < N; ++k) {
            double dt = t[k] - t[k - 1];
            double norm2 = 0.0;
            for (int i = 0; i < e[k - 1].length; ++i) {
                double v = e[k - 1][i];
                norm2 += v * v;
            }
            J += norm2 * dt;
        }
        return J;
    }

    public static double[] computeRMS(double[] t, double[][] e) {
        int N = t.length;
        int n = e[0].length;
        double[] sqInt = new double[n];
        double T = 0.0;

        for (int k = 1; k < N; ++k) {
            double dt = t[k] - t[k - 1];
            for (int i = 0; i < n; ++i) {
                double v = e[k - 1][i];
                sqInt[i] += dt * v * v;
            }
            T += dt;
        }

        double[] rms = new double[n];
        for (int i = 0; i < n; ++i) {
            rms[i] = Math.sqrt(sqInt[i] / T);
        }
        return rms;
    }
}
      

The same pattern can be extended to compute multi-objective costs \( J = \int (\mathbf{e}^{\top}\mathbf{Q}\mathbf{e} + \tau^{\top}\mathbf{R}\tau)\,\mathrm{d}t \) by adding weight matrices into the summation.

MATLAB is widely used for both simulation and hardware-in-the-loop control. Suppose you have a Simulink model capstone_demo.slx that produces logs of q, qd, and tau using To Workspace blocks with structure and time. The script below runs the simulation and computes ISE and RMS metrics.

% Run the Simulink model
mdl = 'capstone_demo';
load_system(mdl);

% Set simulation parameters if needed
set_param(mdl, 'StopTime', 'Tf');  % assume Tf defined in workspace

simOut = sim(mdl, 'SrcWorkspace', 'current');

% Assume simOut contains logs: q, qd, tau (timeseries)
q_ts  = simOut.q;
qd_ts = simOut.qd;
tau_ts = simOut.tau;

t = q_ts.Time;
q = q_ts.Data;   % N-by-n
qd = qd_ts.Data; % N-by-n
tau = tau_ts.Data;

Ts = mean(diff(t));
e = qd - q;

% ISE approximation
J_ISE = Ts * sum(vecnorm(e(1:end-1, :), 2, 2).^2);

% RMS per joint
Ttotal = Ts * (size(e, 1) - 1);
sq_int = Ts * sum(e(1:end-1, :).^2, 1);
rms_joints = sqrt(sq_int / Ttotal);

% Quadratic effort with identity weighting
J_u = Ts * sum(vecnorm(tau(1:end-1, :), 2, 2).^2);

fprintf('ISE = %.4f\n', J_ISE);
fprintf('RMS per joint:\n');
disp(rms_joints);
fprintf('Effort J_u = %.4f\n', J_u);
      

In the report, you should include screenshots of the Simulink diagram (or schematic diagrams drawn with vector graphics or LaTeX) and clearly indicate where saturations, filters, and safety blocks are inserted in the control loop.

Mathematica is convenient for symbolic manipulations and high-order numerical integration. The following code illustrates how to compute an ISE-like metric for a linearized closed-loop model from earlier chapters.

(* State-space model of closed-loop error dynamics *)
Acl = { {-2.0, 1.0}, {-5.0, -3.0} };
Bcl = { {0.0}, {1.0} };
Ccl = IdentityMatrix[2];
Dcl = { {0.0}, {0.0} };

sys = StateSpaceModel[{Acl, Bcl, Ccl, Dcl}];

(* Reference and input: step in first error component *)
u[t_] := {1.0};
x0 = {0.0, 0.0};

(* Output response (e(t)) over [0, Tf] *)
Tf = 5.0;
eSol = OutputResponse[sys, u, {t, 0, Tf}, x0];

(* eSol is a vector function of t *)
eNormSq[t_] := eSol[t].eSol[t];

(* Integral of squared error *)
JISE = NIntegrate[eNormSq[t], {t, 0, Tf}];

JISE
      

By replacing sys with the linearized or exact closed-loop model of your robot and adjusting u[t] to match the input scenario, you can analytically or numerically cross-check the metrics obtained from sampled logs.

Problem 1 (Discrete ISE Approximation Error): Let \( \mathbf{e}(t) \) be a continuously differentiable tracking error on \( [0, T_f] \) and define the discrete approximation \( \widehat{J}_{\mathrm{ISE}} = T_s \sum_{k=0}^{N-1} \|\mathbf{e}(t_k)\|_2^2 \) with \( t_k = kT_s \), \( N T_s = T_f \). Show that \( \widehat{J}_{\mathrm{ISE}} \rightarrow J_{\mathrm{ISE}} \) as \( T_s \rightarrow 0 \).

Solution: Because \( \mathbf{e}(t) \) is continuously differentiable, the scalar function \( g(t) = \|\mathbf{e}(t)\|_2^2 \) is continuous on a compact interval and thus uniformly continuous and bounded. The integral \( J_{\mathrm{ISE}} = \int_{0}^{T_f} g(t)\,\mathrm{d}t \) is the limit of Riemann sums:

\[ \lim_{T_s \rightarrow 0} T_s \sum_{k=0}^{N-1} g(t_k) = \int_{0}^{T_f} g(t)\,\mathrm{d}t. \]

This is a standard result from analysis: for continuous \( g \), Riemann sums converge to the definite integral as the mesh size \( T_s \) goes to zero. Hence \( \widehat{J}_{\mathrm{ISE}} \rightarrow J_{\mathrm{ISE}} \).

Problem 2 (Lyapunov Bound on ISE): Suppose there exists a positive definite Lyapunov function \( V(\mathbf{e}) \) for the error dynamics such that \( \dot{V}(\mathbf{e}(t)) \leq -\alpha \|\mathbf{e}(t)\|_2^2 \) for some \( \alpha > 0 \). Prove that \( J_{\mathrm{ISE}} \leq V(\mathbf{e}(0))/\alpha \).

Solution: Integrate the inequality from \( 0 \) to \( T_f \):

\[ \int_{0}^{T_f} \dot{V}(\mathbf{e}(t))\,\mathrm{d}t \leq -\alpha \int_{0}^{T_f}\|\mathbf{e}(t)\|_2^2\,\mathrm{d}t. \]

The left-hand side is \( V(\mathbf{e}(T_f)) - V(\mathbf{e}(0)) \). Positivity of \( V \) implies \( V(\mathbf{e}(T_f)) \geq 0 \), so

\[ -V(\mathbf{e}(0)) \leq -\alpha J_{\mathrm{ISE}}. \]

Multiplying by \( -1 \) gives \( J_{\mathrm{ISE}} \leq V(\mathbf{e}(0))/\alpha \).

Problem 3 (Multi-Objective Cost as Weighted Sum): Let \( J_1,\dots,J_K \) be nonnegative scalar metrics and consider the vector-valued objective \( \mathbf{J} = (J_1,\dots,J_K) \). Show that for any choice of strictly positive weights \( w_k > 0 \), the scalar cost \( J_{\mathrm{tot}} = \sum_{k=1}^{K} w_k J_k \) defines a total ordering on project variants such that if one variant dominates another in all metrics (i.e., all \( J_k \) are no larger and at least one is strictly smaller), then its scalar cost is strictly smaller.

Solution: Suppose variant A satisfies \( J_k^{A} \leq J_k^{B} \) for all \( k \) and \( J_{k^{\star}}^{A} < J_{k^{\star}}^{B} \) for at least one index \( k^{\star} \). Then

\[ J_{\mathrm{tot}}^{A} - J_{\mathrm{tot}}^{B} = \sum_{k=1}^{K} w_k \big(J_k^{A} - J_k^{B}\big) \leq w_{k^{\star}}\big(J_{k^{\star}}^{A} - J_{k^{\star}}^{B}\big) < 0, \]

because all \( w_k \geq 0 \) and the only strictly negative term in the sum corresponds to \( k^{\star} \). Hence \( J_{\mathrm{tot}}^{A} < J_{\mathrm{tot}}^{B} \). Therefore, the scalarization preserves strict Pareto dominance.

Problem 4 (Confidence Interval for Mean ISE): For a fixed scenario, you run the demo \( M \) times and measure ISE values \( J_1,\dots,J_M \). Assuming these are i.i.d. and approximately normal with unknown variance, derive a \( 95\% \) confidence interval for the true mean ISE \( \mu_J \).

Solution: The sample mean and variance are

\[ \bar{J} = \frac{1}{M}\sum_{m=1}^{M}J_m,\quad s_J^2 = \frac{1}{M-1}\sum_{m=1}^{M}(J_m-\bar{J})^2. \]

Under the normality assumption, \( (\bar{J} - \mu_J)/(s_J/\sqrt{M}) \) has a Student-t distribution with \( M-1 \) degrees of freedom. Therefore,

\[ \mathbb{P}\Big[ \bar{J} - t_{0.975,M-1}\frac{s_J}{\sqrt{M}} \leq \mu_J \leq \bar{J} + t_{0.975,M-1}\frac{s_J}{\sqrt{M}} \Big] \approx 0.95. \]

This gives the desired confidence interval.

Problem 5 (Trade-off Between Tracking and Effort): Consider two controllers with costs \( J^{(1)} = J_{\mathrm{ISE}}^{(1)} + \lambda J_u^{(1)} \) and \( J^{(2)} = J_{\mathrm{ISE}}^{(2)} + \lambda J_u^{(2)} \) for some fixed weight \( \lambda > 0 \). Controller 1 has better tracking but larger effort: \( J_{\mathrm{ISE}}^{(1)} < J_{\mathrm{ISE}}^{(2)} \), \( J_u^{(1)} > J_u^{(2)} \). Derive the condition on \( \lambda \) under which controller 1 is preferred.

Solution: Controller 1 is preferred if \( J^{(1)} < J^{(2)} \), i.e.

\[ J_{\mathrm{ISE}}^{(1)} + \lambda J_u^{(1)} < J_{\mathrm{ISE}}^{(2)} + \lambda J_u^{(2)}. \]

Rearranging,

\[ \lambda\big(J_u^{(1)} - J_u^{(2)}\big) < J_{\mathrm{ISE}}^{(2)} - J_{\mathrm{ISE}}^{(1)}. \]

Since \( J_u^{(1)} - J_u^{(2)} > 0 \) by assumption, we obtain

\[ \lambda < \frac{J_{\mathrm{ISE}}^{(2)} - J_{\mathrm{ISE}}^{(1)}} {J_u^{(1)} - J_u^{(2)}}. \]

Hence, for weights \( \lambda \) below this threshold, the gain in tracking performance compensates for the increased effort and controller 1 is preferred according to the scalar criterion.

This lesson has formalized how to complete the capstone robot control project: define precise performance and robustness metrics, design a reproducible experiment protocol, compute quantitative indices from sampled data, and communicate the results in a structured technical report. The mathematical connection between Lyapunov-based stability analysis, optimal control cost functions, and experimentally measured performance indices allows you to close the loop between theory and practice. Multi-language code snippets in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrate how to implement the metric computations in a realistic toolchain.