controllers

Controller

Bases: ABC

Controller interface

Source code in bayes_cbf/controllers.py

class Controller(ABC):
    """
    Controller interface
    """
    needs_ground_truth = False
    @abstractmethod
    def control(self, xi, t=None):
        pass

NamedAffineFunc

Bases: ABC

Source code in bayes_cbf/controllers.py

class NamedAffineFunc(ABC):
    @property
    def __name__(self):
        """
        Name used for plots
        """
        return self.name

    @abstractmethod
    def value(self, x):
        """
        Scalar value function
        """

    @abstractmethod
    def b(self, x):
        """
        A(x) @ u - b(x)
        """

    @abstractmethod
    def A(self, x):
        """
        A(x) @ u - b(x)
        """

    def __call__(self, x, u):
        """
        A(x) @ u - b(x)
        """
        return self.A(x) @ u - self.b(x)

A(x) abstractmethod

A(x) @ u - b(x)

Source code in bayes_cbf/controllers.py

@abstractmethod
def A(self, x):
    """
    A(x) @ u - b(x)
    """

__call__(x, u)

A(x) @ u - b(x)

Source code in bayes_cbf/controllers.py

def __call__(self, x, u):
    """
    A(x) @ u - b(x)
    """
    return self.A(x) @ u - self.b(x)

__name__() property

Name used for plots

Source code in bayes_cbf/controllers.py

@property
def __name__(self):
    """
    Name used for plots
    """
    return self.name

b(x) abstractmethod

A(x) @ u - b(x)

Source code in bayes_cbf/controllers.py

@abstractmethod
def b(self, x):
    """
    A(x) @ u - b(x)
    """

value(x) abstractmethod

Scalar value function

Source code in bayes_cbf/controllers.py

@abstractmethod
def value(self, x):
    """
    Scalar value function
    """

QPController

Bases: Controller

Source code in bayes_cbf/controllers.py

