Added controllers based on control lyapunov functions

- refactor state space models to return function info as dictionaries
- move riccati solver to misc
- add derivative calculation to empirical drag model
- add min norm controller for relative degree 1 control (accel -> v)
- add min norm controller for relative degree 2 control (accel -> pos)
- add documentation for the min norm controllers

controls/control_affine_clf.py

+"""
+Implements control laws formed by control lyapunov functions (CLF)
+for processes that are affine in the control:
+
+x_dot = f(x) + g(x)u
+
+(Let x \in \reals^m denote state, u \in \reals^n denote control,
+so g(x) \in \reals^{m,n}).
+The CLFs are based on feedback linearization, which looks at the
+output we seek to control, say,
+
+drive y(x) = Cx - d -> 0
+
+and chooses the control u based on the the output's derivative
+
+y_dot = L_f y(x) + L_g y(x) u
+
+where L_f is the Lie derivative wrt f(x); L_f y(x) = dy(x)/dx f(x).
+The control is chosen like u = [L_g y(x)]^-1 (-L_f y(x) + mu) to obtain
+
+y_dot = mu
+
+This mu can then be chosen to satisfy desirable CLF properties such
+as exponential stabilization.
+"""
+import numpy as np
+from misc import riccati
+from scipy import optimize as opt
+
+
+class MinNormRd1ClfController(object):
+    """
+    Let x \in \reals^m denote state, y \in \reals^r be an output we
+    seek to control.
+    Use a CLF to drive
+    y = Cx - d -> 0
+    at a given exponential rate, selecting the minimum norm control.
+    This controller applies only for y with a relative degree 1,
+    i.e., if u = acceleration, y must be velocities.
+
+    Specifically, the set of controls satisfying the rate is taken as:
+    {u \in \reals^n | L_f V(x) + L_g V(x) u + eps V(x) <= 0}
+    =
+    {u \in \reals^n | phi_0(x) + phi_1(x)' u <= 0}
+    where
+    V(x) = y(x)'y(x)
+    phi_0(x) = L_f V(x) + eps V(x)
+             = 2y'Cf(x) + eps y'y
+    phi_1(x) = (L_g V(x) )'
+             = [2y'Cg(x)]'
+    The minimum norm control is given by choosing the minimum norm element
+    of the set. The minimum norm control is
+    u = {  0                                  if phi_0(x) <= 0
+         -phi_0(x)phi_1(x)/||phi_1(x)||^2_2   else
+
+    This implementation closely follows material from:
+    A. D. Ames, J. W. Grizzle, and P. Tabuada. "Control barrier function based
+    quadratic programs with application to adaptive cruise control," in
+    IEEE Conference on Decision and Control (CDC), 2014, pp. 6271-6278.
+    """
+
+    def __init__(self, eps_rate):
+        """
+        :param eps_rate: rate for exponential convergence to setpoint
+        """
+        self.eps_rate = eps_rate
+        self.C = np.zeros(())
+        self.d = np.zeros(())
+
+    def set_setpoint(self, C, d):
+        self.C = C
+        self.d = d
+
+    def __call__(self, x, t, fx=None, gx=None, **kwargs):
+        y = self.C.dot(x) - self.d
+        phi_zero = 2 * y.T.dot(self.C).dot(fx) + self.eps_rate * y.dot(y)  # scalar
+        phi_one = 2 * y.T.dot(self.C).dot(gx)  # n,
+        if phi_zero <= 0:
+            return 0 * phi_one
+        bot = np.linalg.norm(phi_one) ** 2
+        u = -phi_zero * phi_one / max(bot, 1e-8)
+        return u
+
+
+class MinNormRd2ClfController(object):
+    """
+    Let x \in \reals^m denote state, y \in \reals^r be an output we
+    seek to control.
+    Use a CLF to drive
+    y = Cx - d -> 0
+    at a given exponential rate, selecting the minimum norm control.
+    This controller applies only for y with a relative degree 2,
+    i.e., if u = acceleration, y must be position/heading.
+
+    Compared to the case of relative degree 1, we need to take one
+    more derivative to get to the form y_dot = mu.
+
+    This implementation closely follows material from:
+    A. D. Ames and M. Powell. "Towards the unification of locomotion and
+    manipulation through control lyapunov functions and quadratic programs,"
+    in Control of Cyber-Physical Systems. Springer, 2013, pp. 219-240.
+    """
+
+    def __init__(self, eps_rate, c_delta, u_min, u_max):
+        """
+        :param eps_rate: rate for exponential convergence to setpoint
+        :param c_delta: penalty for violating CLF constraint
+        :param u_min: n, | minimum input values
+        :param u_max: n, | maximum input values
+        """
+        self.eps_rate = eps_rate
+        self.c_delta = c_delta
+        self.u_min = u_min
+        self.u_max = u_max
+        self.C = np.zeros(())
+        self.d = np.zeros(())
+        self.C_dot = np.zeros(())  # for extracting y_dot(x_t) from x_t
+        self.F = np.zeros(())
+        self.G = np.zeros(())
+        self.Q = np.zeros(())
+        self.P = np.zeros(())
+
+    def set_setpoint(self, C, d, C_dot, Q=()):
+        self.C = C
+        self.d = d
+        self.C_dot = C_dot
+        r = C.shape[0]
+        self.F = np.zeros((2*r, 2*r))
+        self.F[:r, r:] = np.eye(r)
+        self.G = np.zeros((2*r, r))
+        self.G[r:, :r] = np.eye(r)
+        self.Q = np.eye(2*r) if len(Q) == 0 else Q  # equal weighting
+        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):
+        # CLF
+        y = self.C.dot(x) - self.d
+        y_dot = self.C_dot.dot(x)
+        eta = np.hstack((y, y_dot))
+        V = eta.dot(self.P).dot(eta)
+        PF = self.P.dot(self.F)
+        LfV = eta.dot((PF.T + PF).dot(eta))
+        LgV = 2*eta.dot(self.P).dot(self.G)
+        phi_zero = LfV + self.eps_rate * V
+        phi_one = LgV
+        # input constraints
+        A = self.C.dot(dfxdx).dot(gx)
+        # 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)
+        # solve the QP, writing constraints as Ac z <= bc
+        r = y.size
+        z0 = np.zeros((r+1,))  # z = [mu, delta]
+        hess_vec = np.ones((r+1,))
+        hess_vec[-1] = self.c_delta
+        Ac = np.vstack((phi_one, Ainv, -Ainv))
+        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))
+        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),
+            z0, method='trust-constr',
+            jac=lambda z, *args: hess_vec * z,
+            hessp=lambda x, p, *args: hess_vec * p,
+            constraints=[linear_constraint, ],
+        )
+        z_star = res.x
+        mu_star = z_star[:-1]
+        u_star = Ainv.dot(-Lf + mu_star)
+        return u_star
+
+
+def make_C_matrix(select_inds, m):
+    """
+    Build C matrix for common choices of y = Cx - d where
+    y is a selection of x's values (such as position) and
+    d = y_desired (so Cx - d = 0 implies we are at the desired position)
+    :param select_inds: list of indices of state x
+    :param m: dimension of state
+    :return: r, n | C matrix
+    """
+    r = len(select_inds)
+    C = np.zeros((r, m))
+    C[np.arange(r), select_inds] = 1.
+    return C

