In previous lessons, we derived ideal model-based controllers for a robot manipulator with dynamics of the form

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

where \( \mathbf{q} \in \mathbb{R}^n \) is the joint configuration, \( \boldsymbol{\tau} \in \mathbb{R}^n \) the control torques, and \( \mathbf{d} \) represents disturbances and modeling errors. In practice, three major implementation effects intervene between the ideal control law and the physical actuators:

• Signal filtering: joint positions are measured by encoders, and velocities are usually estimated by numerical differentiation plus low-pass filtering to attenuate sensor noise.
• Actuator saturation: motors and current amplifiers can only generate torques within bounds \( \boldsymbol{\tau}_{\min}, \boldsymbol{\tau}_{\max} \).
• Rate limits: physical and safety limits constrain \( \dot{\boldsymbol{\tau}} \) and sometimes \( \ddot{\mathbf{q}} \).

If these effects are ignored, the closed-loop behavior can deviate drastically from the theoretical Lyapunov analysis of computed-torque or feedback-linearized controllers. Correct implementation requires:

1. Mathematical models for saturation and rate limiters.
2. Discretization consistent with the controller sampling time.
3. Awareness of their impact on stability and tracking accuracy.

flowchart TD
  Qd["Desired joint trajectory q_d(t)"] --> CT["Model-based controller (ideal)"]
  CT --> LIM["Saturation + rate limiting block"]
  LIM --> ACT["Actuators"]
  ACT --> ROB["Robot dynamics"]
  ROB --> SENS["Encoders / sensors"]
  SENS --> FILT["Digital filters (q, q_dot)"]
  FILT --> CT
        

Consider a single joint control input \( u \) (e.g., torque command). A saturation nonlinearity with lower and upper bounds \( u_{\min}, u_{\max} \) is

\[ \operatorname{sat}(u) = \begin{cases} u_{\max} & \text{if } u \geq u_{\max},\\ u_{\min} & \text{if } u \leq u_{\min},\\ u & \text{otherwise.} \end{cases} \]

