def test_inertia():
    N = ReferenceFrame('N')
    ixx, iyy, izz = symbols('ixx iyy izz')
    ixy, iyz, izx = symbols('ixy iyz izx')
    assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
            (N.y | N.y) + izz * (N.z | N.z))
    assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
    assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
            ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
        (N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
            N.y) + izz * (N.z | N.z))

    def _create_inertia_dyadics(self):

        leg_inertia_dyadic = me.inertia(self.frames['leg'], 0, 0,
                                        self.parameters['leg_inertia'])

        torso_inertia_dyadic = me.inertia(self.frames['torso'], 0, 0,
                                          self.parameters['torso_inertia'])

        self.central_inertias = OrderedDict()
        self.central_inertias['leg'] = (leg_inertia_dyadic,
                                        self.points['leg_mass_center'])
        self.central_inertias['torso'] = (torso_inertia_dyadic,
                                          self.points['torso_mass_center'])

def test_specifying_coordinate_issue_339():
    """This test ensures that you can use derivatives as specified values."""

    # beta will be a specified angle
    beta = me.dynamicsymbols('beta')
    q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4')
    u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4')

    N = me.ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', (q1, N.x))
    B = A.orientnew('B', 'Axis', (beta, A.y))

    No = me.Point('No')
    Ao = No.locatenew('Ao', q2 * N.x + q3 * N.y + q4 * N.z)
    Bo = Ao.locatenew('Bo', 10 * A.x + 10 * A.y + 10 * A.z)

    A.set_ang_vel(N, u1 * N.x)
    B.ang_vel_in(N)  # compute it automatically

    No.set_vel(N, 0)
    Ao.set_vel(N, u2 * N.x + u3 * N.y + u4 * N.z)
    Bo.v2pt_theory(Ao, N, B)

    body_A = me.RigidBody('A', Ao, A, 1.0, (me.inertia(A, 1, 2, 3), Ao))
    body_B = me.RigidBody('B', Bo, B, 1.0, (me.inertia(A, 3, 2, 1), Bo))

    bodies = [body_A, body_B]
    # TODO : This should be able to be simple an empty iterable.
    loads = [(No, 0 * N.x)]

    kdes = [u1 - q1.diff(),
            u2 - q2.diff(),
            u3 - q3.diff(),
            u4 - q4.diff()]

    kane = me.KanesMethod(N, q_ind=[q1, q2, q3, q4],
                          u_ind=[u1, u2, u3, u4], kd_eqs=kdes)

    if sympy_newer_than('1.0'):
        fr, frstar = kane.kanes_equations(bodies, loads)
    else:
        fr, frstar = kane.kanes_equations(loads, bodies)

    sys = System(kane)

    sys.specifieds = {(beta, beta.diff(), beta.diff().diff()):
                      lambda x, t: np.array([1.0, 1.0, 1.0])}

    sys.times = np.linspace(0, 10, 20)

    sys.integrate()

    def __init__(self):
        #We define some quantities required for tests here..
        self.p = dynamicsymbols('p:3')
        self.q = dynamicsymbols('q:3')
        self.dynamic = list(self.p) + list(self.q)
        self.states = [radians(45) for x in self.p] + \
                               [radians(30) for x in self.q]

        self.I = ReferenceFrame('I')
        self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
        self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')

        self.O = Point('O')
        self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
                                      10 * self.I.y + 10 * self.I.z)
        self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
                                    10 * self.I.y + 10 * self.I.z)

        self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
        self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]

        self.shape1 = Cylinder()
        self.shape2 = Cylinder()


        self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
        self.mass = symbols('mass')
        self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
        self.param_vals = [0, 0, 0, 0]

        self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)

        self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \
                                 self.mass, (self.inertia, self.P1))

        self.global_frame1 = VisualizationFrame('global_frame1', \
                                self.A, self.P1, self.shape1)

        self.global_frame2 = VisualizationFrame('global_frame2', \
                                self.B, self.P2, self.shape2)

        self.scene1 = Scene(self.I, self.O, \
                            (self.global_frame1, self.global_frame2), \
                                             name='scene')

        self.particle = Particle('particle1', self.P1, self.mass)

        #To make it more readable
        p = self.p
        q = self.q
        #Here is the dragon ..
        self.transformation_matrix = \
            [[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
             [sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
                  sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
                  sin(p[0])*cos(p[1]), 0], \
             [sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
                 -sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
                  cos(p[0])*cos(p[1]), 0], \
             [10, 10, 10, 1]]

def test_pendulum_angular_momentum():
    """Consider a pendulum of length OA = 2a, of mass m as a rigid body of
    center of mass G (OG = a) which turn around (O,z). The angle between the
    reference frame R and the rod is q.  The inertia of the body is I =
    (G,0,ma^2/3,ma^2/3). """

    m, a = symbols('m, a')
    q = dynamicsymbols('q')

    R = ReferenceFrame('R')
    R1 = R.orientnew('R1', 'Axis', [q, R.z])
    R1.set_ang_vel(R, q.diff() * R.z)

    I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)

    O = Point('O')

    A = O.locatenew('A', 2*a * R1.x)
    G = O.locatenew('G', a * R1.x)

    S = RigidBody('S', G, R1, m, (I, G))

    O.set_vel(R, 0)
    A.v2pt_theory(O, R, R1)
    G.v2pt_theory(O, R, R1)

    assert (4 * m * a**2 / 3 * q.diff() * R.z -
            S.angular_momentum(O, R).express(R)) == 0

