In previous chapters you have designed controllers (PD/PID, computed-torque, impedance, MPC, CBF-QP, etc.) as if they were used in isolation. A capstone project, however, must integrate many such components into a coherent architecture: a structured interconnection of sensing, estimation, nominal control, safety filtering, and possibly learning-based modules.

Let the robot dynamics be

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

where \( \mathbf{q} \) are joint coordinates, \( \boldsymbol{\tau} \) are joint torques, and \( \mathbf{d}(t) \) collects disturbances and unmodeled dynamics. The controller architecture specifies how references, measurements, and internal states are processed to generate \( \boldsymbol{\tau} \).

Abstractly, we write the plant as

\[ \dot{\mathbf{x}} = f(\mathbf{x}, \boldsymbol{\tau}, \mathbf{d}), \quad \mathbf{y} = h(\mathbf{x}), \]

where \( \mathbf{x} \) collects joint positions, velocities, and possibly actuator states, and \( \mathbf{y} \) are measured signals. An architecture is a structured mapping \( \mathcal{A}: (r(\cdot),\mathbf{y}(\cdot)) \mapsto \boldsymbol{\tau}(\cdot) \).

flowchart TD
  R["Task reference r(t)"] --> PLNR["Trajectory / setpoint generator"]
  PLNR --> CREF["Reference signals (joint / task space)"]
  CREF --> EST["State estimator / observer"]
  EST --> CNOM["Nominal controller (PD / CT / MPC / etc.)"]
  CNOM --> LEARN["Optional learning residual"]
  LEARN --> SAFETY["Safety / constraint layer (CBF-QP, limits)"]
  SAFETY --> ACT["Actuators & robot plant"]
  ACT --> SENSE["Sensors"]
  SENSE --> EST
        

The capstone design problem is not to invent entirely new controllers, but to decide which modules to include, how to interconnect them, and on which signals they operate so that the closed-loop system meets performance, robustness, and implementation constraints.

We model a controller architecture as a composite dynamical system (plant + internal controller states). Let \( a \) index a candidate architecture (e.g., joint-space PD, computed-torque with impedance outer loop, MPC with CBF-QP). Denote by \( \mathbf{x}_a \) the augmented state (robot + estimators + filters + controller memory).

\[ \dot{\mathbf{x}}_a = f_a(\mathbf{x}_a, r, \mathbf{d}), \quad \mathbf{z}_a = g_a(\mathbf{x}_a, r), \]

where \( \mathbf{z}_a \) collects performance outputs: tracking errors, control effort, safety margins, etc. For example,

\[ \mathbf{z}_a(t) = \begin{bmatrix} \mathbf{e}_q(t) \\ \mathbf{e}_{\dot{q}}(t) \\ \boldsymbol{\tau}(t) \\ \sigma_{\text{safety}}(t) \end{bmatrix}, \quad \mathbf{e}_q(t) = \mathbf{q}_d(t) - \mathbf{q}(t), \]

where \( \sigma_{\text{safety}} \) could represent signed distances to joint or workspace constraints (positive inside the safe set).

Given a time horizon \( [0,T] \) and a running cost \( \ell(\mathbf{z}) \) and terminal cost \( \phi(\mathbf{z}(T)) \), an architecture has associated cost

\[ J_a(r,\mathbf{d}) = \int_0^T \ell(\mathbf{z}_a(t))\,\mathrm{d}t + \phi(\mathbf{z}_a(T)). \]

A common choice is quadratic penalties,

\[ \ell(\mathbf{z}) = \mathbf{e}_q^\top Q_q \mathbf{e}_q + \mathbf{e}_{\dot{q}}^\top Q_{\dot{q}} \mathbf{e}_{\dot{q}} + \boldsymbol{\tau}^\top R \boldsymbol{\tau} + \rho_{\text{safe}}(\sigma_{\text{safety}}), \]

where \( Q_q, Q_{\dot{q}}, R \) are positive semidefinite, and \( \rho_{\text{safe}} \) is large when safety margins are small.

The architecture selection problem can be abstracted as

\[ \min_{a \in \mathcal{A}} \; \min_{\theta_a} \; J_a(r,\mathbf{d};\theta_a) \quad \text{s.t.} \quad \text{stability and real-time constraints}, \]

where \( \theta_a \) collects continuous parameters of architecture \( a \) (gains, weights, prediction horizon, etc.), and \( \mathcal{A} \) is the discrete set of candidate structures. In practice, you will:

• reduce \( \mathcal{A} \) to a small, well-justified list of candidates;
• for each candidate, tune \( \theta_a \) using methods from previous chapters;
• compare architectures with respect to project metrics and constraints.

To compare architectures rigorously, we linearize the closed-loop dynamics about a nominal trajectory (or equilibrium). For architecture \( a \), suppose we obtain a linear time-invariant approximation

\[ \delta\dot{\mathbf{x}}_a = A_a \delta\mathbf{x}_a + B_a \delta\mathbf{w}, \quad \delta\mathbf{z}_a = C_a \delta\mathbf{x}_a + D_a \delta\mathbf{w}, \]

where \( \delta\mathbf{w} \) stacks disturbance and reference perturbations, and \( \delta\mathbf{z}_a \) are small deviations of performance outputs.

The induced \( \mathcal{L}_2 \)-gain of the closed-loop map \( G_a: \delta\mathbf{w} \mapsto \delta\mathbf{z}_a \) is

\[ \|G_a\|_2 = \sup_{\delta\mathbf{w}\neq 0} \frac{\|\delta\mathbf{z}_a\|_2}{\|\delta\mathbf{w}\|_2}, \quad \|\delta\mathbf{z}_a\|_2^2 = \int_0^\infty \|\delta\mathbf{z}_a(t)\|^2 \,\mathrm{d}t. \]

Smaller \( \|G_a\|_2 \) indicates better disturbance attenuation. Similarly, you may consider eigenvalue locations of \( A_a \) (for convergence rate), overshoot and settling time of step responses, and steady-state error (via static gains).

Collecting key metrics into a vector \( \boldsymbol{\rho}(a) \in \mathbb{R}^p \), e.g.

\[ \boldsymbol{\rho}(a) = \begin{bmatrix} \rho_{\text{track}}(a) \\ \rho_{\text{robust}}(a) \\ \rho_{\text{safe}}(a) \\ \rho_{\text{effort}}(a) \\ \rho_{\text{complexity}}(a) \end{bmatrix}, \]

you can perform:

• Weighted sum comparison: choose weights \( w_i > 0 \) and compare \( J_{\text{multi}}(a) = \sum_i w_i \rho_i(a) \);
• Pareto analysis: declare \( a_1 \) better than \( a_2 \) if \( \rho_i(a_1) \leq \rho_i(a_2) \) for all \( i \), with strict improvement in at least one component.

Lemma (Pareto dominance). If for architectures \( a_1, a_2 \) we have \( \boldsymbol{\rho}(a_1) \leq \boldsymbol{\rho}(a_2) \) component-wise and \( \rho_j(a_1) < \rho_j(a_2) \) for some \( j \), then for every weight vector \( \mathbf{w} > 0 \) we have \( J_{\text{multi}}(a_1) < J_{\text{multi}}(a_2) \).

Proof. By definition, \( J_{\text{multi}}(a_k) = \sum_i w_i \rho_i(a_k) \). Then \[ J_{\text{multi}}(a_2) - J_{\text{multi}}(a_1) = \sum_i w_i\big(\rho_i(a_2)-\rho_i(a_1)\big) \geq w_j\big(\rho_j(a_2)-\rho_j(a_1)\big) > 0, \] because \( w_j > 0 \) and \( \rho_j(a_2)-\rho_j(a_1) > 0 \). Hence \( J_{\text{multi}}(a_1) < J_{\text{multi}}(a_2) \) for all \( \mathbf{w} > 0 \).

Many robot controllers use cascaded loops: an inner loop with high bandwidth (e.g., torque or velocity control) and an outer loop with lower bandwidth (e.g., position or task-space control). You have already seen examples in joint-space PD and computed-torque schemes; we now formalize cascade selection and stability.

4.1 One-DOF example

Consider a 1-DOF joint with dynamics

\[ J\ddot{q} + b\dot{q} = \tau, \]

where \( J > 0 \) and \( b \geq 0 \). An inner torque loop tracks a reference \( \tau_{\text{ref}} \):

\[ G_{\text{inner}}(s) = \frac{\tau(s)}{\tau_{\text{ref}}(s)}. \]

The outer loop computes \( \tau_{\text{ref}} \) from position error with a PD controller:

\[ \tau_{\text{ref}}(s) = K_p\big(q_d(s)-q(s)\big) + K_d\big(s q_d(s) - s q(s)\big). \]

