Spaces:
Sleeping
Sleeping
| __all__ = ['Linearizer'] | |
| from sympy 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, _parse_linear_solver | |
| 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. The notation and method is described in [1]_. | |
| 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 | |
| References | |
| ========== | |
| .. [1] D. L. Peterson, G. Gede, and M. Hubbard, "Symbolic linearization of | |
| equations of motion of constrained multibody systems," Multibody | |
| Syst Dyn, vol. 33, no. 2, pp. 143-161, Feb. 2015, doi: | |
| 10.1007/s11044-014-9436-5. | |
| """ | |
| 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, | |
| linear_solver='LU'): | |
| """ | |
| 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 | |
| linear_solver : str, callable | |
| Method used to solve the several symbolic linear systems of the | |
| form ``A*x=b`` in the linearization process. If a string is | |
| supplied, it should be a valid method that can be used with the | |
| :meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is | |
| supplied, it should have the format ``x = f(A, b)``, where it | |
| solves the equations and returns the solution. The default is | |
| ``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``. | |
| ``LUsolve()`` is fast to compute but will often result in | |
| divide-by-zero and thus ``nan`` results. | |
| """ | |
| self.linear_solver = _parse_linear_solver(linear_solver) | |
| # 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 * | |
| self.linear_solver(f_c_jac_q*self._Pqd, | |
| 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 * self.linear_solver(temp, f_v_jac_q) | |
| else: | |
| self._C_1 = zeros(o, n) | |
| self._C_2 = (eye(o) - self._Pud * | |
| self.linear_solver(temp, 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 for all or a subset of the generalized | |
| coordinates, generalized speeds, and time derivatives of the | |
| generalized speeds. These will be substituted into the linearized | |
| system before the linearization is complete. Leave set to ``None`` | |
| if you want the operating point to be an arbitrary set of symbols. | |
| 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 and of | |
| A_and_B=True, (A, B) is returned. See below. | |
| simplify : bool, optional | |
| Determines if returned values are simplified before return. | |
| For large expressions this may be time consuming. Default is False. | |
| Returns | |
| ======= | |
| M, A, B : Matrices, ``A_and_B=False`` | |
| Matrices from the implicit form: | |
| ``[M]*[q', u']^T = [A]*[q_ind, u_ind]^T + [B]*r`` | |
| A, B : Matrices, ``A_and_B=True`` | |
| Matrices from the explicit form: | |
| ``[q_ind', u_ind']^T = [A]*[q_ind, u_ind]^T + [B]*r`` | |
| Notes | |
| ===== | |
| 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 * self.linear_solver(M_eq, Amat_eq) | |
| if Bmat_eq: | |
| B_cont = self.perm_mat.T * self.linear_solver(M_eq, 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 | |