def test_body_add_force():
    # Body with RigidBody.
    rigidbody_masscenter = Point('rigidbody_masscenter')
    rigidbody_mass = Symbol('rigidbody_mass')
    rigidbody_frame = ReferenceFrame('rigidbody_frame')
    body_inertia = inertia(rigidbody_frame, 1, 0, 0)
    rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
                      rigidbody_frame, body_inertia)

    l = Symbol('l')
    Fa = Symbol('Fa')
    point = rigid_body.masscenter.locatenew(
        'rigidbody_body_point0',
        l * rigid_body.frame.x)
    point.set_vel(rigid_body.frame, 0)
    force_vector = Fa * rigid_body.frame.z
    # apply_force with point
    rigid_body.apply_force(force_vector, point)
    assert len(rigid_body.loads) == 1
    force_point = rigid_body.loads[0][0]
    frame = rigid_body.frame
    assert force_point.vel(frame) == point.vel(frame)
    assert force_point.pos_from(force_point) == point.pos_from(force_point)
    assert rigid_body.loads[0][1] == force_vector
    # apply_force without point
    rigid_body.apply_force(force_vector)
    assert len(rigid_body.loads) == 2
    assert rigid_body.loads[1][1] == force_vector
    # passing something else than point
    raises(TypeError, lambda: rigid_body.apply_force(force_vector,  0))
    raises(TypeError, lambda: rigid_body.apply_force(0))