The combined transfer from \( q_d \) to \( q \) is

\[ \frac{q(s)}{q_d(s)} = \frac{G_{\text{inner}}(s)\big(K_p + K_d s\big)} {J s^2 + b s + G_{\text{inner}}(s)\big(K_p + K_d s\big)}. \]

If the inner loop is very fast on the frequency range where the outer loop has significant gain (i.e., \( G_{\text{inner}}(s) \approx 1 \)), then the effective dynamics are approximately

\[ J s^2 + (b + K_d) s + K_p = 0, \]

which is the familiar second-order PD-controlled joint dynamics.

4.2 Small-gain reasoning

Let \( \Delta(s) = G_{\text{inner}}(s) - 1 \) describe inner-loop imperfections. The exact closed-loop is a feedback interconnection between the nominal outer loop (designed for \( G_{\text{inner}}=1 \)) and the uncertainty \( \Delta \).

Proposition. Assume the outer loop (with ideal \( G_{\text{inner}}=1 \)) is internally stable and has input-output gain \( \|G_{\text{outer}}\|_2 \leq \gamma < \infty \). If the inner-loop error satisfies

\[ \|\Delta\|_2 < \frac{1}{\gamma}, \]

then the cascaded architecture (outer PD + imperfect inner torque loop) is still internally stable.

Sketch of proof. This is a direct application of the small-gain theorem from robust control: the feedback of two stable systems with gains \( \|G_{\text{outer}}\|_2 \leq \gamma \) and \( \|\Delta\|_2 < 1/\gamma \) is stable. Intuitively, the outer loop never amplifies the inner-loop imperfection enough to destabilize the overall system.

This provides a quantitative criterion for selecting a cascade: if your chosen inner-loop architecture (e.g., motor current controller, low-level joint servo) is precise enough in the bandwidth of interest, you can treat it as an ideal actuator when designing the outer loop.

4.3 Nominal + safety QP layer

A common modern architecture is nominal controller + safety filter using control barrier functions (from Chapter 13). Let \( \mathbf{u}_{\text{nom}} \) be the torque command from a nominal controller (PD, computed-torque, MPC, etc.). The safety layer solves at each time \( t \) the quadratic program

\[ \mathbf{u}^\star(t) = \operatorname*{arg\,min}_{\mathbf{u}} \frac{1}{2}\|\mathbf{u} - \mathbf{u}_{\text{nom}}(t)\|^2 \] \[ \text{s.t.}\quad L_f h(\mathbf{x}) + L_g h(\mathbf{x})\,\mathbf{u} + \alpha\big(h(\mathbf{x})\big) \geq 0, \]

where \( h(\mathbf{x}) \geq 0 \) describes the safe set and \( \alpha \) is an extended class-\(\mathcal{K}\) function.

Proposition (minimal modification). The solution \( \mathbf{u}^\star \) is the unique control within the constraint set that is closest in Euclidean norm to \( \mathbf{u}_{\text{nom}} \). Thus the architecture preserves nominal behavior whenever safety allows.

Proof. The cost is strictly convex and the constraints are affine; hence the feasible set is convex and the minimizer is unique. The optimization problem is exactly the Euclidean projection of \( \mathbf{u}_{\text{nom}} \) onto the feasible set. ◻

This interpretation is crucial in architecture selection: you can combine any nominal architecture (inner loops, outer loops, learning) with a safety layer, knowing that you only deviate when required by constraints.

Using the metrics and structural ideas above, you can design a decision flow for your project. The following diagram captures typical choices for manipulator control using concepts from previous chapters (task-space control, impedance, MPC, CBF-QP, learning residuals, etc.).

flowchart TD
  START["Start from project specs \n(tracking, force, constraints)"]
    --> C1["Is the task \npurely motion tracking \nwithout contact?"]
  C1 -->|Yes| C2["Moderate bandwidth and \nsmall modeling error?"]
  C2 -->|Yes| A1["Inner joint servo + \nouter joint PD or \ncomputed-torque"]
  C2 -->|No| A2["Use robust / adaptive layer \nor MPC in joint space"]
  C1 -->|No| C3["Is accurate \ncontact force or \ncompliance critical?"]
  C3 -->|Yes| A3["Task-space impedance or \nhybrid position/force control"]
  C3 -->|No| C4["Strong state/input constraints \n(safety, workspace)?"]
  C4 -->|Yes| A4["Nominal controller + CBF-QP \nor whole-body QP"]
  C4 -->|No| C5["Large modeling uncertainty \nor unmodeled effects?"]
  C5 -->|Yes| A5["Add disturbance observer \nor learning residual"]
  C5 -->|No| A6["Keep simplest architecture \nsatisfying specs"]
        

