From Lesson 1, a control barrier function (CBF) is a smooth scalar function \( h(x) \) whose super-level set \( \mathcal{C} = \{x \mid h(x) \ge 0\} \) represents the safe set. For a control-affine system \( \dot{x} = f(x) + g(x)u \), a (zeroing) CBF imposes the inequality

\[ \sup_{u \in \mathcal{U}} \left( L_f h(x) + L_g h(x) u + \alpha(h(x)) \right) \ge 0,\quad \forall x \]

for some extended class-\(\mathcal{K}\) function \( \alpha(\cdot) \). Any feedback \( u = k(x) \) that satisfies

\[ L_f h(x) + L_g h(x) k(x) + \alpha(h(x)) \ge 0,\quad \forall x \]

renders \( \mathcal{C} \) forward invariant. In CBF-QP control, instead of hand-crafting such a \( k(x) \), we:

• Design a nominal controller \( u_{\mathrm{nom}}(x) \) for tracking/performance.
• At each control step, solve a QP to find the closest control to \( u_{\mathrm{nom}}(x) \) that satisfies all CBF constraints.

\[ \begin{aligned} \min_{u \in \mathbb{R}^m}\quad & \tfrac{1}{2} (u - u_{\mathrm{nom}}(x))^\top H (u - u_{\mathrm{nom}}(x)) \\ \text{s.t.}\quad & L_f h_i(x) + L_g h_i(x) u + \alpha_i(h_i(x)) \ge 0,\quad i=1,\dots,p,\\ & u \in \mathcal{U}. \end{aligned} \]

Here \(H \succ 0\) is a weighting matrix, typically \( H = I \). Because the QP constraints exactly encode the CBF inequalities, any feasible solution defines a safety-preserving controller.

flowchart TD
  X["State x (q, qdot, task)"] --> N["Nominal controller u_nom(x)"]
  X --> CBF["CBF constraints h_i(x) >= 0"]
  N --> QP["QP: min ||u - u_nom||^2 s.t. CBF + actuation constraints"]
  CBF --> QP
  QP --> U["Safe control u*"]
  U --> ACT["Robot actuators (joints)"]
  ACT --> X
        

For an \( n \)-DOF manipulator with joint coordinates \( q \in \mathbb{R}^n \), joint velocities \( \dot{q} \), and joint torques \( u = \tau \in \mathbb{R}^n \), the standard dynamics are

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

with positive-definite inertia matrix \(M(q)\), Coriolis matrix \(C(q,\dot{q})\), and gravity vector \(g(q)\). Introducing the state \(x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix}\), we can rewrite this as a control-affine system:

\[ \dot{x} = \begin{bmatrix} \dot{q} \\ -M(q)^{-1}\left(C(q,\dot{q})\dot{q} + g(q)\right) \end{bmatrix} + \begin{bmatrix} 0 \\ M(q)^{-1} \end{bmatrix} \tau \;=\; f(x) + g(x)u. \]

In some controllers (e.g., velocity-level control), we treat joint velocity commands \( u = \dot{q}_{\mathrm{cmd}} \) as inputs and consider the reduced first-order model

\[ \dot{q} = u,\quad x := q. \]

In task space, we consider an end-effector map \( x_{\mathrm{task}} = \phi(q) \in \mathbb{R}^k \). The differential kinematics are

\[ \dot{x}_{\mathrm{task}} = J(q)\dot{q}, \]

where \( J(q) \) is the manipulator Jacobian. For velocity-level controllers, we may use task-space input \( u_{\mathrm{task}} = \dot{x}_{\mathrm{task}} \), with joint velocities constrained through \( \dot{x}_{\mathrm{task}} = J(q)\dot{q} \). CBFs can be defined either in joint coordinates or in task coordinates; the resulting CBF constraints must ultimately be enforced on the actual control variable (joint velocities or torques).

Consider one joint \( q_i \) with limits \( q_i^{\min} \) and \( q_i^{\max} \). We design two scalar CBFs:

