Skip to content
Snippets Groups Projects
Commit dcd553b9 authored by andersct's avatar andersct
Browse files

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
parent 0d4428c5
No related branches found
No related tags found
No related merge requests found
"""
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
......@@ -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
......@@ -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(
......
......@@ -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
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()
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
......@@ -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
......@@ -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()
......@@ -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()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment