feat: Introduce CBFController for managing multiple Control Barrier F…

cbfpy/__init__.py

@@ -10,10 +10,12 @@
     UnicyclePnorm2dCBF,
     rotation_matrix_2d,
 )
+from cbfpy.cbf_controller import CBFController
 from cbfpy.cbf_qp_solver import CBFNomQPSolver, CBFQPSolver
 
 __all__ = [
     "CBFBase",
+    "CBFController",
     "CBFNomQPSolver",
     "CBFQPSolver",
     "CircleCBF",

cbfpy/cbf_controller.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+
+from collections.abc import Sequence
+
+from numpy.typing import NDArray
+
+from cbfpy.cbf import CBFBase
+from cbfpy.cbf_qp_solver import CBFNomQPSolver
+
+
+class CBFController:
+    """High-level controller that combines multiple CBFs with a QP solver.
+
+    Collects constraints from a list of CBFs and solves the CBF-QP to produce
+    a safe input that tracks the given nominal input.
+
+    Args:
+        cbf_list: list of CBF instances (constraints must be pre-computed via calc_constraints)
+        P: weight matrix for the QP cost. shape=(N, N)
+
+    Example:
+        >>> import numpy as np
+        >>> from cbfpy import CBFController, CircleCBF
+        >>>
+        >>> cbf = CircleCBF(center=np.zeros(2), radius=2.0, keep_inside=True)
+        >>> controller = CBFController([cbf], P=np.eye(2))
+        >>>
+        >>> # Update constraints based on current state
+        >>> cbf.calc_constraints(agent_position=np.array([1.5, 0.0]))
+        >>>
+        >>> # Get safe input
+        >>> status, safe_input = controller.optimize(nominal_input=np.array([1.0, 0.0]))
+    """
+
+    def __init__(self, cbf_list: Sequence[CBFBase], P: NDArray) -> None:
+        self._cbf_list = cbf_list
+        self._P = P
+        self._solver = CBFNomQPSolver()
+
+    @property
+    def cbf_list(self) -> Sequence[CBFBase]:
+        return self._cbf_list
+
+    @property
+    def P(self) -> NDArray:
+        return self._P
+
+    @P.setter
+    def P(self, value: NDArray) -> None:
+        self._P = value
+
+    def optimize(self, nominal_input: NDArray) -> tuple[str, NDArray]:
+        """Collect constraints from all CBFs and solve the CBF-QP.
+
+        All CBFs must have their constraints updated (via calc_constraints)
+        before calling this method.
+
+        Args:
+            nominal_input: desired control input. shape=(N,)
+
+        Returns:
+            status: "optimal" on success
+            optimal_input: safe control input. shape=(N,)
+        """
+        G_list: list[NDArray] = []
+        alpha_h_list: list[float] = []
+        for cbf in self._cbf_list:
+            G, alpha_h = cbf.get_constraints()
+            G_list.append(G)
+            alpha_h_list.append(alpha_h)
+
+        return self._solver.optimize(nominal_input, self._P, G_list, alpha_h_list)

cbfpy/cbf_qp_solver.py

@@ -18,8 +18,8 @@ def _assemble_constraints(G_list: Sequence[NDArray]) -> NDArray:
         Assembled constraint matrix. shape=(M, N)
     """
     if len(G_list) > 1:
-        return np.array([g.flatten() for g in G_list])
-    return G_list[0].reshape(1, -1)
+        return np.array([np.atleast_1d(g).flatten() for g in G_list])
+    return np.atleast_1d(G_list[0]).reshape(1, -1)
 
 
 def _solve_qp(P: NDArray, q: NDArray, G_ineq: NDArray, h_ineq: NDArray) -> NDArray:

docs/source/cbfpy.rst

@@ -8,6 +8,13 @@ CBF Classes
    :members:
    :show-inheritance:
 
+Controller
+----------
+
+.. automodule:: cbfpy.cbf_controller
+   :members:
+   :show-inheritance:
+
 QP Solvers
 ----------
 

docs/source/examples.rst

@@ -88,3 +88,25 @@ enabling reactive navigation through environments with multiple obstacles.
 .. literalinclude:: ../../examples/example_lidar_cbf.py
    :language: python
    :caption: examples/example_lidar_cbf.py
+
+
+Multi-Agent Collision Avoidance
+-------------------------------
+
+Four agents swap positions diagonally while avoiding each other.
+Uses :class:`~cbfpy.cbf_controller.CBFController` with multiple ``CircleCBF`` instances per agent.
+
+.. literalinclude:: ../../examples/example_multi_agent_cbf.py
+   :language: python
+   :caption: examples/example_multi_agent_cbf.py
+
+
+Path Following with Obstacle Avoidance
+---------------------------------------
+
+An agent follows a sequence of waypoints while avoiding circular obstacles.
+Demonstrates :class:`~cbfpy.cbf_controller.CBFController` composing multiple CBF constraints.
+
+.. literalinclude:: ../../examples/example_path_following_cbf.py
+   :language: python
+   :caption: examples/example_path_following_cbf.py

docs/source/index.rst

@@ -20,6 +20,7 @@ Features
 - Multiple CBF types: scalar, circular, p-norm, unicycle, LiDAR-based
 - Lightweight QP solver via `quadprog <https://pypi.org/project/quadprog/>`_ (Goldfarb/Idnani algorithm)
 - Runtime parameter reconfiguration via ``set_parameters()``
+- :class:`~cbfpy.cbf_controller.CBFController` for easy multi-CBF composition
 
 Quick Start
 -----------
@@ -41,10 +42,23 @@ Quick Start
    nominal_input = np.array([1.0, 0.0])  # desired direction
    status, safe_input = solver.optimize(nominal_input, np.eye(2), [G], [alpha_h])
 
+Or using :class:`~cbfpy.cbf_controller.CBFController` for a simpler API:
+
+.. code-block:: python
+
+   from cbfpy import CBFController, CircleCBF
+
+   cbf = CircleCBF(center=np.zeros(2), radius=2.0, keep_inside=True)
+   controller = CBFController([cbf], P=np.eye(2))
+
+   cbf.calc_constraints(agent_position=np.array([1.5, 0.0]))
+   status, safe_input = controller.optimize(nominal_input=np.array([1.0, 0.0]))
+
 
 .. toctree::
    :maxdepth: 4
    :caption: Contents:
 
+   theory
    cbfpy
    examples

docs/source/theory.rst

@@ -0,0 +1,159 @@
+Theory
+======
+
+This page provides the mathematical background for Control Barrier Functions (CBFs)
+implemented in cbfpy.
+
+
+Control Barrier Functions
+-------------------------
+
+A Control Barrier Function (CBF) provides formal safety guarantees for dynamical systems.
+Consider an affine control system:
+
+.. math::
+
+   \dot{x} = f(x) + g(x) u
+
+where :math:`x \in \mathbb{R}^n` is the state and :math:`u \in \mathbb{R}^m` is the control input.
+
+A **safe set** :math:`\mathcal{C}` is defined as the 0-superlevel set of a continuously
+differentiable function :math:`h : \mathbb{R}^n \to \mathbb{R}`:
+
+.. math::
+
+   \mathcal{C} = \{ x \in \mathbb{R}^n \mid h(x) \geq 0 \}
+
+The function :math:`h` is a CBF if there exists an extended class-:math:`\mathcal{K}` function
+:math:`\alpha` such that for all :math:`x \in \mathcal{C}`:
+
+.. math::
+
+   \sup_{u} \left[ \frac{\partial h}{\partial x} (f(x) + g(x) u) + \alpha(h(x)) \right] \geq 0
+
+
+CBF-QP Formulation
+------------------
+
+To find the safe input closest to a desired nominal input :math:`u_{\text{nom}}`,
+we solve a Quadratic Program (QP):
+
+.. math::
+
+   \min_{u} \quad & \frac{1}{2} (u - u_{\text{nom}})^T P (u - u_{\text{nom}}) \\
+   \text{s.t.} \quad & \frac{\partial h}{\partial x} g(x) \, u + \alpha(h(x)) \geq 0
+
+In cbfpy, we denote:
+
+- :math:`G = \frac{\partial h}{\partial x} g(x)` — the constraint gradient (computed by each CBF class)
+- :math:`\alpha(h)` — the class-K function output (default: :math:`\alpha(h) = h`)
+- :math:`P` — the weight matrix (positive definite)
+
+Multiple CBFs can be combined by stacking their constraints.
+
+
+Barrier Functions in cbfpy
+--------------------------
+
+Scalar CBF
+^^^^^^^^^^
+
+For a 1D state :math:`x` with limit :math:`l`:
+
+.. math::
+
+   h(x) = \text{sign} \cdot (x - l)
+
+where :math:`\text{sign} = +1` for ``keep_upper=True`` (stay above :math:`l`)
+and :math:`\text{sign} = -1` for ``keep_upper=False`` (stay below :math:`l`).
+
+Scalar Range CBF
+^^^^^^^^^^^^^^^^
+
+For a 1D state :math:`x` within range :math:`[a, b]`:
+
+.. math::
+
+   h(x) = \text{sign} \cdot \left[ \left( \frac{b - a}{2} \right)^2 - \left( x - \frac{a + b}{2} \right)^2 \right]
+
+This is positive inside the range and negative outside.
+
+Circle CBF
+^^^^^^^^^^
+
+For a 2D agent position :math:`p` with a circular region of center :math:`c` and radius :math:`r`:
+
+.. math::
+
+   h(p) = \text{sign} \cdot \left( 1 - \left\| \frac{p - c}{r} \right\|_2 \right)
+
+where :math:`\text{sign} = +1` for ``keep_inside=True``.
+
+P-norm 2D CBF
+^^^^^^^^^^^^^^
+
+Generalizes the circle to a p-norm shaped region. For center :math:`c`,
+semi-axes :math:`w`, rotation :math:`\theta`, and norm order :math:`p`:
+
+.. math::
+
+   \tilde{p} = R(-\theta) \frac{p - c}{w}
+
+.. math::
+
+   h(\tilde{p}) = \text{sign} \cdot \left( 1 - \| \tilde{p} \|_p \right)
+
+where :math:`R(\theta)` is the 2D rotation matrix. Setting :math:`p = 2` gives an ellipse,
+:math:`p = 1` gives a diamond, and :math:`p \to \infty` gives a rectangle.
+
+Unicycle Variants
+^^^^^^^^^^^^^^^^^
+
+For unicycle models with state :math:`(x, y, \theta)` and input :math:`(v, \omega)`,
+the constraint gradient is transformed by the input matrix:
+
+.. math::
+
+   A = \begin{pmatrix} \cos\theta & 0 \\ \sin\theta & 0 \\ 0 & 1 \end{pmatrix}
+
+so that the CBF constraint becomes:
+
+.. math::
+
+   \frac{\partial h}{\partial x} A \, u + \alpha(h) \geq 0
+
+LiDAR CBF
+^^^^^^^^^^
+
+For LiDAR-based obstacle avoidance, each laser ray at range :math:`r` and angle :math:`\phi`
+defines a barrier function:
+
+.. math::
+
+   h(r, \phi) = r - r_c(\phi)
+
+where :math:`r_c(\phi) = \sqrt{(w_x \cos\phi)^2 + (w_y \sin\phi)^2}` is the
+critical distance defined by the ellipsoidal safety margin with semi-axes :math:`(w_x, w_y)`.
+
+
+CBFController
+-------------
+
+The :class:`~cbfpy.cbf_controller.CBFController` class simplifies the common pattern of
+combining multiple CBFs with a QP solver:
+
+.. code-block:: python
+
+   from cbfpy import CBFController, CircleCBF
+
+   # Define CBFs
+   cbf1 = CircleCBF(center1, radius1, keep_inside=False)
+   cbf2 = CircleCBF(center2, radius2, keep_inside=False)
+
+   # Create controller
+   controller = CBFController([cbf1, cbf2], P=np.eye(2))
+
+   # In control loop:
+   cbf1.calc_constraints(agent_position)
+   cbf2.calc_constraints(agent_position)
+   status, safe_input = controller.optimize(nominal_input)

examples/example_circle_cbf.py

@@ -8,29 +8,12 @@
 from numpy.typing import NDArray
 
 from cbfpy.cbf import CircleCBF
-from cbfpy.cbf_qp_solver import CBFNomQPSolver
-
-
-class CBFOptimizer:
-    def __init__(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
-        self.qp_nom_solver = CBFNomQPSolver()
-        self.P = np.eye(2)
-        self.circle_cbf = CircleCBF(center, radius, keep_inside)
-
-    def set_parameters(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
-        self.circle_cbf.set_parameters(center, radius, keep_inside)
-
-    def get_parameters(self) -> tuple[NDArray, float, bool]:
-        return self.circle_cbf.get_parameters()
-
-    def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> tuple[str, NDArray]:
-        self.circle_cbf.calc_constraints(agent_position)
-        G, alpha_h = self.circle_cbf.get_constraints()
-        return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h])
+from cbfpy.cbf_controller import CBFController
 
 
 def main() -> None:
-    optimizer_list = [CBFOptimizer(np.zeros(2)), CBFOptimizer(np.zeros(2))]
+    cbf_list = [CircleCBF(np.zeros(2), 1.0, keep_inside=False), CircleCBF(np.zeros(2), 1.0, keep_inside=False)]
+    controller_list = [CBFController([cbf], P=np.eye(2)) for cbf in cbf_list]
 
     initial_position_array = np.array([[-2, -2.5], [2, 2]])
     agent_position_list: list[NDArray] = [initial_position_array]
@@ -53,10 +36,11 @@ def update(
 
             keep_inside = False
 
-            optimizer_list[agent_id].set_parameters(another_agent_position, 2 * radius, keep_inside)
+            cbf_list[agent_id].set_parameters(another_agent_position, 2 * radius, keep_inside)
             nominal_input = np.array([-2 * agent_id + 1] * 2)
             curr_position = curr_position_array[agent_id]
-            _, optimal_input = optimizer_list[agent_id].optimize(nominal_input, curr_position)
+            cbf_list[agent_id].calc_constraints(curr_position)
+            _, optimal_input = controller_list[agent_id].optimize(nominal_input)
 
             curr_position_array[agent_id] = curr_position_array[agent_id] + dt * optimal_input