Up to this point, robot controllers were mostly derived without explicit limits: torques could be arbitrarily large and joints could move freely. Real robots, however, are subject to inequality constraints:

• Actuator torque / current limits: \( \tau_{\min,i} \le \tau_i \le \tau_{\max,i} \).
• Velocity limits: \( -\dot{q}_{\max,i} \le \dot{q}_i \le \dot{q}_{\max,i} \).
• Position (joint) limits: \( q_{\min,i} \le q_i \le q_{\max,i} \).
• Contact force limits (e.g., friction): typically of the form \( h(q,\dot{q},\boldsymbol{\tau}) \le 0 \).

Let the robot dynamics be known from previous courses:

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

and suppose a nominal (unconstrained) control law \( \boldsymbol{\tau}_{\text{nom}}(q,\dot{q},r) \) has been designed (for instance, a PD, computed-torque, or task-space controller). Inequality-constrained control replaces this by a feasible control \( \boldsymbol{\tau} \) that satisfies:

\[ \boldsymbol{\tau} \in \mathcal{U} = \left\{ \boldsymbol{\tau} \in \mathbb{R}^m \;\middle|\; \boldsymbol{\tau}_{\min} \le \boldsymbol{\tau} \le \boldsymbol{\tau}_{\max} \right\}, \]

and possibly additional state constraints. A simple and widely used approach is to project the nominal control onto \( \mathcal{U} \), which yields the familiar notion of saturation.

flowchart TD
  REF["Task / joint reference"] --> CTRL["Nominal controller computes tau_nom"]
  CTRL --> SAT["Project / saturate into feasible set U"]
  SAT --> FEAS["Check feasibility of task (optional)"]
  FEAS --> ACT["Send feasible tau to actuators"]
        

Consider a general nonlinear control system in state-space form (for a manipulator, \( x = [q^\top \;\; \dot{q}^\top]^\top \)):

\[ \dot{x} = f(x) + G(x)\,\boldsymbol{\tau}. \]

We encode inequality constraints by functions \( h_i(x,\boldsymbol{\tau}) \le 0 \). Typical cases:

• Actuator limits (pure input constraints):

  \[ h_i(x,\boldsymbol{\tau}) = \tau_i - \tau_{\max,i} \le 0, \quad h_{i+m}(x,\boldsymbol{\tau}) = \tau_{\min,i} - \tau_i \le 0. \]

• Velocity limits: approximate by constraints on the control that prevent violation in a short horizon, e.g. in discrete time with sampling time \( T_s \):

  \[ q_{k+1} = q_k + T_s \dot{q}_k, \quad \dot{q}_{k+1} \approx \dot{q}_k + T_s\,\ddot{q}_k(x_k,\boldsymbol{\tau}_k), \]

  and enforce inequalities such as \( -\dot{q}_{\max,i} \le \dot{q}_{k+1,i} \le \dot{q}_{\max,i} \).

• Joint limits: treated similarly, ensuring \( q_{k+1,i} \in [q_{\min,i}, q_{\max,i}] \).

For most of this lesson, we focus on pure input constraints, since they are always present (actuator limits) and already lead to rich behavior.

Let \( \boldsymbol{\tau}_{\text{nom}} \in \mathbb{R}^m \) be the nominal torque vector produced by any controller (PD, computed-torque, operational-space, etc.). We define the feasible set of torques as an axis-aligned box:

\[ \mathcal{U} = \left\{ \boldsymbol{\tau} \in \mathbb{R}^m \;\middle|\; \tau_{\min,i} \le \tau_i \le \tau_{\max,i},\; i=1,\dots,m \right\}. \]

A natural constrained control law is to choose the feasible torque closest (in Euclidean norm) to the nominal torque:

\[ \boldsymbol{\tau}^\star = \arg\min_{\boldsymbol{\tau} \in \mathcal{U}} \frac{1}{2}\left\|\boldsymbol{\tau}-\boldsymbol{\tau}_{\text{nom}}\right\|^2. \]

Because \( \mathcal{U} \) is a Cartesian product of intervals, the minimization problem decouples per joint. For each component \( i \) we solve

\[ \tau_i^\star = \arg\min_{\tau_{\min,i} \le \tau_i \le \tau_{\max,i}} \frac{1}{2}\left(\tau_i - \tau_{\text{nom},i}\right)^2. \]

The scalar problem has the well-known closed form

