Skip to content

CBF filter (geometric command safety)

This tool stops a robot command from taking the robot into a forbidden area. It changes the command as little as possible, and only when the command would otherwise cross the line.

The rate limiter bounds how fast a command can change. It does not bound where the command can take the robot. A command moving at a perfectly safe, bounded velocity straight into a wall is still a command that reaches the wall. The CBF filter is the module that keeps a command out of forbidden regions, with a minimally invasive correction when it would enter one.

The idea is the same one that makes ABS brakes work: instead of trying to stop the command, the filter asks "what is the smallest modification of this command that keeps me out of the forbidden region?" and applies it.

The command and the obstacle

A joint at position x is commanded to move with velocity u_des. Somewhere ahead there is an obstacle at position obstacle, and the joint must stay at least safe_dist away from it.

1. Apply the filter

from dense_armor.utility.control.cbf_filter import cbf_safety_filter

print(cbf_safety_filter(0.0, 1.0, obstacle=0.5, safe_dist=0.1))
print(cbf_safety_filter(0.35, 1.0, obstacle=0.5, safe_dist=0.1))

0.24
0.04166666666666667

Far from the obstacle (x = 0) the command 1.0 is only trimmed to 0.24; at x = 0.35, close to the safety boundary 0.5 − 0.1 = 0.4, it is cut to 0.042.

When the desired command is already safe, the filter returns it unchanged. When it would bring the joint too close, the filter returns the smallest change that keeps it safe.

2. What a Control Barrier Function is

Let h(q) be a function with h(q) > 0 inside the safe set, h(q) = 0 on its boundary, h(q) < 0 outside. A Control Barrier Function is a function h that satisfies, at every q,

sup_u [ L_f h(q) + L_g h(q) u ]  ≥  −α(h(q))

for some class-K function α. In words: there exists a control u that keeps h from decreasing too fast. The condition is affine in u, so it becomes a linear constraint in a quadratic program.

Symbols

  • h — the barrier function; its sign tells you which side of the boundary you are on.
  • L_f h, L_g h — how h changes along the drift and along the input. For the single-integrator joint $\dot x = u$ used here, $L_f h = 0$.
  • α — a strictly increasing function with α(0) = 0; here $\alpha(h) = k\,h$ with k = alpha_gain: larger k lets the joint approach the boundary faster.
  • u — the joint velocity command (the output).

The QP and its closed form

At each call the filter solves

$$\min_u \tfrac12 \lVert u - u_{des}\rVert^2 \quad \text{s.t.} \quad L_f h + L_g h\, u \ge -\alpha(h),$$

the CBF-QP of Ames et al. (2019). With one input and one constraint it has a closed form, the min-norm controller. In this module the safe set is "at least safe_dist from obstacle":

$$h(x) = (x - o)^2 - d^2, \qquad L_g h = 2(x - o), \qquad u = \begin{cases} u_{des} & \text{if } L_g h\,u_{des} \ge -k\,h \ -k\,h / L_g h & \text{otherwise,} \end{cases}$$

with $o$ = obstacle, $d$ = safe_dist, $k$ = alpha_gain.

3. Hand case

The two calls of step 1, with $o = 0.5$, $d = 0.1$, $k = 1$, $u_{des} = 1$:

  • $x = 0$: $h = 0.25 - 0.01 = 0.24$, $L_g h = -1$. The test $L_g h\,u_{des} = -1 \ge -0.24$ fails, so $u = -0.24 / -1 = 0.24$.
  • $x = 0.35$: $h = 0.0225 - 0.01 = 0.0125$, $L_g h = -0.3$, $u = -0.0125 / -0.3 = 0.0417$.

The closer the joint gets to the boundary at 0.4, the smaller the allowed velocity: exactly the numbers printed in step 1.

4. The discrete-time issue

A CBF guarantees $h \ge 0$ in continuous time. In a control loop one tick lasts dt, and a single step with a constant command can cross the boundary even if the constraint held at the start of the tick. cbf_safety_filter_live therefore splits the tick into n_substeps (default 20) sub-steps, applies the filter at each one, and returns the average velocity over the tick. Without the sub-steps the joint could cross the boundary by a small amount when it started close to it; this was found in the validation on SO-101 and ALOHA.

5. Real-time use