def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in Kane. The inertia of the pendulum is
    # defined about the hinge, not about the center of mass.

    # Defining the constants and knowns of the system
    gravity        = symbols('g')
    k, ls          = symbols('k ls')
    a, mA, mC      = symbols('a mA mC')
    F              = dynamicsymbols('F')
    Ix, Iy, Iz     = symbols('Ix Iy Iz')

    # Declaring the Generalized coordinates and speeds
    q1, q2   = dynamicsymbols('q1 q2')
    q1d, q2d = dynamicsymbols('q1 q2', 1)
    u1, u2   = dynamicsymbols('u1 u2')
    u1d, u2d = dynamicsymbols('u1 u2', 1)

    # Creating reference frames
    N = ReferenceFrame('N')
    A = ReferenceFrame('A')

    A.orient(N, 'Axis', [-q2, N.z])
    A.set_ang_vel(N, -u2 * N.z)

    # Origin of Newtonian reference frame
    O = Point('O')

    # Creating and Locating the positions of the cart, C, and the
    # center of mass of the pendulum, A
    C  = O.locatenew('C',  q1 * N.x)
    Ao = C.locatenew('Ao', a * A.y)

    # Defining velocities of the points
    O.set_vel(N, 0)
    C.set_vel(N, u1 * N.x)
    Ao.v2pt_theory(C, N, A)
    Cart     = Particle('Cart', C, mC)
    Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))

    # kinematical differential equations

    kindiffs  = [q1d - u1, q2d - u2]

    bodyList  = [Cart, Pendulum]

    forceList = [(Ao, -N.y * gravity * mA),
                 (C,  -N.y * gravity * mC),
                 (C,  -N.x * k * (q1 - ls)),
                 (C,   N.x * F)]

    km=Kane(N)
    km.coords([q1, q2])
    km.speeds([u1, u2])
    km.kindiffeq(kindiffs)
    (fr,frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == -Iz

def test_aux():
    # Same as above, except we have 2 auxiliary speeds for the ground contact
    # point, which is known to be zero. In one case, we go through then
    # substitute the aux. speeds in at the end (they are zero, as well as their
    # derivative), in the other case, we use the built-in auxiliary speed part
    # of Kane. The equations from each should be the same.
    q1, q2, q3, u1, u2, u3  = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
    u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
    u4d, u5d = dynamicsymbols('u4, u5', 1)
    r, m, g = symbols('r m g')

    N = ReferenceFrame('N')
    Y = N.orientnew('Y', 'Axis', [q1, N.z])
    L = Y.orientnew('L', 'Axis', [q2, Y.x])
    R = L.orientnew('R', 'Axis', [q3, L.y])
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
    R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
        R.ang_vel_in(N)))

    C = Point('C')
    C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)
    Dmc.a2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]

    ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
    BodyD = RigidBody()
    BodyD.mc = Dmc
    BodyD.inertia = (I, Dmc)
    BodyD.frame = R
    BodyD.mass = m
    BodyList = [BodyD]

    KM = Kane(N)
    KM.coords([q1, q2, q3])
    KM.speeds([u1, u2, u3, u4, u5])
    KM.kindiffeq(kd)
    kdd = KM.kindiffdict()
    (fr, frstar) = KM.kanes_equations(ForceList, BodyList)
    fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
    frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})

    KM2 = Kane(N)
    KM2.coords([q1, q2, q3])
    KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
    KM2.kindiffeq(kd)
    (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
    fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
    frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})

    assert fr.expand() == fr2.expand()
    assert frstar.expand() == frstar2.expand()

def test_aux():
    # Same as above, except we have 2 auxiliary speeds for the ground contact
    # point, which is known to be zero. In one case, we go through then
    # substitute the aux. speeds in at the end (they are zero, as well as their
    # derivative), in the other case, we use the built-in auxiliary speed part
    # of KanesMethod. The equations from each should be the same.
    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
    u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
    u4d, u5d = dynamicsymbols('u4, u5', 1)
    r, m, g = symbols('r m g')

    N = ReferenceFrame('N')
    Y = N.orientnew('Y', 'Axis', [q1, N.z])
    L = Y.orientnew('L', 'Axis', [q2, Y.x])
    R = L.orientnew('R', 'Axis', [q3, L.y])
    w_R_N_qd = R.ang_vel_in(N)
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)

    C = Point('C')
    C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)
    Dmc.a2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]

    ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
    BodyList = [BodyD]

    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
                     kd_eqs=kd)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = KM.kanes_equations(ForceList, BodyList)
    fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
                      u_auxiliary=[u4, u5])
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
    fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
    frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})

    frstar.simplify()
    frstar2.simplify()

    assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
    assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])

 def parallel_axis(self, point):
     """Returns the inertia of the body about another point."""
     # TODO : What if the new point is not fixed in the rigid body's frame?
     a, b, c = self.masscenter.pos_from(point).to_matrix(self.frame)
     return self.central_inertia + self.mass * inertia(self.frame,
                                                       b**2 + c**2,
                                                       c**2 + a**2,
                                                       a**2 + b**2,
                                                       -a * b,
                                                       -b * c,
                                                       -a * c)

