444 lines
15 KiB
Python
444 lines
15 KiB
Python
|
__all__ = ['Linearizer']
|
||
|
|
|
||
|
|
from sympy.core.backend import Matrix, eye, zeros
|
||
|
|
from sympy.core.symbol 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
|
||
|
|
from collections.abc import Iterable
|
||
|
|
|
||
|
|
class Linearizer:
|
||
|
|
"""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
|
||
|
|
does not 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._Pq = None
|
||
|
|
self._Pqi = None
|
||
|
|
self._Pqd = None
|
||
|
|
self._Pu = None
|
||
|
|
self._Pui = None
|
||
|
|
self._Pud = None
|
||
|
|
self._C_0 = None
|
||
|
|
self._C_1 = None
|
||
|
|
self._C_2 = None
|
||
|
|
self.perm_mat = None
|
||
|
|
|
||
|
|
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()
|
||
|
|
|
||
|
|
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
|