\[ \tau_i^\star = \operatorname{sat}\!\left( \tau_{\text{nom},i}; \tau_{\min,i}, \tau_{\max,i} \right) = \min\!\bigl(\tau_{\max,i},\;\max(\tau_{\min,i}, \tau_{\text{nom},i})\bigr). \]

Proof sketch. The objective is strictly convex in \( \tau_i \), its unconstrained minimizer is \( \tau_{\text{nom},i} \), and the feasible set is a closed interval. Projecting a point onto an interval yields the above formula. Since the cost decomposes into a sum of independent scalar costs and \( \mathcal{U} \) decomposes into a product of intervals, the full vector solution is the component-wise projection.

Thus, simple component-wise saturation of each joint torque is exactly the solution of a Euclidean projection problem onto the actuator-limited feasible set.

Consider a single revolute joint with dynamics (gravity compensated):

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

where \( m > 0 \) is an equivalent inertia and \( b \ge 0 \) a viscous damping coefficient. Let \( q_r \) be a constant reference and define the error \( e = q - q_r \). A nominal PD control law is

\[ \tau_{\text{nom}} = -k_p e - k_d \dot{q}, \quad k_p > 0,\; k_d > 0. \]

With torque saturation \( \tau \in [-\tau_{\max}, \tau_{\max}] \) we apply

\[ \tau = \operatorname{sat}\!\left(\tau_{\text{nom}};\, -\tau_{\max}, \tau_{\max}\right). \]

Consider the Lyapunov function

\[ V(e,\dot{q}) = \tfrac{1}{2} m \dot{q}^2 + \tfrac{1}{2} k_p e^2. \]

Its time derivative along trajectories is

\[ \dot{V} = m \dot{q}\ddot{q} + k_p e \dot{q} = \dot{q}(\tau - b\dot{q}) + k_p e \dot{q}. \]

If \( \tau = \tau_{\text{nom}} \) (no saturation), we obtain the classical PD result

\[ \dot{V} = -k_d \dot{q}^2 - b \dot{q}^2 \le 0, \]

and the equilibrium \( (e,\dot{q}) = (0,0) \) is globally asymptotically stable. Under saturation, we can write

\[ \tau = \phi(\tau_{\text{nom}}), \]

where \( \phi \) is a static nonlinearity that is odd, monotone nondecreasing, and satisfies the sector condition

\[ 0 \le \phi(s)\,s \le s^2 \quad \text{for all } s \in \mathbb{R}. \]

This means the feedback interconnection of the linear joint dynamics and the saturation nonlinearity belongs to a class of systems for which absolute stability results (e.g. circle and Popov criteria) guarantee stability if the unsaturated loop is sufficiently damped. Intuitively, saturation reduces control authority but does not inject energy, ensuring a form of robust stability.

Practically, one ensures that:

• The unsaturated closed loop is strongly stable (sufficiently large \( k_p,k_d \)).
• \( \tau_{\max} \) is large enough that near the equilibrium the controller never saturates (local linear behavior).
• Joint trajectories are planned so that required torques stay within limits for typical tasks.

In Lesson 2 of this chapter, equality constraints were enforced by projecting control actions into subspaces (e.g. to satisfy a contact or task constraint exactly). With inequality constraints (limits), tasks may become infeasible: there may be no torque satisfying both the control equality and the torque bounds.

Suppose a joint-space or task-space controller prescribes a torque of the form

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

Torque limits impose

\[ \boldsymbol{\tau}_{\min} \le \mathbf{M}(q)\ddot{q}_{\text{des}} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{g}(q) \le \boldsymbol{\tau}_{\max}. \]

Thus, desired accelerations must belong to the set

\[ \mathcal{A}(q,\dot{q}) = \left\{ \ddot{q} \in \mathbb{R}^n \;\middle|\; \boldsymbol{\tau}_{\min} - \mathbf{C}(q,\dot{q})\dot{q} - \mathbf{g}(q) \le \mathbf{M}(q)\ddot{q} \le \boldsymbol{\tau}_{\max} - \mathbf{C}(q,\dot{q})\dot{q} - \mathbf{g}(q) \right\}. \]

If a desired task-space acceleration \( \ddot{x}_{\text{des}} \) is specified, and \( \ddot{q}_{\text{des}} \) is obtained by solving \( \ddot{x}_{\text{des}} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q} \), feasibility requires \( \ddot{q}_{\text{des}} \in \mathcal{A}(q,\dot{q}) \). When this is not the case, the task must be relaxed (e.g. by reducing acceleration, de-prioritizing certain tasks, or switching to a safer maneuver).