def test_parallel_axis():
    # This is for a 2 dof inverted pendulum on a cart.
    # This tests the parallel axis code in KanesMethod. The inertia of the
    # pendulum is defined about the hinge, not about the center of mass.

    # Defining the constants and knowns of the system
    gravity = symbols("g")
    k, ls = symbols("k ls")
    a, mA, mC = symbols("a mA mC")
    F = dynamicsymbols("F")
    Ix, Iy, Iz = symbols("Ix Iy Iz")

    # Declaring the Generalized coordinates and speeds
    q1, q2 = dynamicsymbols("q1 q2")
    q1d, q2d = dynamicsymbols("q1 q2", 1)
    u1, u2 = dynamicsymbols("u1 u2")
    u1d, u2d = dynamicsymbols("u1 u2", 1)

    # Creating reference frames
    N = ReferenceFrame("N")
    A = ReferenceFrame("A")

    A.orient(N, "Axis", [-q2, N.z])
    A.set_ang_vel(N, -u2 * N.z)

    # Origin of Newtonian reference frame
    O = Point("O")

    # Creating and Locating the positions of the cart, C, and the
    # center of mass of the pendulum, A
    C = O.locatenew("C", q1 * N.x)
    Ao = C.locatenew("Ao", a * A.y)

    # Defining velocities of the points
    O.set_vel(N, 0)
    C.set_vel(N, u1 * N.x)
    Ao.v2pt_theory(C, N, A)
    Cart = Particle("Cart", C, mC)
    Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))

    # kinematical differential equations

    kindiffs = [q1d - u1, q2d - u2]

    bodyList = [Cart, Pendulum]

    forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)]

    km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
    with warnings.catch_warnings():
        warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
        (fr, frstar) = km.kanes_equations(forceList, bodyList)
    mm = km.mass_matrix_full
    assert mm[3, 3] == Iz

def test_rolling_disc():
    # Rolling Disc Example
    # Here the rolling disc is formed from the contact point up, removing the
    # need to introduce generalized speeds. Only 3 configuration and 3
    # speed variables are need to describe this system, along with the
    # disc's mass and radius, and the local gravity.
    q1, q2, q3 = dynamicsymbols('q1 q2 q3')
    q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
    r, m, g = symbols('r m g')

    # The kinematics are formed by a series of simple rotations. Each simple
    # rotation creates a new frame, and the next rotation is defined by the new
    # frame's basis vectors. This example uses a 3-1-2 series of rotations, or
    # Z, X, Y series of rotations. Angular velocity for this is defined using
    # the second frame's basis (the lean frame).
    N = ReferenceFrame('N')
    Y = N.orientnew('Y', 'Axis', [q1, N.z])
    L = Y.orientnew('L', 'Axis', [q2, Y.x])
    R = L.orientnew('R', 'Axis', [q3, L.y])

    # This is the translational kinematics. We create a point with no velocity
    # in N; this is the contact point between the disc and ground. Next we form
    # the position vector from the contact point to the disc's center of mass.
    # Finally we form the velocity and acceleration of the disc.
    C = Point('C')
    C.set_vel(N, 0)
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)

    # Forming the inertia dyadic.
    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))

    # Finally we form the equations of motion, using the same steps we did
    # before. Supply the Lagrangian, the generalized speeds.
    BodyD.set_potential_energy(- m * g * r * cos(q2))
    Lag = Lagrangian(N, BodyD)
    q = [q1, q2, q3]
    q1 = Function('q1')
    q2 = Function('q2')
    q3 = Function('q3')
    l = LagrangesMethod(Lag, q)
    l.form_lagranges_equations()
    RHS = l.rhs()
    RHS.simplify()
    t = symbols('t')

    assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
    assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
        )*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
        )*Derivative(q3(t), t))/(10*r)
    assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
        )*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
        )*Derivative(q2(t), t)