For a vector input \( \mathbf{u} \in \mathbb{R}^n \), saturation is typically applied componentwise, i.e. \( \boldsymbol{\operatorname{sat}}(\mathbf{u}) = [\operatorname{sat}(u_1),\dots,\operatorname{sat}(u_n)]' \).

A rate limiter constrains the rate of change of the commanded input. In continuous time, with internal state \( u_r(t) \),

\[ \dot{u}_r(t) = \begin{cases} \dot{u}_{\max} & \text{if } \dot{u}_{\text{cmd}}(t) \geq \dot{u}_{\max},\\ \dot{u}_{\min} & \text{if } \dot{u}_{\text{cmd}}(t) \leq \dot{u}_{\min},\\ \dot{u}_{\text{cmd}}(t) & \text{otherwise,} \end{cases} \quad u_{\text{out}}(t) = u_r(t), \]

where \( \dot{u}_{\text{cmd}}(t) \) is the derivative of the ideal input (e.g., from computed-torque law), and \( \dot{u}_{\min}\leq 0 \leq \dot{u}_{\max} \). In discrete time with sampling time \( T_s \), a simple approximation is

\[ u_{\text{out}}[k] = \operatorname{clip}\!\Big( u_{\text{out}}[k-1] + \delta u[k],\; u_{\min},\; u_{\max} \Big), \]

where \( \delta u[k] = \operatorname{clip}(u_{\text{cmd}}[k] - u_{\text{out}}[k-1],\; \dot{u}_{\min} T_s,\; \dot{u}_{\max} T_s) \) and \( \operatorname{clip} \) denotes projection onto the specified interval.

Both saturation and rate limiting are memoryless or low-order nonlinearities. In Lyapunov analysis they are often treated via sector bounds, e.g., for scalar saturation one can show

\[ 0 \leq \frac{\operatorname{sat}(u)}{u} \leq 1 \quad \text{for all } u \neq 0, \]

which means \( \operatorname{sat}(u) \) lies in the sector \( [0,1] \) and allows the use of absolute stability tools. We will not go into full Lur'e system theory here, but it is important to understand that saturation is a structured nonlinearity with useful bounds.

Joint positions \( \mathbf{q}[k] \) are measured by encoders at discrete instants. Numerical differentiation magnifies high-frequency noise, so velocities are estimated using a dirty derivative:

\[ \dot{\mathbf{q}}_{\text{est}}[k] = \alpha \,\dot{\mathbf{q}}_{\text{est}}[k-1] + (1-\alpha)\,\frac{\mathbf{q}[k]-\mathbf{q}[k-1]}{T_s}, \quad 0 < \alpha < 1. \]

This is a first-order low-pass filter applied to the raw finite difference. The corresponding transfer function in the \( z \)-domain for each joint is

\[ H(z) = \frac{(1-\alpha)(1 - z^{-1})}{1 - \alpha z^{-1}}. \]

For small sampling time \( T_s \), the approximate continuous-time pole is located at \( s = -\frac{1-\alpha}{\alpha T_s} \). Choosing a cutoff frequency \( \omega_c \) yields

\[ \alpha \approx \frac{1}{1 + \omega_c T_s}. \]

Proof (approximate): Comparing the discrete pole \( z_p = \alpha \) with the first-order mapping \( z_p \approx \mathrm{e}^{-T_s \omega_c} \) gives \( \alpha \approx \mathrm{e}^{-T_s \omega_c} \). For small \( T_s \omega_c \), using the first-order expansion \( \mathrm{e}^{-T_s \omega_c} \approx 1 - T_s \omega_c \) and the continuous-time model \( \dot{x} = -\omega_c x + \omega_c u \), one obtains the rational approximation \( \alpha \approx \frac{1}{1 + \omega_c T_s} \), which is convenient for controller tuning.

In many implementations, \( \mathbf{q} \) is also low-pass filtered to remove quantization noise:

\[ \mathbf{q}_{\text{f}}[k] = \beta\,\mathbf{q}_{\text{f}}[k-1] + (1-\beta)\,\mathbf{q}[k], \quad 0 < \beta < 1. \]

The filtered signals \( \mathbf{q}_{\text{f}}, \dot{\mathbf{q}}_{\text{est}} \) are then fed into the nonlinear control law (e.g. computed-torque) instead of the raw measurements.

Recall the ideal computed-torque structure for tracking a desired trajectory \( \mathbf{q}_d, \dot{\mathbf{q}}_d, \ddot{\mathbf{q}}_d \):

\[ \boldsymbol{\tau}_{\text{ideal}} = \mathbf{M}(\mathbf{q})\Big( \ddot{\mathbf{q}}_d + \mathbf{K}_d \tilde{\dot{\mathbf{q}}} + \mathbf{K}_p \tilde{\mathbf{q}} \Big) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}), \]

with tracking errors \( \tilde{\mathbf{q}} = \mathbf{q}_d - \mathbf{q} \) and \( \tilde{\dot{\mathbf{q}}} = \dot{\mathbf{q}}_d - \dot{\mathbf{q}} \). In implementation, we replace \( \mathbf{q} \) and \( \dot{\mathbf{q}} \) by their filtered counterparts \( \mathbf{q}_{\text{f}}, \dot{\mathbf{q}}_{\text{est}} \), and we apply saturation and rate limiting:

\[ \begin{aligned} \tilde{\mathbf{q}}_{\text{f}} &= \mathbf{q}_d - \mathbf{q}_{\text{f}},\\ \tilde{\dot{\mathbf{q}}}_{\text{f}} &= \dot{\mathbf{q}}_d - \dot{\mathbf{q}}_{\text{est}},\\[4pt] \boldsymbol{\tau}_{\text{pre}} &= \mathbf{M}(\mathbf{q}_{\text{f}})\Big( \ddot{\mathbf{q}}_d + \mathbf{K}_d \tilde{\dot{\mathbf{q}}}_{\text{f}} + \mathbf{K}_p \tilde{\mathbf{q}}_{\text{f}} \Big) + \mathbf{C}(\mathbf{q}_{\text{f}},\dot{\mathbf{q}}_{\text{est}})\dot{\mathbf{q}}_{\text{est}} + \mathbf{g}(\mathbf{q}_{\text{f}}),\\[4pt] \boldsymbol{\tau}_{\text{sat}}[k] &= \boldsymbol{\operatorname{sat}}\Big( \boldsymbol{\tau}_{\text{rl}}[k] \Big),\\[4pt] \boldsymbol{\tau}_{\text{rl}}[k] &= \text{rate_limit}\Big( \boldsymbol{\tau}_{\text{pre}}[k],\; \boldsymbol{\tau}_{\text{sat}}[k-1] \Big). \end{aligned} \]

Here \( \boldsymbol{\tau}_{\text{rl}} \) is the rate-limited torque command before final saturation (for safety), and \( \boldsymbol{\tau}_{\text{sat}} \) is the actual torque sent to actuators. In practice, saturation and rate limiting may be implemented in the low-level drive firmware; when that is the case, the high-level controller should still be designed under the knowledge that these nonlinearities exist.

Local stability insight. If the reference trajectory and gain matrices are chosen so that the ideal controller is locally exponentially stabilizing, then there exists a bounded neighborhood of the origin in which all components of \( \boldsymbol{\tau}_{\text{pre}} \) stay strictly inside the saturation limits. In that region, the actual closed-loop is identical to the ideal system, and the Lyapunov proof carries over. Thus, for small enough tracking errors, the original stability properties are preserved. The practical design task is to choose gains and reference trajectories so that the operating region of interest lies inside this domain of linear behavior.

The following flow summarizes a typical discrete-time joint control loop for model-based nonlinear control, incorporating measurement filtering, saturation, and rate limiting.

flowchart TD
  START["Start of control cycle (k)"] --> READ["Read encoders q[k]"]
  READ --> QF["Update filters for q_f[k], qdot_est[k]"]
  QF --> ERRS["Compute errors: e_q, e_qdot"]
  ERRS --> TAUIP["Compute tau_pre from model-based law"]
  TAUIP --> RL["Apply rate limiter: tau_rl[k]"]
  RL --> SAT["Apply saturation: tau_sat[k]"]
  SAT --> SEND["Send tau_sat[k] to actuators"]
  SEND --> NEXT["Wait until next sample k+1"]
        

In a real-time implementation, steps from reading encoders to sending torques must be completed within the sampling period \( T_s \). Multi-rate schemes (inner current loop, outer torque loop, and outermost kinematic loop) are often used, but we postpone that discussion to the chapter on digital control.

In Python, typical libraries for robot dynamics and kinematics include roboticstoolbox-python and the Python bindings of pinocchio. Below is a simplified joint-space controller loop using numpy only for clarity; in a real system, roboticstoolbox can be used to obtain \( \mathbf{M}(\mathbf{q}), \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}(\mathbf{q}) \).

import numpy as np

class JointFilter:
    def __init__(self, n_joints, Ts, omega_c_vel, omega_c_pos=None):
        self.n = n_joints
        self.Ts = Ts
        # Dirty derivative parameters
        self.alpha_vel = 1.0 / (1.0 + omega_c_vel * Ts)
        self.q_prev = np.zeros(n_joints)
        self.qf = np.zeros(n_joints)
        self.qdot_est = np.zeros(n_joints)
        if omega_c_pos is not None:
            self.beta_pos = 1.0 / (1.0 + omega_c_pos * Ts)
        else:
            self.beta_pos = None

    def update(self, q_meas):
        if self.beta_pos is None:
            self.qf = q_meas.copy()
        else:
            self.qf = self.beta_pos * self.qf + (1.0 - self.beta_pos) * q_meas

        dq_raw = (q_meas - self.q_prev) / self.Ts
        self.qdot_est = (self.alpha_vel * self.qdot_est
                         + (1.0 - self.alpha_vel) * dq_raw)
        self.q_prev = q_meas.copy()
        return self.qf, self.qdot_est