This flowchart is not a substitute for analysis: for each suggested architecture, you should derive or simulate the closed-loop dynamics and compute the performance metrics introduced in Lesson 1 and above.

We now build a Python skeleton for representing and selecting controller architectures. The idea is to define an abstract controller interface and specific implementations (PD, computed-torque, MPC) plus optional safety and learning layers. In practice, you would connect this to a robotics library such as pinocchio or roboticstoolbox-python to evaluate \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \), and \( \mathbf{g}(\mathbf{q}) \).

from abc import ABC, abstractmethod
import numpy as np

# -------------------------------
# Abstract controller interface
# -------------------------------

class Controller(ABC):
    @abstractmethod
    def compute_tau(self, t, q, dq, ref):
        """
        Parameters
        ----------
        t   : float
        q   : np.ndarray, shape (n,)
        dq  : np.ndarray, shape (n,)
        ref : dict, e.g. {"qd": qd, "dqd": dqd, "xdd": xdd, ...}

        Returns
        -------
        tau : np.ndarray, shape (n,)
        """
        pass

# -------------------------------
# Joint-space PD controller
# -------------------------------

class JointPD(Controller):
    def __init__(self, Kp, Kd):
        self.Kp = np.diag(Kp)
        self.Kd = np.diag(Kd)

    def compute_tau(self, t, q, dq, ref):
        qd = ref["qd"]
        dqd = ref.get("dqd", np.zeros_like(q))
        e = qd - q
        de = dqd - dq
        return self.Kp @ e + self.Kd @ de

# -------------------------------
# Computed-torque controller
# -------------------------------

class ComputedTorque(Controller):
    def __init__(self, model, Kp, Kd):
        """
        model must provide:
          M(q), C(q, dq), g(q)
        """
        self.model = model
        self.Kp = np.diag(Kp)
        self.Kd = np.diag(Kd)

    def compute_tau(self, t, q, dq, ref):
        qd = ref["qd"]
        dqd = ref.get("dqd", np.zeros_like(q))
        ddqd = ref.get("ddqd", np.zeros_like(q))

        e = qd - q
        de = dqd - dq
        v = ddqd + self.Kd @ de + self.Kp @ e

        M = self.model.M(q)
        C = self.model.C(q, dq)
        g = self.model.g(q)

        # tau = M(q) v + C(q, dq) dq + g(q)
        tau = M @ v + C @ dq + g
        return tau

# -------------------------------
# Safety filter (CBF-inspired QP)
# Here we use a simple projection placeholder
# -------------------------------

class SafetyFilter:
    def __init__(self, umin, umax):
        self.umin = np.asarray(umin)
        self.umax = np.asarray(umax)

    def filter(self, x, u_nom):
        # Simple box projection; replace by QP with CBF constraints in real project
        return np.clip(u_nom, self.umin, self.umax)

# -------------------------------
# Architecture wrapper
# -------------------------------

class Architecture:
    def __init__(self, controller: Controller,
                 safety: SafetyFilter | None = None):
        self.controller = controller
        self.safety = safety

    def compute_tau(self, t, x, ref):
        q, dq = x["q"], x["dq"]
        tau_nom = self.controller.compute_tau(t, q, dq, ref)
        if self.safety is not None:
            return self.safety.filter(x, tau_nom)
        return tau_nom

# -------------------------------
# Architecture selection
# -------------------------------

def select_architecture(spec, model):
    """
    spec: dict with keys like
        {"contact": False,
         "constraints": True,
         "uncertainty": "medium",
         "compute_budget": "moderate"}
    """
    n = model.nq

    if not spec["contact"]:
        if spec["uncertainty"] == "low":
            # inner joint servo assumed; outer PD is enough
            ctrl = JointPD(Kp=5.0 * np.ones(n), Kd=1.0 * np.ones(n))
        else:
            # use computed-torque for better robustness
            ctrl = ComputedTorque(model, Kp=10.0 * np.ones(n), Kd=3.0 * np.ones(n))
    else:
        # For contact-sensitive tasks, you might choose an impedance control
        # implementation here (not shown for brevity).
        ctrl = JointPD(Kp=3.0 * np.ones(n), Kd=0.8 * np.ones(n))

    if spec.get("constraints", False):
        safety = SafetyFilter(umin=-20.0 * np.ones(n), umax=20.0 * np.ones(n))
    else:
        safety = None

    return Architecture(ctrl, safety)
      