def test_default():
    body = Body('body')
    assert body.name == 'body'
    assert body.loads == []
    point = Point('body_masscenter')
    point.set_vel(body.frame, 0)
    com = body.masscenter
    frame = body.frame
    assert com.vel(frame) == point.vel(frame)
    assert body.mass == Symbol('body_mass')
    ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
    ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
    assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
                            body.masscenter)

def test_custom_rigid_body():
    # Body with RigidBody.
    rigidbody_masscenter = Point('rigidbody_masscenter')
    rigidbody_mass = Symbol('rigidbody_mass')
    rigidbody_frame = ReferenceFrame('rigidbody_frame')
    body_inertia = inertia(rigidbody_frame, 1, 0, 0)
    rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
                      rigidbody_frame, body_inertia)
    com = rigid_body.masscenter
    frame = rigid_body.frame
    rigidbody_masscenter.set_vel(rigidbody_frame, 0)
    assert com.vel(frame) == rigidbody_masscenter.vel(frame)
    assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)

    assert rigid_body.mass == rigidbody_mass
    assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)

    assert hasattr(rigid_body, 'masscenter')
    assert hasattr(rigid_body, 'mass')
    assert hasattr(rigid_body, 'frame')
    assert hasattr(rigid_body, 'inertia')

    def __init__(self, name, masscenter=None, mass=None, frame=None,
                 central_inertia=None):

        self.name = name
        self.loads = []

        if frame is None:
            frame = ReferenceFrame(name + '_frame')

        if masscenter is None:
            masscenter = Point(name + '_masscenter')

        if central_inertia is None and mass is None:
            ixx = Symbol(name + '_ixx')
            iyy = Symbol(name + '_iyy')
            izz = Symbol(name + '_izz')
            izx = Symbol(name + '_izx')
            ixy = Symbol(name + '_ixy')
            iyz = Symbol(name + '_iyz')
            _inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz, izx),
                        masscenter)
        else:
            _inertia = (central_inertia, masscenter)

        if mass is None:
            _mass = Symbol(name + '_mass')
        else:
            _mass = mass

        masscenter.set_vel(frame, 0)

        # If user passes masscenter and mass then a particle is created
        # otherwise a rigidbody. As a result a body may or may not have inertia.
        if central_inertia is None and mass is not None:
            self.frame = frame
            self.masscenter = masscenter
            Particle.__init__(self, name, masscenter, _mass)
        else:
            RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)

def test_linearize_rolling_disc_lagrange():
    q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
    q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 1)
    r, m, g = symbols("r m g")

    N = ReferenceFrame("N")
    Y = N.orientnew("Y", "Axis", [q1, N.z])
    L = Y.orientnew("L", "Axis", [q2, Y.x])
    R = L.orientnew("R", "Axis", [q3, L.y])

    C = Point("C")
    C.set_vel(N, 0)
    Dmc = C.locatenew("Dmc", r * L.z)
    Dmc.v2pt_theory(C, N, R)

    I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
    BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
    BodyD.potential_energy = -m * g * r * cos(q2)

    Lag = Lagrangian(N, BodyD)
    l = LagrangesMethod(Lag, q)
    l.form_lagranges_equations()

    # Linearize about steady-state upright rolling
    op_point = {q1: 0, q2: 0, q3: 0, q1d: 0, q2d: 0, q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
    A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
    sol = Matrix(
        [
            [0, 0, 0, 1, 0, 0],
            [0, 0, 0, 0, 1, 0],
            [0, 0, 0, 0, 0, 1],
            [0, 0, 0, 0, -6 * q3d, 0],
            [0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
            [0, 0, 0, 0, 0, 0],
        ]
    )

    assert A == sol

body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)

r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, body_frame)