def saturate(u, u_min, u_max):
    return np.minimum(np.maximum(u, u_min), u_max)


def rate_limit(u_cmd, u_prev, du_min, du_max):
    du = u_cmd - u_prev
    du = np.minimum(np.maximum(du, du_min), du_max)
    return u_prev + du


class ComputedTorqueController:
    def __init__(self, robot_model, Kp, Kd, Ts,
                 tau_min, tau_max, dtau_min, dtau_max,
                 omega_c_vel, omega_c_pos=None):
        """
        robot_model: object with methods M(q), C(q, qdot), g(q)
        Kp, Kd: diagonal gain matrices (as np.array)
        tau_min, tau_max: joint torque bounds
        dtau_min, dtau_max: per-sample torque increment bounds
        """
        self.robot = robot_model
        self.Kp = Kp
        self.Kd = Kd
        self.Ts = Ts
        self.tau_min = tau_min
        self.tau_max = tau_max
        self.dtau_min = dtau_min
        self.dtau_max = dtau_max
        self.filter = JointFilter(len(tau_min), Ts,
                                  omega_c_vel, omega_c_pos)
        self.tau_prev = np.zeros_like(tau_min)

    def step(self, q_meas, qd, qd_dot, qd_ddot):
        # 1) Filtering
        qf, qdot_est = self.filter.update(q_meas)

        # 2) Errors
        e_q = qd - qf
        e_qdot = qd_dot - qdot_est

        # 3) Dynamics terms
        M = self.robot.M(qf)
        C = self.robot.C(qf, qdot_est)
        g = self.robot.g(qf)

        v = qd_ddot + self.Kd @ e_qdot + self.Kp @ e_q
        tau_pre = M @ v + C @ qdot_est + g

        # 4) Rate limiting then saturation
        tau_rl = rate_limit(tau_pre, self.tau_prev,
                            self.dtau_min, self.dtau_max)
        tau_sat = saturate(tau_rl, self.tau_min, self.tau_max)

        self.tau_prev = tau_sat.copy()
        return tau_sat
      

In a real application, robot_model can be a wrapper around roboticstoolbox or pinocchio, and step is called at each control cycle with the latest encoder readings and reference trajectory values.

In C++, matrix computations are typically handled with Eigen, while common robotics dynamics libraries include pinocchio, RBDL, and Orocos KDL. The snippet below sketches a discrete-time controller using Eigen, assuming that a RobotModel interface provides M(q), C(q, qdot), g(q).

#include <Eigen/Dense>
#include <algorithm>

using Vector = Eigen::VectorXd;
using Matrix = Eigen::MatrixXd;

struct RobotModel {
    Matrix M(const Vector& q) const;
    Matrix C(const Vector& q, const Vector& qdot) const;
    Vector g(const Vector& q) const;
};

class JointFilter {
public:
    JointFilter(int n, double Ts, double omega_c_vel, double omega_c_pos = -1.0)
        : n_(n), Ts_(Ts),
          q_prev_(Vector::Zero(n)),
          qf_(Vector::Zero(n)),
          qdot_est_(Vector::Zero(n)) {

        alpha_vel_ = 1.0 / (1.0 + omega_c_vel * Ts_);
        if (omega_c_pos > 0.0) {
            beta_pos_ = 1.0 / (1.0 + omega_c_pos * Ts_);
        } else {
            beta_pos_ = -1.0; // disabled
        }
    }

