We consider an n-DOF manipulator controlled at the joint-velocity level. The low-level robot interface accepts a command \( \mathbf{v} \in \mathbb{R}^n \) and updates joint positions via

\[ \dot{\mathbf{q}}(t) = \mathbf{v}(t), \quad \mathbf{q}(t) \in \mathbb{R}^n \]

where \( \mathbf{q} \) collects joint angles (or generalized coordinates). A nominal controller (from previous chapters) provides a desired joint velocity \( \mathbf{v}_{\text{nom}}(\mathbf{q}, \mathbf{q}_d, \dot{\mathbf{q}}_d) \) for tracking some reference. Our goal is to compute instead a safe velocity \( \mathbf{v}_{\text{safe}} \) that:

• Stays as close as possible to \( \mathbf{v}_{\text{nom}} \),
• Guarantees joint limits are never violated,
• Ensures the end-effector stays outside forbidden workspace regions (e.g., obstacles).

This is achieved by wrapping the nominal controller with a CBF-QP safety filter:

flowchart TD
  R["Reference / Task"] --> N["Nominal Controller (PD, CT, etc.)"]
  N --> VN["Joint velocity v_nom"]
  VN --> SF["Safety Filter (QP with CBF constraints)"]
  SF --> VS["Joint velocity v_safe"]
  VS --> ROB["Robot (joint-level interface)"]
  ROB --> MEAS["Sensors (q, qdot, ee pose)"]
  MEAS --> N
        

The lab is conceptually the same for simulated or real robots. In practice, you will implement the safety filter in a loop with sampling time \( T_s \), using measured \( \mathbf{q}(k) \) and possibly end-effector pose \( \mathbf{p}(k) \).

At the joint-velocity level, the system is control-affine in the state \( \mathbf{x} = \mathbf{q} \) with input \( \mathbf{u} = \mathbf{v} \):

\[ \dot{\mathbf{x}} = \dot{\mathbf{q}} = \mathbf{v}. \]

A safe set is defined by a continuously differentiable function \( h : \mathbb{R}^n \to \mathbb{R} \) as

\[ \mathcal{C} = \{ \mathbf{q} \in \mathbb{R}^n \,\vert\, h(\mathbf{q}) \ge 0 \}. \]

The set \( \mathcal{C} \) is forward invariant if any trajectory starting in \( \mathcal{C} \) never leaves it. A zeroing control barrier function (ZCBF) condition at velocity level is:

\[ \dot{h}(\mathbf{q}, \mathbf{v}) = \nabla h(\mathbf{q})^{\top} \mathbf{v} \ge -\alpha\big(h(\mathbf{q})\big), \]

where \( \alpha \) is an extended class-\(\mathcal{K}\) function (e.g. \( \alpha(s) = \gamma s \) with \( \gamma > 0 \)). If the above inequality is satisfied for all time, and the system is locally Lipschitz, then \( \mathcal{C} \) is forward invariant, i.e., safety is guaranteed.

In a QP-based safety filter, we enforce this inequality as a linear constraint in \( \mathbf{v} \):

\[ \nabla h(\mathbf{q})^{\top} \mathbf{v} + \alpha\big(h(\mathbf{q})\big) \ge 0. \]

For multiple CBFs \( h_j \), the safe set is the intersection \( \mathcal{C} = \bigcap_j \mathcal{C}_j \), and the QP includes one linear inequality per CBF.

For each joint \( i \in \{1,\dots,n\} \), let \( q_{i,\min} \) and \( q_{i,\max} \) denote its mechanical limits. We want \( q_{i,\min} \le q_i \le q_{i,\max} \) for all time. We encode this as two scalar CBFs:

\[ h_i^{\min}(\mathbf{q}) = q_i - q_{i,\min}, \quad h_i^{\max}(\mathbf{q}) = q_{i,\max} - q_i. \]

The safe set is \( h_i^{\min} \ge 0 \) and \( h_i^{\max} \ge 0 \). Their gradients are \( \nabla h_i^{\min} = \mathbf{e}_i \) and \( \nabla h_i^{\max} = -\mathbf{e}_i \), where \( \mathbf{e}_i \) is the i-th canonical basis vector. The CBF conditions become

\[ \begin{aligned} \dot{h}_i^{\min}(\mathbf{q}, \mathbf{v}) &= \mathbf{e}_i^{\top} \mathbf{v} \ge -\alpha\big(h_i^{\min}(\mathbf{q})\big), \\ \dot{h}_i^{\max}(\mathbf{q}, \mathbf{v}) &= -\mathbf{e}_i^{\top} \mathbf{v} \ge -\alpha\big(h_i^{\max}(\mathbf{q})\big). \end{aligned} \]