controls/sdre.py

@@ -19,7 +19,7 @@ J. R. Cloutier, "State-dependent riccati equation techniques: an overview,"
 in Proceedings of the American Control Conference, 1997, pp. 932-936.
 """
 import numpy as np
-from scipy.linalg import schur
+from misc import riccati
 
 
 class SdreController(object):
@@ -38,25 +38,7 @@ class SdreController(object):
 
     def __call__(self, x, t, A=(), B=(), **kwargs):
         Rinv = np.linalg.inv(self.R)
-        P = solve_cts_riccati_schur(A, B, self.Q, Rinv)
+        P = riccati.solve_cts_riccati_schur(A, B, self.Q, Rinv)
         u = -Rinv.dot(B.T.dot(P.dot(x - self.x_goal)))
         return u
 
-
-def solve_cts_riccati_schur(A, B, Q, Rinv):
-    """
-    Solve via Schur decomposition.
-    :param A:
-    :param B:
-    :param Q:
-    :param Rinv:
-    :return:
-    """
-    m12 = -B.dot(Rinv).dot(B.T)
-    M = np.block([[A, m12], [-Q, -A.T]])
-    T, U = schur(M, sort='lhp')[:2]
-    n = A.shape[0]
-    U11 = U.T[:n, :n]
-    U12 = U.T[:n, n:]
-    P = U12.T.dot(np.linalg.inv(U11).T)
-    return P

controls/stepping.py

@@ -20,8 +20,8 @@ def solve_discrete_difference_eqns(discrete_state_model, x0, u_fcn, DT, T):
     u = np.zeros((n, t.size))
     x[:, 0] = x0.copy()
     for i in range(0, t.size-1):
-        discrete_state_model.update_AB(x[:, i])
-        u[:, i] = u_fcn(x[:, i], i*DT)
+        operators = discrete_state_model.get_process_operators(x[:, i])
+        u[:, i] = u_fcn(x[:, i], i*DT, **operators)
         x[:, i+1] = discrete_state_model.step(x[:, i], u[:, i])
     return x, u
 
@@ -45,10 +45,8 @@ def solve_cts_eqns(cts_state_model, x0, u_fcn, DT, T, cts_sim_model=None):
     u = np.zeros((n, t.size))
     x[:, 0] = x0.copy()
     for i in range(0, t.size - 1):
-        cts_state_model.update_AB(x[:, i])
-        cts_sim_model.update_AB(x[:, i])
-        A, B = cts_state_model.get_AB()
-        u[:, i] = u_fcn(x[:, i], i * DT, A=A, B=B)
+        operators = cts_state_model.get_process_operators(x[:, i])
+        u[:, i] = u_fcn(x[:, i], i * DT, **operators)
         t_span = (i*DT, (i+1 + 0.5)*DT)
         t_eval = (i+1)*DT,
         sol = itg.solve_ivp(

controls/time_based.py

@@ -4,19 +4,20 @@ import numpy as np
 def make_control_signal(time_list, u_list):
     """
 