    void update(const Vector& q_meas) {
        if (beta_pos_ > 0.0) {
            qf_ = beta_pos_ * qf_ + (1.0 - beta_pos_) * q_meas;
        } else {
            qf_ = q_meas;
        }

        Vector dq_raw = (q_meas - q_prev_) / Ts_;
        qdot_est_ = alpha_vel_ * qdot_est_
                  + (1.0 - alpha_vel_) * dq_raw;

        q_prev_ = q_meas;
    }

    const Vector& q_filtered() const { return qf_; }
    const Vector& qdot_est() const { return qdot_est_; }

private:
    int n_;
    double Ts_;
    double alpha_vel_;
    double beta_pos_;
    Vector q_prev_;
    Vector qf_;
    Vector qdot_est_;
};

inline Vector saturate(const Vector& u,
                       const Vector& u_min,
                       const Vector& u_max) {
    Vector out = u;
    for (int i = 0; i < u.size(); ++i) {
        out[i] = std::min(std::max(out[i], u_min[i]), u_max[i]);
    }
    return out;
}

inline Vector rateLimit(const Vector& u_cmd,
                        const Vector& u_prev,
                        const Vector& du_min,
                        const Vector& du_max) {
    Vector out = u_prev;
    for (int i = 0; i < u_cmd.size(); ++i) {
        double du = u_cmd[i] - u_prev[i];
        du = std::min(std::max(du, du_min[i]), du_max[i]);
        out[i] += du;
    }
    return out;
}

class ComputedTorqueController {
public:
    ComputedTorqueController(const RobotModel* robot,
                             const Vector& Kp_diag,
                             const Vector& Kd_diag,
                             double Ts,
                             const Vector& tau_min,
                             const Vector& tau_max,
                             const Vector& dtau_min,
                             const Vector& dtau_max,
                             double omega_c_vel,
                             double omega_c_pos = -1.0)
        : robot_(robot),
          Kp_(Kp_diag.asDiagonal()),
          Kd_(Kd_diag.asDiagonal()),
          Ts_(Ts),
          tau_min_(tau_min),
          tau_max_(tau_max),
          dtau_min_(dtau_min),
          dtau_max_(dtau_max),
          filter_(tau_min.size(), Ts, omega_c_vel, omega_c_pos),
          tau_prev_(Vector::Zero(tau_min.size())) {}

    Vector step(const Vector& q_meas,
                const Vector& qd,
                const Vector& qd_dot,
                const Vector& qd_ddot) {
        // 1) Filtering
        filter_.update(q_meas);
        const Vector& qf = filter_.q_filtered();
        const Vector& qdot_est = filter_.qdot_est();

        // 2) Errors
        Vector e_q = qd - qf;
        Vector e_qdot = qd_dot - qdot_est;

        // 3) Dynamics
        Matrix M = robot_->M(qf);
        Matrix C = robot_->C(qf, qdot_est);
        Vector g = robot_->g(qf);

        Vector v = qd_ddot + Kd_ * e_qdot + Kp_ * e_q;
        Vector tau_pre = M * v + C * qdot_est + g;

        // 4) Rate limit and saturate
        Vector tau_rl = rateLimit(tau_pre, tau_prev_, dtau_min_, dtau_max_);
        Vector tau_sat = saturate(tau_rl, tau_min_, tau_max_);
        tau_prev_ = tau_sat;
        return tau_sat;
    }

private:
    const RobotModel* robot_;
    Matrix Kp_;
    Matrix Kd_;
    double Ts_;
    Vector tau_min_, tau_max_;
    Vector dtau_min_, dtau_max_;
    JointFilter filter_;
    Vector tau_prev_;
};
      

This structure can be integrated into a ros_control controller by calling step inside update, using joint state and reference messages from ROS topics.

Java has fewer robotics-specific libraries, but the linear algebra can be handled with EJML. Robot dynamics may be computed in native code (C++) and accessed via JNI, or implemented directly in Java for simpler manipulators.

import org.ejml.simple.SimpleMatrix;

public class JointFilter {
    private final double Ts;
    private final double alphaVel;
    private final double betaPos;
    private SimpleMatrix qPrev;
    private SimpleMatrix qf;
    private SimpleMatrix qdotEst;
    private final boolean filterPos;

    public JointFilter(int n, double Ts, double omegaCVel, Double omegaCPos) {
        this.Ts = Ts;
        this.alphaVel = 1.0 / (1.0 + omegaCVel * Ts);
        this.qPrev = new SimpleMatrix(n, 1);
        this.qf = new SimpleMatrix(n, 1);
        this.qdotEst = new SimpleMatrix(n, 1);
        if (omegaCPos != null && omegaCPos > 0.0) {
            this.betaPos = 1.0 / (1.0 + omegaCPos * Ts);
            this.filterPos = true;
        } else {
            this.betaPos = -1.0;
            this.filterPos = false;
        }
    }

    public void update(SimpleMatrix qMeas) {
        if (filterPos) {
            qf = qf.scale(betaPos).plus(qMeas.scale(1.0 - betaPos));
        } else {
            qf = qMeas.copy();
        }
        SimpleMatrix dqRaw = qMeas.minus(qPrev).divide(Ts);
        qdotEst = qdotEst.scale(alphaVel)
                .plus(dqRaw.scale(1.0 - alphaVel));
        qPrev = qMeas.copy();
    }

    public SimpleMatrix getQf() { return qf; }
    public SimpleMatrix getQdotEst() { return qdotEst; }
}

public class Saturation {
    public static SimpleMatrix saturate(SimpleMatrix u,
                                        SimpleMatrix uMin,
                                        SimpleMatrix uMax) {
        SimpleMatrix out = u.copy();
        for (int i = 0; i < u.getNumElements(); ++i) {
            double val = out.get(i);
            val = Math.min(Math.max(val, uMin.get(i)), uMax.get(i));
            out.set(i, val);
        }
        return out;
    }

    public static SimpleMatrix rateLimit(SimpleMatrix uCmd,
                                         SimpleMatrix uPrev,
                                         SimpleMatrix duMin,
                                         SimpleMatrix duMax) {
        SimpleMatrix out = uPrev.copy();
        for (int i = 0; i < uCmd.getNumElements(); ++i) {
            double du = uCmd.get(i) - uPrev.get(i);
            du = Math.min(Math.max(du, duMin.get(i)), duMax.get(i));
            out.set(i, uPrev.get(i) + du);
        }
        return out;
    }
}

public interface RobotModel {
    SimpleMatrix M(SimpleMatrix q);
    SimpleMatrix C(SimpleMatrix q, SimpleMatrix qdot);
    SimpleMatrix g(SimpleMatrix q);
}

public class ComputedTorqueController {
    private final RobotModel robot;
    private final SimpleMatrix Kp;
    private final SimpleMatrix Kd;
    private final double Ts;
    private final SimpleMatrix tauMin;
    private final SimpleMatrix tauMax;
    private final SimpleMatrix dtauMin;
    private final SimpleMatrix dtauMax;
    private final JointFilter filter;
    private SimpleMatrix tauPrev;

    public ComputedTorqueController(RobotModel robot,
                                    SimpleMatrix KpDiag,
                                    SimpleMatrix KdDiag,
                                    double Ts,
                                    SimpleMatrix tauMin,
                                    SimpleMatrix tauMax,
                                    SimpleMatrix dtauMin,
                                    SimpleMatrix dtauMax,
                                    double omegaCVel,
                                    Double omegaCPos) {
        this.robot = robot;
        this.Kp = SimpleMatrix.diag(KpDiag.getDDRM());
        this.Kd = SimpleMatrix.diag(KdDiag.getDDRM());
        this.Ts = Ts;
        this.tauMin = tauMin;
        this.tauMax = tauMax;
        this.dtauMin = dtauMin;
        this.dtauMax = dtauMax;
        int n = tauMin.getNumElements();
        this.filter = new JointFilter(n, Ts, omegaCVel, omegaCPos);
        this.tauPrev = new SimpleMatrix(n, 1);
    }