For a linear choice \( \alpha(s) = \gamma s \) with \( \gamma > 0 \), we obtain the pair of inequalities

\[ \begin{aligned} \mathbf{e}_i^{\top} \mathbf{v} + \gamma (q_i - q_{i,\min}) &\ge 0, \\ -\mathbf{e}_i^{\top} \mathbf{v} + \gamma (q_{i,\max} - q_i) &\ge 0. \end{aligned} \]

These are affine constraints in \( \mathbf{v} \) and can be directly inserted into the QP. Near the joint limits, the allowable velocity range shrinks and prevents violation of the constraints.

Let \( \mathbf{p}(\mathbf{q}) \in \mathbb{R}^3 \) denote the end-effector position (known from forward kinematics). Assume there is a spherical forbidden region with center \( \mathbf{c} \in \mathbb{R}^3 \) and radius \( R > 0 \). A natural safety function is

\[ h_{\text{ws}}(\mathbf{q}) = \big\|\mathbf{p}(\mathbf{q}) - \mathbf{c}\big\|^2 - R^2. \]

The safe set \( \mathcal{C}_{\text{ws}} \) consists of configurations where the end-effector is outside (or on) the sphere. Using the manipulator Jacobian \( \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{3 \times n} \) such that \( \dot{\mathbf{p}} = \mathbf{J}(\mathbf{q}) \mathbf{v} \), we compute

\[ \nabla h_{\text{ws}}(\mathbf{q}) = 2\big(\mathbf{p}(\mathbf{q}) - \mathbf{c}\big)^{\top} \mathbf{J}(\mathbf{q}), \]

and thus

\[ \dot{h}_{\text{ws}}(\mathbf{q}, \mathbf{v}) = 2\big(\mathbf{p}(\mathbf{q}) - \mathbf{c}\big)^{\top} \mathbf{J}(\mathbf{q}) \mathbf{v}. \]

The CBF constraint for workspace avoidance is

\[ 2\big(\mathbf{p}(\mathbf{q}) - \mathbf{c}\big)^{\top} \mathbf{J}(\mathbf{q}) \mathbf{v} + \alpha\big(h_{\text{ws}}(\mathbf{q})\big) \ge 0. \]

When the end-effector approaches the forbidden ball (i.e., \( h_{\text{ws}} \to 0 \)), the constraint shapes the permitted velocity directions to steer the robot away from the obstacle while minimally deviating from the nominal command.

Collect all CBF inequalities in the standard form \( \mathbf{A}(\mathbf{q}) \mathbf{v} \le \mathbf{b}(\mathbf{q}) \) or equivalently \( \mathbf{A}_{\text{cbf}} \mathbf{v} \ge \mathbf{c}_{\text{cbf}} \). For implementation, we fix the sign convention as

\[ \mathbf{A}(\mathbf{q}) \mathbf{v} \le \mathbf{b}(\mathbf{q}), \]

where each row comes from one CBF inequality. For example, for a joint lower bound constraint \( \mathbf{e}_i^{\top} \mathbf{v} + \gamma (q_i - q_{i,\min}) \ge 0 \), we can rewrite as \( -\mathbf{e}_i^{\top} \mathbf{v} \le \gamma (q_i - q_{i,\min}) \).

A typical QP-based safety filter solves at each sampling step:

\[ \begin{aligned} \mathbf{v}_{\text{safe}}(\mathbf{q}) &= \arg\min_{\mathbf{v},\,\delta} \frac{1}{2} \big\|\mathbf{v} - \mathbf{v}_{\text{nom}}\big\|^2 + \frac{\lambda}{2} \delta^2 \\ \text{s.t. } \quad \mathbf{A}(\mathbf{q}) \mathbf{v} &\le \mathbf{b}(\mathbf{q}) + \delta \mathbf{1}, \\ \delta &\ge 0, \end{aligned} \]

where \( \delta \) is a slack variable (shared or per-constraint) that guarantees feasibility. A large \( \lambda \gg 1 \) penalizes any violation of safety constraints, effectively implementing a “soft” barrier.

Continuous-time safety argument. If \( \delta \equiv 0 \), the CBF constraints enforce \( \dot{h}_j \ge -\alpha(h_j) \) for all j, which ensures forward invariance of each safe set \( \mathcal{C}_j \) under standard CBF assumptions. Thus, the QP solution is guaranteed to be safe if the QP is feasible and solved at sufficiently high rate (\( T_s \) small) and the dynamics are well approximated by the velocity-level model.

The implementation is a repeated loop at sampling time \( T_s \). At each step, we:

1. Read the current joint state \( \mathbf{q}(k) \) and (optionally) \( \dot{\mathbf{q}}(k) \).
2. Evaluate forward kinematics \( \mathbf{p}(\mathbf{q}(k)) \) and Jacobian \( \mathbf{J}(\mathbf{q}(k)) \).
3. Compute the nominal command \( \mathbf{v}_{\text{nom}}(k) \).
4. Construct CBF inequalities for joint limits and workspace.
5. Solve the QP for \( \mathbf{v}_{\text{safe}}(k) \).
6. Send \( \mathbf{v}_{\text{safe}}(k) \) to the robot.

flowchart TD
  S["Start at time k"] --> M["Measure q(k), qdot(k)"]
  M --> KIN["Compute ee pose p(q) and Jacobian J(q)"]
  KIN --> NOM["Compute nominal velocity v_nom(k)"]
  NOM --> CBF["Build CBF constraints (joint + workspace)"]
  CBF --> QP["Solve QP: min ||v - v_nom||^2 s.t. CBF"]
  QP --> CMD["Apply v_safe(k) to robot"]
  CMD --> ADV["Advance time: k := k+1"]
  ADV --> S
        

Numerically, it is crucial that the QP solver runs within the control period \( T_s \). For small n and few constraints, this is easily satisfied with standard solvers.

We consider a planar 2-DOF arm with joint limits and a single spherical (circular in plane) workspace forbidden region. Forward kinematics and Jacobian are implemented directly. For the QP, you may use a package such as qpsolvers or cvxopt; here we assume a generic solve_qp interface.

import numpy as np

# Robot parameters for planar 2-DOF arm (for illustration)
L1, L2 = 1.0, 1.0  # link lengths

def forward_kinematics(q):
    q1, q2 = q
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return np.array([x, y])

def jacobian(q):
    q1, q2 = q
    s1 = np.sin(q1); c1 = np.cos(q1)
    s12 = np.sin(q1 + q2); c12 = np.cos(q1 + q2)
    J11 = -L1 * s1 - L2 * s12
    J12 = -L2 * s12
    J21 =  L1 * c1 + L2 * c12
    J22 =  L2 * c12
    return np.array([[J11, J12],
                     [J21, J22]])

# Joint limits
q_min = np.array([-np.pi / 2.0, -np.pi / 2.0])
q_max = np.array([ np.pi / 2.0,  np.pi / 2.0])

# Workspace forbidden circle
c_obs = np.array([0.5, 0.5])
R_obs = 0.3

gamma_joint = 5.0
gamma_ws = 5.0
lambda_slack = 1e3

def build_cbf_constraints(q):
    """
    Build A, b for inequalities A v <= b.
    Includes:
      - 2 inequalities per joint (min, max)
      - 1 workspace-avoidance inequality
    """
    n = q.shape[0]
    A_list = []
    b_list = []

    # Joint limit constraints
    for i in range(n):
        # h_min = q_i - q_min_i
        h_min = q[i] - q_min[i]
        # e_i^T v + gamma * h_min >= 0
        # Rewrite: -e_i^T v <= gamma * h_min
        e_i = np.zeros(n)
        e_i[i] = 1.0
        A_list.append(-e_i)
        b_list.append(gamma_joint * h_min)

        # h_max = q_max_i - q_i
        h_max = q_max[i] - q[i]
        # -e_i^T v + gamma * h_max >= 0
        # Rewrite: e_i^T v <= gamma * h_max
        A_list.append(e_i)
        b_list.append(gamma_joint * h_max)

    # Workspace avoidance
    p = forward_kinematics(q)
    J = jacobian(q)
    diff = p - c_obs
    h_ws = np.dot(diff, diff) - R_obs**2
    # 2 diff^T J v + gamma_ws * h_ws >= 0
    # Rewrite: -2 diff^T J v <= gamma_ws * h_ws
    a_ws = -2.0 * diff @ J  # row vector
    A_list.append(a_ws)
    b_list.append(gamma_ws * h_ws)

    A = np.vstack(A_list)      # shape (m, n)
    b = np.array(b_list)       # shape (m,)
    return A, b