\[ h_i^{\mathrm{low}}(q) = q_i - q_i^{\min},\quad h_i^{\mathrm{up}}(q) = q_i^{\max} - q_i. \]

The safe set for joint \(i\) is \( \mathcal{C}_i = \{q \mid h_i^{\mathrm{low}}(q) \ge 0, \; h_i^{\mathrm{up}}(q) \ge 0\} \). For the first-order model \( \dot{q} = u \), the Lie derivatives are:

\[ \dot{h}_i^{\mathrm{low}} = \nabla_q h_i^{\mathrm{low}} \dot{q} = u_i,\quad \dot{h}_i^{\mathrm{up}} = \nabla_q h_i^{\mathrm{up}} \dot{q} = -u_i. \]

Taking linear class-\(\mathcal{K}\) functions \( \alpha(h) = \gamma h \) with \( \gamma > 0 \), the CBF inequalities become

\[ \begin{aligned} &\dot{h}_i^{\mathrm{low}} + \gamma h_i^{\mathrm{low}} \ge 0 &\quad\Rightarrow\quad u_i + \gamma (q_i - q_i^{\min}) \ge 0 \;\Rightarrow\; u_i \ge -\gamma (q_i - q_i^{\min}),\\[4pt] &\dot{h}_i^{\mathrm{up}} + \gamma h_i^{\mathrm{up}} \ge 0 &\quad\Rightarrow\quad -u_i + \gamma (q_i^{\max} - q_i) \ge 0 \;\Rightarrow\; u_i \le \gamma (q_i^{\max} - q_i). \end{aligned} \]

For all joints, we stack inequalities into \( A(q)u \le b(q) \) with \( u = \dot{q}_{\mathrm{cmd}} \). Introducing joint-velocity bounds \( u^{\min} \le u \le u^{\max} \), a typical joint-space CBF-QP is

\[ \begin{aligned} \min_{u \in \mathbb{R}^n}\quad & \tfrac{1}{2}(u - u_{\mathrm{nom}}(q,\dot{q}))^\top H (u - u_{\mathrm{nom}}(q,\dot{q}))\\ \text{s.t.}\quad & u_i \ge -\gamma (q_i - q_i^{\min}), \\ & u_i \le \gamma (q_i^{\max} - q_i), \quad i=1,\dots,n,\\ & u^{\min} \le u \le u^{\max}. \end{aligned} \]

Forward invariance (sketch). Suppose \( q(0) \in \mathcal{C}_i \). If the controller \( u(t) \) satisfies both inequalities for all \( t \), then \( h_i^{\mathrm{low}}(q(t)) \) and \( h_i^{\mathrm{up}}(q(t)) \) solve differential inequalities of the form \( \dot{h} \ge -\gamma h \) with non-negative initial condition, implying \( h(t) \ge 0 \) for all \( t \ge 0 \). Thus the joint limits are never violated.

Now consider a task-space safety constraint, such as keeping the end-effector away from an obstacle or inside a workspace. Let \( x_{\mathrm{task}} = \phi(q) \in \mathbb{R}^k \) and define a CBF \( h_{\mathrm{task}}(x_{\mathrm{task}}) \), with safe set \( \mathcal{C}_{\mathrm{task}} = \{q \mid h_{\mathrm{task}}(\phi(q)) \ge 0\} \). With \( x_{\mathrm{task}} \) as state and \( \dot{x}_{\mathrm{task}} = u_{\mathrm{task}} \) as input, we have the first-order system

\[ \dot{x}_{\mathrm{task}} = u_{\mathrm{task}}. \]

The differential kinematics enforce \( u_{\mathrm{task}} = J(q)\dot{q} \), while the actual control variable in the low-level controller is typically \( u = \dot{q}_{\mathrm{cmd}} \). The CBF inequality at task level is