# Mass
# ====

l_leg_mass, body_mass, r_leg_mass = symbols("m_L, m_B, m_R")

# Inertia
# =======

l_leg_inertia, body_inertia, r_leg_inertia = symbols("I_Lz, I_Bz, I_Rz")

l_leg_inertia_dyadic = inertia(l_leg_frame, 0, 0, l_leg_inertia)

l_leg_central_inertia = (l_leg_inertia_dyadic, l_leg_mass_center)

body_inertia_dyadic = inertia(body_frame, 0, 0, body_inertia)

body_central_inertia = (body_inertia_dyadic, body_mass_center)

r_leg_inertia_dyadic = inertia(body_frame, 0, 0, r_leg_inertia)

r_leg_central_inertia = (r_leg_inertia_dyadic, r_leg_mass_center)

# Rigid Bodies
# ============

l_leg = RigidBody("Lower Leg", l_leg_mass_center, l_leg_frame, l_leg_mass, l_leg_central_inertia)

#.........这里部分代码省略.........
    #                                is q[3].
    # Velocity constraints: f_v, for u3, u4 and u5.
    # Acceleration constraints: f_a.
    f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
    f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
        O.pos_from(P))), ai).expand() for ai in A])
    v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
    a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
    f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])

    # Solve for constraint equations in the form of
    #                           u_dependent = A_rs * [u_i; u_aux].
    # First, obtain constraint coefficient matrix:  M_v * [u; ua] = 0;
    # Second, taking u[0], u[1], u[2] as independent,
    #         taking u[3], u[4], u[5] as dependent,
    #         rearranging the matrix of M_v to be A_rs for u_dependent.
    # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
    M_v = zeros(3, 9)
    for i in range(3):
        for j, ui in enumerate(u + ua):
            M_v[i, j] = f_v[i].diff(ui)

    M_v_i = M_v[:, :3]
    M_v_d = M_v[:, 3:6]
    M_v_aux = M_v[:, 6:]
    M_v_i_aux = M_v_i.row_join(M_v_aux)
    A_rs = - M_v_d.inv() * M_v_i_aux

    u_dep = A_rs[:, :3] * Matrix(u[:3])
    u_dep_dict = dict(zip(u[3:], u_dep))
    #u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())}

    # Active forces: F_O acting on point O; F_P acting on point P.
    # Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
    F_O = m*g*A.z
    F_P = Fx * A.x + Fy * A.y + Fz * A.z
    Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
            zip(partial_v_O, partial_v_P)])

    # Inertia force: R_star_O.
    # Inertia of disc: I_C_O, where J is a inertia component about principal axis.
    # Inertia torque: T_star_C.
    # Generalized inertia forces (unconstrained): Fr_star_u.
    R_star_O = -m*O.acc(N)
    I_C_O = inertia(B, I, J, I)
    T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
                 + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
    Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
                        zip(partial_v_O, partial_w_C)])

    # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
    # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
    Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
    Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
                + A_rs.T * Fr_star_u[3:6, :]
    Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
            .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()


    # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.

    # Rigid Bodies: disc, with inertia I_C_O.
    iner_tuple = (I_C_O, O)
    disc = RigidBody('disc', O, C, m, iner_tuple)
    bodyList = [disc]

    # Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
    F_o = (O, F_O)
    F_p = (P, F_P)
    forceList = [F_o,  F_p]

    # KanesMethod.
    kane = KanesMethod(
        N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
        q_dependent=q[3:], configuration_constraints = f_c,
        u_dependent=u[3:], velocity_constraints= f_v,
        u_auxiliary=ua
        )

    # fr, frstar, frstar_steady and kdd(kinematic differential equations).
    (fr, frstar)= kane.kanes_equations(forceList, bodyList)
    frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
                    .subs({q[3]: -r*cos(q[1])}).expand()
    kdd = kane.kindiffdict()


    # Test
    # First try Fr_c == fr;
    # Second try Fr_star_c == frstar;
    # Third try Fr_star_steady == frstar_steady.
    # Both signs are checked in case the equations were found with an inverse
    # sign.
    assert ((Matrix(Fr_c).expand() == fr.expand()) or
             (Matrix(Fr_c).expand() == (-fr).expand()))

    assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or
             (Matrix(Fr_star_c).expand() == (-frstar).expand()))

    assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or
             (Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))

