Skip to content

CBF filter (geometric command safety)

A command moving at a perfectly safe, bounded velocity straight into an obstacle is still dangerous -- rate_limiter.md bounds rate of change, this module bounds WHERE a command can go. cbf_safety_filter/cbf_filtered_trajectory implement a Control Barrier Function (CBF) safety filter: never let the applied command enter a forbidden region, with a minimally invasive correction when it would. cbf_safety_filter_live is the same math packaged for a real-time control loop that reacts to one sensor callback at a time (a real dt per tick) instead of filtering a whole pre-recorded array offline -- 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). Grounded in Ames et al. (2019)'s CBF-QP theory (2019 ECC, arXiv:1903.11199) -- the same theory SAFER-Splat uses, applied to a known geometric obstacle instead of SAFER-Splat's GPU-bound Gaussian-Splatting perception (not available on every machine -- see the module's own docstring). Promoted from Dense-Evolution-Discovery after validation on two independent real physical domains (SO-101, ALOHA) -- see the module's own docstring for the real numbers, including a real numerical finding about discrete-time sub-stepping.

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/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/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/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

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.