In [1]:
%pylab inline
import control
import scipy.integrate
import sympy
import sympy.physics.mechanics as mech
from sympy.utilities.autowrap import ufuncify
sympy.init_printing()
rcParams['lines.linewidth']=3
rcParams['font.size']=15
rcParams['figure.figsize']=(15,5)
In [2]:
def derive_eoms(c_dict):
# variables
theta, m, g, J, t, l, T = sympy.symbols('theta, m, g, J, t, l, T')
# frames
frame_e = mech.ReferenceFrame('e')
frame_b = frame_e.orientnew('b', 'Axis', (theta(t), frame_e.y))
# point p at axle
point_p = mech.Point('p')
point_p.set_vel(frame_e, 0)
# center of mass
point_cm = point_p.locatenew('cm', -l*frame_b.z)
point_cm.set_vel(frame_b, 0)
point_cm.v1pt_theory(otherpoint=point_p, outframe=frame_e, interframe=frame_b)
# rigid body
body_pend = mech.RigidBody(
name='pend',
masscenter=point_p,
frame=frame_b,
mass=m,
inertia=(mech.inertia(frame_b, ixx=0, iyy=J, izz=0), point_p))
# lagrangian
L = mech.Lagrangian(frame_e, body_pend)
# lagranges method
lm = mech.LagrangesMethod(
Lagrangian=L,
q_list=[theta(t)],
coneqs=None,
forcelist= [(point_cm, m*g*frame_e.z), (frame_b, T(t)*frame_b.y)],
frame=frame_e)
lm.form_lagranges_equations()
rhs = lm.rhs()
x = sympy.DeferredVector('x')
u = sympy.DeferredVector('u')
x_dict = {theta(t).diff(t):x[1],theta(t):x[0]}
u_dict = {T(t):u[0]}
f_sim = sympy.lambdify((t,x,u), rhs.subs(x_dict).subs(u_dict).subs(c_dict))
x_vect = [theta(t), theta(t).diff(t)]
u_vect = [T(t)]
return f_sim, rhs.jacobian(x_vect).subs(c_dict), rhs.jacobian(u_vect).subs(c_dict), rhs
In [3]:
def linearize_eoms(A_sym, B_sym, x0):
theta, t = sympy.symbols('theta, t')
A = array(A_sym.subs(theta(t), x0[0])).astype(float)
B = array(B_sym.subs(theta(t), x0[0])).astype(float)
return A, B
In [4]:
def f_cont_lqr(x, K, x0, u0):
return u0 - K.dot(x - x0)
In [5]:
def derive_dyn_inv_contr(A_closed, rhs, c_dict):
theta, t, T, theta_0 = sympy.symbols('theta, t, T, theta_0')
x = sympy.Matrix([theta(t) - theta_0, theta(t).diff(t)])
x_dot_des = A_closed*x
assert(sympy.simplify(x_dot_des[0] - rhs[0]) == 0)
theta_ddot = rhs[1]
theta_ddot_des = x_dot_des[1]
dyn_inv_expr = sympy.solve(theta_ddot_des - theta_ddot, T(t))[0].subs(c_dict)
x = sympy.DeferredVector('x')
u = sympy.DeferredVector('u')
x_dict = {theta(t).diff(t):x[1],theta(t):x[0]}
f_dyn_inv = sympy.lambdify((x, theta_0), sympy.Matrix([dyn_inv_expr.subs(x_dict)]))
return f_dyn_inv, dyn_inv_expr
In [6]:
def derive_dyn_inv_saturation(dyn_inv_expr):
theta, t, a, b, u_max, theta_0 = sympy.symbols('theta, t, a, b, u_max, theta_0')
dyn_inv_sat_expr = sympy.solve(dyn_inv_expr.subs(theta(t).diff(t), b).subs(theta(t), a) - u_max, b)[0]
f_dyn_inv_sat = ufuncify((a, u_max, theta_0), dyn_inv_sat_expr)
return f_dyn_inv_sat, dyn_inv_sat_expr
In [7]:
def sim_trajectory(f_sim, f_cont, t, x0, u_max):
sim = scipy.integrate.ode(f_sim)
n_t = len(t)
n_x = len(x0)
sim.set_initial_value(y=x0, t=t[0])
y = zeros((n_t, len(x0)))
y[0,:] = x0
for i in range(1, len(t)):
u = f_cont(sim.y)
if (abs(u) > u_max):
u = sign(u)*u_max
sim.set_f_params(u)
sim.integrate(t[i])
while sim.y[0] > pi:
sim.y[0] -= 2*pi
while sim.y[0] < -pi:
sim.y[0] += 2*pi
if not sim.successful():
break
y[i,:] = sim.y.T
return y
In [8]:
def sim_x0_box(f_sim, f_cont, x0_list, t=arange(0, 10, 0.02), u_max=10):
y = zeros((len(x0_list), len(t), len(x0_list[0])))
for i in range(len(x0_list)):
y[i,:,:] = sim_trajectory(f_sim, f_cont, t, x0_list[i,:], u_max)
return y
In [9]:
def do_plotting(op, show_legend=False):
x0 = op['x0']
x0_list = op['x0_list']
y_lqr = op['y_lqr']
y_dyn_inv = op['y_dyn_inv']
f_dyn_inv_sat = op['f_dyn_inv_sat']
u_max = op['u_max']
hold(True)
for i in range(len(x0_list)):
h_lqr = plot(rad2deg(y_lqr[i,:,0]), rad2deg(y_lqr[i,:,1]), 'k-')
h_dyn_inv = plot(rad2deg(y_dyn_inv[i,:,0]), rad2deg(y_dyn_inv[i,:,1]), 'g-')
h_x0 = plot(rad2deg(x0_list[:,0]), rad2deg(x0_list[:,1]),'ro')
x = linspace(y_dyn_inv[:,:,0].min(),y_dyn_inv[:,:,0].max())
h_sat = plot(rad2deg(x), rad2deg(f_dyn_inv_sat(x, x0[0], u_max)), 'k--')
plot(rad2deg(x), rad2deg(f_dyn_inv_sat(x, x0[0], -u_max)), 'k--')
xlabel('$\\theta$, deg')
ylabel('$\dot{\\theta}$, deg/s')
if show_legend:
legend([h_lqr[0], h_dyn_inv[0], h_x0[0], h_sat[0]],
['LQR', 'DI', '$x_0$', 'saturation'], loc='best', ncol=4)
#axis(rad2deg([y_dyn_inv[:,:,0].min(), y_dyn_inv[:,:,0].max(),
# y_dyn_inv[:,:,1].min(), y_dyn_inv[:,:,1].max()]))
In [10]:
def operating_point(x0, c_dict, d=None, n_x0=3):
if d is None:
d = pi/10*ones(len(x0))
u_max = c_dict['u_max']
f_sim, A_sym, B_sym, rhs = derive_eoms(c_dict)
x0_list = x0 + array([[i, j] for i in linspace(-d[0], d[0], n_x0) for j in linspace(-d[1], d[1], n_x0)])
# lqr control
A, B = linearize_eoms(A_sym, B_sym, x0)
Q = diag([1,0])
R = eye(1)
K = control.care(A, B, Q=Q, R=R)[2]
ss_open = control.ss(A, B, eye(2), zeros((2,1)))
wn_open, zeta_open, P_open = ss_open.damp()
ss_closed = ss_open.feedback(K)
ss_closed *= 1.0/norm(ss_closed.horner(0))
wn_closed, zeta_closed, P_closed = ss_closed.damp()
# use same speed as lqr for dyn. inv.
f_dyn_inv, dyn_inv_expr = derive_dyn_inv_contr(ss_closed.A, rhs, c_dict)
f_dyn_inv_sat, dyn_inv_sat_expr = derive_dyn_inv_saturation(dyn_inv_expr)
u0 = f_dyn_inv(x0, x0[0])
y_lqr = sim_x0_box(f_sim, lambda x: f_cont_lqr(x, K, x0, u0), x0_list, u_max=u_max)
y_dyn_inv = sim_x0_box(f_sim, lambda x: f_dyn_inv(x, x0[0]), x0_list, u_max=u_max)
return {
'x0': x0,
'x0_list': x0_list,
'y_lqr': y_lqr,
'y_dyn_inv': y_dyn_inv,
'f_dyn_inv_sat': f_dyn_inv_sat,
'ss_closed': ss_closed,
'ss_open': ss_closed,
'u_max': u_max,
'n_x0': n_x0,
'd': d,
'c_dict': c_dict,
}
In [11]:
# dynamics
c_dict = {'m':1, 'J':1, 'g':9.8, 'l':1, 'u_max': 10}
d = [pi/10, pi/5]
n_x0 = 3
op1 = operating_point([-pi/4,0], c_dict, d, n_x0)
op2 = operating_point([0,0], c_dict, d, n_x0)
op3 = operating_point([pi/4,0], c_dict, d, n_x0)
In [12]:
figure(figsize(15,10))
do_plotting(op1, show_legend=True)
do_plotting(op2)
do_plotting(op3)
grid()
text(-60, -250, 'back mode')
text(0, -250, 'stop mode')
text(40, -250, 'forward mode')
title('control modes')
Out[12]:
In [13]:
A = op1['ss_closed'].A
B = op1['ss_closed'].B
control.lyap(A.T.dot(A), eye(2))
Out[13]:
In [14]:
P = row_stack([column_stack([A.T*A, A.T*B]),
column_stack([B.T*A, B.T*B])])
Q = 10*eye(2)
P = rand(3,3)
control.lyap(A, Q)
Out[14]:
In [15]:
P
Out[15]:
In [16]:
shape(P), shape(Q)
Out[16]:
In [17]:
P.shape
Out[17]:
In [18]:
linalg.eig(P)
Out[18]:
In [19]:
linalg.eig(-P)
Out[19]:
$ \newcommand{\pd}[2]{\frac{\partial#1}{\partial#2}} $ Click to see latex command definitions.
source: AAE 666 Class Notes (Martin Corless) pg. 178/ pg. 265
Consider a disturbed linear system:
\begin{align*} \dot{x} = Ax + Bw\\ z = Cx + Dw \end{align*}where all eigenvalues of A have negative real part and w is a bounded input. Suppose there exiss a positive real scalar $\alpha$ such that:
\begin{align*} \begin{pmatrix} PA + A^TP + \alpha P & PB\\ B^TP & -\alpha \mu_1 I \end{pmatrix} < 0\\ \begin{pmatrix} C^TC - P & C^T D \\ D^TC & D^TD - \mu_2 I \end{pmatrix} < 0 \end{align*}Then $\lim \sup_{t \rightarrow \infty} ||z(t)||_{\infty} \leq \gamma ||w(t)||_{\infty}$ where
\begin{align*} \gamma = \sqrt{\mu_1 + \mu_2} \end{align*}
In [20]:
import picos as pic
import cvxopt as cvx
import numpy as np
import scipy.linalg
import scipy.optimize
import control
from IPython.core.display import Image
In [21]:
def solve_bounded_disturbance():
def solve_lmi(A_data, B_data, C_data, D_data, alpha, verbose=False):
sdp = pic.Problem()
# variables
P = sdp.add_variable('P', (2,2), vtype='symmetric')
alpha = pic.new_param('alpha', alpha)
mu_1 = sdp.add_variable('mu_1')
mu_2 = sdp.add_variable('mu_2')
# parameters
A = pic.new_param('A', cvx.matrix(A_data))
B = pic.new_param('B', cvx.matrix(B_data))
C = pic.new_param('C', cvx.matrix(C_data))
D = pic.new_param('D', cvx.matrix(D_data))
I = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(2))))
sdp.add_constraint(
(P*A + A.T*P + alpha*P & P*B) //
(B.T*P & -alpha*mu_1*I) << 0)
sdp.add_constraint(
(C.T*C - P & C.T*D) //
(D.T*C & D.T*D - mu_2*I) << 0)
sdp.add_constraint(P >> 1e-10*I)
sdp.solve(verbose=verbose)
return sdp
A = np.array([[-1,0],[0,-2]])
B = np.eye(2)
C = np.eye(2)
D = np.zeros((2,2))
sdp = solve_lmi(A, B, C, D, 10.0)
# we use fmin to solve for minimum alpha by iteratively solving lmi
alpha_opt = scipy.optimize.fmin(lambda alpha: -alpha if (
alpha > 0 and solve_lmi(A, B, C, D, alpha).status == 'optimal')
else 0.5*abs(alpha), 0)[0]
sdp = solve_lmi(A, B, C, D, alpha_opt)
print sdp
print 'optimal alpha: ', alpha_opt
print 'gamma: ', np.sqrt(
sdp.variables['mu_1'].value[0,0] +
sdp.variables['mu_2'].value[0,0])
solve_bounded_disturbance()