\[ \dot{h}_{\mathrm{task}} + \alpha(h_{\mathrm{task}}) = \nabla_{x_{\mathrm{task}}} h_{\mathrm{task}}(x_{\mathrm{task}}) \dot{x}_{\mathrm{task}} + \alpha(h_{\mathrm{task}}(x_{\mathrm{task}})) \ge 0. \]

Substituting \( \dot{x}_{\mathrm{task}} = J(q)\dot{q} \) and assigning \( u = \dot{q}_{\mathrm{cmd}} \), we get

\[ \nabla_{x_{\mathrm{task}}} h_{\mathrm{task}}(\phi(q))\, J(q)\,u + \alpha(h_{\mathrm{task}}(\phi(q))) \ge 0. \]

This has the standard affine-in-control CBF form \( a(q)^\top u + \alpha(h_{\mathrm{task}}(\phi(q))) \ge 0 \) with \( a(q)^\top := \nabla_{x_{\mathrm{task}}} h_{\mathrm{task}}(\phi(q))\,J(q) \). For multiple task-space constraints and joint constraints, all inequalities are stacked into a unified QP in the joint velocity variable \( u = \dot{q}_{\mathrm{cmd}} \).

In torque-level formulations, the mapping is more involved (relative degree > 1) and requires higher-order CBFs (HOCBFs). Those techniques build CBFs in higher derivatives of task variables and enforce inequalities on \(\tau\) through Lie derivatives of appropriate order, but the basic QP structure remains the same.

In practice, robot control stacks performance objectives (tracking) with safety constraints (CBFs). At each sampling instant:

1. Measure \( q,\dot{q} \) and compute task variables.
2. Compute nominal command \( u_{\mathrm{nom}} \) (e.g. joint-space PD or operational-space controller from previous chapters).
3. Evaluate all CBFs \( h_i \) and their gradients.
4. Formulate a joint-level QP in the actual actuator variable \(u\) that enforces all joint and task CBF inequalities.

We can conceptually separate where the constraint is defined (joint vs task space) from where the control lives (joint space or torque input), but computationally all constraints are expressed in terms of the final decision variable \(u\).

flowchart TD
  S["State (q, qdot)"] --> JSAFE["Joint constraints \nh_joint(q) >= 0"]
  S --> TSAFE["Task constraints \nh_task(phi(q)) >= 0"]
  S --> NOM["Nominal controller \n(joint or task space)"]
  NOM --> MAP["Map to joint \ninput u_nom"]
  JSAFE --> FORM["Form CBF inequalities \nin u (joint)"]
  TSAFE --> FORM
  MAP --> QP["Joint-space QP in u"]
  FORM --> QP
  QP --> USTAR["Safe u* (velocities or torques)"]
        

We implement the velocity-level joint-limit CBF-QP for a single revolute joint using cvxpy. The dynamics are \( \dot{q} = u \), nominal control is a simple PD law, and CBFs enforce joint limits \( q^{\min} \le q(t) \le q^{\max} \).

import numpy as np
import cvxpy as cp