This skeleton lets you quickly swap architectures in your simulation environment and compute metrics \( \boldsymbol{\rho}(a) \) for each choice.

In C++, typical robotics stacks (ROS 2, OROCOS, custom embedded code) leverage Eigen and kinematics/dynamics libraries such as orocos_kdl or pinocchio. The following sketch shows an abstract controller interface and an architecture wrapper.

#pragma once

#include <Eigen/Dense>
#include <memory>

struct Reference {
  Eigen::VectorXd qd;
  Eigen::VectorXd dqd;
  Eigen::VectorXd ddqd;
};

class RobotModel {
public:
  virtual ~RobotModel() {}
  virtual int dof() const = 0;
  virtual Eigen::MatrixXd M(const Eigen::VectorXd& q) const = 0;
  virtual Eigen::MatrixXd C(const Eigen::VectorXd& q,
                            const Eigen::VectorXd& dq) const = 0;
  virtual Eigen::VectorXd g(const Eigen::VectorXd& q) const = 0;
};

class Controller {
public:
  virtual ~Controller() {}
  virtual Eigen::VectorXd
  computeTau(double t,
             const Eigen::VectorXd& q,
             const Eigen::VectorXd& dq,
             const Reference& ref) = 0;
};

class JointPDController : public Controller {
public:
  JointPDController(const Eigen::VectorXd& kp,
                    const Eigen::VectorXd& kd)
    : Kp(kp.asDiagonal()), Kd(kd.asDiagonal()) {}

  Eigen::VectorXd computeTau(double /*t*/,
                             const Eigen::VectorXd& q,
                             const Eigen::VectorXd& dq,
                             const Reference& ref) override {
    Eigen::VectorXd e  = ref.qd  - q;
    Eigen::VectorXd de = ref.dqd - dq;
    return Kp * e + Kd * de;
  }

private:
  Eigen::MatrixXd Kp, Kd;
};

class ComputedTorqueController : public Controller {
public:
  ComputedTorqueController(std::shared_ptr<RobotModel> model,
                           const Eigen::VectorXd& kp,
                           const Eigen::VectorXd& kd)
    : model_(std::move(model)),
      Kp(kp.asDiagonal()), Kd(kd.asDiagonal()) {}

  Eigen::VectorXd computeTau(double /*t*/,
                             const Eigen::VectorXd& q,
                             const Eigen::VectorXd& dq,
                             const Reference& ref) override {
    Eigen::VectorXd e  = ref.qd  - q;
    Eigen::VectorXd de = ref.dqd - dq;
    Eigen::VectorXd v  = ref.ddqd + Kd * de + Kp * e;

    Eigen::MatrixXd M = model_->M(q);
    Eigen::MatrixXd C = model_->C(q, dq);
    Eigen::VectorXd g = model_->g(q);

    return M * v + C * dq + g;
  }

private:
  std::shared_ptr<RobotModel> model_;
  Eigen::MatrixXd Kp, Kd;
};

class SafetyFilter {
public:
  SafetyFilter(const Eigen::VectorXd& umin,
               const Eigen::VectorXd& umax)
    : umin_(umin), umax_(umax) {}

  Eigen::VectorXd filter(const Eigen::VectorXd& u_nom) const {
    Eigen::VectorXd u = u_nom;
    for (int i = 0; i < u.size(); ++i) {
      if (u[i] < umin_[i]) u[i] = umin_[i];
      if (u[i] > umax_[i]) u[i] = umax_[i];
    }
    return u;
  }

private:
  Eigen::VectorXd umin_, umax_;
};

class Architecture {
public:
  Architecture(std::shared_ptr<Controller> controller,
               std::shared_ptr<SafetyFilter> safety = nullptr)
    : controller_(std::move(controller)), safety_(std::move(safety)) {}

  Eigen::VectorXd computeTau(double t,
                             const Eigen::VectorXd& q,
                             const Eigen::VectorXd& dq,
                             const Reference& ref) {
    Eigen::VectorXd u_nom = controller_->computeTau(t, q, dq, ref);
    if (safety_) {
      return safety_->filter(u_nom);
    }
    return u_nom;
  }

private:
  std::shared_ptr<Controller> controller_;
  std::shared_ptr<SafetyFilter> safety_;
};
      

