from __future__ import print_function, division
__all__ = ['Linearizer']
from sympy.core.backend import Matrix, eye, zeros
from sympy.core.compatibility import Iterable
from sympy import Dummy
from sympy.utilities.iterables import flatten
from sympy.physics.vector import dynamicsymbols
from sympy.physics.mechanics.functions import msubs
from collections import namedtuple
[docs]class Linearizer(object):
"""This object holds the general model form for a dynamic system.
This model is used for computing the linearized form of the system,
while properly dealing with constraints leading to dependent
coordinates and speeds.
Attributes
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix
Matrices holding the general system form.
q, u, r : Matrix
Matrices holding the generalized coordinates, speeds, and
input vectors.
q_i, u_i : Matrix
Matrices of the independent generalized coordinates and speeds.
q_d, u_d : Matrix
Matrices of the dependent generalized coordinates and speeds.
perm_mat : Matrix
Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T
"""
def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u,
q_i=None, q_d=None, u_i=None, u_d=None, r=None, lams=None):
"""
Parameters
----------
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like
System of equations holding the general system form.
Supply empty array or Matrix if the parameter
doesn't exist.
q : array_like
The generalized coordinates.
u : array_like
The generalized speeds
q_i, u_i : array_like, optional
The independent generalized coordinates and speeds.
q_d, u_d : array_like, optional
The dependent generalized coordinates and speeds.
r : array_like, optional
The input variables.
lams : array_like, optional
The lagrange multipliers
"""
# Generalized equation form
self.f_0 = Matrix(f_0)
self.f_1 = Matrix(f_1)
self.f_2 = Matrix(f_2)
self.f_3 = Matrix(f_3)
self.f_4 = Matrix(f_4)
self.f_c = Matrix(f_c)
self.f_v = Matrix(f_v)
self.f_a = Matrix(f_a)
# Generalized equation variables
self.q = Matrix(q)
self.u = Matrix(u)
none_handler = lambda x: Matrix(x) if x else Matrix()
self.q_i = none_handler(q_i)
self.q_d = none_handler(q_d)
self.u_i = none_handler(u_i)
self.u_d = none_handler(u_d)
self.r = none_handler(r)
self.lams = none_handler(lams)
# Derivatives of generalized equation variables
self._qd = self.q.diff(dynamicsymbols._t)
self._ud = self.u.diff(dynamicsymbols._t)
# If the user doesn't actually use generalized variables, and the
# qd and u vectors have any intersecting variables, this can cause
# problems. We'll fix this with some hackery, and Dummy variables
dup_vars = set(self._qd).intersection(self.u)
self._qd_dup = Matrix([var if var not in dup_vars else Dummy()
for var in self._qd])
# Derive dimesion terms
l = len(self.f_c)
m = len(self.f_v)
n = len(self.q)
o = len(self.u)
s = len(self.r)
k = len(self.lams)
dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k'])
self._dims = dims(l, m, n, o, s, k)
self._setup_done = False
def _setup(self):
# Calculations here only need to be run once. They are moved out of
# the __init__ method to increase the speed of Linearizer creation.
self._form_permutation_matrices()
self._form_block_matrices()
self._form_coefficient_matrices()
self._setup_done = True
def _form_permutation_matrices(self):
"""Form the permutation matrices Pq and Pu."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Compute permutation matrices
if n != 0:
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
if l > 0:
self._Pqi = self._Pq[:, :-l]
self._Pqd = self._Pq[:, -l:]
else:
self._Pqi = self._Pq
self._Pqd = Matrix()
if o != 0:
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
if m > 0:
self._Pui = self._Pu[:, :-m]
self._Pud = self._Pu[:, -m:]
else:
self._Pui = self._Pu
self._Pud = Matrix()
# Compute combination permutation matrix for computing A and B
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
if P_col1:
if P_col2:
self.perm_mat = P_col1.row_join(P_col2)
else:
self.perm_mat = P_col1
else:
self.perm_mat = P_col2
def _form_coefficient_matrices(self):
"""Form the coefficient matrices C_0, C_1, and C_2."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Build up the coefficient matrices C_0, C_1, and C_2
# If there are configuration constraints (l > 0), form C_0 as normal.
# If not, C_0 is I_(nxn). Note that this works even if n=0
if l > 0:
f_c_jac_q = self.f_c.jacobian(self.q)
self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
else:
self._C_0 = eye(n)
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
# o = 0.
if m > 0:
f_v_jac_u = self.f_v.jacobian(self.u)
temp = f_v_jac_u * self._Pud
if n != 0:
f_v_jac_q = self.f_v.jacobian(self.q)
self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
else:
self._C_1 = zeros(o, n)
self._C_2 = (eye(o) - self._Pud *
temp.LUsolve(f_v_jac_u)) * self._Pui
else:
self._C_1 = zeros(o, n)
self._C_2 = eye(o)
def _form_block_matrices(self):
"""Form the block matrices for composing M, A, and B."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Block Matrix Definitions. These are only defined if under certain
# conditions. If undefined, an empty matrix is used instead
if n != 0:
self._M_qq = self.f_0.jacobian(self._qd)
self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q)
else:
self._M_qq = Matrix()
self._A_qq = Matrix()
if n != 0 and m != 0:
self._M_uqc = self.f_a.jacobian(self._qd_dup)
self._A_uqc = -self.f_a.jacobian(self.q)
else:
self._M_uqc = Matrix()
self._A_uqc = Matrix()
if n != 0 and o - m + k != 0:
self._M_uqd = self.f_3.jacobian(self._qd_dup)
self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q)
else:
self._M_uqd = Matrix()
self._A_uqd = Matrix()
if o != 0 and m != 0:
self._M_uuc = self.f_a.jacobian(self._ud)
self._A_uuc = -self.f_a.jacobian(self.u)
else:
self._M_uuc = Matrix()
self._A_uuc = Matrix()
if o != 0 and o - m + k != 0:
self._M_uud = self.f_2.jacobian(self._ud)
self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u)
else:
self._M_uud = Matrix()
self._A_uud = Matrix()
if o != 0 and n != 0:
self._A_qu = -self.f_1.jacobian(self.u)
else:
self._A_qu = Matrix()
if k != 0 and o - m + k != 0:
self._M_uld = self.f_4.jacobian(self.lams)
else:
self._M_uld = Matrix()
if s != 0 and o - m + k != 0:
self._B_u = -self.f_3.jacobian(self.r)
else:
self._B_u = Matrix()
[docs] def linearize(self, op_point=None, A_and_B=False, simplify=False):
"""Linearize the system about the operating point. Note that
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
These may be either symbolic or numeric.
Parameters
----------
op_point : dict or iterable of dicts, optional
Dictionary or iterable of dictionaries containing the operating
point conditions. These will be substituted in to the linearized
system before the linearization is complete. Leave blank if you
want a completely symbolic form. Note that any reduction in
symbols (whether substituted for numbers or expressions with a
common parameter) will result in faster runtime.
A_and_B : bool, optional
If A_and_B=False (default), (M, A, B) is returned for forming
[M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
(A, B) is returned for forming dx = [A]x + [B]r, where
x = [q_ind, u_ind]^T.
simplify : bool, optional
Determines if returned values are simplified before return.
For large expressions this may be time consuming. Default is False.
Potential Issues
----------------
Note that the process of solving with A_and_B=True is
computationally intensive if there are many symbolic parameters.
For this reason, it may be more desirable to use the default
A_and_B=False, returning M, A, and B. More values may then be
substituted in to these matrices later on. The state space form can
then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
P = Linearizer.perm_mat.
"""
# Run the setup if needed:
if not self._setup_done:
self._setup()
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif isinstance(op_point, Iterable):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
else:
op_point_dict = {}
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Rename terms to shorten expressions
M_qq = self._M_qq
M_uqc = self._M_uqc
M_uqd = self._M_uqd
M_uuc = self._M_uuc
M_uud = self._M_uud
M_uld = self._M_uld
A_qq = self._A_qq
A_uqc = self._A_uqc
A_uqd = self._A_uqd
A_qu = self._A_qu
A_uuc = self._A_uuc
A_uud = self._A_uud
B_u = self._B_u
C_0 = self._C_0
C_1 = self._C_1
C_2 = self._C_2
# Build up Mass Matrix
# |M_qq 0_nxo 0_nxk|
# M = |M_uqc M_uuc 0_mxk|
# |M_uqd M_uud M_uld|
if o != 0:
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
if k != 0:
col3 = Matrix([zeros(n + m, k), M_uld])
if n != 0:
col1 = Matrix([M_qq, M_uqc, M_uqd])
if o != 0 and k != 0:
M = col1.row_join(col2).row_join(col3)
elif o != 0:
M = col1.row_join(col2)
else:
M = col1
elif k != 0:
M = col2.row_join(col3)
else:
M = col2
M_eq = msubs(M, op_point_dict)
# Build up state coefficient matrix A
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
# Col 1 is only defined if n != 0
if n != 0:
r1c1 = A_qq
if o != 0:
r1c1 += (A_qu * C_1)
r1c1 = r1c1 * C_0
if m != 0:
r2c1 = A_uqc
if o != 0:
r2c1 += (A_uuc * C_1)
r2c1 = r2c1 * C_0
else:
r2c1 = Matrix()
if o - m + k != 0:
r3c1 = A_uqd
if o != 0:
r3c1 += (A_uud * C_1)
r3c1 = r3c1 * C_0
else:
r3c1 = Matrix()
col1 = Matrix([r1c1, r2c1, r3c1])
else:
col1 = Matrix()
# Col 2 is only defined if o != 0
if o != 0:
if n != 0:
r1c2 = A_qu * C_2
else:
r1c2 = Matrix()
if m != 0:
r2c2 = A_uuc * C_2
else:
r2c2 = Matrix()
if o - m + k != 0:
r3c2 = A_uud * C_2
else:
r3c2 = Matrix()
col2 = Matrix([r1c2, r2c2, r3c2])
else:
col2 = Matrix()
if col1:
if col2:
Amat = col1.row_join(col2)
else:
Amat = col1
else:
Amat = col2
Amat_eq = msubs(Amat, op_point_dict)
# Build up the B matrix if there are forcing variables
# |0_(n + m)xs|
# B = |B_u |
if s != 0 and o - m + k != 0:
Bmat = zeros(n + m, s).col_join(B_u)
Bmat_eq = msubs(Bmat, op_point_dict)
else:
Bmat_eq = Matrix()
# kwarg A_and_B indicates to return A, B for forming the equation
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
if A_and_B:
A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq)
if Bmat_eq:
B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq)
else:
# Bmat = Matrix([]), so no need to sub
B_cont = Bmat_eq
if simplify:
A_cont.simplify()
B_cont.simplify()
return A_cont, B_cont
# Otherwise return M, A, B for forming the equation
# [M]dx = [A]x + [B]r, where x = [q, u]^T
else:
if simplify:
M_eq.simplify()
Amat_eq.simplify()
Bmat_eq.simplify()
return M_eq, Amat_eq, Bmat_eq
def permutation_matrix(orig_vec, per_vec):
"""Compute the permutation matrix to change order of
orig_vec into order of per_vec.
Parameters
----------
orig_vec : array_like
Symbols in original ordering.
per_vec : array_like
Symbols in new ordering.
Returns
-------
p_matrix : Matrix
Permutation matrix such that orig_vec == (p_matrix * per_vec).
"""
if not isinstance(orig_vec, (list, tuple)):
orig_vec = flatten(orig_vec)
if not isinstance(per_vec, (list, tuple)):
per_vec = flatten(per_vec)
if set(orig_vec) != set(per_vec):
raise ValueError("orig_vec and per_vec must be the same length, " +
"and contain the same symbols.")
ind_list = [orig_vec.index(i) for i in per_vec]
p_matrix = zeros(len(orig_vec))
for i, j in enumerate(ind_list):
p_matrix[i, j] = 1
return p_matrix