    public SimpleMatrix step(SimpleMatrix qMeas,
                             SimpleMatrix qd,
                             SimpleMatrix qdDot,
                             SimpleMatrix qdDdot) {
        filter.update(qMeas);
        SimpleMatrix qf = filter.getQf();
        SimpleMatrix qdotEst = filter.getQdotEst();

        SimpleMatrix eQ = qd.minus(qf);
        SimpleMatrix eQdot = qdDot.minus(qdotEst);

        SimpleMatrix M = robot.M(qf);
        SimpleMatrix C = robot.C(qf, qdotEst);
        SimpleMatrix g = robot.g(qf);

        SimpleMatrix v = qdDdot
                .plus(Kd.mult(eQdot))
                .plus(Kp.mult(eQ));
        SimpleMatrix tauPre = M.mult(v).plus(C.mult(qdotEst)).plus(g);

        SimpleMatrix tauRl = Saturation.rateLimit(tauPre, tauPrev, dtauMin, dtauMax);
        SimpleMatrix tauSat = Saturation.saturate(tauRl, tauMin, tauMax);
        tauPrev = tauSat.copy();
        return tauSat;
    }
}
      

Such Java code can be integrated into real-time JVM frameworks or used in simulation environments for teaching and prototyping.

MATLAB has built-in support for robotic dynamics via the Robotics System Toolbox and functions such as robotics.RigidBodyTree. Simulink offers ready-made blocks for saturation, rate limiting, and low-pass filtering. The MATLAB function below encapsulates the discrete-time controller for use in a Simulink MATLAB Function block.

function tau_sat = ct_controller_step(q_meas, qd, qd_dot, qd_ddot)
%#codegen
% Global or persistent variables can store state between calls
% (for q_prev, qf, qdot_est, tau_prev)

persistent Ts Kp Kd tau_min tau_max dtau_min dtau_max ...
           q_prev qf qdot_est tau_prev alpha_vel beta_pos robot

if isempty(Ts)
    Ts = 0.001;
    n = numel(q_meas);

    Kp = diag(100 * ones(1,n));
    Kd = diag(20 * ones(1,n));

    tau_min = -50 * ones(n,1);
    tau_max =  50 * ones(n,1);
    dtau_min = -200 * Ts * ones(n,1);
    dtau_max =  200 * Ts * ones(n,1);

    omega_c_vel = 2 * pi * 20;   % 20 Hz cut-off
    omega_c_pos = 2 * pi * 5;    % 5 Hz cut-off

    alpha_vel = 1 / (1 + omega_c_vel * Ts);
    beta_pos  = 1 / (1 + omega_c_pos * Ts);

    q_prev = q_meas;
    qf = q_meas;
    qdot_est = zeros(n,1);
    tau_prev = zeros(n,1);

    % robot: RigidBodyTree created in base workspace
    robot = evalin('base', 'robot');
end

% 1) Filtering
qf = beta_pos * qf + (1 - beta_pos) * q_meas;
dq_raw = (q_meas - q_prev) / Ts;
qdot_est = alpha_vel * qdot_est + (1 - alpha_vel) * dq_raw;
q_prev = q_meas;

% 2) Errors
e_q = qd - qf;
e_qdot = qd_dot - qdot_est;

% 3) Dynamics using RigidBodyTree
M = massMatrix(robot, qf');
Cqdot = velocityProduct(robot, qf', qdot_est');
g = gravityTorque(robot, qf');

v = qd_ddot + Kd * e_qdot + Kp * e_q;
tau_pre = M * v + Cqdot' + g';

% 4) Rate limiting
du = tau_pre - tau_prev;
du = min(max(du, dtau_min), dtau_max);
tau_rl = tau_prev + du;

% 5) Saturation
tau_sat = min(max(tau_rl, tau_min), tau_max);
tau_prev = tau_sat;
      

