Extend clf controller to control barrier functions

- add cbf controller for relative degree 2 control
- add display for cbf controller

controls/control_affine_clf.py

@@ -133,7 +133,20 @@ class MinNormRd2ClfController(object):
         self.P = riccati.solve_cts_riccati_schur(
             self.F, self.G, self.Q, np.eye(r))  # 2r, 2r
 
-    def __call__(self, x, t, fx=None, gx=None, dfxdx=None, **kwargs):
+    def __call__(self, x, t, fx=None, gx=None, dfxdx=None,
+                 A_cbf=(), b_cbf=(), **kwargs):
+        """
+        :param x:
+        :param t:
+        :param fx: m, |
+        :param gx: m, n |
+        :param dfxdx: m, m | jacobian of fx
+        :param A_cbf: k, n | for k control barrier function
+            constraints, if specified
+        :param b_cbf: k,
+        :param kwargs:
+        :return:
+        """
         # CLF
         y = self.C.dot(x) - self.d
         y_dot = self.C_dot.dot(x)
@@ -145,11 +158,13 @@ class MinNormRd2ClfController(object):
         phi_zero = LfV + self.eps_rate * V
         phi_one = LgV
         # input constraints
-        A = self.C.dot(dfxdx).dot(gx)
+        dLfydx = self.C.dot(dfxdx)  # r, m
+        A = dLfydx.dot(gx)  # r, n
         # at minimum need r <= m,n for right inverse, so u is defined
-        Ainv = np.linalg.pinv(A)
-        Lf = self.C.dot(dfxdx).dot(fx)
-        AinvLf = Ainv.dot(Lf)
+        # - should have this inverse by relative degree 2 assumption
+        Ainv = np.linalg.pinv(A)  # n, r
+        Lf = dLfydx.dot(fx)  # r,
+        AinvLf = Ainv.dot(Lf)  # n,
         # solve the QP, writing constraints as Ac z <= bc
         r = y.size
         z0 = np.zeros((r+1,))  # z = [mu, delta]
@@ -159,6 +174,11 @@ class MinNormRd2ClfController(object):
         Ac = np.hstack((Ac, 0 * Ac[:, [0]]))  # for delta
         Ac[0, -1] = -1
         bc = np.hstack((-phi_zero, self.u_max + AinvLf, -self.u_min - AinvLf))