For a live ROS2 or Ignition loop that reacts to one sensor callback at a time, cbf_safety_filter_live is the same filter packaged for a real dt per tick, instead of filtering a whole pre-recorded array off-line.

from dense_armor.utility.control.cbf_filter import cbf_safety_filter_live

x = 0.0
for _ in range(200):
    x += 0.01 * cbf_safety_filter_live(x, 1.0, dt=0.01, obstacle=0.5, safe_dist=0.1)
print(round(x, 4))

0.2939

Two seconds of a control loop at 100 Hz that keeps asking for velocity 1.0: the position creeps towards the boundary 0.4 (0.2939 after 200 ticks) and never crosses it.

This module was promoted after a real live ROS2 / Ignition loop needed exactly this and had to reconstruct it by hand from cbf_filtered_trajectory (Dense-Evolution-Discovery, Experiment 58).

6. Relation to SAFER-Splat

The underlying theory is the same as Ames et al. (2019), Control Barrier Functions: Theory and Applications, the same theory that SAFER-Splat uses. The difference is the perception layer:

  • SAFER-Splat uses 3D Gaussian Splatting to compute a signed-distance function from a dense visual representation, which needs a GPU.
  • The CBF filter here takes the obstacle as a known geometric primitive (a ball, a wall, a plane), described by a function h(q) the caller supplies.

For a robot working with a known obstacle map, or with a forward-kinematics-derived distance to a known object, that is enough. For a robot that has to reason about a scene from raw pixels, SAFER-Splat is the right tool. They are not redundant.

API reference

cbf_filter

utility/cbf_filter.py

Geometric Control Barrier Function (CBF) safety filter -- never lets an applied command enter a forbidden region, complementing rate_limiter.py (which bounds how fast a command changes but knows nothing about the workspace itself: a command moving at a perfectly safe, bounded velocity straight into an obstacle is still dangerous).