class JointCBFQP1DOF:
    def __init__(self, q_min, q_max,
                 gamma=5.0,
                 kp=20.0, kd=5.0,
                 u_min=-2.0, u_max=2.0):
        self.q_min = q_min
        self.q_max = q_max
        self.gamma = gamma
        self.kp = kp
        self.kd = kd
        self.u_min = u_min
        self.u_max = u_max

    def nominal_velocity(self, q, qdot, q_ref, qdot_ref=0.0):
        # Simple PD in velocity space (tracks q_ref)
        e = q_ref - q
        edot = qdot_ref - qdot
        return self.kp * e + self.kd * edot

    def cbf_bounds(self, q):
        # h_low = q - q_min, h_up = q_max - q
        h_low = q - self.q_min
        h_up = self.q_max - q

        # alpha(h) = gamma * h
        alpha_low = self.gamma * h_low
        alpha_up = self.gamma * h_up

        # CBF inequalities:
        # u >= -alpha_low, u <= alpha_up
        return -alpha_low, alpha_up

    def solve_qp(self, q, qdot, q_ref):
        u_nom = self.nominal_velocity(q, qdot, q_ref)

        u = cp.Variable()  # scalar joint velocity
        alpha_low, alpha_up = self.cbf_bounds(q)

        constraints = []
        # CBF constraints
        constraints += [u >= alpha_low]   # note: alpha_low = -gamma (q - q_min)
        constraints += [u <= alpha_up]
        # actuator limits
        constraints += [u >= self.u_min, u <= self.u_max]

        # Minimize (u - u_nom)^2
        objective = cp.Minimize(0.5 * cp.square(u - u_nom))
        prob = cp.Problem(objective, constraints)
        prob.solve(solver=cp.OSQP, warm_start=True)

        if u.value is None:
            # Fall back to saturated nominal if QP fails
            u_safe = np.clip(u_nom, self.u_min, self.u_max)
        else:
            u_safe = float(u.value)
        return u_safe

# Example usage in a simple integration loop
if __name__ == "__main__":
    dt = 0.01
    q_min, q_max = -1.0, 1.0
    controller = JointCBFQP1DOF(q_min, q_max)
    q, qdot = 0.0, 0.0
    q_ref = 0.9  # reference position inside limits

    for k in range(1000):
        u_safe = controller.solve_qp(q, qdot, q_ref)
        # Forward Euler integration of qdot = u
        qdot = u_safe
        q += dt * qdot
        # Here you would log/plot q, qdot, and u_safe
      

Extending this to \( n \) joints simply replaces u by an \( n \)-dimensional vector, stacks the inequalities for each joint, and uses cp.Variable(n) with a quadratic objective \( \|u - u_{\mathrm{nom}}\|^2 \).

We now sketch a multi-DOF joint-space CBF-QP in C++ using Eigen and qpOASES. We consider \( n \) joints, joint velocity input \( u \in \mathbb{R}^n \), and stacked CBF constraints for joint limits as in Section 3.

#include <Eigen/Dense>
#include <qpOASES.hpp>

using Eigen::VectorXd;
using Eigen::MatrixXd;

class JointCBFQPMultiDOF {
public:
    JointCBFQPMultiDOF(const VectorXd& q_min,
                       const VectorXd& q_max,
                       double gamma,
                       const VectorXd& u_min,
                       const VectorXd& u_max)
    : q_min_(q_min), q_max_(q_max),
      gamma_(gamma),
      u_min_(u_min), u_max_(u_max),
      n_(q_min.size()),
      qp_(n_, 2*n_) // n vars, 2n inequality constraints
    {
        H_.setIdentity(n_, n_); // objective 0.5 * (u - u_nom)^T (u - u_nom)
    }

