======================================
A rolling disc using Lagrange's Method
======================================

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 needed to describe this system, along with the
disc's mass and radius, and the local gravity. ::

  >>> from sympy import symbols, cos, sin
  >>> from sympy.physics.mechanics import *
  >>> mechanics_printing(pretty_print=False)
  >>> 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)
  r*(sin(q2)*q1' + q3')*L.x - r*q2'*L.y

Forming the inertia dyadic. ::

  >>> I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
  >>> mprint(I)
  m*r**2/4*(L.x|L.x) + m*r**2/2*(L.y|L.y) + m*r**2/4*(L.z|L.z)
  >>> BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))

We then set the potential energy and determine the Lagrangian of the rolling
disc. ::

  >>> BodyD.potential_energy = - m * g * r * cos(q2)
  >>> Lag = Lagrangian(N, BodyD)

Then the equations of motion are generated by initializing the
``LagrangesMethod`` object. Finally we solve for the generalized
accelerations(q double dots) with the ``rhs`` method. ::

  >>> q = [q1, q2, q3]
  >>> l = LagrangesMethod(Lag, q)
  >>> le = l.form_lagranges_equations()
  >>> le.simplify(); le
  Matrix([
  [m*r**2*(6*sin(q2)*q3'' + 5*sin(2*q2)*q1'*q2' + 6*cos(q2)*q2'*q3' - 5*cos(2*q2)*q1''/2 + 7*q1''/2)/4],
  [                      m*r*(4*g*sin(q2) - 5*r*sin(2*q2)*q1'**2/2 - 6*r*cos(q2)*q1'*q3' + 5*r*q2'')/4],
  [                                                 3*m*r**2*(sin(q2)*q1'' + cos(q2)*q1'*q2' + q3'')/2]])
  >>> lrhs = l.rhs(); lrhs.simplify(); lrhs
  Matrix([
  [                                                          q1'],
  [                                                          q2'],
  [                                                          q3'],
  [                       -2*(2*tan(q2)*q1' + 3*q3'/cos(q2))*q2'],
  [-4*g*sin(q2)/(5*r) + sin(2*q2)*q1'**2/2 + 6*cos(q2)*q1'*q3'/5],
  [         (-5*cos(q2)*q1' + 6*tan(q2)*q3' + 4*q1'/cos(q2))*q2']])
