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_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_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_rigidbody2():
    M, v, r, omega = dynamicsymbols('M v r omega')
    N = ReferenceFrame('N')
    b = ReferenceFrame('b')
    b.set_ang_vel(N, omega * b.x)
    P = Point('P')
    I = outer (b.x, b.x)
    Inertia_tuple = (I, P)
    B = RigidBody('B', P, b, M, Inertia_tuple)
    P.set_vel(N, v * b.x)
    assert B.angularmomentum(P, N) == omega * b.x
    O = Point('O')
    O.set_vel(N, v * b.x)
    P.set_pos(O, r * b.y)
    assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z

def test_disc_on_an_incline_plane():
    # Disc rolling on an inclined plane
    # First the generalized coordinates are created. The mass center of the
    # disc is located from top vertex of the inclined plane by the generalized
    # coordinate 'y'. The orientation of the disc is defined by the angle
    # 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
    # the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
    # gravitational constant.
    y, theta = dynamicsymbols('y theta')
    yd, thetad = dynamicsymbols('y theta', 1)
    m, g, R, l, alpha = symbols('m g R l alpha')

    # Next, we create the inertial reference frame 'N'. A reference frame 'A'
    # is attached to the inclined plane. Finally a frame is created which is attached to the disk.
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z])
    B = A.orientnew('B', 'Axis', [-theta, A.z])

    # Creating the disc 'D'; we create the point that represents the mass
    # center of the disc and set its velocity. The inertia dyadic of the disc
    # is created. Finally, we create the disc.
    Do = Point('Do')
    Do.set_vel(N, yd * A.x)
    I = m * R**2 / 2 * B.z | B.z
    D = RigidBody('D', Do, B, m, (I, Do))

    # To construct the Lagrangian, 'L', of the disc, we determine its kinetic
    # and potential energies, T and U, respectively. L is defined as the
    # difference between T and U.
    D.set_potential_energy(m * g * (l - y) * sin(alpha))
    L = Lagrangian(N, D)

    # We then create the list of generalized coordinates and constraint
    # equations. The constraint arises due to the disc rolling without slip on
    # on the inclined path. Also, the constraint is holonomic but we supply the
    # differentiated holonomic equation as the 'LagrangesMethod' class requires
    # that. We then invoke the 'LagrangesMethod' class and supply it the
    # necessary arguments and generate the equations of motion. The'rhs' method
    # solves for the q_double_dots (i.e. the second derivative with respect to
    # time  of the generalized coordinates and the lagrange multiplers.
    q = [y, theta]
    coneq = [yd - R * thetad]
    m = LagrangesMethod(L, q, coneq)
    m.form_lagranges_equations()
    rhs = m.rhs()
    rhs.simplify()
    assert rhs[2] == 2*g*sin(alpha)/3

def test_rigidbody():
    m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega')
    A = ReferenceFrame('A')
    A2 = ReferenceFrame('A2')
    P = Point('P')
    P2 = Point('P2')
    I = Dyadic(0)
    I2 = Dyadic(0)
    B = RigidBody('B', P, A, m, (I, P))
    assert B.mass == m
    assert B.frame == A
    assert B.masscenter == P
    assert B.inertia == (I, B.masscenter)

    B.mass = m2
    B.frame = A2
    B.masscenter = P2
    B.inertia = (I2, B.masscenter)
    raises(TypeError, lambda: RigidBody(P, P, A, m, (I, P)))
    raises(TypeError, lambda: RigidBody('B', P, P, m, (I, P)))
    raises(TypeError, lambda: RigidBody('B', P, A, m, (P, P)))
    raises(TypeError, lambda: RigidBody('B', P, A, m, (I, I)))
    assert B.__str__() == 'B'
    assert B.mass == m2
    assert B.frame == A2
    assert B.masscenter == P2
    assert B.inertia == (I2, B.masscenter)
    assert B.masscenter == P2
    assert B.inertia == (I2, B.masscenter)

    # Testing linear momentum function assuming A2 is the inertial frame
    N = ReferenceFrame('N')
    P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
    assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)