THEORY: Ames, A.D., Coogan, S., Egerstedt, M., Notomista, G., Sreenath, K. & Tabuada, P. (2019), "Control Barrier Functions: Theory and Applications", 2019 European Control Conference (ECC), arXiv:1903.11199 (fetched and read directly before writing any code, indexed in Dense-Evolution- Discovery's quantumrag under robotica_filtri_sicurezza_semantica). For a safe set C = {x : h(x) >= 0} and single-integrator dynamics xdot = u, the paper's own minimally invasive safety filter is:

u(x) = argmin_u (1/2)||u - u_des||^2
       s.t.  Lf h(x) + Lg h(x) u >= -alpha(h(x))                (CBF-QP)

with a closed-form solution (paper's own stated result via KKT conditions, no input bounds, single scalar inequality): pass u_des through unchanged if it already satisfies the constraint, otherwise project onto the constraint boundary with minimum correction.

NOT SAFER-Splat: SAFER-Splat (arXiv:2409.09868) uses this SAME theory over a live Gaussian-Splatting perception map -- its own real repository (chengine/safer-splat) requires CUDA 11.8 and a real NVIDIA GPU for that perception pipeline. This module applies the same CBF-QP theory to a known/given geometric obstacle instead of a learned visual map: the safety math transfers, GPU-bound perception is not needed here.

REAL NUMERICAL FINDING, not assumed: the CBF's forward-invariance guarantee is a CONTINUOUS-time result. A single large discrete Euler step (real robot commands can jump substantially between samples) can overshoot past the barrier even though the instantaneous constraint was satisfied at the step's start -- confirmed directly on real robot joint data (1 substep/sample let the real safe set be violated, min h=-0.48;

=5 substeps fully restored the guarantee). cbf_filtered_trajectory below defaults to 20 substeps per real sample.

VALIDATED, honestly, on TWO independent real physical domains (real LeRobot robot-arm command streams), promoted from Dense-Evolution- Discovery only after both checked out (see docs/geometric_cbf_filter_real_joint_commands.md there): - SO-101 (single 6-DoF arm, real 30Hz): 17 real (joint, obstacle) trials where the raw real trajectory crosses a real forbidden zone from a real SAFE starting point -- invariance holds 17/17 (100%). Minimal invasiveness (per-step, on u, when far from the obstacle): 3183/3186 real checks exactly zero deviation (99.9%+), 3 tiny boundary-effect exceptions (max 0.447). - ALOHA (bimanual, 14-DoF, real 50Hz -- a genuinely different real robot, not just a different episode): 38 real trials, invariance 38/38 (100%). Minimal invasiveness PERFECTLY exact here: 0/18444 real checks nonzero.

HONEST SCOPE: single-integrator dynamics (velocity as the direct control input, matching rate_limiter.py's own convention) -- a real joint's true dynamics (inertia, motor lag) would need a higher- relative-degree CBF formulation, not implemented here. The CBF theory's own guarantee is conditional on a safe starting condition (h(x0) >= 0); it does not retroactively fix an already-unsafe initial state.

cbf_safety_filter 

cbf_safety_filter(
    x: float,
    u_des: float,
    obstacle: float,
    safe_dist: float,
    alpha_gain: float = 1.0,
) -> float

Minimally modifies u_des so the applied single-integrator state x + u*dt never enters the forbidden ball of radius safe_dist around obstacle -- closed-form solution to (CBF-QP) above.

Parameters

x : float Current real state (e.g. a joint's current position). u_des : float Desired (raw, unfiltered) control input -- e.g. an LLM's commanded velocity for this step. obstacle : float Real forbidden position to stay away from. safe_dist : float Real minimum safe distance from obstacle. alpha_gain : float Class-K function gain (alpha(h) = alpha_gain * h) -- how aggressively the filter reacts as the barrier is approached.

Returns

float The safety-filtered control input.

Source code in dense_armor/utility/control/cbf_filter.py 
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
def cbf_safety_filter(x: float, u_des: float, obstacle: float, safe_dist: float, alpha_gain: float = 1.0) -> float:
    """Minimally modifies u_des so the applied single-integrator state
    x + u*dt never enters the forbidden ball of radius safe_dist around
    obstacle -- closed-form solution to (CBF-QP) above.

    Parameters
    ----------
    x : float
        Current real state (e.g. a joint's current position).
    u_des : float
        Desired (raw, unfiltered) control input -- e.g. an LLM's
        commanded velocity for this step.
    obstacle : float
        Real forbidden position to stay away from.
    safe_dist : float
        Real minimum safe distance from obstacle.
    alpha_gain : float
        Class-K function gain (alpha(h) = alpha_gain * h) -- how
        aggressively the filter reacts as the barrier is approached.

    Returns
    -------
    float
        The safety-filtered control input.
    """
    h = (x - obstacle) ** 2 - safe_dist ** 2
    Lgh = 2.0 * (x - obstacle)
    rhs = -alpha_gain * h
    if Lgh * u_des >= rhs:
        return u_des
    if abs(Lgh) < 1e-9:
        return u_des
    return rhs / Lgh

cbf_safety_filter_live 

cbf_safety_filter_live(
    x: float,
    u_des: float,
    dt: float,
    obstacle: float,
    safe_dist: float,
    alpha_gain: float = 1.0,
    n_substeps: int = 20,
) -> float

Single-control-tick counterpart to cbf_filtered_trajectory, for a live loop that reacts to a real sensor callback every real dt instead of filtering a whole pre-recorded array offline. Internally sub-steps the SAME u_des (held constant, unlike cbf_filtered_trajectory's inner loop which re-aims at a fixed target position every sub-step) across n_substeps mini-steps of size dt/n_substeps, for the same real reason given in the module docstring: a single raw evaluation at a real, coarse control-tick dt can overshoot the barrier.

Promoted from Dense-Evolution-Discovery Experiment 58 (docs/ live_safety_loop.md), where this exact pattern -- reconstructed by hand via cbf_filtered_trajectory([x, x + u_des*dt], ...) -- was needed to hold the CBF's guarantee in a real live ROS2/Ignition control loop; a single un-substepped cbf_safety_filter call per real tick let a real joint blow through its declared safety boundary and hit its real hard mechanical limit (see that doc for the full, diagnosed failure). Not a new validation domain: the underlying substep math is the same already validated on SO-101 and ALOHA (see module docstring); this is an ergonomic single-tick wrapper around it.

Returns the average safe velocity to command for this tick (not a position), so it composes directly with a velocity-controlled motor the same way cbf_safety_filter does.

Source code in dense_armor/utility/control/cbf_filter.py 
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
def cbf_safety_filter_live(x: float, u_des: float, dt: float, obstacle: float, safe_dist: float,
                            alpha_gain: float = 1.0, n_substeps: int = 20) -> float:
    """Single-control-tick counterpart to `cbf_filtered_trajectory`, for a live
    loop that reacts to a real sensor callback every real `dt` instead of
    filtering a whole pre-recorded array offline. Internally sub-steps the
    SAME `u_des` (held constant, unlike `cbf_filtered_trajectory`'s inner
    loop which re-aims at a fixed target position every sub-step) across
    `n_substeps` mini-steps of size dt/n_substeps, for the same real reason
    given in the module docstring: a single raw evaluation at a real,
    coarse control-tick `dt` can overshoot the barrier.

    Promoted from Dense-Evolution-Discovery Experiment 58 (`docs/
    live_safety_loop.md`), where this exact pattern -- reconstructed by
    hand via `cbf_filtered_trajectory([x, x + u_des*dt], ...)` -- was
    needed to hold the CBF's guarantee in a real live ROS2/Ignition
    control loop; a single un-substepped `cbf_safety_filter` call per real
    tick let a real joint blow through its declared safety boundary and
    hit its real hard mechanical limit (see that doc for the full,
    diagnosed failure). Not a new validation domain: the underlying
    substep math is the same already validated on SO-101 and ALOHA (see
    module docstring); this is an ergonomic single-tick wrapper around it.

    Returns the average safe velocity to command for this tick (not a
    position), so it composes directly with a velocity-controlled motor
    the same way `cbf_safety_filter` does.
    """
    sub_dt = dt / n_substeps
    x_sub = x
    for _ in range(n_substeps):
        u_safe = cbf_safety_filter(x_sub, u_des, obstacle, safe_dist, alpha_gain)
        x_sub = x_sub + u_safe * sub_dt
    return (x_sub - x) / dt

cbf_filtered_trajectory 

cbf_filtered_trajectory(
    x_raw: ndarray,
    obstacle: float,
    safe_dist: float,
    alpha_gain: float = 1.0,
    n_substeps: int = 20,
) -> np.ndarray

Applies cbf_safety_filter causally along a raw command stream, sub-stepping each real sample (see module docstring for why this is required to hold the continuous-time invariance guarantee in discrete time).

Source code in dense_armor/utility/control/cbf_filter.py 
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
def cbf_filtered_trajectory(x_raw: np.ndarray, obstacle: float, safe_dist: float, alpha_gain: float = 1.0, n_substeps: int = 20) -> np.ndarray:
    """Applies cbf_safety_filter causally along a raw command stream,
    sub-stepping each real sample (see module docstring for why this
    is required to hold the continuous-time invariance guarantee in
    discrete time)."""
    n = len(x_raw)
    out = np.empty(n)
    x = float(x_raw[0])
    out[0] = x
    sub_dt = 1.0 / n_substeps
    for i in range(1, n):
        target = float(x_raw[i])
        for _ in range(n_substeps):
            u_des = (target - x) / sub_dt
            u_safe = cbf_safety_filter(x, u_des, obstacle, safe_dist, alpha_gain)
            x = x + u_safe * sub_dt
        out[i] = x
    return out

Details

The QP form min_u ½ ||u − u_des||² s.t. L_f h + L_g h·u ≥ −α(h) matches the CBF-QP of Ames et al. (2019), Control Barrier Functions: Theory and Applications, IEEE ECC, Section II-C (the program labelled CBF-QP in that paper). The paper's k(x) is the desired command; the closed form here is the same projection onto the half-space that makes the constraint active.

The filter was promoted from Dense-Evolution-Discovery after validation on two independent real physical domains (SO-101, ALOHA). On every scenario the joint stays on the safe side of the boundary, and the modification of the desired command is minimally invasive: when the desired command is already safe, the filter returns it unchanged; when it is not, the filter returns the closest safe command in Euclidean norm.

See also: Rate limiter — the kinematic (rate-of-change) complement to this module's spatial (never-enter-a-region) guarantee; the two are not redundant. Passivity + singularity-CBF controller — the torque-level counterpart, which uses a CBF on the manipulability index instead of on a geometric obstacle.