def test_rolling_disc():
    # Rolling Disc Example
    # Here the rolling disc is formed from the contact point up, removing the
    # need to introduce generalized speeds. Only 3 configuration and three
    # speed variables are need to describe this system, along with the disc's
    # mass and radius, and the local graivty (note that mass will drop out).
    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
    r, m, g = symbols('r m g')

    # The kinematics are formed by a series of simple rotations. Each simple
    # rotation creates a new frame, and the next rotation is defined by the new
    # frame's basis vectors. This example uses a 3-1-2 series of rotations, or
    # Z, X, Y series of rotations. Angular velocity for this is defined using
    # the second frame's basis (the lean frame).
    N = ReferenceFrame('N')
    Y = N.orientnew('Y', 'Axis', [q1, N.z])
    L = Y.orientnew('L', 'Axis', [q2, Y.x])
    R = L.orientnew('R', 'Axis', [q3, L.y])
    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
    R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))

    # This is the translational kinematics. We create a point with no velocity
    # in N; this is the contact point between the disc and ground. Next we form
    # the position vector from the contact point to the disc mass center.
    # Finally we form the velocity and acceleration of the disc.
    C = Point('C')
    C.set_vel(N, 0)
    Dmc = C.locatenew('Dmc', r * L.z)
    Dmc.v2pt_theory(C, N, R)
    Dmc.a2pt_theory(C, N, R)

    # This is a simple way to form the inertia dyadic.
    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

    # Kinematic differential equations; how the generalized coordinate time
    # derivatives relate to generalized speeds.
    kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]

    # Creation of the force list; it is the gravitational force at the mass
    # center of the disc. Then we create the disc by assigning a Point to the
    # mass center attribute, a ReferenceFrame to the frame attribute, and mass
    # and inertia. Then we form the body list.
    ForceList = [(Dmc, - m * g * Y.z)]
    BodyD = RigidBody()
    BodyD.mc = Dmc
    BodyD.inertia = (I, Dmc)
    BodyD.frame = R
    BodyD.mass = m
    BodyList = [BodyD]

    # Finally we form the equations of motion, using the same steps we did
    # before. Specify inertial frame, supply generalized speeds, supply
    # kinematic differential equation dictionary, compute Fr from the force
    # list and Fr* fromt the body list, compute the mass matrix and forcing
    # terms, then solve for the u dots (time derivatives of the generalized
    # speeds).
    KM = Kane(N)
    KM.coords([q1, q2, q3])
    KM.speeds([u1, u2, u3])
    KM.kindiffeq(kd)
    KM.kanes_equations(ForceList, BodyList)
    MM = KM.mass_matrix
    forcing = KM.forcing
    rhs = MM.inv() * forcing
    kdd = KM.kindiffdict()
    rhs = rhs.subs(kdd)
    assert rhs.expand() == Matrix([(10*u2*u3*r - 5*u3**2*r*tan(q2) +
        4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()

    l: 2,
    w: 1,
    f: 2,
    v0: 20}

N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])


O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))

#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)


L = Lagrangian(N, rb)
lm = LagrangesMethod(
    L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d])
lm.form_lagranges_equations()
rhs = lm.rhs()
print('{} = {}'.format(msprint(q1d.diff(t)),
    msprint(rhs[3].simplify())))
print('{} = {}'.format(msprint(q2d.diff(t)),
    msprint(rhs[4].simplify())))
print('{} = {}'.format(msprint(q3d.diff(t)),
    msprint(rhs[5].simplify())))