def test_potential_energy():
    m, M, l1, g, h, H = symbols('m M l1 g h H')
    omega = dynamicsymbols('omega')
    N = ReferenceFrame('N')
    O = Point('O')
    O.set_vel(N, 0 * N.x)
    Ac = O.locatenew('Ac', l1 * N.x)
    P = Ac.locatenew('P', l1 * N.x)
    a = ReferenceFrame('a')
    a.set_ang_vel(N, omega * N.z)
    Ac.v2pt_theory(O, N, a)
    P.v2pt_theory(O, N, a)
    Pa = Particle('Pa', P, m)
    I = outer(N.z, N.z)
    A = RigidBody('A', Ac, a, M, (I, Ac))
    Pa.set_potential_energy(m * g * h)
    A.set_potential_energy(M * g * H)
    assert potential_energy(A, Pa) == m * g * h + M * g * H

def test_rigidbody2():
    M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
    N = ReferenceFrame('N')
    b = ReferenceFrame('b')
    b.set_ang_vel(N, omega * b.x)
    P = Point('P')
    I = outer(b.x, b.x)
    Inertia_tuple = (I, P)
    B = RigidBody('B', P, b, M, Inertia_tuple)
    P.set_vel(N, v * b.x)
    assert B.angular_momentum(P, N) == omega * b.x
    O = Point('O')
    O.set_vel(N, v * b.x)
    P.set_pos(O, r * b.y)
    assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
    B.potential_energy = M * g * h
    assert B.potential_energy == M * g * h
    assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2

def test_potential_energy():
    m, M, l1, g, h, H = symbols("m M l1 g h H")
    omega = dynamicsymbols("omega")
    N = ReferenceFrame("N")
    O = Point("O")
    O.set_vel(N, 0 * N.x)
    Ac = O.locatenew("Ac", l1 * N.x)
    P = Ac.locatenew("P", l1 * N.x)
    a = ReferenceFrame("a")
    a.set_ang_vel(N, omega * N.z)
    Ac.v2pt_theory(O, N, a)
    P.v2pt_theory(O, N, a)
    Pa = Particle("Pa", P, m)
    I = outer(N.z, N.z)
    A = RigidBody("A", Ac, a, M, (I, Ac))
    Pa.potential_energy = m * g * h
    A.potential_energy = M * g * H
    assert potential_energy(A, Pa) == m * g * h + M * g * H

def test_rigidbody3():
    q1, q2, q3, q4 = dynamicsymbols('q1:5')
    p1, p2, p3 = symbols('p1:4')
    m = symbols('m')

    A = ReferenceFrame('A')
    B = A.orientnew('B', 'axis', [q1, A.x])
    O = Point('O')
    O.set_vel(A, q2*A.x + q3*A.y + q4*A.z)
    P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z)
    I = outer(B.x, B.x)

    rb1 = RigidBody('rb1', P, B, m, (I, P))
    # I_S/O = I_S/S* + I_S*/O
    rb2 = RigidBody('rb2', P, B, m,
                    (I + inertia_of_point_mass(m, P.pos_from(O), B), O))

    assert rb1.central_inertia == rb2.central_inertia
    assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)

def test_rigidbody():
    m = Symbol('m')
    A = ReferenceFrame('A')
    P = Point('P')
    I = Dyadic([])
    B = RigidBody()
    assert B.mass == None
    assert B.mc == None
    assert B.inertia == (None, None)
    assert B.frame == None

    B.mass = m
    B.frame = A
    B.cm = P
    B.inertia = (I, B.cm)
    assert B.mass == m
    assert B.frame == A
    assert B.cm == P
    assert B.inertia == (I, B.cm)

    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_rigidbody():
    m, m2 = symbols('m m2')
    A = ReferenceFrame('A')
    A2 = ReferenceFrame('A2')
    P = Point('P')
    P2 = Point('P2')
    I = Dyadic([])
    I2 = Dyadic([])
    B = RigidBody('B', P, A, m, (I, P))
    assert B.mass == m
    assert B.frame == A
    assert B.mc == P
    assert B.inertia == (I, B.mc)

    B.mass = m2
    B.frame = A2
    B.mc = P2
    B.inertia = (I2, B.mc)
    assert B.mass == m2
    assert B.frame == A2
    assert B.mc == P2
    assert B.inertia == (I2, B.mc)

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