+        if len(A_cbf) > 0:
+            A_cbf_mu = A_cbf.dot(Ainv)  # k, r
+            A_cbf_mu = np.hstack((A_cbf_mu, 0 * A_cbf_mu[:, [0]]))
+            Ac = np.vstack((Ac, A_cbf_mu))
+            bc = np.hstack((bc, b_cbf + A_cbf.dot(AinvLf)))
         linear_constraint = opt.LinearConstraint(Ac, bc - np.inf, bc)
         res = opt.minimize(
             lambda z: 0.5 * (self.c_delta * z[-1]**2 + np.linalg.norm(z[:-1])**2),
@@ -173,6 +193,83 @@ class MinNormRd2ClfController(object):
         return u_star
 
 
+class MinNormRd2CbfObstacleController(object):
+    """
+    Use a control barrier function (CBF) to enforce a minimum distance
+    to point obstacles.
+    Assumption:
+        The control input affects acceleration so the CBF constraint on
+        position is of relative degree two.
+
+    To avoid obstacles with distance d, use barrier function:
+    b_i(x_t) = ||p_t - p_i||^2_2 - d^2
+    where p_t = Cx_t \in \reals^2 is current position
+    and p_i \in \reals^2 is the ith obstacle's position.
+    The full barrier function for the k obstacles is
+    b(x_t) = (b_1(x_t); ...; b_k(x_t))
+
+    The CBF is enforced using linear class K functions having
+    the same coefficient. This controller basically makes the
+    constraints corresponding to the CBF, and hands these to
+    a CLF-based controller to use when selecting controls.
+
+    This implementation closely follows material from:
+    W. Xiao and C. Belta. "Control barrier functions for systems with
+    high relative degree," in IEEE Conference on Decision and Control
+    (CDC), 2019, pp. 474-479.
+    """
+    def __init__(self, rd2clf_controller, C, c_alpha, d):
+        """
+        :param rd2clf_controller: clf controller for systems of relative degree 2
+        :param C: 2, m | selects position from current state, p_t = Cx_t
+        :param c_alpha: coefficient for linear class K functions
+        :param d: minimum distance allowed to each obstacle
+        """
+        self.rd2clf_controller = rd2clf_controller
+        self.C = C
+        self.c_alpha = c_alpha
+        self.d = d
+        self.p = np.zeros((0, 2))
+
+    def set_obstacles(self, p):
+        """
+        :param p: k, 2 | obstacle positions
+        :return:
+        """
+        self.p = p.reshape(-1, 2)
+
+    def set_setpoint(self, *args, **kwargs):
+        self.rd2clf_controller.set_setpoint(*args, **kwargs)
+
+    def __call__(self, x, t, fx=None, gx=None, dfxdx=None, **kwargs):
+        m = x.size
+        d_t = self.C.dot(x) - self.p  # k, 2
+        b = np.linalg.norm(d_t, axis=1)**2 - self.d**2  # k,
+        dbdx = 2 * d_t.dot(self.C)  # k, m
+        Lfb = dbdx.dot(fx)  # k,
+
+        # only enforce barrier for valid initial conditions
+        is_phi_zero = b >= 0
+        is_phi_one = Lfb + self.c_alpha * b >= 0
+        is_valid = is_phi_zero & is_phi_one
+        k = is_valid.sum()
+        if k < 1:
+            return self.rd2clf_controller(x, t, fx=fx, gx=gx, dfxdx=dfxdx, **kwargs)
+
+        b = b[is_valid]
+        dbdx = dbdx[is_valid, :]
+        Lfb = Lfb[is_valid]
+        d2bdx2_i = 2 * self.C.T.dot(self.C)  # m, m since d^2b/dx^2 is (k, m, m) tensor
+        dLfbdx = np.broadcast_to(d2bdx2_i.dot(fx), (k, m)) \
+            + dbdx.dot(dfxdx)  # k, m
+        Lf2b = dLfbdx.dot(fx)  # k,
+        LgLfb = dLfbdx.dot(gx)  # k, n
+        b_cbf = Lf2b + 2*self.c_alpha*Lfb + self.c_alpha**2 * b
+        A_cbf = -LgLfb
+        return self.rd2clf_controller(x, t, fx=fx, gx=gx, dfxdx=dfxdx,
+                                      A_cbf=A_cbf, b_cbf=b_cbf, **kwargs)
+
+
 def make_C_matrix(select_inds, m):
     """
     Build C matrix for common choices of y = Cx - d where

examples/display_control_affine_cbf.py

+import numpy as np
+from rigid_body_models import boat
+from state_space_models import empirical_drag
+from controls import stepping
+from controls import control_affine_clf as ca
+from vis import vis_mounted_thruster_model
+from scene_elements.scene import Scene
+from scene_elements.sphere import Sphere
+from scene_elements.water import Water
+
+
+def main_clf_rd2():
+    seed = np.random.randint(1000)
+    # seed = 539
+    np.random.seed(seed)
+    print('seed: {}'.format(seed))
+
+    DT = 0.1
+    boat_model = boat.DiamondMountedThrusters()
+    cts_sim_model = empirical_drag.EmpiricalDragCts(
+        boat_model,
+        np.array([.1, .5]) * 1.,
+        np.array([.5, .1]) * 1.,
+    )
+    x0 = np.array([0., 4, 1, 2, np.pi/2, np.pi/4 * 1])
+    x0[2:4] = np.random.rand(2) * 3
+    x0[5] *= np.random.randn()
+    x1 = np.array([0, 0, 0, 0, -np.pi/2, 0])
+    obstacle_xy = np.array([
+        [0., 2],
+    ])
+
+    eps_rate = 1.
+    c_delta = 1.
+    c_alpha = 1.
+    min_distance = 1.
+    n = boat_model.f.shape[1]
+    u_min = np.ones((n,)) * -2.
+    u_max = np.ones((n,)) * 2.
+    clf_controller = ca.MinNormRd2ClfController(
+        eps_rate, c_delta, u_min, u_max,
+    )
+    m = x0.size
+    pos_selection_inds = [0, 1, 4]
+    vel_selection_inds = [2, 3, 5]
+    C = ca.make_C_matrix(pos_selection_inds, m)
+    cbf_controller = ca.MinNormRd2CbfObstacleController(
+        clf_controller, C[:2, :], c_alpha, min_distance,
+    )
+    C_dot = ca.make_C_matrix(vel_selection_inds, m)
+    cbf_controller.set_setpoint(C, x1[pos_selection_inds], C_dot)
+    cbf_controller.set_obstacles(obstacle_xy)
+
+    scene = Scene([Sphere(obstacle_xy[i], color='red')
+                   for i in range(obstacle_xy.shape[0])] + [Water()])
+    T = 10
+    x, u = stepping.solve_cts_eqns(
+        cts_sim_model, x0, cbf_controller, DT, T,
+        cts_sim_model=cts_sim_model,
+    )
+    np.set_printoptions(suppress=True)
+    print(x.T.round(2))
+    print(u.T.round(2))
+    print(np.linalg.norm(x.T[:, :2] - obstacle_xy[0], axis=1) > min_distance)
+    vis_mounted_thruster_model.loop_sim(
+        boat_model, x.T[:, [0, 1]], x.T[:, 4], u.T, DT, scene=scene)
+
+
+if __name__ == '__main__':
+    main_clf_rd2()

vis/vis_mounted_thruster_model.py

@@ -17,7 +17,7 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
     :param camera:
     :return:
     """
-    fig, ax = plt.subplots(1, 2)
+    fig, ax = plt.subplots(1, 2, figsize=(8, 4))
     k = theta.size
     ax[0].set_xticks(np.mgrid[-50:50:1])
     ax[0].set_yticks(np.mgrid[-50:50:1])
@@ -26,6 +26,7 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
     ax[0].set_aspect('equal')
     ax[1].set_aspect('equal')
     ax[0].grid(True)
+    ax[0].set_axisbelow(True)
 
     artists = draw_model(ax[0], model, xy[0], theta[0])
     scene.draw_top_view(ax[0])
@@ -45,11 +46,11 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
         scene.draw_image_view(ax[1], camera)
 
     ani = animation.FuncAnimation(
-        fig, draw_fcn, k, fargs=(artists,), interval=50, repeat_delay=500, blit=False)
+        fig, draw_fcn, k, fargs=(artists,), interval=100, repeat_delay=500, blit=False)
     plt.show()
 
 
-def loop_sim(model, xy, theta, u, dt, **kwargs):
+def loop_sim(model, xy, theta, u, dt, scene=None, **kwargs):
     """
 
     :param model:
@@ -57,6 +58,7 @@ def loop_sim(model, xy, theta, u, dt, **kwargs):
     :param theta: k, | orientation of rigid body
     :param u: k, n | control inputs
     :param dt: time between sampled positions
+    :param scene:
     :return:
     """
     fig, ax = plt.subplots()
@@ -67,8 +69,11 @@ def loop_sim(model, xy, theta, u, dt, **kwargs):
     ax.set_ylim([-3, 3])
     ax.set_aspect('equal')
     plt.grid(True)
+    ax.set_axisbelow(True)
 
     artists = draw_model(ax, model, xy[0], theta[0])
+    if scene:
+        scene.draw_top_view(ax)
     time_text = ax.text(0.5, 1.05, 't = {:2.2f}s'.format(0), fontsize=14,
                         horizontalalignment='center',
                         verticalalignment='center', transform=ax.transAxes)