This structure can be embedded into a real-time loop (from Chapter 12) where the selected architecture is fixed at configuration time.

While heavy-duty robot control is less common in Java, similar ideas apply for simulation environments or Java-based middleware. The following example uses simple arrays; in a project you may use a linear algebra library such as EJML.

public interface Controller {
    double[] computeTau(double t,
                        double[] q,
                        double[] dq,
                        Reference ref);
}

public class Reference {
    public double[] qd;
    public double[] dqd;
    public double[] ddqd;
}

public class JointPD implements Controller {
    private final double[] kp;
    private final double[] kd;

    public JointPD(double[] kp, double[] kd) {
        this.kp = kp;
        this.kd = kd;
    }

    @Override
    public double[] computeTau(double t,
                               double[] q,
                               double[] dq,
                               Reference ref) {
        int n = q.length;
        double[] tau = new double[n];
        for (int i = 0; i < n; ++i) {
            double e  = ref.qd[i]  - q[i];
            double de = ref.dqd[i] - dq[i];
            tau[i] = kp[i] * e + kd[i] * de;
        }
        return tau;
    }
}

public class SafetyFilter {
    private final double[] umin;
    private final double[] umax;

    public SafetyFilter(double[] umin, double[] umax) {
        this.umin = umin;
        this.umax = umax;
    }

    public double[] filter(double[] uNom) {
        int n = uNom.length;
        double[] u = new double[n];
        for (int i = 0; i < n; ++i) {
            double v = uNom[i];
            if (v < umin[i]) v = umin[i];
            if (v > umax[i]) v = umax[i];
            u[i] = v;
        }
        return u;
    }
}

public class Architecture {
    private final Controller controller;
    private final SafetyFilter safety;

    public Architecture(Controller controller, SafetyFilter safety) {
        this.controller = controller;
        this.safety = safety;
    }

    public double[] computeTau(double t,
                               double[] q,
                               double[] dq,
                               Reference ref) {
        double[] uNom = controller.computeTau(t, q, dq, ref);
        if (safety != null) {
            return safety.filter(uNom);
        }
        return uNom;
    }
}
      

This skeleton is suitable for simulation-based architecture comparison where the dynamics and metrics are computed in Java.

9.1 MATLAB script driving Simulink architectures

In MATLAB, Simulink is ideal for assembling cascaded loops and safety layers graphically. A small MATLAB driver can switch architectures by configuring mask parameters and running simulations.

% Define project specs
spec.contact       = false;
spec.constraints   = true;
spec.uncertainty   = "medium";
spec.computeBudget = "moderate";

% Select architecture type
if ~spec.contact
    if spec.uncertainty == "low"
        archType = "JointPD";
    else
        archType = "ComputedTorque";
    end
else
    archType = "Impedance";
end

% Set controller gains based on archType
switch archType
    case "JointPD"
        Kp = 5 * ones(6,1);
        Kd = 1 * ones(6,1);
    case "ComputedTorque"
        Kp = 15 * ones(6,1);
        Kd = 4  * ones(6,1);
    case "Impedance"
        Kp = 3 * ones(6,1);
        Kd = 0.8 * ones(6,1);
end

% Push parameters to Simulink model
modelName = "capstone_robot_architecture";
set_param(modelName, "SimulationCommand", "stop");

set_param([modelName "/Controller"], ...
          "ArchType", archType, ...
          "Kp", mat2str(Kp), ...
          "Kd", mat2str(Kd));

if spec.constraints
    set_param([modelName "/SafetyLayer"], "EnableSafety", "on");
else
    set_param([modelName "/SafetyLayer"], "EnableSafety", "off");
end

% Run simulation and collect metrics
simOut = sim(modelName, "StopTime", "5");

e_q   = simOut.logsout.getElement("e_q").Values.Data;
tau   = simOut.logsout.getElement("tau").Values.Data;
sigma = simOut.logsout.getElement("sigmaSafety").Values.Data;

rho_track   = max(vecnorm(e_q, 2, 2));
rho_effort  = sqrt(mean(vecnorm(tau, 2, 2).^2));
rho_safe    = min(sigma);

