How to use the sympy.Matrix function in sympy

def test_Matrix3():
    x = Symbol("x")
    a = array([[2, 4],[5, 1]])
    assert Matrix(a) == Matrix([[2, 4], [5, 1]])
    assert Matrix(a) != Matrix([[2, 4], [5, 2]])
    a = array([[sin(2), 4], [5, 1]])
    assert Matrix(a) == Matrix([[sin(2), 4],[5, 1]])
    assert Matrix(a) != Matrix([[sin(0), 4],[5, 1]])

import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np

x, y = me.dynamicsymbols('x y')
xd, yd = me.dynamicsymbols('x y', 1)
e1 = (x+y)**2+(x-y)**3
e2 = (x-y)**2
e3 = x**2+y**2+2*x*y
m1 = sm.Matrix([e1,e2]).reshape(2, 1)
m2 = sm.Matrix([(x+y)**2,(x-y)**2]).reshape(1, 2)
m3 = m1+sm.Matrix([x,y]).reshape(2, 1)
am = sm.Matrix([i.expand() for i in m1]).reshape((m1).shape[0], (m1).shape[1])
cm = sm.Matrix([i.expand() for i in sm.Matrix([(x+y)**2,(x-y)**2]).reshape(1, 2)]).reshape((sm.Matrix([(x+y)**2,(x-y)**2]).reshape(1, 2)).shape[0], (sm.Matrix([(x+y)**2,(x-y)**2]).reshape(1, 2)).shape[1])
em = sm.Matrix([i.expand() for i in m1+sm.Matrix([x,y]).reshape(2, 1)]).reshape((m1+sm.Matrix([x,y]).reshape(2, 1)).shape[0], (m1+sm.Matrix([x,y]).reshape(2, 1)).shape[1])
f = (e1).expand()
g = (e2).expand()
a = sm.factor((e3), x)
bm = sm.Matrix([sm.factor(i, x) for i in m1]).reshape((m1).shape[0], (m1).shape[1])
cm = sm.Matrix([sm.factor(i, x) for i in m1+sm.Matrix([x,y]).reshape(2, 1)]).reshape((m1+sm.Matrix([x,y]).reshape(2, 1)).shape[0], (m1+sm.Matrix([x,y]).reshape(2, 1)).shape[1])
a = (e3).diff(x)
b = (e3).diff(y)
cm = sm.Matrix([i.diff(x) for i in m2]).reshape((m2).shape[0], (m2).shape[1])
dm = sm.Matrix([i.diff(x) for i in m1+sm.Matrix([x,y]).reshape(2, 1)]).reshape((m1+sm.Matrix([x,y]).reshape(2, 1)).shape[0], (m1+sm.Matrix([x,y]).reshape(2, 1)).shape[1])
frame_a=me.ReferenceFrame('a')
frame_b=me.ReferenceFrame('b')
frame_b.orient(frame_a, 'DCM', sm.Matrix([1,0,0,1,0,0,1,0,0]).reshape(3, 3))

or momentum.__class__ == MomentumNitscheStokes \
		   or momentum.__class__ == MomentumDukowiczStokes:
			s   = "    - using full-Stokes formulation -"
			# strain-rate tensor :
			self.epsdot = 0.5 * (self.grad_u + self.grad_u.T)

		# first-order Blatter-Pattyn approximation :
		elif    momentum.__class__ == MomentumBP \
		     or momentum.__class__ == MomentumDukowiczBP:
			s   = "    - using Blatter-Pattyn formulation -"
			# strain-rate tensor :
			epi         = 0.5 * (self.grad_u + self.grad_u.T)
			epi02       = 0.5*self.du_x_dz
			epi12       = 0.5*self.du_y_dz
			epi22       = -self.du_x_dx - self.du_y_dy  # incompressibility
			self.epsdot = sp.Matrix([[epi[0,0],  epi[0,1],  epi02],
			                         [epi[1,0],  epi[1,1],  epi12],
			                         [epi02,     epi12,     epi22]])

		print_text(s, cls=self)

		# effective strain-rate squared :
		self.epsdot2      = self.epsdot.multiply(self.epsdot)
		self.epsdot_eff   = 0.5 * ( + self.epsdot2[0,0] \
		                            + self.epsdot2[1,1] \
		                            + self.epsdot2[2,2])

		# flow enhancement factor :
		def E(x,y,z):
			return 1.0

		# water content :