flowchart TD
  XDD["Desired task acceleration x_ddot_des"] --> SOLVE["Solve for q_ddot_des"]
  SOLVE --> CHECK["Check torque feasibility with dynamic model"]
  CHECK -->|feasible| USE["Use q_ddot_des and tau_nom"]
  CHECK -->|infeasible| MODIFY["Relax task / scale reference / re-plan"]
        

In Python, we typically use libraries such as numpy for linear algebra and robotics libraries like pinocchio or roboticstoolbox for dynamics. The core saturation logic, however, is independent of the robot model.

import numpy as np

# Example: n-DOF joint-space PD with torque saturation
class SaturatedPDController:
    def __init__(self, Kp, Kd, tau_min, tau_max):
        """
        Kp, Kd: diagonal gains as 1D numpy arrays of length n
        tau_min, tau_max: joint-wise torque limits (1D arrays)
        """
        self.Kp = np.asarray(Kp)
        self.Kd = np.asarray(Kd)
        self.tau_min = np.asarray(tau_min)
        self.tau_max = np.asarray(tau_max)

    @staticmethod
    def saturate(tau_nom, tau_min, tau_max):
        # Component-wise projection onto [tau_min, tau_max]
        return np.minimum(np.maximum(tau_nom, tau_min), tau_max)

    def compute_tau(self, q, qd, q_ref, qd_ref):
        e = q - q_ref
        ed = qd - qd_ref
        tau_nom = -self.Kp * e - self.Kd * ed
        tau = self.saturate(tau_nom, self.tau_min, self.tau_max)
        return tau

# Example usage in a simulation loop with Pinocchio for dynamics
import pinocchio as pin

robot = ...  # load robot model
controller = SaturatedPDController(
    Kp=np.array([100.0, 80.0, 60.0]),
    Kd=np.array([20.0, 16.0, 12.0]),
    tau_min=np.array([-50.0, -40.0, -30.0]),
    tau_max=np.array([50.0, 40.0, 30.0])
)

dt = 0.001
q = robot.q0.copy()
qd = np.zeros_like(q)
q_ref = q.copy()
qd_ref = np.zeros_like(q)

for k in range(10000):
    # Compute dynamics terms
    pin.computeAllTerms(robot.model, robot.data, q, qd)
    M = robot.data.M
    b = robot.data.nle  # C(q,qd)*qd + g(q)

    tau = controller.compute_tau(q, qd, q_ref, qd_ref)

    # Forward dynamics: qdd = M^{-1}(tau - b)
    qdd = np.linalg.solve(M, tau - b)

    # Simple Euler integration
    qd = qd + dt * qdd
    q = q + dt * qd
      

This example shows how saturation naturally fits into a model-based simulation using Pinocchio. For task-space controllers, \( \boldsymbol{\tau}_{\text{nom}} \) would be computed via operational-space formulas, then passed through the same saturator.

In C++, Eigen is widely used for linear algebra, and many robotics libraries (e.g. Pinocchio, RBDL) are Eigen-based. The saturation logic is again a component-wise projection.

#include <Eigen/Dense>

class SaturatedPDController {
public:
    SaturatedPDController(const Eigen::VectorXd& Kp,
                          const Eigen::VectorXd& Kd,
                          const Eigen::VectorXd& tau_min,
                          const Eigen::VectorXd& tau_max)
        : Kp_(Kp), Kd_(Kd), tau_min_(tau_min), tau_max_(tau_max) {}

    Eigen::VectorXd computeTau(const Eigen::VectorXd& q,
                               const Eigen::VectorXd& qd,
                               const Eigen::VectorXd& q_ref,
                               const Eigen::VectorXd& qd_ref) const
    {
        Eigen::VectorXd e  = q  - q_ref;
        Eigen::VectorXd ed = qd - qd_ref;
        Eigen::VectorXd tau_nom = -Kp_.cwiseProduct(e)
                                  -Kd_.cwiseProduct(ed);
        return saturate(tau_nom);
    }

private:
    Eigen::VectorXd saturate(const Eigen::VectorXd& tau_nom) const
    {
        Eigen::VectorXd tau = tau_nom;
        for (int i = 0; i < tau.size(); ++i) {
            if (tau(i) > tau_max_(i)) tau(i) = tau_max_(i);
            if (tau(i) < tau_min_(i)) tau(i) = tau_min_(i);
        }
        return tau;
    }