metrics = struct("track", rho_track, ...
                 "effort", rho_effort, ...
                 "safe", rho_safe);
      

In the Simulink diagram, the Controller block implements PD, computed-torque, or impedance control, while the SafetyLayer block enforces actuator and state constraints (e.g., via a QP or saturation logic).

9.2 Wolfram Mathematica comparison of two architectures

Mathematica is convenient for symbolic derivations and numerical simulation of closed-loop dynamics for different architectures.

(* 1-DOF joint model *)
J = 1.0; b = 0.1;
qd[t_] := 0.5*Sin[2 t];

(* PD architecture: tau = Kp (qd - q) + Kd (dqd - dq) *)
Kp = 20; Kd = 5;

eqPD = J*q''[t] + b*q'[t] == 
   Kp (qd[t] - q[t]) + Kd (qd'[t] - q'[t]);

(* Computed-torque-like architecture:
   tau = J (qdd_d + Kd e' + Kp e) + b q' *)
eqCT = J*q''[t] + b*q'[t] == 
   J (qd''[t] + Kd (qd'[t] - q'[t]) + Kp (qd[t] - q[t])) + b*q'[t];

ic = {q[0] == 0, q'[0] == 0};

solPD = NDSolve[{eqPD, ic}, q, {t, 0, 10}][[1]];
solCT = NDSolve[{eqCT, ic}, q, {t, 0, 10}][[1]];

ePD[t_] := qd[t] - (q[t] /. solPD);
eCT[t_] := qd[t] - (q[t] /. solCT);

rhoTrackPD = MaxValue[Abs[ePD[t]], 0 <= t <= 10];
rhoTrackCT = MaxValue[Abs[eCT[t]], 0 <= t <= 10];

{rhoTrackPD, rhoTrackCT}
      

By evaluating tracking error or energy metrics for different symbolic architectures, you obtain quantitative guidance for the final choice in your capstone project.

Problem 1 (Computed-torque vs PD error dynamics). Consider an \( n \)-DOF manipulator with dynamics \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} \). Compare the closed-loop error dynamics under:

1. Joint-space PD: \( \boldsymbol{\tau} = K_p(\mathbf{q}_d - \mathbf{q}) + K_d(\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) \);
2. Computed torque: \( \boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\mathbf{v} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) \) with \( \mathbf{v} = \ddot{\mathbf{q}}_d + K_d \dot{\mathbf{e}} + K_p \mathbf{e} \), \( \mathbf{e} = \mathbf{q}_d - \mathbf{q} \).

Solution.

For PD control, substituting \( \boldsymbol{\tau} = K_p\mathbf{e} + K_d\dot{\mathbf{e}} \) gives

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

Rearranging in terms of \( \mathbf{e} \) yields a nonlinear, configuration-dependent error system whose explicit form is complicated and coupled through \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \). PD stability is typically shown via Lyapunov arguments using properties of \( \mathbf{M} \), but the dynamics are not exactly linear.

For computed torque, substituting \( \boldsymbol{\tau} \) into the dynamics gives

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{M}(\mathbf{q})\big(\ddot{\mathbf{q}}_d + K_d\dot{\mathbf{e}} + K_p\mathbf{e}\big) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}), \]

and canceling yields

\[ \mathbf{M}(\mathbf{q}) \big(\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_d - K_d\dot{\mathbf{e}} - K_p\mathbf{e}\big) = \mathbf{0}. \]

Using \( \ddot{\mathbf{e}} = \ddot{\mathbf{q}}_d - \ddot{\mathbf{q}} \), we obtain

\[ \ddot{\mathbf{e}} + K_d\dot{\mathbf{e}} + K_p\mathbf{e} = \mathbf{0}, \]

a linear decoupled error system whose eigenvalues are directly controlled by the gains. Thus, for architecture selection, computed torque offers predictable linear error dynamics (under perfect modeling), whereas PD is less predictable but simpler and cheaper.

Problem 2 (Minimal modification in CBF-QP architecture). For the QP safety layer in Section 4.3, prove rigorously that \( \mathbf{u}^\star \) is the unique minimizer if the feasible set is nonempty, and that it is the Euclidean projection of \( \mathbf{u}_{\text{nom}} \) onto the feasible set.

Solution.