nonhol_coneqs: array_like, optional
            The nonholonomic constraint equations

        forcelist : iterable, optional
            Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
            tuples which represent the force at a point or torque on a frame.
            This feature is primarily to account for the nonconservative forces
            and/or moments.

        frame : ReferenceFrame, optional
            Supply the inertial frame. This is used to determine the
            generalized forces due to non-conservative forces.
        """

        self._L = Matrix([sympify(Lagrangian)])
        self.eom = None
        self._m_cd = Matrix()           # Mass Matrix of differentiated coneqs
        self._m_d = Matrix()            # Mass Matrix of dynamic equations
        self._f_cd = Matrix()           # Forcing part of the diff coneqs
        self._f_d = Matrix()            # Forcing part of the dynamic equations
        self.lam_coeffs = Matrix()      # The coeffecients of the multipliers

        forcelist = forcelist if forcelist else []
        if not iterable(forcelist):
            raise TypeError('Force pairs must be supplied in an iterable.')
        self._forcelist = forcelist
        if frame and not isinstance(frame, ReferenceFrame):
            raise TypeError('frame must be a valid ReferenceFrame')
        self.inertial = frame

        self.lam_vec = Matrix()

cg_sa = c_gamma * s_alpha
    # t matrix elements
    t11 = (c_gamma * c_theta) - (sg_ca * s_theta)
    t12 = -(c_gamma * s_theta) - (sg_ca * c_theta)
    t13 = sg_sa
    t14 = (d * c_gamma) + (r * sg_sa)
    t21 = (s_gamma * c_theta) + (cg_ca * s_theta)
    t22 = -(s_gamma * s_theta) + (cg_ca * c_theta)
    t23 = -cg_sa
    t24 = (d * s_gamma) - (r * cg_sa)
    t31 = s_alpha * s_theta
    t32 = s_alpha * c_theta
    t33 = c_alpha
    t34 = (r * c_alpha) + b
    # t matrix
    tmat = sympy.Matrix([
        [t11, t12, t13, t14],
        [t21, t22, t23, t24],
        [t31, t32, t33, t34],
        [0, 0, 0, 1]
    ])
    return tmat

def f_compute_analytical_jacobians(prob: SymBVP, in_place=False) -> (SymBVP, IdentityMapper):
    if not in_place:
        prob = copy.deepcopy(prob)

    y = sympy.Matrix([state['sym'] for state in prob.states])
    p = sympy.Matrix([parameter['sym'] for parameter in prob.parameters])
    q = sympy.Matrix([quad['sym'] for quad in prob.quads])
    f = sympy.Matrix([state['eom'] for state in prob.states])
    phi_0 = sympy.Matrix([bc['expr'] for bc in prob.constraints['initial']])
    phi_f = sympy.Matrix([bc['expr'] for bc in prob.constraints['terminal']])

    prob.func_jac['df_dy'] = f.jacobian(y)
    prob.bc_jac['initial']['dbc_dy'] = phi_0.jacobian(y)
    prob.bc_jac['terminal']['dbc_dy'] = phi_f.jacobian(y)

    if len(p) > 0:
        prob.func_jac.update({'df_dp': f.jacobian(p)})
        prob.bc_jac['initial']['dbc_dp'] = phi_0.jacobian(p)
        prob.bc_jac['terminal']['dbc_dp'] = phi_f.jacobian(p)

    if len(q) > 0:
        prob.bc_jac['initial']['dbc_dq'] = phi_0.jacobian(q)
        prob.bc_jac['terminal']['dbc_dq'] = phi_f.jacobian(q)

    sol_mapper = IdentityMapper()

* The symbol ``t`` used in the polynomial
        * The triangle :math:`B(s, t)`.
    """
    # NOTE: We import SymPy at runtime to avoid the import-time cost for users
    #       that don't want to do symbolic computation. The ``sympy`` import is
    #       a tad expensive.
    import sympy  # pylint: disable=import-outside-toplevel

    nodes_sym = to_symbolic(nodes)

    s, t = sympy.symbols("s, t")
    b_polynomial = nodes_sym * triangle_weights(degree, s, t)
    b_polynomial.simplify()

    factored = [value.factor() for value in b_polynomial]
    return s, t, sympy.Matrix(factored).reshape(*b_polynomial.shape)

def jacobian(self):
        eq = sympy.Matrix(self.generated_equations)        
        J = eq.jacobian(self.variables)
        return J

def RotX_S(t):
  return(sp.Matrix([
    [1,         0,           0],
    [0, sp.cos(t),  -sp.sin(t)],
    [0, sp.sin(t),   sp.cos(t)]
    ]))

# calculate controllability matrix
    QsT = Matrix([B.transpose(),\
                 (A*B).transpose(),\
                 (A**2*B).transpose(),\
                 (A**3*B).transpose()])
    Qs = QsT.transpose()
    print('Qs: ', Qs)
    det_Qs = Qs.det()
#    print det_Qs.expand()
    
    
    t1T = Matrix([t1T1, t1T2, t1T3, t1T4])
    
    e_4 = Matrix([[0,0,0,1]])
    
    t1T = e_4*Qs**-1
    
    K = t1T*(A**4 + p[3]*A**(3) + p[2]*A**2 + p[1]*A + p[0]*sp.eye(4))
    return K