-    :param time_list: [t1, t2, ..., tn] | times specifying intervals for controls
+    :param time_list: [t1, t2, ..., tk] | times specifying intervals for controls
     - time assumed to start at 0, first interval being t \in [0, t1)
     - after T, u = 0 is supplied
-    :param u_list: m, n | ith column is control corresponding to ith time interval
+    :param u_list: n, k | ith column is control corresponding to ith time interval
     :return:
+        u_fcn: (x, t, **kwargs) -> n, | control input
     """
     n = len(time_list)
     assert u_list.shape[1] == n
 
-    def u(x, t):
+    def u_fcn(x, t, **kwargs):
         if t < 0 or time_list[-1] <= t:
             return 0 * u_list[:, 0]
         i = np.digitize(t, time_list)
         return u_list[:, i]
 
-    return u
+    return u_fcn

examples/display_control_affine_clf.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
+from vis import vis_mounted_thruster_model
+
+
+def main_clf_rd1():
+    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., 0, 1, 3, np.pi/2, np.pi/4 * 1])
+    x1 = np.array([0, 0, 0, 0, -np.pi/2, 0])
+    m = x0.size
+    selection_inds = [2, 3, 5]
+    C = control_affine_clf.make_C_matrix(selection_inds, m)
+    eps_rate = 3.
+    min_norm_controller = control_affine_clf.MinNormRd1ClfController(eps_rate)
+    min_norm_controller.set_setpoint(C, x1[selection_inds])
+    T = 30
+    x, u = stepping.solve_cts_eqns(
+        cts_sim_model, x0, min_norm_controller, DT, T,
+        cts_sim_model=cts_sim_model,
+    )
+    np.set_printoptions(suppress=True)
+    print(x.T.round(2))
+    print(u.T.round(2))
+    vis_mounted_thruster_model.loop_sim(
+        boat_model, x.T[:, [0, 1]], x.T[:, 4], u.T, DT)
+
+
+def main_clf_rd2():
+    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., 0, 1, 2, np.pi/2, np.pi/4 * 1])
+    x1 = np.array([0, 0, 0, 0, -np.pi/2, 0])
+
+    eps_rate = 1.
+    c_delta = 1.
+    n = boat_model.f.shape[1]
+    u_min = np.ones((n,)) * -2.
+    u_max = np.ones((n,)) * 2.
+    min_norm_controller = control_affine_clf.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 = control_affine_clf.make_C_matrix(pos_selection_inds, m)
+    C_dot = control_affine_clf.make_C_matrix(vel_selection_inds, m)
+    min_norm_controller.set_setpoint(C, x1[pos_selection_inds], C_dot)
+
+    T = 10
+    x, u = stepping.solve_cts_eqns(
+        cts_sim_model, x0, min_norm_controller, DT, T,
+        cts_sim_model=cts_sim_model,
+    )
+    np.set_printoptions(suppress=True)
+    print(x.T.round(2))
+    print(u.T.round(2))
+    vis_mounted_thruster_model.loop_sim(
+        boat_model, x.T[:, [0, 1]], x.T[:, 4], u.T, DT)
+
+
+if __name__ == '__main__':
+    # main_clf_rd1()
+    main_clf_rd2()

misc/riccati.py

+import numpy as np
+from scipy.linalg import schur
+
+
+def solve_cts_riccati_schur(A, B, Q, Rinv):
+    """
+    Solve via Schur decomposition.
+    :param A:
+    :param B:
+    :param Q:
+    :param Rinv:
+    :return:
+    """
+    m12 = -B.dot(Rinv).dot(B.T)
+    M = np.block([[A, m12], [-Q, -A.T]])
+    T, U = schur(M, sort='lhp')[:2]
+    n = A.shape[0]
+    U11 = U.T[:n, :n]
+    U12 = U.T[:n, n:]
+    P = U12.T.dot(np.linalg.inv(U11).T)
+    return P

state_space_models/empirical_drag.py

@@ -72,27 +72,39 @@ class EmpiricalDragCts(object):
         self.angular_drag_c = angular_drag_c
 
     def step(self, x_t, u_t):
-        fx, gx = self.get_fx_gx(x_t)
+        operators = self.get_process_operators(x_t)
+        fx, gx = operators['fx'], operators['gx']
         x_dot = fx + gx.dot(u_t)
         return x_dot
 
-    def update_AB(self, x_t):
-        B_t = mb.rot_2d(x_t[4]).dot(self.b_part)
-        self.Bc_t[2:4, :] = B_t
-
-    def get_AB(self):
-        return self.Ac, self.Bc_t
-
-    def get_fx_gx(self, x_t):
-        A_t, B_t = self.get_AB()
+    def get_process_operators(self, x_t):
+        B_t = self.Bc_t
+        B_t[2:4, :] = mb.rot_2d(x_t[4]).dot(self.b_part)
+        A = self.Ac
         f_drag = np.zeros((self.m,))
+        norm_v = np.linalg.norm(x_t[2:4])
         f_drag[2:4] = (self.drag_c[0] +
-                       self.drag_c[1] * np.linalg.norm(x_t[2:4])) * x_t[2:4]
+                       self.drag_c[1] * norm_v) * x_t[2:4]
+        abs_omega = np.abs(x_t[5])
         f_drag[5] = (self.angular_drag_c[0] +
-                     self.angular_drag_c[1] * np.abs(x_t[5])) * x_t[5]
-        fx = A_t.dot(x_t) - f_drag
+                     self.angular_drag_c[1] * abs_omega) * x_t[5]
+        fx = A.dot(x_t) - f_drag
         gx = B_t
-        return fx, gx
+        
+        Ca = np.zeros((2, self.m))
+        dfxdx_drag = self.drag_c[0] * Ca +\
+            self.drag_c[1] * Ca * norm_v +\
+            self.drag_c[1] * np.outer(x_t[2:4], x_t[2:4].dot(Ca) / max(norm_v, 1e-8))
+        Cb = np.zeros((1, self.m))
+        dfxdx_angular_drag = \
+            self.angular_drag_c[0] * Cb + \
+            self.angular_drag_c[1] * Cb * abs_omega + \
+            self.angular_drag_c[1] * x_t[5] * x_t[5] * Cb / max(abs_omega, 1e-8)
+        dfxdx_all_drag = np.zeros((self.m, self.m))
+        dfxdx_all_drag[2:4, :] = dfxdx_drag
+        dfxdx_all_drag[5, :] = dfxdx_angular_drag
+        dfxdx = A - dfxdx_all_drag
+        return dict(A=A, B=B_t, fx=fx, gx=gx, dfxdx=dfxdx)
 
     def get_mn(self):
         return self.m, self.n

state_space_models/simple_planar_integrator.py

@@ -48,21 +48,20 @@ class NoDragCts(object):
             mounted_thruster_model.im
 
     def step(self, x_t, u_t):
-        A_t, B_t = self.get_AB()
-        x_dot = A_t.dot(x_t) + B_t.dot(u_t)
+        operators = self.get_process_operators(x_t)
+        A, B_t = operators['A'], operators['B']
+        x_dot = A.dot(x_t) + B_t.dot(u_t)
         return x_dot
 
-    def update_AB(self, x_t):
+    def get_process_operators(self, x_t):
         """