def safety_filter(q, v_nom, solve_qp):
    """
    Given q and nominal velocity v_nom, solve QP:
      min_v,delta  0.5 ||v - v_nom||^2 + 0.5 * lambda_slack * delta^2
      s.t.         A v <= b + delta
                   delta >= 0
    The solve_qp callback should accept (H, f, A, b, C, d)
    representing:
      min 0.5 x^T H x + f^T x
      s.t. A x <= b, C x = d
    """
    n = v_nom.shape[0]
    A, b = build_cbf_constraints(q)

    # Decision variable x = [v; delta]
    H = np.zeros((n + 1, n + 1))
    # Quadratic term for v
    H[:n, :n] = np.eye(n)
    # Quadratic term for delta
    H[n, n] = lambda_slack

    f = np.zeros(n + 1)
    f[:n] = -v_nom

    # Inequalities: A v - 1 * delta <= b
    m = A.shape[0]
    A_qp = np.zeros((m + 1, n + 1))
    b_qp = np.zeros(m + 1)

    # CBF constraints
    A_qp[:m, :n] = A
    A_qp[:m, n] = -1.0
    b_qp[:m] = b

    # delta >= 0  becomes  -delta <= 0
    A_qp[m, n] = -1.0
    b_qp[m] = 0.0

    # No equality constraints
    C = None
    d = None

    x_opt = solve_qp(H, f, A_qp, b_qp, C, d)
    v_safe = x_opt[:n]
    return v_safe

# Example usage (assuming some solve_qp implementation):
# def solve_qp(H, f, A, b, C, d):
#     ...
# In a control loop:
# while running:
#     q = get_current_joint_positions()
#     v_nom = nominal_controller(q, q_des, qdot_des)
#     v_safe = safety_filter(q, v_nom, solve_qp)
#     send_velocity_command(v_safe)
      

In a real implementation, solve_qp can be backed by qpsolvers, osqp, cvxopt, or similar packages. For robotics kinematics, you may replace the hand-coded planar model by libraries such as pinocchio or roboticstoolbox-python.

In C++, we can use Eigen for linear algebra and a QP solver such as OsqpEigen or qpOASES. Below is a simplified skeleton that assumes:

• forwardKinematics(q) returns end-effector position,
• jacobian(q) returns the Jacobian,
• solveQP(H, f, A, b) solves the QP with inequality constraints.

#include <Eigen/Dense>

// Global parameters (for illustration)
const double gamma_joint = 5.0;
const double gamma_ws    = 5.0;
const double lambda_slack = 1e3;

Eigen::Vector2d forwardKinematics(const Eigen::Vector2d& q);
Eigen::Matrix<double, 2, 2> jacobian(const Eigen::Vector2d& q);

// Solve QP: min 0.5 x^T H x + f^T x  s.t. A x <= b
Eigen::VectorXd solveQP(const Eigen::MatrixXd& H,
                        const Eigen::VectorXd& f,
                        const Eigen::MatrixXd& A,
                        const Eigen::VectorXd& b);

void buildCBFConstraints(const Eigen::Vector2d& q,
                         Eigen::MatrixXd& A,
                         Eigen::VectorXd& b)
{
    const int n = 2;
    const int m = 2 * n + 1; // 2 constraints per joint + workspace
    A.resize(m, n);
    b.resize(m);

    Eigen::Vector2d q_min(-M_PI / 2.0, -M_PI / 2.0);
    Eigen::Vector2d q_max( M_PI / 2.0,  M_PI / 2.0);

    int row = 0;
    for (int i = 0; i < n; ++i)
    {
        // Lower limit: q_i - q_min_i >= 0
        double h_min = q(i) - q_min(i);
        Eigen::Vector2d e_i = Eigen::Vector2d::Zero();
        e_i(i) = 1.0;

        // e_i^T v + gamma_joint * h_min >= 0  =>  -e_i^T v <= gamma_joint * h_min
        A.row(row) = -e_i.transpose();
        b(row) = gamma_joint * h_min;
        ++row;

        // Upper limit: q_max_i - q_i >= 0
        double h_max = q_max(i) - q(i);
        // -e_i^T v + gamma_joint * h_max >= 0  =>  e_i^T v <= gamma_joint * h_max
        A.row(row) = e_i.transpose();
        b(row) = gamma_joint * h_max;
        ++row;
    }

    // Workspace constraint
    Eigen::Vector2d c_obs(0.5, 0.5);
    double R_obs = 0.3;

    Eigen::Vector2d p = forwardKinematics(q);
    Eigen::Matrix<double, 2, 2> J = jacobian(q);
    Eigen::Vector2d diff = p - c_obs;
    double h_ws = diff.squaredNorm() - R_obs * R_obs;

    // 2 diff^T J v + gamma_ws * h_ws >= 0
    // => -(2 diff^T J) v <= gamma_ws * h_ws
    Eigen::RowVector2d a_ws = -2.0 * diff.transpose() * J;
    A.row(row) = a_ws;
    b(row) = gamma_ws * h_ws;
}