The cost is \( J(\mathbf{u}) = \tfrac{1}{2}\|\mathbf{u}-\mathbf{u}_{\text{nom}}\|^2 \), whose Hessian is the identity \( I \), hence positive definite. Therefore, \( J \) is strictly convex. The constraint set defined by the linear inequality is convex (intersection of half-spaces). A strictly convex function over a nonempty convex set has a unique minimizer. Moreover, this minimizer is precisely the Euclidean projection of \( \mathbf{u}_{\text{nom}} \) onto the set, by the standard theory of projections in Hilbert spaces. ◻

Problem 3 (Cascade stability margin via small-gain). Suppose the outer loop with ideal actuator has induced gain \( \|G_{\text{outer}}\|_2 \leq \gamma \), and the inner loop satisfies \( \|\Delta\|_2 \leq \varepsilon \). Derive a sufficient condition on \( \varepsilon \) for the cascaded architecture to remain stable and explain its architectural interpretation.

Solution.

The closed-loop is a feedback interconnection of \( G_{\text{outer}} \) and \( \Delta \). The small-gain theorem guarantees stability if \( \|G_{\text{outer}}\|_2 \|\Delta\|_2 < 1 \). Since \( \|G_{\text{outer}}\|_2 \leq \gamma \) and \( \|\Delta\|_2 \leq \varepsilon \), a sufficient condition is

\[ \gamma \varepsilon < 1 \quad \Rightarrow \quad \varepsilon < \frac{1}{\gamma}. \]

Architecturally, this means the inner torque loop must be accurate enough (small \( \varepsilon \)) relative to how aggressive the outer loop is (large \( \gamma \) implies outer loop amplifies inner-loop errors).

Problem 4 (Multi-objective dominance). Let \( \boldsymbol{\rho}(a) = (\rho_1(a),\dots,\rho_p(a)) \) be the metric vector of architecture \( a \). Suppose architecture \( a_1 \) satisfies \( \rho_i(a_1) \leq \rho_i(a_2) \) for all \( i \) and \( \rho_j(a_1) < \rho_j(a_2) \) for at least one \( j \). Show that for any weight vector \( \mathbf{w} > 0 \), the weighted sum performance satisfies \( \sum_i w_i \rho_i(a_1) < \sum_i w_i \rho_i(a_2) \).

Solution.

The difference is \( \sum_i w_i\big(\rho_i(a_2)-\rho_i(a_1)\big) \). Each term is nonnegative because \( \rho_i(a_2)-\rho_i(a_1) \geq 0 \) and \( w_i > 0 \). At least one term is strictly positive (for index \( j \)), hence the sum is strictly positive, which implies the inequality. ◻

Problem 5 (Choosing between task-space impedance and MPC). You must select between two architectures for a surface-following task: (A) task-space impedance control; (B) joint-space MPC with a force tracking term in the cost. Qualitatively list at least three quantitative metrics \( \rho_i \) that would favor (A) over (B) and vice versa.

Solution.

Possible metrics include:

• \( \rho_{\text{track}} \): end-effector position/force tracking error;
• \( \rho_{\text{robust}} \): sensitivity to contact stiffness uncertainty;
• \( \rho_{\text{effort}} \): average torque magnitude;
• \( \rho_{\text{complexity}} \): CPU load and memory usage;
• \( \rho_{\text{safety}} \): minimal distance to joint/force limits.

Impedance (A) often yields better robustness to unknown surface geometry (good \( \rho_{\text{robust}} \)), simpler implementation (good \( \rho_{\text{complexity}} \)), but may have limited constraint handling (worse \( \rho_{\text{safety}} \) if limits are tight). MPC (B) can handle hard constraints and multi-objective costs (good \( \rho_{\text{safety}} \) and likely smaller \( \rho_{\text{track}} \)), but is computationally heavier (worse \( \rho_{\text{complexity}} \)). The final architecture choice depends on which metrics dominate in your capstone project.

In this lesson you elevated from controller design to controller architecture selection. By modeling each candidate architecture as an augmented closed-loop system \( \dot{\mathbf{x}}_a = f_a(\mathbf{x}_a,r,\mathbf{d}) \) with performance outputs \( \mathbf{z}_a \), you obtained a principled way to compare cascaded and hierarchical controllers using quantitative metrics. You saw how small-gain reasoning justifies cascade choices, how CBF-QP layers act as minimal modifications of nominal commands, and how to encode architecture options in Python, C++, Java, MATLAB/Simulink, and Mathematica. These tools prepare you to instantiate and justify a concrete architecture in your capstone implementation and debugging (Lesson 3).