-        Before taking initial or next step, update to new A_t, B_t matrices
+        Return operators which may be based on current state
         :param x_t:
-        :return:
+        :return: dict{A, B(x_t)}
         """
-        B_t = mb.rot_2d(x_t[4]).dot(self.b_part)
-        self.Bc_t[2:4, :] = B_t
-
-    def get_AB(self):
-        return self.Ac, self.Bc_t
+        B_t = self.Bc_t
+        B_t[2:4, :] = mb.rot_2d(x_t[4]).dot(self.b_part)
+        return dict(A=self.Ac, B=B_t)
 
     def get_mn(self):
         return self.m, self.n
@@ -83,15 +82,13 @@ class AccelApproxDragCts(object):
         self.alpha = alpha
 
     def step(self, x_t, u_t):
-        A_t, B_t = self.get_AB()
-        x_dot = A_t.dot(x_t) + B_t.dot(u_t)
+        operators = self.get_process_operators(x_t)
+        A, B_t = operators['A'], operators['B']
+        x_dot = A.dot(x_t) + B_t.dot(u_t)
         return x_dot
 
-    def update_AB(self, x_t):
-        self.no_drag_cts_model.update_AB(x_t)
-
-    def get_AB(self):
-        return self.no_drag_cts_model.get_AB()
+    def get_process_operators(self, x_t):
+        return self.no_drag_cts_model.get_process_operators(x_t)
 
     def get_mn(self):
         return self.no_drag_cts_model.get_mn()
@@ -122,33 +119,21 @@ class ZeroOrderHoldDiscrete(object):
         self.Bd_t = np.zeros(())
 
     def step(self, x_t, u_t):
-        x_p = self.Ad_t.dot(x_t) + self.Bd_t.dot(u_t)
+        operators = self.get_process_operators(x_t)
+        Ad_t, Bd_t = operators['A'], operators['B']
+        x_p = Ad_t.dot(x_t) + Bd_t.dot(u_t)
         return x_p
 
-    def update_AB(self, x_t):
-        self.cts_model.update_AB(x_t)
-        Ac_t, Bc_t = self.cts_model.get_AB()
+    def get_process_operators(self, x_t):
+        operators = self.cts_model.get_process_operators(x_t)
+        Ac_t, Bc_t = operators['A'], operators['B']
         m = Ac_t.shape[0]
         M = np.block([[Ac_t, np.eye(m)],
                       [np.zeros_like(Ac_t), np.zeros_like(Ac_t)]])
         eM = la.expm(M * self.dt)
         self.Ad_t = eM[0:m, 0:m]
         self.Bd_t = eM[0:m, m:].dot(Bc_t)
-
-    def get_AB(self):
-        return self.Ad_t, self.Bd_t
+        return dict(A=self.Ad_t, B=self.Bd_t)
 
     def get_mn(self):
         return self.cts_model.get_mn()
-
-
-
-
-
-
-
-
-
-
-
-

vis/vis_mounted_thruster_model.py

@@ -80,7 +80,7 @@ def loop_sim(model, xy, theta, u, dt, **kwargs):
         draw_model(ax, model, xy[i], theta[i], u[i], artists=artists)
 
     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()