Eigen::Vector2d safetyFilter(const Eigen::Vector2d& q,
                             const Eigen::Vector2d& v_nom)
{
    const int n = 2;
    Eigen::MatrixXd A_cbf;
    Eigen::VectorXd b_cbf;
    buildCBFConstraints(q, A_cbf, b_cbf);
    int m = static_cast<int>(b_cbf.size());

    // Decision x = [v; delta] in R^{n+1}
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n + 1, n + 1);
    H.block(0, 0, n, n) = Eigen::Matrix2d::Identity();
    H(n, n) = lambda_slack;

    Eigen::VectorXd f = Eigen::VectorXd::Zero(n + 1);
    f.segment(0, n) = -v_nom;

    // Inequalities: A_cbf v - delta <= b_cbf   and   -delta <= 0
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(m + 1, n + 1);
    Eigen::VectorXd b(m + 1);

    A.block(0, 0, m, n) = A_cbf;
    A.block(0, n, m, 1) = -Eigen::VectorXd::Ones(m);
    b.segment(0, m) = b_cbf;

    A(m, n) = -1.0;
    b(m) = 0.0;

    Eigen::VectorXd x_opt = solveQP(H, f, A, b);
    return x_opt.segment(0, n);
}
      

Here, solveQP can be implemented with OsqpEigen, qpOASES, or any other convex QP solver. Robot kinematics can be implemented with Pinocchio, KDL, or custom code, provided you supply forwardKinematics and jacobian.

Java has fewer off-the-shelf QP solvers, but one can use optimization libraries such as ojAlgo or Apache Commons Math. Below, we sketch a simple projected gradient method for the QP, suitable for small problems.

public class SafetyFilter2D {

    private static final double GAMMA_JOINT = 5.0;
    private static final double GAMMA_WS = 5.0;
    private static final double R_OBS = 0.3;
    private static final double[] C_OBS = {0.5, 0.5};

    // Forward kinematics and Jacobian for planar 2-DOF arm
    public static double[] forwardKinematics(double[] q, double L1, double L2) {
        double q1 = q[0], q2 = q[1];
        double x = L1 * Math.cos(q1) + L2 * Math.cos(q1 + q2);
        double y = L1 * Math.sin(q1) + L2 * Math.sin(q1 + q2);
        return new double[]{x, y};
    }

    public static double[][] jacobian(double[] q, double L1, double L2) {
        double q1 = q[0], q2 = q[1];
        double s1 = Math.sin(q1), c1 = Math.cos(q1);
        double s12 = Math.sin(q1 + q2), c12 = Math.cos(q1 + q2);
        double J11 = -L1 * s1 - L2 * s12;
        double J12 = -L2 * s12;
        double J21 =  L1 * c1 + L2 * c12;
        double J22 =  L2 * c12;
        return new double[][]{
                {J11, J12},
                {J21, J22}
        };
    }

    // Build inequalities A v <= b for given q
    public static void buildConstraints(double[] q,
                                        double[][] A,
                                        double[] b) {
        double[] qMin = {-Math.PI / 2.0, -Math.PI / 2.0};
        double[] qMax = { Math.PI / 2.0,  Math.PI / 2.0};

        int row = 0;
        // Joint limits
        for (int i = 0; i < 2; ++i) {
            double hMin = q[i] - qMin[i];
            // -e_i^T v <= gamma * hMin
            A[row][0] = (i == 0) ? -1.0 : 0.0;
            A[row][1] = (i == 1) ? -1.0 : 0.0;
            b[row] = GAMMA_JOINT * hMin;
            row++;

            double hMax = qMax[i] - q[i];
            // e_i^T v <= gamma * hMax
            A[row][0] = (i == 0) ? 1.0 : 0.0;
            A[row][1] = (i == 1) ? 1.0 : 0.0;
            b[row] = GAMMA_JOINT * hMax;
            row++;
        }

        // Workspace
        double L1 = 1.0, L2 = 1.0;
        double[] p = forwardKinematics(q, L1, L2);
        double[][] J = jacobian(q, L1, L2);
        double dx = p[0] - C_OBS[0];
        double dy = p[1] - C_OBS[1];
        double hWs = dx * dx + dy * dy - R_OBS * R_OBS;

        double a1 = -2.0 * (dx * J[0][0] + dy * J[1][0]);
        double a2 = -2.0 * (dx * J[0][1] + dy * J[1][1]);

        A[row][0] = a1;
        A[row][1] = a2;
        b[row] = GAMMA_WS * hWs;
    }

    // Simple projected gradient descent for small QP:
    // min 0.5 ||v - vNom||^2  s.t.  A v <= b
    public static double[] projectQP(double[] vNom,
                                     double[][] A,
                                     double[] b,
                                     int maxIter,
                                     double stepSize) {
        double[] v = vNom.clone();
        int m = b.length;
        for (int it = 0; it < maxIter; ++it) {
            // Gradient of objective: v - vNom
            double g0 = v[0] - vNom[0];
            double g1 = v[1] - vNom[1];
            v[0] -= stepSize * g0;
            v[1] -= stepSize * g1;

            // Project onto each half-space sequentially
            for (int i = 0; i < m; ++i) {
                double ai0 = A[i][0];
                double ai1 = A[i][1];
                double lhs = ai0 * v[0] + ai1 * v[1];
                if (lhs > b[i]) {
                    double norm2 = ai0 * ai0 + ai1 * ai1;
                    if (norm2 > 1e-8) {
                        double alpha = (lhs - b[i]) / norm2;
                        v[0] -= alpha * ai0;
                        v[1] -= alpha * ai1;
                    }
                }
            }
        }
        return v;
    }

    public static double[] safetyFilter(double[] q, double[] vNom) {
        // A has 5 rows (4 joint constraints + 1 workspace) and 2 columns
        double[][] A = new double[5][2];
        double[] b = new double[5];
        buildConstraints(q, A, b);
        return projectQP(vNom, A, b, 50, 0.1);
    }
}
      

The simple projected gradient method is not as numerically robust as a full QP solver, but it is often sufficient for low-dimensional lab examples and does not require external native libraries.

10.1 MATLAB/Simulink (Robotics System Toolbox + quadprog)

MATLAB provides both robotics modeling tools and a QP solver quadprog. Assume we have a rigidBodyTree model robot and we wish to implement the safety filter in a MATLAB Function block in Simulink.

function v_safe = cbf_safety_filter(q, v_nom, robot)
% CBF-based safety filter at joint-velocity level
% q      : current joint positions (nx1)
% v_nom  : nominal joint velocities (nx1)
% robot  : rigidBodyTree (for FK and Jacobian)

n = numel(q);
gamma_joint = 5.0;
gamma_ws    = 5.0;
lambda_slack = 1e3;

% Joint limits from robot model
q_min = robot.homeConfiguration;
q_max = robot.homeConfiguration;
for i = 1:n
    q_min(i) = robot.Bodies{i}.Joint.PositionLimits(1);
    q_max(i) = robot.Bodies{i}.Joint.PositionLimits(2);
end

% Forbidden sphere
c_obs = [0.5; 0.5; 0.0];
R_obs = 0.3;

% Build constraints A v <= b
A = [];
b = [];

% Joint limits
for i = 1:n
    h_min = q(i) - q_min(i);
    e_i   = zeros(1, n); e_i(i) = 1;
    % -e_i v <= gamma_joint * h_min
    A = [A; -e_i];
    b = [b; gamma_joint * h_min];

    h_max = q_max(i) - q(i);
    % e_i v <= gamma_joint * h_max
    A = [A; e_i];
    b = [b; gamma_joint * h_max];
end

% Workspace avoidance using end-effector body
eeName = robot.BodyNames{end};
T = getTransform(robot, q, eeName);
p = T(1:3, 4);
J = geometricJacobian(robot, q, eeName);
Jp = J(1:3, :);  % translational part

diff = p - c_obs;
h_ws = diff' * diff - R_obs^2;

% 2 diff^T Jp v + gamma_ws * h_ws >= 0
% => -(2 diff^T Jp) v <= gamma_ws * h_ws
a_ws = -2 * diff' * Jp;
A = [A; a_ws];
b = [b; gamma_ws * h_ws];

% QP: min 0.5 [v;delta]' H [v;delta] + f' [v;delta]
H = blkdiag(eye(n), lambda_slack);
f = [-v_nom; 0];

m = size(A, 1);
A_qp = [A, -ones(m, 1);  % A v - delta <= b
        zeros(1, n), -1]; % -delta <= 0
b_qp = [b; 0];

% No equality constraints
Aeq = [];
beq = [];

opts = optimoptions('quadprog', 'Display', 'off');
x_opt = quadprog(H, f, A_qp, b_qp, Aeq, beq, [], [], [], opts);
v_safe = x_opt(1:n);
      

In Simulink, you can call cbf_safety_filter from a MATLAB Function block, feeding it measured joint positions and nominal velocities, and routing its output to the joint velocity command block.

10.2 Wolfram Mathematica (QuadraticOptimization)

Mathematica has a high-level quadratic optimization interface that can directly express the CBF-QP.

(* Joint-limit and workspace CBF-QP for 2-DOF planar arm *)