    Eigen::VectorXd Kp_, Kd_;
    Eigen::VectorXd tau_min_, tau_max_;
};

// In a dynamics loop (e.g., using Pinocchio):
// - compute M(q), b(q,qd)
// - compute tau_nom from controller
// - apply controller.computeTau(...)
// - integrate q, qd forward in time
      

Integrating this into a real-time control loop requires careful consideration of computation time and avoiding dynamic memory allocation inside the loop (pre-allocate Eigen vectors and reuse them).

In Java, linear algebra can be handled using libraries such as EJML (org.ejml) or Apache Commons Math. Here we show a simple array-based implementation of a saturated PD controller that can be integrated with a Java-based simulator or a ROS Java node.

public class SaturatedPDController {
    private final double[] Kp;
    private final double[] Kd;
    private final double[] tauMin;
    private final double[] tauMax;

    public SaturatedPDController(double[] Kp, double[] Kd,
                                 double[] tauMin, double[] tauMax) {
        this.Kp = Kp.clone();
        this.Kd = Kd.clone();
        this.tauMin = tauMin.clone();
        this.tauMax = tauMax.clone();
    }

    public void computeTau(double[] q, double[] qd,
                           double[] qRef, double[] qdRef,
                           double[] tauOut) {
        int n = q.length;
        for (int i = 0; i < n; i++) {
            double e  = q[i]  - qRef[i];
            double ed = qd[i] - qdRef[i];
            double tauNom = -Kp[i] * e - Kd[i] * ed;

            // Saturation
            double tau = tauNom;
            if (tau > tauMax[i]) tau = tauMax[i];
            if (tau < tauMin[i]) tau = tauMin[i];
            tauOut[i] = tau;
        }
    }
}
      

The method computeTau writes the saturated torque directly into tauOut to avoid object allocation in real-time loops. Dynamics integration can be implemented in Java or delegated to a physics engine accessible from Java.

9.1 MATLAB / Simulink

MATLAB offers the Robotics System Toolbox for dynamics and Simulink for block-diagram design. Saturation is often implemented either as a dedicated Saturate block or via simple code.

function tau = saturated_pd(q, qd, q_ref, qd_ref, Kp, Kd, tau_min, tau_max)
% q, qd, q_ref, qd_ref: n-by-1 vectors
% Kp, Kd, tau_min, tau_max: n-by-1 vectors

e  = q  - q_ref;
ed = qd - qd_ref;

tau_nom = -Kp .* e - Kd .* ed;
tau     = min(max(tau_nom, tau_min), tau_max);
      

In Simulink, one can use:

• A block that computes \( \tau_{\text{nom}} \) from \( e, \dot{e} \).
• A Saturation block with upper and lower limits \( \tau_{\max}, \tau_{\min} \).
• A dynamics block (e.g. Forward Dynamics from Robotics System Toolbox).

9.2 Wolfram Mathematica

Mathematica is convenient for symbolic derivations and quick simulations.

(* Define saturation *)
sat[v_, vmin_, vmax_] = Min[Max[v, vmin], vmax];

(* Parameters *)
m = 1.0;
b = 0.2;
kp = 50.0;
kd = 10.0;
tauMax = 5.0;

(* Closed-loop dynamics for a single joint about q_ref = 0 *)
eq = q''[t] == (sat[-kp q[t] - kd q'[t], -tauMax, tauMax] - b q'[t])/m;

sol = NDSolve[
  {eq, q[0] == 0.5, q'[0] == 0.0},
  q, {t, 0, 5}
];

Plot[Evaluate[q[t] /. sol], {t, 0, 5},
     AxesLabel -> {"t", "q(t)"},
     PlotRange -> All]
      

This script compares the response with saturated torque to the ideal PD response and can be extended to multi-DOF robots using matrix-valued dynamics.

In this lesson we formalized inequality constraints in robot control, focusing on actuator saturation and feasibility of joint- and task-space commands. We showed that simple saturation is the Euclidean projection of the nominal torque onto a box-constrained feasible set, and discussed how this interacts with stability through sector bounds and Lyapunov analysis. We also examined feasibility of equality tasks under torque limits, emphasizing that desired task accelerations must correspond to dynamically achievable joint accelerations. Finally, we implemented saturated controllers in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, preparing for more advanced constrained and contact-aware controllers in subsequent lessons.