    VectorXd solveQP(const VectorXd& q,
                     const VectorXd& u_nom)
    {
        using namespace qpOASES;

        // Build QP: min 0.5 u^T H u + g^T u
        // with H = 2I, g = -2 u_nom (so objective = ||u - u_nom||^2)
        MatrixXd H = 2.0 * H_;
        VectorXd g = -2.0 * u_nom;

        // CBF-based bounds: u_i >= -gamma (q_i - q_min_i)
        //                    u_i <=  gamma (q_max_i - q_i)
        VectorXd cbf_lb(n_), cbf_ub(n_);
        cbf_lb = -gamma_ * (q - q_min_);
        cbf_ub =  gamma_ * (q_max_ - q);

        // Inequalities of form A u <= b
        // We use:
        //   u_i <= cbf_ub_i
        //  -u_i <= -cbf_lb_i  (equivalently u_i >= cbf_lb_i)
        MatrixXd A(2*n_, n_);
        VectorXd b(2*n_);
        A.setZero();
        b.setZero();
        // Upper CBF bounds
        A.topRows(n_) =  MatrixXd::Identity(n_, n_);
        b.head(n_)   =  cbf_ub;
        // Lower CBF bounds
        A.bottomRows(n_) = -MatrixXd::Identity(n_, n_);
        b.tail(n_)       = -cbf_lb;

        // Variable bounds (actuator limits)
        VectorXd lb = u_min_;
        VectorXd ub = u_max_;

        // Convert Eigen to qpOASES arrays
        real_t* H_qp = const_cast<real_t*>(H.data());
        real_t* g_qp = const_cast<real_t*>(g.data());
        real_t* A_qp = const_cast<real_t*>(A.data());
        real_t* lb_qp = const_cast<real_t*>(lb.data());
        real_t* ub_qp = const_cast<real_t*>(ub.data());
        real_t* lbA_qp = nullptr;
        real_t* ubA_qp = b.data(); // lower bounds for A u, here  -inf < A u <= b

        // For simplicity, we set lower bounds on A u to -inf
        VectorXd lbA_vec = VectorXd::Constant(2*n_, -1e9);
        lbA_qp = lbA_vec.data();

        int nWSR = 20;
        qp_.init(H_qp, g_qp, A_qp,
                 lb_qp, ub_qp,
                 lbA_qp, ubA_qp,
                 nWSR);

        VectorXd u_opt(n_);
        qp_.getPrimalSolution(u_opt.data());
        return u_opt;
    }

private:
    VectorXd q_min_, q_max_;
    double gamma_;
    VectorXd u_min_, u_max_;
    int n_;
    MatrixXd H_;
    qpOASES::QProblem qp_;
};
      

In a real system, one would reuse the same QProblem object across timesteps, warm-start the solver, and add more rows to A, b for task-space CBFs or other inequality constraints.

8.1 Java with ojAlgo (1-DOF example)

Java does not have a standard QP library in the core language, but packages like ojAlgo provide quadratic optimization capabilities. Below is a 1-DOF joint-limit CBF-QP using ExpressionsBasedModel.

import org.ojalgo.optimisation.ExpressionsBasedModel;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.Variable;
import org.ojalgo.optimisation.Expression;

public class JointCBFQP1DOF {

    private final double qMin, qMax, gamma, uMin, uMax;

    public JointCBFQP1DOF(double qMin, double qMax,
                          double gamma,
                          double uMin, double uMax) {
        this.qMin = qMin;
        this.qMax = qMax;
        this.gamma = gamma;
        this.uMin = uMin;
        this.uMax = uMax;
    }

    private double nominalVelocity(double q, double qdot,
                                   double qRef) {
        double kp = 20.0;
        double kd = 5.0;
        double e = qRef - q;
        double edot = -qdot;
        return kp * e + kd * edot;
    }

    public double solveQP(double q, double qdot, double qRef) {
        double uNom = nominalVelocity(q, qdot, qRef);

        // CBF bounds
        double cbfLower = -gamma * (q - qMin);    // u >= cbfLower
        double cbfUpper =  gamma * (qMax - q);    // u <= cbfUpper

        ExpressionsBasedModel model = new ExpressionsBasedModel();

        Variable u = Variable.make("u")
                .lower(uMin).upper(uMax)
                .weight(1.0); // for quadratic objective scaling
        model.addVariable(u);

        // Objective: minimize (u - uNom)^2
        Expression objective = model.addExpression("objective");
        objective.setQuadraticFactor(u, u, 1.0);
        objective.setLinearFactor(u, -2.0 * uNom);
        objective.weight(1.0);

        // CBF constraints: cbfLower <= u <= cbfUpper
        Expression lowerCbf = model.addExpression("cbfLower");
        lowerCbf.setLinearFactor(u, 1.0);
        lowerCbf.lower(cbfLower);

        Expression upperCbf = model.addExpression("cbfUpper");
        upperCbf.setLinearFactor(u, 1.0);
        upperCbf.upper(cbfUpper);

        Optimisation.Result result = model.minimise();
        if (result.getState().isFeasible()) {
            return result.get(u).doubleValue();
        } else {
            // Fallback to saturated nominal
            double uSat = Math.max(uMin, Math.min(uMax, uNom));
            return uSat;
        }
    }
}
      

8.2 MATLAB/Simulink with quadprog

In MATLAB, quadprog solves QPs of the form \( \min \frac{1}{2}u^\top H u + f^\top u \) subject to linear inequalities \( A u \le b \) and bounds. The following script implements the 1-DOF joint-limit CBF-QP.

function u_safe = joint_cbf_qp_1dof(q, qdot, q_ref, params)
    % params: struct with fields q_min, q_max, gamma, u_min, u_max, kp, kd

    q_min = params.q_min;
    q_max = params.q_max;
    gamma = params.gamma;
    u_min = params.u_min;
    u_max = params.u_max;
    kp = params.kp;
    kd = params.kd;

    % Nominal PD velocity
    e = q_ref - q;
    edot = -qdot;
    u_nom = kp * e + kd * edot;

    % CBF bounds
    cbf_lower = -gamma * (q - q_min);  % u >= cbf_lower
    cbf_upper =  gamma * (q_max - q);  % u <= cbf_upper

    % QP: min (u - u_nom)^2
    H = 2;           % scalar (2 * 1)
    f = -2 * u_nom;  % so objective = u^2 - 2 u_nom u + const

    % Inequalities A u <= b
    A = [ 1;   % u <= cbf_upper
         -1]; % -u <= -cbf_lower  (u >= cbf_lower)
    b = [cbf_upper; -cbf_lower];

    % Bounds
    lb = u_min;
    ub = u_max;

    options = optimoptions('quadprog', 'Display', 'off');
    [u_opt,~,exitflag] = quadprog(H, f, A, b, [], [], lb, ub, [], options);

    if exitflag <= 0
        u_safe = max(u_min, min(u_max, u_nom));
    else
        u_safe = u_opt;
    end
end
      

In Simulink, one can wrap the joint_cbf_qp_1dof function in a MATLAB Function block, feed in q, qdot, and q_ref, and connect the block output to the joint-level velocity command, with the robot simulator providing q, qdot feedback.

Mathematica can solve QPs via QuadraticOptimization. For the 1-DOF CBF-QP:

(* Parameters *)
qMin = -1.0;
qMax =  1.0;
gamma = 5.0;
uMin = -2.0;
uMax =  2.0;
kp = 20.0;
kd = 5.0;

(* Nominal controller *)
nominalVelocity[q_, qdot_, qRef_] :=
  Module[{e, edot},
    e = qRef - q;
    edot = -qdot;
    kp*e + kd*edot
  ];

jointCBFQP1DOF[q_, qdot_, qRef_] :=
 Module[{u, uNom, cbfLower, cbfUpper, objective, constraints, sol},
  uNom = nominalVelocity[q, qdot, qRef];

  (* CBF bounds *)
  cbfLower = -gamma*(q - qMin);  (* u >= cbfLower *)
  cbfUpper =  gamma*(qMax - q);  (* u <= cbfUpper *)

  (* Objective (u - uNom)^2 = 1/2 (2) (u - uNom)^2 *)
  objective = 1/2 (u - uNom)^2;

  constraints = {
    cbfLower <= u <= cbfUpper,
    uMin <= u <= uMax
  };

  sol = Quiet@Check[
    QuadraticOptimization[objective, constraints, u],
    $Failed
  ];

  If[sol === $Failed,
    (* Fallback: saturated nominal *)
    Max[uMin, Min[uMax, uNom]],
    sol
  ]
 ]

(* Example call *)
q = 0.0;
qdot = 0.0;
qRef = 0.9;
uSafe = jointCBFQP1DOF[q, qdot, qRef];
      