L1 = 1.0; L2 = 1.0;
qMin = {-Pi/2, -Pi/2};
qMax = { Pi/2,  Pi/2};
cObs = {0.5, 0.5};
rObs = 0.3;
gammaJoint = 5.0;
gammaWs = 5.0;
lambdaSlack = 1000.0;

forwardKinematics[{q1_, q2_}] := {
  L1 Cos[q1] + L2 Cos[q1 + q2],
  L1 Sin[q1] + L2 Sin[q1 + q2]
};

jacobian[{q1_, q2_}] := {
  {-L1 Sin[q1] - L2 Sin[q1 + q2], -L2 Sin[q1 + q2]},
  { L1 Cos[q1] + L2 Cos[q1 + q2],  L2 Cos[q1 + q2]}
};

cbfSafetyFilter[qVec_List, vNom_List] := Module[
  {q1, q2, v1, v2, delta, q, p, J, diff, hMin, hMax, hWs,
   ineqs, vSafe},
  {q1, q2} = qVec;
  q = {q1, q2};

  p = forwardKinematics[q];
  J = jacobian[q];
  diff = p - cObs;
  hWs = diff.diff - rObs^2;

  (* Decision variables *)
  {v1, v2, delta} = {Subscript[v, 1], Subscript[v, 2], \[Delta]};

  ineqs = {};

  (* Joint CBFs *)
  Do[
    hMin = q[[i]] - qMin[[i]];
    AppendTo[ineqs, -{If[i == 1, 1, 0], If[i == 2, 1, 0]}.{v1, v2}
      <= gammaJoint hMin + delta],
    {i, 1, 2}
  ];
  Do[
    hMax = qMax[[i]] - q[[i]];
    AppendTo[ineqs, {If[i == 1, 1, 0], If[i == 2, 1, 0]}.{v1, v2}
      <= gammaJoint hMax + delta],
    {i, 1, 2}
  ];

  (* Workspace CBF *)
  With[{a = -2 diff.J},
    AppendTo[ineqs, a.{v1, v2} <= gammaWs hWs + delta]
  ];

  AppendTo[ineqs, delta >= 0];

  vSafe = {v1, v2} /. 
    QuadraticOptimization[
      0.5 ((v1 - vNom[[1]])^2 + (v2 - vNom[[2]])^2) +
        0.5 lambdaSlack delta^2,
      ineqs,
      {v1, v2, delta}
    ];

  vSafe
]
      

The function cbfSafetyFilter takes the current joint angles and nominal velocities and returns the safe velocity command, using QuadraticOptimization to enforce CBF inequalities.

Problem 1 (Invariance for Joint-Limit CBF): Consider a single joint with dynamics \( \dot{q} = v \) and safety function \( h(q) = q - q_{\min} \). Show that if the control input \( v(t) \) satisfies \( v(t) + \gamma h(q(t)) \ge 0 \) for all \( t \ge 0 \) with \( \gamma > 0 \), then the set \( \mathcal{C} = \{ q \,\vert\, h(q) \ge 0 \} \) is forward invariant.

Solution: The derivative of \( h \) is

\[ \dot{h}(q(t)) = \frac{\mathrm{d}}{\mathrm{d}t}\big(q(t) - q_{\min}\big) = \dot{q}(t) = v(t). \]

By assumption, \( v(t) + \gamma h(q(t)) \ge 0 \), so

\[ \dot{h}(t) \ge -\gamma h(t). \]

This scalar differential inequality has the comparison solution \( \tilde{h}(t) = h(0) e^{-\gamma t} \). Standard comparison lemmas imply \( h(t) \ge \tilde{h}(t) \) for all \( t \ge 0 \). If \( h(0) \ge 0 \), then \( \tilde{h}(t) \ge 0 \) for all \( t \ge 0 \), and thus \( h(t) \ge 0 \). Hence \( q(t) \in \mathcal{C} \) for all \( t \ge 0 \), and the safe set is forward invariant.

Problem 2 (Gradient of Workspace CBF): Let \( h_{\text{ws}}(\mathbf{q}) = \|\mathbf{p}(\mathbf{q}) - \mathbf{c}\|^2 - R^2 \), where \( \mathbf{p}(\mathbf{q}) \) is the end-effector position and \( \mathbf{J}(\mathbf{q}) \) is its Jacobian. Derive \( \nabla h_{\text{ws}}(\mathbf{q}) \) and confirm the expression used in this lesson.

Solution: Write \( \mathbf{r}(\mathbf{q}) = \mathbf{p}(\mathbf{q}) - \mathbf{c} \). Then

\[ h_{\text{ws}}(\mathbf{q}) = \mathbf{r}(\mathbf{q})^{\top}\mathbf{r}(\mathbf{q}) - R^2. \]

