Consider a robot with state \( x \in \mathbb{R}^n \) and control input \( u \in \mathbb{R}^m \), modeled in the control-affine form introduced in the previous lesson:

\[ \dot{x} = f(x) + g(x)u , \]

A safe set is a subset \( \mathcal{C} \subset \mathbb{R}^n \) of the state space such that, if the robot starts in \( \mathcal{C} \) and the controller respects certain constraints, then \( x(t) \in \mathcal{C} \) for all future times. Formally, a set \( \mathcal{C} \) is forward invariant if

\[ x(0) \in \mathcal{C} \;\Rightarrow\; x(t) \in \mathcal{C}\ \text{for all}\ t \ge 0 . \]

Control barrier functions (CBFs) represent such sets via a scalar function \( h:\mathbb{R}^n \to \mathbb{R} \):

\[ \mathcal{C} = \{ x \in \mathbb{R}^n \mid h(x) \ge 0 \}, \quad \partial \mathcal{C} = \{ x \mid h(x) = 0 \}, \quad \mathrm{Int}(\mathcal{C}) = \{ x \mid h(x) > 0 \}. \]

The CBF condition (from Lesson 1) guarantees that a suitable control input can keep the system inside \( \mathcal{C} \). For relative degree 1 constraints, a (zeroing) CBF is a function \( h \in C^1 \) that satisfies, for some class-\( \mathcal{K} \) function \( \alpha \),

\[ \sup_{u \in \mathbb{R}^m} \bigl( L_f h(x) + L_g h(x) u + \alpha(h(x)) \bigr) \ge 0 \quad \text{for all } x \in \mathcal{C}. \]

In this lesson, we do not design the controller yet; instead, we design the safe set itself, i.e., the function \( h(x) \), so that:

• It accurately encodes physical safety requirements (joint limits, distances, etc.),
• It is sufficiently smooth (usually \( C^1 \) or \( C^2 \)),
• It is compatible with the CBF condition and the robot dynamics.

flowchart TD
  S["High-level safety spec (e.g. 'no collision', 'respect joint limits'"] --> G["Geometric constraint in joint/task space"]
  G --> H["Design smooth constraint function h(x)"]
  H --> CBF["Check CBF & relative-degree conditions"]
  CBF --> Cset["Safe set C = {x | h(x) >= 0}"]
  Cset --> CTRL["Used by safety filter / CBF-QP (Lesson 3)"]
        

Let the robot state be \( x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \), where \( q \in \mathbb{R}^{n_q} \) is the vector of joint angles, and consider the standard manipulator dynamics:

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

where \( M(q) \) is the inertia matrix, \( C(q,\dot{q}) \) the Coriolis/centrifugal terms, \( g(q) \) the gravity vector, and \( B \) the actuation matrix. In state-space form, this is control-affine:

\[ \dot{x} = \begin{bmatrix} \dot{q} \\ M(q)^{-1}\bigl( -C(q,\dot{q})\dot{q} - g(q) + B u \bigr) \end{bmatrix} = f(x) + g(x)u. \]

Most safety constraints are naturally expressed in terms of positions \( q \) and possibly task-space positions \( p(q) \), while the dynamics are second-order. The generic safe set will have the form

\[ \mathcal{C} = \bigcap_{i=1}^N \mathcal{C}_i, \quad \mathcal{C}_i = \{ x \in \mathbb{R}^n \mid h_i(x) \ge 0 \}, \]

where each \( h_i \) encodes a particular safety requirement, such as a joint limit or minimum distance from an obstacle. A well-designed \( h_i \) should satisfy:

• Consistency: \( h_i(x) \ge 0 \) if and only if the requirement is satisfied.
• Regularity on the boundary: \( \nabla h_i(x) \neq 0 \) on \( \{x \mid h_i(x) = 0\} \).
• Smoothness: \( h_i \) is at least \( C^1 \) (often \( C^2 \) for second-order systems).

These conditions ensure that the CBF inequalities are well-defined and that the safe set boundary is a smooth manifold, which simplifies both analysis and numerical optimization in later lessons.