For multi-DOF manipulators, one defines u as a vector, constructs a quadratic objective \( (u - u_{\mathrm{nom}})^\top (u - u_{\mathrm{nom}}) \), and stacks CBF inequalities and bounds accordingly.

Problem 1 (Joint-Limit CBF-QP Derivation). Consider a single joint obeying \( \dot{q} = u \) with limits \( q^{\min} \), \( q^{\max} \). Let \( h^{\mathrm{low}}(q) = q - q^{\min} \), \( h^{\mathrm{up}}(q) = q^{\max} - q \), and \( \alpha(h) = \gamma h \) with \( \gamma > 0 \). Show that the CBF-QP

\[ \begin{aligned} \min_{u \in \mathbb{R}} \quad & (u - u_{\mathrm{nom}})^2\\ \text{s.t.}\quad & u \ge -\gamma (q - q^{\min}),\\ & u \le \gamma (q^{\max} - q) \end{aligned} \]

guarantees that any trajectory starting from \( q(0) \in [q^{\min}, q^{\max}] \) remains in that interval for all time.

Solution. The CBF invariance theorem states that if for all \( t \ge 0 \) the control satisfies

\[ \dot{h}^{\mathrm{low}} + \gamma h^{\mathrm{low}} \ge 0,\quad \dot{h}^{\mathrm{up}} + \gamma h^{\mathrm{up}} \ge 0, \]

then the set where both CBFs are non-negative is forward invariant. Here \( \dot{h}^{\mathrm{low}} = u \) and \( \dot{h}^{\mathrm{up}} = -u \), so the inequalities become

\[ u + \gamma (q - q^{\min}) \ge 0,\quad -u + \gamma (q^{\max} - q) \ge 0. \]

These are equivalent to the constraints in the QP. The QP always chooses \( u \) satisfying both (if feasible), so along the solution we have \( \dot{h}^{\mathrm{low}} \ge -\gamma h^{\mathrm{low}} \), \( \dot{h}^{\mathrm{up}} \ge -\gamma h^{\mathrm{up}} \). Solving these differential inequalities gives \( h(t) \ge e^{-\gamma t} h(0) \ge 0 \) when \( h(0) \ge 0 \). Thus \( h^{\mathrm{low}}(q(t)) \ge 0 \) and \( h^{\mathrm{up}}(q(t)) \ge 0 \) for all \( t \ge 0 \), i.e., \( q^{\min} \le q(t) \le q^{\max} \).

Problem 2 (Task-Space CBF Constraint in Joint Velocities). Let \( x = \phi(q) \in \mathbb{R}^3 \) denote the end-effector position, and suppose we want to keep the end-effector at least distance \( d_{\min} \) away from an obstacle at position \( x_{\mathrm{obs}} \). Define

\[ h_{\mathrm{task}}(x) = \|x - x_{\mathrm{obs}}\|^2 - d_{\min}^2. \]

With input \( u = \dot{q} \) and \( \dot{x} = J(q)\dot{q} \), derive the inequality on \( u \) corresponding to the CBF condition \( \dot{h}_{\mathrm{task}} + \gamma h_{\mathrm{task}} \ge 0 \).

Solution. First compute the gradient

\[ \nabla_x h_{\mathrm{task}}(x) = 2(x - x_{\mathrm{obs}})^\top. \]

Thus

\[ \dot{h}_{\mathrm{task}} = \nabla_x h_{\mathrm{task}}(x)\,\dot{x} = 2(x - x_{\mathrm{obs}})^\top J(q) u. \]

The CBF inequality \( \dot{h}_{\mathrm{task}} + \gamma h_{\mathrm{task}} \ge 0 \) becomes

\[ 2(x - x_{\mathrm{obs}})^\top J(q) u + \gamma\left(\|x - x_{\mathrm{obs}}\|^2 - d_{\min}^2\right) \ge 0. \]

This is an affine inequality in \( u \) of the form \( a(q,x)^\top u + b(q,x) \ge 0 \), where \( a(q,x)^\top = 2(x - x_{\mathrm{obs}})^\top J(q) \) and \( b(q,x) = \gamma(\|x - x_{\mathrm{obs}}\|^2 - d_{\min}^2) \).

Problem 3 (Forward Invariance for Multiple CBFs). Let \( \mathcal{C}_i = \{x \mid h_i(x) \ge 0\} \), \( i = 1,\dots,p \), be safe sets with corresponding CBFs. Assume a controller \( u(x) \) satisfies \( \dot{h}_i(x) + \alpha_i(h_i(x)) \ge 0 \) for all \( i \). Show that the intersection \( \mathcal{C} = \bigcap_i \mathcal{C}_i \) is forward invariant.

Solution. Each inequality \( \dot{h}_i + \alpha_i(h_i) \ge 0 \), together with \( h_i(0) \ge 0 \), implies \( h_i(t) \ge 0 \) for all \( t \ge 0 \) by the comparison lemma and the same exponential lower bound argument used in Problem 1. Therefore any trajectory starting in \( \mathcal{C}_i \) never leaves \( \mathcal{C}_i \). If \( x(0) \in \mathcal{C} \), then \( x(0) \in \mathcal{C}_i \) for all \( i \), so \( x(t) \in \mathcal{C}_i \) for all \( t \ge 0 \) and all \( i \), hence \( x(t) \in \mathcal{C} \) for all \( t \ge 0 \).

Problem 4 (Design Flow for Joint + Task-Space CBF-QP). Outline the design steps for a controller that enforces both joint limits and task-space distance constraints using a single joint-level QP.

Solution (flow outline).

flowchart TD
  START["Start with robot model (q, qdot)"] --> J["Specify joint limits"]
  START --> T["Specify task safety h_task(phi(q))"]
  J --> HJ["Define joint CBFs h_joint_i(q)"]
  T --> HT["Define task CBFs h_task_j(phi(q))"]
  HJ --> LIE["Compute derivatives wrt joint input u"]
  HT --> LIE
  LIE --> QPFORM["Form affine inequalities in u"]
  QPFORM --> NOM["Design nominal controller u_nom(q, qdot)"]
  NOM --> SOLVE["Build QP: min ||u - u_nom||^2 with all inequalities"]
  QPFORM --> SOLVE
  SOLVE --> CTRL["Implement solver in control loop"]
        

Problem 5 (Feasibility Considerations). For the joint-limit CBF-QP in Problem 1, assume \( q \) is exactly at the upper limit \( q^{\max} \). Show what the CBF constraints reduce to, and discuss why the QP remains feasible and what control is generated.

Solution. At \( q = q^{\max} \), we have \( h^{\mathrm{up}} = 0 \) and \( h^{\mathrm{low}} = q^{\max} - q^{\min} \ge 0 \). The inequalities become

\[ u \ge -\gamma (q^{\max} - q^{\min}),\quad u \le \gamma (q^{\max} - q^{\max}) = 0. \]

Thus any \( u \le 0 \) satisfying the lower bound is allowed; in particular, \( u = 0 \) and negative values that move the joint away from the limit are feasible. The QP picks the value closest to \( u_{\mathrm{nom}} \) subject to \( u \le 0 \). If the nominal control tries to push beyond the limit (\( u_{\mathrm{nom}} > 0 \)), the QP will clip it to \( u = 0 \) or to a small negative value if that is closer while remaining feasible.

In this lesson, we embedded control barrier functions in QP-based controllers for joint and task space. Starting from robot control-affine models, we derived explicit CBF inequalities for joint limits and task-space distance constraints, and showed how to stack them into a single QP that minimally deviates from a nominal controller while guaranteeing forward invariance of safety sets. We also presented implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica, highlighting how CBF-QP controllers can be realized in real-time robot control software.

The next lesson will address safety under input limits and how to guarantee feasibility and robustness of CBF-QP controllers when actuation is constrained.