Differentiating with respect to \( \mathbf{q} \),

\[ \nabla h_{\text{ws}}(\mathbf{q}) = 2\,\mathbf{r}(\mathbf{q})^{\top} \frac{\partial \mathbf{r}(\mathbf{q})}{\partial \mathbf{q}} = 2\,\big(\mathbf{p}(\mathbf{q}) - \mathbf{c}\big)^{\top} \frac{\partial \mathbf{p}(\mathbf{q})}{\partial \mathbf{q}}. \]

By definition of the geometric Jacobian, \( \frac{\partial \mathbf{p}(\mathbf{q})}{\partial \mathbf{q}} = \mathbf{J}(\mathbf{q}) \), so

\[ \nabla h_{\text{ws}}(\mathbf{q}) = 2\big(\mathbf{p}(\mathbf{q}) - \mathbf{c}\big)^{\top} \mathbf{J}(\mathbf{q}), \]

which matches the expression used earlier. The CBF time derivative is then \( \dot{h}_{\text{ws}} = \nabla h_{\text{ws}} \mathbf{v} \).

Problem 3 (Feasibility with Slack Variable): Consider the QP

\[ \begin{aligned} \min_{\mathbf{v},\delta} \quad &\frac{1}{2}\|\mathbf{v} - \mathbf{v}_{\text{nom}}\|^2 + \frac{\lambda}{2} \delta^2 \\ \text{s.t.} \quad &\mathbf{A}\mathbf{v} \le \mathbf{b} + \delta \mathbf{1}, \\ &\delta \ge 0. \end{aligned} \]

Show that this QP is always feasible for any \( \mathbf{v}_{\text{nom}} \) and any \( \mathbf{A}, \mathbf{b} \), and explain the effect of letting \( \lambda \to \infty \).

Solution: Feasibility is demonstrated by constructing at least one feasible point. Choose \( \mathbf{v} = \mathbf{v}_{\text{nom}} \). For each constraint row \( i \), \( \mathbf{a}_i^{\top} \mathbf{v}_{\text{nom}} \le b_i + \delta \) is required. Let

\[ \delta^{\ast} = \max\Big\{0, \max_i \big(\mathbf{a}_i^{\top} \mathbf{v}_{\text{nom}} - b_i\big)\Big\}. \]

Then \( \delta^{\ast} \ge 0 \) and \( \mathbf{a}_i^{\top} \mathbf{v}_{\text{nom}} - b_i \le \delta^{\ast} \) for all \( i \), so all constraints are satisfied. Hence the feasible set is non-empty. As \( \lambda \to \infty \), the optimizer is increasingly penalized for non-zero \( \delta \), so optimal solutions tend to enforce \( \delta \approx 0 \) whenever the hard constraints \( \mathbf{A}\mathbf{v} \le \mathbf{b} \) are feasible. Thus, a large \( \lambda \) approximates strict CBF constraints, while still guaranteeing feasibility when the nominal command would otherwise violate safety.

Problem 4 (Discrete-Time Approximation): Suppose the continuous-time condition \( \dot{h}(\mathbf{q},\mathbf{v}) \ge -\gamma h(\mathbf{q}) \) holds and the system is sampled with period \( T_s \). Show that a first-order Euler approximation yields

\[ h_{k+1} \approx h_k + T_s \dot{h}_k \ge h_k - \gamma T_s h_k = (1 - \gamma T_s) h_k. \]

Under what condition on \( \gamma T_s \) does this imply that \( h_k \ge 0 \) for all \( k \) if \( h_0 \ge 0 \)?

Solution: From the inequality, we have \( h_{k+1} \ge (1 - \gamma T_s) h_k \). If \( 0 \le \gamma T_s \le 1 \), then the scalar factor \( 1 - \gamma T_s \) lies in \( [0,1] \), and therefore \( h_{k+1} \ge 0 \) whenever \( h_k \ge 0 \). By induction, \( h_k \ge 0 \) for all \( k \) if \( h_0 \ge 0 \). Thus, a sufficient condition is \( \gamma T_s \le 1 \), which is consistent with intuitive requirements that the sampling period be small compared to the barrier feedback rate.

In this lab, we implemented a CBF-QP safety filter that enforces joint-limit and workspace constraints at the joint-velocity level. We derived joint and workspace CBFs, showed how to form linear inequality constraints in the velocity command, and embedded them into a quadratic program that minimally distorts an arbitrary nominal controller. We also presented implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, emphasizing how kinematic models and QP solvers interface with the safety filter loop. These ideas extend naturally to more complex robots and richer safe sets (e.g., self-avoidance, human-robot separation) while keeping the same mathematical structure.