In Simulink, one can instead construct the loop using built-in Derivative (with filter), Transfer Fcn for low-pass filters, and the Saturation and Rate Limiter blocks, with robot dynamics implemented via a MATLAB Function block or prebuilt dynamics block.

Wolfram Mathematica is useful both for symbolic manipulation of dynamics and for simulating the effect of saturation, rate limiting, and filtering in continuous or discrete time.

(* Example: scalar joint with computed torque-like control *)

Ts = 0.001;
omegaCVel = 2 Pi 20;
alphaVel = 1/(1 + omegaCVel Ts);

(* Low-pass "dirty derivative" *)
updateFilter[{qPrev_, qdotEst_}, qMeas_] :=
 Module[{dqRaw, qdotEstNew},
  dqRaw = (qMeas - qPrev)/Ts;
  qdotEstNew = alphaVel qdotEst + (1 - alphaVel) dqRaw;
  {qMeas, qdotEstNew}
 ]

saturate[u_, uMin_, uMax_] := Min[Max[u, uMin], uMax];

rateLimit[uCmd_, uPrev_, duMin_, duMax_] :=
 Module[{du = uCmd - uPrev},
  du = Min[Max[du, duMin], duMax];
  uPrev + du
 ]

(* Toy single-DOF dynamics: M(q) = m, C = 0, g(q) = g0 Sin[q] *)
m = 1.0; g0 = 9.81;
Kp = 100.; Kd = 20.;

tauMin = -5.; tauMax = 5.;
dtauMin = -200. Ts; dtauMax = 200. Ts;

(* Discrete-time simulation *)
steps = 5000;
qd[k_] := 0.5 Sin[0.5 k Ts]; (* desired trajectory *)
qdDot[k_] := 0.5 0.5 Cos[0.5 k Ts];
qdDdot[k_] := -0.5 0.5^2 Sin[0.5 k Ts];

q = ConstantArray[0., steps + 1];
qdot = ConstantArray[0., steps + 1];
tau = ConstantArray[0., steps + 1];

qPrev = 0.; qdotEst = 0.;
tauPrev = 0.;

Do[
  (* filter update *)
  {qPrev, qdotEst} = updateFilter[{qPrev, qdotEst}, q[[k]]];

  (* errors *)
  eQ = qd[k] - qPrev;
  eQdot = qdDot[k] - qdotEst;

  (* computed torque *)
  v = qdDdot[k] + Kd eQdot + Kp eQ;
  tauPre = m v + g0 Sin[qPrev];

  (* rate limit and saturation *)
  tauRl = rateLimit[tauPre, tauPrev, dtauMin, dtauMax];
  tau[[k]] = saturate[tauRl, tauMin, tauMax];
  tauPrev = tau[[k]];

  (* update dynamics (semi-implicit Euler) *)
  qdot[[k + 1]] = qdot[[k]] + Ts (1/m) (tau[[k]] - g0 Sin[q[[k]]]);
  q[[k + 1]] = q[[k]] + Ts qdot[[k + 1]];
 , {k, 1, steps}];

ListLinePlot[
 {
  Table[{k Ts, q[[k]]}, {k, 1, steps}],
  Table[{k Ts, qd[k]}, {k, 1, steps}]
 },
 PlotLegends -> {"q(t)", "q_d(t)"},
 AxesLabel -> {"t [s]", "angle"}
]
      

Symbolic tools in Mathematica can also be used to derive the robot dynamics \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) and then automatically generate C or C++ code for use in real-time controllers that contain the implementation nonlinearities discussed.

This lesson translated ideal computed-torque and feedback-linearizing controllers into realizable digital implementations. We modeled actuator saturation and rate limits mathematically, derived discrete-time filtering schemes for joint measurements, and examined their impact on closed-loop stability and performance. We then implemented a complete controller loop in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, tying each to common robotics libraries. These implementation details are essential for bridging theoretical nonlinear control with actual robot hardware, and they set the stage for robustness and adaptive strategies in later chapters.