3.1 Joint Limit Constraints

A common safety requirement is to keep each joint angle within manufacturer-specified limits:

\[ q_i^{\min} \le q_i \le q_i^{\max}, \quad i = 1,\dots,n_q. \]

For each joint \( i \), define two scalar functions:

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

The safe set corresponding to joint \( i \) is \( \mathcal{C}_i = \{ x \mid h_i^{\mathrm{low}}(q) \ge 0,\, h_i^{\mathrm{up}}(q) \ge 0 \} \). Both functions are affine and \( C^\infty \), and their gradients are nonzero on the boundary:

\[ \nabla_q h_i^{\mathrm{low}}(q) = e_i, \qquad \nabla_q h_i^{\mathrm{up}}(q) = -e_i, \]

where \( e_i \) is the \( i \)-th canonical basis vector. The overall joint-safe set is the intersection over all joints and both bounds.

3.2 Task-Space Workspace Boundaries

Let \( p(q) \in \mathbb{R}^3 \) be the end-effector position given by forward kinematics. Suppose we want to constrain \( p(q) \) to remain within a ball of radius \( R \) around a center \( p_c \):

\[ \| p(q) - p_c \|^2 \le R^2. \]

A natural choice of barrier function is

\[ h_{\mathrm{ws}}(q) = R^2 - \| p(q) - p_c \|^2. \]

Then the workspace-safe set is \( \mathcal{C}_{\mathrm{ws}} = \{ x \mid h_{\mathrm{ws}}(q) \ge 0 \} \). Its gradient with respect to \( q \) is

\[ \nabla_q h_{\mathrm{ws}}(q) = -2 J(q)^\top (p(q) - p_c), \]

where \( J(q) = \frac{\partial p}{\partial q} \) is the manipulator Jacobian. On the boundary \( \|p(q) - p_c\|^2 = R^2 \), the gradient is nonzero unless the Jacobian loses rank and the motion of the end-effector becomes tangent to the sphere in a singular way. Such singular configurations are already known from kinematics and can be excluded by additional safe sets.

3.3 Obstacle Avoidance in Task Space

Consider a spherical obstacle with center \( c \in \mathbb{R}^3 \) and radius \( r_{\mathrm{obs}} \). To maintain a safety margin \( d_{\min} > 0 \), we require

\[ \| p(q) - c \| \ge r_{\mathrm{obs}} + d_{\min}. \]

Equivalently,

\[ \| p(q) - c \|^2 - (r_{\mathrm{obs}} + d_{\min})^2 \ge 0. \]

Define the obstacle-avoidance barrier function

\[ h_{\mathrm{obs}}(q) = \| p(q) - c \|^2 - (r_{\mathrm{obs}} + d_{\min})^2. \]

Then \( \mathcal{C}_{\mathrm{obs}} = \{ x \mid h_{\mathrm{obs}}(q) \ge 0 \} \) describes a forbidden ball in task space. Its gradient is

\[ \nabla_q h_{\mathrm{obs}}(q) = 2 J(q)^\top (p(q) - c). \]

Intuitively, \( \nabla_q h_{\mathrm{obs}} \) points away from the obstacle, and the CBF constraint used later will enforce that the end-effector velocity has a component that does not decrease \( h_{\mathrm{obs}} \) too fast.

A realistic robot must satisfy many simultaneous safety constraints: joint limits, workspace limits, obstacle avoidance, self-collision avoidance, singularity avoidance, etc. Suppose we have functions \( h_1,\dots,h_N \) with associated sets \( \mathcal{C}_i = \{ x \mid h_i(x) \ge 0 \} \).

4.1 Intersection of Constraints

The most common situation is that all constraints must be enforced:

\[ \mathcal{C} = \bigcap_{i=1}^N \mathcal{C}_i = \bigcap_{i=1}^N \{ x \mid h_i(x) \ge 0 \}. \]

In later lessons, we will enforce each inequality separately in a CBF-QP (quadratic program), but even at the safe-set design stage it is useful to reason about how these sets interact. For example:

• If the intersection is empty, the specification is infeasible and must be relaxed.
• If two constraints are redundant (e.g., a joint limit always implies a workspace bound), one can be removed to simplify optimization.

A single composite barrier can also be defined by

\[ h_{\min}(x) = \min_{i=1,\dots,N} h_i(x), \]

which satisfies \( \mathcal{C} = \{ x \mid h_{\min}(x) \ge 0 \} \). However, \( h_{\min} \) is nonsmooth where two \( h_i \) are equal, so directly using \( h_{\min} \) as a CBF is delicate. This is why, in practice, one keeps the individual \( h_i \) and imposes inequalities per constraint.

4.2 Smooth Approximations of Set Operations

To maintain smoothness, we can use soft-min and soft-max operators. For intersection (all constraints active), define a soft-min:

\[ h_{\mathrm{softmin}}(x) = -\frac{1}{\kappa} \log\!\left( \sum_{i=1}^N e^{-\kappa h_i(x)} \right), \quad \kappa > 0. \]

As \( \kappa \to +\infty \), this approaches \( \min_i h_i(x) \), but for finite \( \kappa \), it is \( C^\infty \). Similarly, a union \( \mathcal{C} = \bigcup_i \mathcal{C}_i \) can be approximated by a soft-max:

\[ h_{\mathrm{softmax}}(x) = \frac{1}{\kappa} \log\!\left( \sum_{i=1}^N e^{\kappa h_i(x)} \right). \]

Whether such smooth composites are used directly or not, they are useful for analysis and for constructing higher-level safety metrics.

flowchart TD
  A["Atomic constraints h_i(x) >= 0"] --> B["Check feasibility of intersection"]
  B --> C["C = intersection of all C_i"]
  A --> D["Optionally build smooth composite \n(softmin/softmax)"]
  C --> E["Used for hard safety (must satisfy all)"]
  D --> F["Used for soft metrics or shaping"]
        

For the manipulator dynamics, most position-based safety constraints have relative degree 2 with respect to the input \( u \): the control does not appear in \( \dot{h}(x) \) but only in \( \ddot{h}(x) \). For example, for a joint limit constraint \( h(q) = q_i^{\max} - q_i \), we have

\[ \dot{h}(q,\dot{q}) = -\dot{q}_i, \]

which does not contain \( u \). To enforce safety with CBFs, we must use a high-order control barrier function (HOCBF) or exponential CBF. A typical construction is:

\[ \begin{aligned} h_0(x) &= h(q), \\ h_1(x) &= \dot{h}_0(x) + \alpha_1(h_0(x)), \\ h_2(x) &= \dot{h}_1(x) + \alpha_2(h_1(x)), \end{aligned} \]

where \( \alpha_1, \alpha_2 \) are class-\( \mathcal{K} \) functions (often linear). For a second-order system, we impose the CBF inequality on \( h_1 \) or \( h_2 \):

\[ L_f h_1(x) + L_g h_1(x) u + \alpha_2(h_1(x)) \ge 0. \]

The design of the safe set remains governed by \( h_0(q) \ge 0 \), but the dynamics of the constraint are shaped by \( \alpha_1, \alpha_2 \). Different choices produce different tradeoffs between how aggressively the system slows down when approaching the boundary.

For a scalar constraint \( h(q) \), a simple linear choice \( \alpha_1(s) = k_1 s \), \( \alpha_2(s) = k_2 s \) with \( k_1, k_2 > 0 \) yields:

\[ \begin{aligned} h_0(q) &= h(q), \\ h_1(q,\dot{q}) &= \dot{h}(q,\dot{q}) + k_1 h(q), \\ L_f h_1(x) + L_g h_1(x) u + k_2 h_1(x) &\ge 0. \end{aligned} \]

Under mild regularity conditions, if these inequalities hold and the system starts in \( \{h_0 \ge 0,\, h_1 \ge 0\} \), then the original safe set \( \{ h_0 \ge 0 \} \) is forward invariant. This links the kinematic description of safety to the dynamical ability of the robot to obey it.

Real controllers are implemented digitally with sampling period \( T_s > 0 \). Let \( x_k = x(k T_s) \) and let the controller compute \( u_k \) at time \( k T_s \). A common approximation is

\[ x_{k+1} \approx x_k + T_s \bigl( f(x_k) + g(x_k) u_k \bigr). \]

Even if the continuous-time CBF condition guarantees invariance for the exact dynamics, discretization introduces an error of order \( T_s^2 \). To compensate, we often design a tightened safe set \( \mathcal{C}_\delta \subset \mathcal{C} \) such that, if \( x_k \in \mathcal{C}_\delta \), then the discrete-time evolution maintains \( x_{k+1} \in \mathcal{C} \).

A simple approach is to shrink each constraint by a margin depending on the worst-case change of \( h_i(x) \) in one sample:

\[ h_i^\delta(x) = h_i(x) - \eta_i, \quad \eta_i \approx \sup_{\substack{x \in \mathcal{C} \\ u \in \mathcal{U}}} \bigl| T_s \dot{h}_i(x,u) \bigr|. \]

This is conservative but conceptually clean: the controller keeps the tightened inequality \( h_i^\delta(x_k) \ge 0 \), and the real state stays inside the original set \( \{h_i \ge 0\} \) despite discretization errors.

We now implement basic safe-set computations for a 2-DOF planar manipulator. The state is \( x = [q_1, q_2, \dot{q}_1, \dot{q}_2]^\top \). We consider:

• Joint limits \( q_i^{\min} \le q_i \le q_i^{\max} \),
• An obstacle in task space with center \( c \in \mathbb{R}^2 \) and radius \( r_{\mathrm{obs}} \),
• A safety margin \( d_{\min} \) around the obstacle.

We assume the planar end-effector position is \( p(q) = [x(q),\, y(q)]^\top \) with FK already implemented from earlier robotics courses.

7.1 Python Implementation (NumPy)

import numpy as np

def forward_kinematics_2d(q, l1, l2):
    """
    Simple 2-DOF planar arm in R^2.
    q: array-like, shape (2,)
    l1, l2: link lengths
    """
    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 joint_limit_barrier(q, q_min, q_max, margin=0.0):
    """
    Elementwise joint limit barrier:
    h_low[i] = q[i] - (q_min[i] + margin)
    h_up[i]  = (q_max[i] - margin) - q[i]
    Returns concatenated h vector.
    """
    q = np.asarray(q)
    q_min = np.asarray(q_min)
    q_max = np.asarray(q_max)
    h_low = q - (q_min + margin)
    h_up = (q_max - margin) - q
    return np.concatenate([h_low, h_up])

def obstacle_barrier(q, l1, l2, c, r_obs, d_min):
    """
    Obstacle avoidance barrier:
    h_obs(q) = ||p(q) - c||^2 - (r_obs + d_min)^2
    """
    p = forward_kinematics_2d(q, l1, l2)
    c = np.asarray(c)
    dist2 = np.dot(p - c, p - c)
    rho = r_obs + d_min
    return dist2 - rho**2

def is_safe(q, q_min, q_max, l1, l2, c, r_obs, d_min, margin=0.0, tol=0.0):
    """
    Check whether state q satisfies all constraints:
    joint limits and obstacle avoidance.
    """
    h_joints = joint_limit_barrier(q, q_min, q_max, margin=margin)
    h_obs = obstacle_barrier(q, l1, l2, c, r_obs, d_min)
    # all constraints must be nonnegative (with tolerance)
    return np.all(h_joints >= -tol) and (h_obs >= -tol)

# Example usage:
if __name__ == "__main__":
    q = np.array([0.0, 0.0])
    q_min = np.array([-1.0, -1.0])
    q_max = np.array([1.0, 1.0])
    l1 = 0.8
    l2 = 0.6
    c = np.array([0.8, 0.0])
    r_obs = 0.1
    d_min = 0.05

    print("Joint barrier:", joint_limit_barrier(q, q_min, q_max))
    print("Obstacle barrier:", obstacle_barrier(q, l1, l2, c, r_obs, d_min))
    print("Is safe?", is_safe(q, q_min, q_max, l1, l2, c, r_obs, d_min))
      