class QPController(Controller):
    def __init__(self, x_dim, u_dim, ctrl_reg, clf_relax_weight, net_model,
                 cbfs, clf, unsafe_controller, summary_writer):
        self.x_dim = x_dim
        self.u_dim = u_dim
        self.ctrl_reg = ctrl_reg
        self.clf_relax_weight = clf_relax_weight
        self.net_model = net_model
        self.cbfs = cbfs
        self.clf = clf
        self.unsafe_controller = unsafe_controller
        self.summary_writer = summary_writer

    def _qp_stability(self, clc, t, x, u0, extravars=None):
        """
        SOCP compatible representation of condition

        d clf(x) / dt + gamma * clf(x) < rho
        """
        # grad_clf(x) @ f(x) + grad_clf(x) @ g(x) @ u + gamma * clf(x) < 0
        # ||[0] [y_1; u] + [0]||_2 <= - [grad_clf(x) @ g(x)] u - grad_clf(x) @ ||f(x) - gamma * clf(x)
        m = u0.shape[-1]
        (bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(
            lambda u: clc(t, u), x, u0)
        A, bfb, bfc, d = SOCPController.convert_cbc_terms_to_socp_terms(
            bfe, e, V, bfv, v, extravars)
        # # We want to return in format?
        # (name, (A, b, c, d))
        # s.t. factor * ||A[y_1; u] + b||_2 <= c'u + d
        return (bfc, d)


    def _plots(self, t, xi, y_uopt_t, extravars):
        uopt = y_uopt_t[extravars:]
        x_p = self.clf._planner.plan(t)
        self.summary_writer.add_scalar('QPController/plan0', x_p[0], t)
        self.summary_writer.add_scalar('QPController/plan1', x_p[1], t)
        self.summary_writer.add_scalar('QPController/plan2', x_p[2], t)
        self.summary_writer.add_scalar('QPController/clf', self.clf._clf(xi, x_p), t)
        self.summary_writer.add_scalar(
            'QPController/clf/dot',
            self.clf._dot_clf_gp(t, x_p, uopt).mean(xi), t)
        self.summary_writer.add_scalar(
            'QPController/clf/dotctrl',
            self.clf._grad_clf_x(xi, x_p) @ self.clf.model.g_func(xi) @ uopt, t)
        self.summary_writer.add_scalar('QPController/clc', self.clf.clc(t, uopt).mean(xi), t)
        self.summary_writer.add_scalar('QPController/ρ', y_uopt_t[0], t)

    def control(self, xi, t=None, extravars=1):
        tic = time.time()
        u_ref = self.unsafe_controller.control(xi, t=t)
        assert extravars == 1, "I assumed extravars to be δ"
        m = u_ref.shape[-1]
        A = np.zeros((extravars+m, extravars+m))
        sqrt_Q = np.eye(self.u_dim) * math.sqrt(self.ctrl_reg)
        A[0, 0] = math.sqrt(self.clf_relax_weight)
        A[extravars:, extravars:] = to_numpy(sqrt_Q)
        bfb = np.zeros((extravars+m,))
        (bfc, d) = list(map(to_numpy, self._qp_stability(
            self.clf.clc, t, xi, u_ref,
            extravars=extravars)))
        y_uopt_init = np.hstack([np.zeros(extravars), u_ref.detach().numpy()])
        y_uopt = optimizer_qp_cvxpy(
            y_uopt_init,
            (A, bfb),
            [('Safety', (bfc, d))])
        # assert bfc @ y_uopt + d >= 0
        y_uopt_t = torch.from_numpy(y_uopt).to(dtype=xi.dtype, device=xi.device)
        uopt = y_uopt_t[extravars:]
        print("Controller step {0:d} took {1:.4f} sec".format(t, time.time()- tic))

        self._plots(t, xi, y_uopt_t, extravars)
        return uopt

SOCPController

Bases: Controller

Source code in bayes_cbf/controllers.py

class SOCPController(Controller):
    def __init__(self, x_dim, u_dim, ctrl_reg, clf_relax_weight, net_model,
                 cbfs, clf, unsafe_controller,
                 summary_writer):
        self.x_dim = x_dim
        self.u_dim = u_dim
        self.ctrl_reg = ctrl_reg
        self.clf_relax_weight = clf_relax_weight
        self.net_model = net_model
        self.cbfs = cbfs
        self.clf = clf
        self.unsafe_controller = unsafe_controller
        self.summary_writer = summary_writer

    def _socp_objective(self, i, x, u0, yidx=0, extravars=None):
        # s.t. ||[0, 1, Q][y_1; ρ; u] - Q u_0||_2 <= [1, 0, 0] [y_1; ρ; u] + 0
        # s.t. ||R[y_1; ρ; u] + h||_2 <= a' [y_1; ρ; u] + b
        assert yidx < extravars

        # R = [ 0, √λ,  0 ]
        #     [ 0, 0 , √Q ]
        # h = [0, - √Q u₀]
        # a = [1, 0, 0]
        # b = 0
        sqrt_Q = torch.eye(self.u_dim) * math.sqrt(self.ctrl_reg)
        λ = self.clf_relax_weight
        assert extravars >= 2
        R = torch.zeros(self.u_dim + 1, self.u_dim + extravars)
        h = torch.zeros(self.u_dim + 1)
        with torch.no_grad():
            assert extravars >= 2
            R[0, 1] = math.sqrt(λ ) # for δ
            R[1:, extravars:] = sqrt_Q
            h[1:] = - sqrt_Q @ u0
        a = torch.zeros((self.u_dim + extravars,))
        a[yidx] = 1
        b = torch.tensor(0.)
        # s.t. ||R[y_1; ρ; u] + h||_2 <= a' [y_1; ρ; u] + b
        return (R, h, a, b)


    @staticmethod
    def convert_cbc_terms_to_socp_terms(bfe, e, V, bfv, v, extravars,
                                        testing=False):
        m = bfe.shape[-1]
        bfc = bfe.new_zeros((m+extravars))
        if testing:
            u0 = torch.zeros((m,))
            u0_hom = torch.cat((torch.tensor([1.]), u0))
        with torch.no_grad():
            # [1, u] Asq [1; u]
            Asq = torch.cat(
                (
                    torch.cat((torch.tensor([[v]]),         (bfv / 2).reshape(1, -1)), dim=-1),
                    torch.cat(((bfv / 2).reshape(-1, 1),    V), dim=-1)
                ),
                dim=-2)
            if testing:
                np.testing.assert_allclose(u0_hom @ Asq @ u0_hom,
                                        u0 @ V @ u0 + bfv @ u0 + v)

            # [1, u] Asq [1; u] = |L[1; u]|_2 = |[0, A] [y_1; y_2; u] + b|_2
            n_constraints = m+1
            try:
                L = torch.cholesky(Asq) # (m+1) x (m+1)
            except RuntimeError as err:
                if "cholesky" in str(err) and "singular" in str(err):
                    L = torch.cholesky(Asq + torch.diag(torch.ones(m+1)*1e-3))
                else:
                    raise
            # try:
            #     n_constraints = m+1
            #     L = torch.cholesky(Asq) # (m+1) x (m+1)
            # except RuntimeError as err:
            #     if "cholesky" in str(err) and "singular" in str(err):
            #         if torch.allclose(v, torch.zeros((1,))) and torch.allclose(bfv, torch.zeros(bfv.shape[0])):
            #             L = torch.zeros((m+1, m+1))
            #             L[1:, 1:] = torch.cholesky(V)
            #         else:
            #             diag_e, U = torch.symeig(Asq, eigenvectors=True)
            #             n_constraints = torch.nonzero(diag_e, as_tuple=True)[0].shape[-1]
            #             L = torch.diag(diag_e).sqrt() @ U[:, -n_constraints:]
            #     else:
            #         raise

        if testing:
            np.testing.assert_allclose(L @ L.T, Asq, rtol=1e-2, atol=1e-3)

        A = torch.zeros((n_constraints, m + extravars))
        A[:, extravars:] = L.T[:, 1:]
        bfb = L.T[:, 0] # (m+1)
        if testing:
            y_u0 = torch.cat((torch.zeros(extravars), u0))
            np.testing.assert_allclose(A @ y_u0 + bfb, L.T @ u0_hom)
            np.testing.assert_allclose(u0_hom @ Asq @ u0_hom, u0_hom @ L @ L.T @ u0_hom, rtol=1e-2, atol=1e-3)
            np.testing.assert_allclose(u0 @ V @ u0 + bfv @ u0 + v, (A @ y_u0 + bfb) @ (A @ y_u0 + bfb), rtol=1e-2, atol=1e-3)
        assert extravars >= 1, "I assumed atleast δ "
        bfc[extravars-1] = 1 # For delta the relaxation factor
        bfc[extravars:] = bfe # only affine terms need to be negative?
        d = e
        return A, bfb, bfc, d

    def _socp_stability(self, clc, t, x, u0, extravars=None):
        """
        SOCP compatible representation of condition

        d clf(x) / dt + gamma * clf(x) < rho
        """
        # grad_clf(x) @ f(x) + grad_clf(x) @ g(x) @ u + gamma * clf(x) < 0
        # ||[0] [y_1; u] + [0]||_2 <= - [grad_clf(x) @ g(x)] u - grad_clf(x) @ ||f(x) - gamma * clf(x)
        m = u0.shape[-1]
        (bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(
            lambda u: clc(t, u), x, u0)
        A, bfb, bfc, d = self.convert_cbc_terms_to_socp_terms(
            bfe, e, V, bfv, v, extravars)
        # # We want to return in format?
        # (name, (A, b, c, d))
        # s.t. factor * ||A[y_1; u] + b||_2 <= c'u + d
        return (A, bfb, bfc, d)

    def _socp_safety(self, cbc2, x, u0,
                     safety_factor=None,
                     extravars=None):
        """
        Var(CBC2) = Au² + b' u + c
        E(CBC2) = e' u + e
        """
        factor = safety_factor
        m = u0.shape[-1]

        (bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(cbc2, x, u0)
        with torch.no_grad():
            # [1, u] Asq [1; u]
            Asq = torch.cat(
                (
                    torch.cat((torch.tensor([[v]]),         (bfv / 2).reshape(1, -1)), dim=-1),
                    torch.cat(((bfv / 2).reshape(-1, 1),    V), dim=-1)
                ),
                dim=-2)

            # [1, u] Asq [1; u] = |L[1; u]|_2 = |[0, A] [y_1; y_2; u] + b|_2
            A = torch.zeros((m + 1, m + extravars))
            try:
                L = torch.cholesky(Asq) # (m+1) x (m+1)
            except RuntimeError as err:
                if "cholesky" in str(err) and "singular" in str(err):
                    diag_e, V = torch.symeig(Asq, eigenvectors=True)
                    L = torch.max(torch.diag(diag_e),
                                    torch.tensor(0.)).sqrt() @ V.t()
                else:
                    raise
            A[:, extravars:] = L[:, 1:]
            b = L[:, 0] # (m+1)
            c = torch.zeros((m+extravars,))
            c[extravars:] = bfe
            # # We want to return in format?
            # (name, (A, b, c, d))
            # s.t. factor * ||A[y_1; u] + b||_2 <= c'x + d
            return (factor * A, factor * b, c, e)

    def _named_socp_constraints(self, t, x, u_ref, convert_out=to_numpy, extravars=None):
        constraints = [
            ("Objective",
             list(map(to_numpy, self._socp_objective(t, x, u_ref, yidx=0,
                                                     extravars=extravars))))
        ] + [
            ("Safety_%d gt 0" % i,
             list(map(to_numpy, self._socp_safety(
                 cbf.cbc,
                 x, u_ref,
                 safety_factor=cbf.safety_factor(),
                 extravars=extravars))))
             for i, cbf in enumerate(self.cbfs)
        ]
        if self.clf is not None:
            constraints += [ ("Stability gt 0",
                              list(map(convert_out,
                                       self._socp_stability(
                                           self.clf.clc,
                                           t, x,
                                           u_ref,
                                           extravars=extravars))))
            ]
        return constraints

    def control(self, xi, t=None, extravars=2):

        tic = time.time()
        u_ref = self.unsafe_controller.control(xi, t=t)
        y_uopt_init = np.hstack([np.zeros(extravars), u_ref.detach().numpy()])
        assert extravars == 2, "I assumed extravars to be δ"
        linear_obj = np.hstack([np.array([1., 0]), np.zeros(u_ref.shape)])
        try:
            y_uopt = optimizer_socp_cvxpy(
                y_uopt_init,
                linear_obj,
                self._named_socp_constraints(t, xi, u_ref,
                                             convert_out=to_numpy, extravars=extravars))
        except InfeasibleProblemError as e:
            y_uopt = torch.from_numpy(y_uopt_init).to(dtype=xi.dtype,
                                                        device=xi.device)
            raise
        y_uopt_t = torch.from_numpy(y_uopt).to(dtype=xi.dtype, device=xi.device)
        uopt = y_uopt_t[extravars:]
        print("Controller step {0:d} took {1:.4f} sec".format(t, time.time()- tic))
        return uopt