From dcd553b97a2ea4fb9cb6708852832b33c7b26395 Mon Sep 17 00:00:00 2001
From: andersct <andersct@umich.edu>
Date: Sat, 26 Sep 2020 23:01:59 -0400
Subject: [PATCH] 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 | 188 ++++++++++++++++++
controls/sdre.py | 22 +-
controls/stepping.py | 10 +-
controls/time_based.py | 9 +-
examples/display_control_affine_clf.py | 77 +++++++
misc/riccati.py | 21 ++
state_space_models/empirical_drag.py | 40 ++--
.../simple_planar_integrator.py | 57 ++----
vis/vis_mounted_thruster_model.py | 2 +-
9 files changed, 345 insertions(+), 81 deletions(-)
create mode 100644 controls/control_affine_clf.py
create mode 100644 examples/display_control_affine_clf.py
create mode 100644 misc/riccati.py
diff --git a/controls/control_affine_clf.py b/controls/control_affine_clf.py
new file mode 100644
index 0000000..2522e59
--- /dev/null
+++ b/controls/control_affine_clf.py
@@ -0,0 +1,188 @@
+"""
+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
diff --git a/controls/sdre.py b/controls/sdre.py
index 69a2de7..77256b6 100644
--- a/controls/sdre.py
+++ b/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
diff --git a/controls/stepping.py b/controls/stepping.py
index 5c7e05b..a34a946 100644
--- a/controls/stepping.py
+++ b/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(
diff --git a/controls/time_based.py b/controls/time_based.py
index d29e3ba..1cb4db2 100644
--- a/controls/time_based.py
+++ b/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
diff --git a/examples/display_control_affine_clf.py b/examples/display_control_affine_clf.py
new file mode 100644
index 0000000..d3df04a
--- /dev/null
+++ b/examples/display_control_affine_clf.py
@@ -0,0 +1,77 @@
+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()
diff --git a/misc/riccati.py b/misc/riccati.py
new file mode 100644
index 0000000..c4c4b11
--- /dev/null
+++ b/misc/riccati.py
@@ -0,0 +1,21 @@
+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
diff --git a/state_space_models/empirical_drag.py b/state_space_models/empirical_drag.py
index 2aaccdd..49b8888 100644
--- a/state_space_models/empirical_drag.py
+++ b/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
diff --git a/state_space_models/simple_planar_integrator.py b/state_space_models/simple_planar_integrator.py
index 6001560..7af129e 100644
--- a/state_space_models/simple_planar_integrator.py
+++ b/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()
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/vis/vis_mounted_thruster_model.py b/vis/vis_mounted_thruster_model.py
index 1926eed..3cac623 100644
--- a/vis/vis_mounted_thruster_model.py
+++ b/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()
--
GitLab