7.2 C++ Implementation (Eigen)

#include <iostream>
#include <Eigen/Dense>

using Eigen::Vector2d;
using Eigen::VectorXd;

Vector2d forwardKinematics2D(const Vector2d& q, double l1, double l2) {
    double q1 = q(0);
    double q2 = q(1);
    double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
    return Vector2d(x, y);
}

VectorXd jointLimitBarrier(const Vector2d& q,
                           const Vector2d& q_min,
                           const Vector2d& q_max,
                           double margin = 0.0) {
    Vector2d h_low = q - (q_min + Vector2d::Constant(margin));
    Vector2d h_up  = (q_max - Vector2d::Constant(margin)) - q;
    VectorXd h(4);
    h.segment<2>(0) = h_low;
    h.segment<2>(2) = h_up;
    return h;
}

double obstacleBarrier(const Vector2d& q,
                       double l1, double l2,
                       const Vector2d& c,
                       double r_obs, double d_min) {
    Vector2d p = forwardKinematics2D(q, l1, l2);
    Vector2d diff = p - c;
    double dist2 = diff.dot(diff);
    double rho = r_obs + d_min;
    return dist2 - rho * rho;
}

bool isSafe(const Vector2d& q,
            const Vector2d& q_min,
            const Vector2d& q_max,
            double l1, double l2,
            const Vector2d& c,
            double r_obs, double d_min,
            double margin = 0.0,
            double tol = 0.0) {
    VectorXd h_joints = jointLimitBarrier(q, q_min, q_max, margin);
    double h_obs = obstacleBarrier(q, l1, l2, c, r_obs, d_min);
    for (int i = 0; i < h_joints.size(); ++i) {
        if (h_joints(i) < -tol) {
            return false;
        }
    }
    if (h_obs < -tol) {
        return false;
    }
    return true;
}

int main() {
    Vector2d q(0.0, 0.0);
    Vector2d q_min(-1.0, -1.0);
    Vector2d q_max( 1.0,  1.0);
    double l1 = 0.8;
    double l2 = 0.6;
    Vector2d c(0.8, 0.0);
    double r_obs = 0.1;
    double d_min = 0.05;

    VectorXd h_joints = jointLimitBarrier(q, q_min, q_max);
    double h_obs = obstacleBarrier(q, l1, l2, c, r_obs, d_min);

    std::cout << "h_joints = " << h_joints.transpose() << std::endl;
    std::cout << "h_obs = " << h_obs << std::endl;
    std::cout << "Is safe? " << isSafe(q, q_min, q_max, l1, l2, c,
                                            r_obs, d_min) << std::endl;
    return 0;
}
      

7.3 Java Implementation

public class SafeSet2D {

