'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.'

def linearize(self, op_point=None, A_and_B=False, simplify=False):

    if (not self._setup_done):
        self._setup()
    if isinstance(op_point, dict):
        op_point_dict = op_point
    elif isinstance(op_point, collections.Iterable):
        op_point_dict = {}
        for op in op_point:
            op_point_dict.update(op)
    else:
        op_point_dict = {}
    (l, m, n, o, s, k) = self._dims
    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
    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)
    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()
    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)
    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()
    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:
            B_cont = Bmat_eq
        if simplify:
            A_cont.simplify()
            B_cont.simplify()
        return (A_cont, B_cont)
    else:
        if simplify:
            M_eq.simplify()
            Amat_eq.simplify()
            Bmat_eq.simplify()
        return (M_eq, Amat_eq, Bmat_eq)