pC_star = Point('pC*') # center of mass of rod
pA_star = pC_star.locatenew('pA*', L/2*C.x) # center of disk A
pB_star = pC_star.locatenew('pB*', -L/2*C.x) # center of disk A
pA_hat = pA_star.locatenew('pA^', -r*C.z) # contact point of disk A and ground
pB_hat = pB_star.locatenew('pB^', -r*C.z) # contact point of disk A and ground

pC_star.set_vel(N, v*C.y)
pA_star.v2pt_theory(pC_star, N, C) # pA* and pC* are both fixed in frame C
pB_star.v2pt_theory(pC_star, N, C) # pB* and pC* are both fixed in frame C
pA_hat.v2pt_theory(pA_star, N, A) # pA* and pA^ are both fixed in frame A
pB_hat.v2pt_theory(pB_star, N, B) # pB* and pB^ are both fixed in frame B


I_rod = inertia(C, 0, m0*L**2/12, m0*L**2/12, 0, 0, 0)
rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star))

I_discA = inertia(A, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star))

I_discB = inertia(B, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star))

print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C))))
print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N))))

qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)],
               [q2d, q3d])
print(msprint(qd_val))

print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val)))))

# bearing A
pA = Point('A')
pA.set_vel(N, 0)
pA.set_vel(F1, 0)

# bearing B, center of mass of disc
pB = pA.locatenew('pB', -R*F1.y)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
print('v_B_N = {}'.format(msprint(pB.vel(N))))

Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(F2, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('disc', pB, B, m, (I_disc, pB))
H = rb_disc.angular_momentum(pB, N).subs(vals).express(F2).simplify()
print("H about B in frame N = {}".format(msprint(H)))

#2b
# disc/ground contact point
pC = pB.locatenew('pC', -r*F2.y)

fAx, fAy, fAz, fBx, fBy, fBz = symbols('fAx fAy fAz fBx fBy fBz')
fCx, fCy, fCz = symbols('fCx fCy fCz')
mAx, mAy, mAz, mBx, mBy, mBz = symbols('mAx mAy mAz mBx mBy mBz')

# forces on rod, disc
fA = fAx*F1.x + fAy*F1.y + fAz*F1.z # force exerted on rod at point A
fB = fBx*F2.x + fBy*F2.y + fBz*F2.z # force exerted on disc by rod at point B
fC = fCx*F2.x + fCy*F2.y + fCz*F2.z # force exerted on ground by disc at point C

# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)

# kinematic differential equations
# kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])]
# kde += [x - y for x, y in zip(qd[3:], u[3:])]
# kde_map = solve(kde, qd)
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])

# define disc rigidbody
IC = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m * g * dot(pC_star.pos_from(pR), A.z))

# potential energy
V = rbC.potential_energy
print("V = {0}".format(msprint(V)))

# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print("K = {0}".format(msprint(K)))

u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print("\ngeneralized active forces, Fr")

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()

q1, q2, q3 = dynamicsymbols('q1, q2 q3')
#omega1, omega2, omega3 = dynamicsymbols('ω1 ω2 ω3')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)

# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')

# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)

# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))

# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))

print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
K_min = dot(H, H) / I_max / 2
K_max = dot(H, H) / I_min / 2
print('K_ω_min = {0}'.format(msprint(K_min)))
print('K_ω_max = {0}'.format(msprint(K_max)))

from sympy.physics.vector import ReferenceFrame
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy import symbols

q = q1, q2, q3, q4, q5, q6 = dynamicsymbols("q1:7")
m, g, k, px, Ip = symbols("m g k px Ip")
N = ReferenceFrame("N")
B = N.orientnew("B", "body", [q4, q5, q6], "zyx")

A = Point("A")
S = A.locatenew("S", q1 * N.x + q2 * N.y + q3 * N.z)
P = S.locatenew("P", px * B.x)
A.set_vel(N, 0)
S.set_vel(N, S.pos_from(A).dt(N))
P.v2pt_theory(S, N, B)

Ixx = Ip / 2
Iyy = Ip / 2
Izz = Ip
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody("rb", S, B, m, (I, S))
rb.set_potential_energy(-m * g * (rb.masscenter.pos_from(A) & N.z) + k / 2 * (P.pos_from(A)).magnitude() ** 2)

L = Lagrangian(N, rb)
print("{} = {}\n".format("L", msprint(L)))

lm = LagrangesMethod(L, q)
print(msprint(lm.form_lagranges_equations()))