    public static double[] forwardKinematics2D(double[] q, double l1, double l2) {
        double q1 = q[0];
        double 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[] jointLimitBarrier(double[] q,
                                             double[] qMin,
                                             double[] qMax,
                                             double margin) {
        double[] h = new double[4];
        // low limits
        h[0] = q[0] - (qMin[0] + margin);
        h[1] = q[1] - (qMin[1] + margin);
        // upper limits
        h[2] = (qMax[0] - margin) - q[0];
        h[3] = (qMax[1] - margin) - q[1];
        return h;
    }

    public static double obstacleBarrier(double[] q,
                                         double l1, double l2,
                                         double[] c,
                                         double rObs, double dMin) {
        double[] p = forwardKinematics2D(q, l1, l2);
        double dx = p[0] - c[0];
        double dy = p[1] - c[1];
        double dist2 = dx * dx + dy * dy;
        double rho = rObs + dMin;
        return dist2 - rho * rho;
    }

    public static boolean isSafe(double[] q,
                                 double[] qMin,
                                 double[] qMax,
                                 double l1, double l2,
                                 double[] c,
                                 double rObs, double dMin,
                                 double margin,
                                 double tol) {
        double[] hJoints = jointLimitBarrier(q, qMin, qMax, margin);
        double hObs = obstacleBarrier(q, l1, l2, c, rObs, dMin);
        for (int i = 0; i < hJoints.length; ++i) {
            if (hJoints[i] < -tol) {
                return false;
            }
        }
        if (hObs < -tol) {
            return false;
        }
        return true;
    }

    public static void main(String[] args) {
        double[] q = {0.0, 0.0};
        double[] qMin = {-1.0, -1.0};
        double[] qMax = { 1.0,  1.0};
        double l1 = 0.8;
        double l2 = 0.6;
        double[] c = {0.8, 0.0};
        double rObs = 0.1;
        double dMin = 0.05;
        double margin = 0.0;
        double tol = 0.0;

        System.out.println("Is safe? " +
            isSafe(q, qMin, qMax, l1, l2, c, rObs, dMin, margin, tol));
    }
}
      

7.4 MATLAB / Simulink Implementation

function demo_safe_set_2d()
    q = [0.0; 0.0];
    q_min = [-1.0; -1.0];
    q_max = [ 1.0;  1.0];
    l1 = 0.8; l2 = 0.6;
    c = [0.8; 0.0];
    r_obs = 0.1;
    d_min = 0.05;
    margin = 0.0;
    tol = 0.0;

    h_joints = joint_limit_barrier(q, q_min, q_max, margin);
    h_obs = obstacle_barrier(q, l1, l2, c, r_obs, d_min);

    fprintf('h_joints = [%f %f %f %f]\n', h_joints);
    fprintf('h_obs = %f\n', h_obs);
    fprintf('Is safe? %d\n', is_safe(q, q_min, q_max, l1, l2, c, r_obs, d_min, margin, tol));
end

function p = fk_2d(q, l1, l2)
    q1 = q(1); q2 = q(2);
    x = l1 * cos(q1) + l2 * cos(q1 + q2);
    y = l1 * sin(q1) + l2 * sin(q1 + q2);
    p = [x; y];
end

function h = joint_limit_barrier(q, q_min, q_max, margin)
    h_low = q - (q_min + margin);
    h_up  = (q_max - margin) - q;
    h = [h_low; h_up];
end

function h = obstacle_barrier(q, l1, l2, c, r_obs, d_min)
    p = fk_2d(q, l1, l2);
    diff = p - c;
    dist2 = diff' * diff;
    rho = r_obs + d_min;
    h = dist2 - rho^2;
end

function safe = is_safe(q, q_min, q_max, l1, l2, c, r_obs, d_min, margin, tol)
    h_joints = joint_limit_barrier(q, q_min, q_max, margin);
    h_obs = obstacle_barrier(q, l1, l2, c, r_obs, d_min);
    safe = all(h_joints >= -tol) && (h_obs >= -tol);
end
      

In Simulink, these functions can be placed inside a MATLAB Function block. The block receives \( q \) (and possibly \( \dot{q} \)) and outputs the vector of constraint values \( h(x) \). The safety filter in later lessons will use these outputs as CBF constraints.

7.5 Wolfram Mathematica Implementation

fk2D[q_List, l1_, l2_] := Module[{q1, q2, x, y},
  {q1, q2} = q;
  x = l1 Cos[q1] + l2 Cos[q1 + q2];
  y = l1 Sin[q1] + l2 Sin[q1 + q2];
  {x, y}
];

jointLimitBarrier[q_List, qMin_List, qMax_List, margin_: 0.0] := Module[
  {hLow, hUp},
  hLow = q - (qMin + margin);
  hUp  = (qMax - margin) - q;
  Join[hLow, hUp]
];

obstacleBarrier[q_List, l1_, l2_, c_List, rObs_, dMin_] := Module[
  {p, diff, dist2, rho},
  p = fk2D[q, l1, l2];
  diff = p - c;
  dist2 = diff.diff;
  rho = rObs + dMin;
  dist2 - rho^2
];

isSafe[q_List, qMin_List, qMax_List, l1_, l2_, c_List,
       rObs_, dMin_, margin_: 0.0, tol_: 0.0] := Module[
  {hJoints, hObs},
  hJoints = jointLimitBarrier[q, qMin, qMax, margin];
  hObs = obstacleBarrier[q, l1, l2, c, rObs, dMin];
  And[
    VectorQ[hJoints, (# >= -tol) &],
    hObs >= -tol
  ]
];

(* Example *)
q = {0.0, 0.0};
qMin = {-1.0, -1.0};
qMax = { 1.0,  1.0};
l1 = 0.8; l2 = 0.6;
c = {0.8, 0.0};
rObs = 0.1; dMin = 0.05;

Print["Is safe? ", isSafe[q, qMin, qMax, l1, l2, c, rObs, dMin]];
      

Problem 1 (1D Position Constraint as HOCBF): Consider a 1D double integrator \( \ddot{q} = u \) with state \( x = [q,\dot{q}]^\top \). The safety requirement is \( q \ge 0 \). Design a second-order CBF structure based on \( h_0(q) = q \) and write the inequalities that must hold to guarantee forward invariance of \( \{ q \ge 0 \} \).

Solution: Define \( h_0(x) = q \). Then

\[ \dot{h}_0(x) = \dot{q}. \]

Choose \( \alpha_1(s) = k_1 s \) with \( k_1 > 0 \), and define

\[ h_1(x) = \dot{h}_0(x) + \alpha_1(h_0(x)) = \dot{q} + k_1 q. \]

Its derivative along trajectories is

\[ \dot{h}_1(x,u) = \ddot{q} + k_1 \dot{q} = u + k_1 \dot{q}. \]

Choose \( \alpha_2(s) = k_2 s \) with \( k_2 > 0 \). The HOCBF inequality is

\[ \dot{h}_1(x,u) + \alpha_2(h_1(x)) = u + k_1 \dot{q} + k_2(\dot{q} + k_1 q) \ge 0. \]

Any controller \( u(x) \) that satisfies this inequality for all states with \( h_0(x) \ge 0 \) and \( h_1(x) \ge 0 \) ensures forward invariance of the set \( \{ q \ge 0 \} \). Intuitively, the inequality forces the acceleration \( u \) to slow down motion toward the boundary.

Problem 2 (Joint Limits as Multiple Constraints): For a single joint with limits \( q^{\min} \le q \le q^{\max} \), define \( h^{\mathrm{low}}(q) = q - q^{\min} \) and \( h^{\mathrm{up}}(q) = q^{\max} - q \). Show that the safe set \( \mathcal{C} = \{ (q,\dot{q}) \mid h^{\mathrm{low}}(q) \ge 0,\, h^{\mathrm{up}}(q) \ge 0 \} \) is equivalent to the interval constraint, and explain why gradients being nonzero on the boundary is useful.

Solution: The inequality \( h^{\mathrm{low}}(q) \ge 0 \) is equivalent to \( q \ge q^{\min} \) and \( h^{\mathrm{up}}(q) \ge 0 \) is equivalent to \( q \le q^{\max} \). Therefore the pair of inequalities is equivalent to \( q^{\min} \le q \le q^{\max} \). The gradients are

\[ \frac{\partial h^{\mathrm{low}}}{\partial q} = 1, \qquad \frac{\partial h^{\mathrm{up}}}{\partial q} = -1 \]

and are nonzero on the boundaries \( q = q^{\min}, q^{\max} \). This ensures that the boundary is a regular one-dimensional manifold and that the Lie derivatives \( L_f h, L_g h \) are well-defined and continuous. This regularity is crucial for both theoretical CBF results and numerical optimization in safety filters.

Problem 3 (Task-Space Obstacle CBF Derivatives): For the 2D planar end-effector position \( p(q) \in \mathbb{R}^2 \), let \( h_{\mathrm{obs}}(q) = \|p(q) - c\|^2 - \rho^2 \) with \( \rho = r_{\mathrm{obs}} + d_{\min} \). Compute \( \dot{h}_{\mathrm{obs}} \) in terms of the Jacobian \( J(q) \) and joint velocities \( \dot{q} \). Assume no explicit dependence on \( \dot{q} \) in \( p \).

Solution: By the chain rule,

\[ h_{\mathrm{obs}}(q) = (p(q) - c)^\top (p(q) - c) - \rho^2. \]

Differentiating with respect to time:

\[ \dot{h}_{\mathrm{obs}} = 2 (p(q) - c)^\top \dot{p}(q,\dot{q}). \]

Since \( \dot{p}(q,\dot{q}) = J(q)\dot{q} \), we obtain

\[ \dot{h}_{\mathrm{obs}}(q,\dot{q}) = 2 (p(q) - c)^\top J(q)\dot{q}. \]

This expression enters directly into the CBF inequalities which constrain \( \dot{q} \) and, through the dynamics, the control input \( u \).

Problem 4 (Intersection of Safe Sets): Let \( \mathcal{C}_1 = \{ x \mid h_1(x) \ge 0 \} \) and \( \mathcal{C}_2 = \{ x \mid h_2(x) \ge 0 \} \) be safe sets that are forward invariant under some control law \( u(x) \). Under what additional assumption does the intersection \( \mathcal{C} = \mathcal{C}_1 \cap \mathcal{C}_2 \) remain forward invariant under the same control law?

Solution: The intersection will be forward invariant if the control law that maintains \( \mathcal{C}_1 \) and \( \mathcal{C}_2 \) simultaneously satisfies both CBF inequalities whenever the state lies in the intersection. In practice, this means that for all \( x \in \mathcal{C} \), the chosen \( u(x) \) must satisfy

\[ L_f h_1(x) + L_g h_1(x) u(x) + \alpha_1(h_1(x)) \ge 0, \quad L_f h_2(x) + L_g h_2(x) u(x) + \alpha_2(h_2(x)) \ge 0. \]

A sufficient condition is that the feasible sets of controls defined by each inequality have nonempty intersection for all \( x \in \mathcal{C} \). In later lessons, this is enforced by solving a QP over the intersection of inequality constraints.

Problem 5 (Safe Set Tightening with Sampling): Consider a scalar constraint \( h(x) \ge 0 \) with continuous-time dynamics \( \dot{h}(x,u) \ge -\gamma h(x) \) for some \( \gamma > 0 \) (a standard CBF condition). Using the Euler approximation \( h_{k+1} \approx h_k + T_s \dot{h}_k \), derive a sufficient condition on \( T_s \) and an initial margin \( h_k \) so that \( h_{k+1} \ge 0 \) holds.

Solution: From the inequality,

\[ \dot{h}_k \ge -\gamma h_k. \]

Then

\[ 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. \]

Thus a sufficient condition for \( h_{k+1} \ge 0 \) is that \( (1 - \gamma T_s) h_k \ge 0 \). If \( h_k \ge 0 \) and \( T_s \le 1/\gamma \), then the lower bound \( (1 - \gamma T_s) h_k \) is nonnegative, and the discretized system preserves safety. If \( T_s > 1/\gamma \), then we must start with a stricter condition \( h_k \ge \eta > 0 \) so that the product remains nonnegative; this is exactly the idea of safe set tightening.

In this lesson we developed a rigorous framework for designing safe sets for robots. Starting from geometric safety specifications, we constructed smooth constraint functions \( h(x) \) for joint limits, workspace boundaries, and obstacle avoidance, and discussed how to combine multiple constraints via intersections and smooth approximations (soft-min/soft-max).

We then addressed the high-order nature of manipulator dynamics by introducing high-order CBF constructions, where the primary safe set is designed at the position level but enforced through cascaded inequalities that involve velocities and accelerations. Finally, we examined the impact of sampling and motivated safe set tightening to compensate for discretization, and showed how to implement these safe sets in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

In the next lesson, these safe sets will be integrated into CBF-QP controllers that synthesize control inputs guaranteeing forward invariance while remaining as close as possible to a nominal (performance-oriented) controller.