===========================
Three Link Conical Pendulum
===========================

.. note::
   You can download this example as a Python script:
   :jupyter-download:script:`three-link-conical-pendulum` or Jupyter notebook:
   :jupyter-download:notebook:`three-link-conical-pendulum`.

This example shows how to simulate a three link conical compound pendulum made
up of cylindrical bars connecting particles at the joints. The is an example of
a 3D holonomic system. Wikipedia provides a basic description of a `conical
pendulum`_.

.. _conical pendulum: https://en.wikipedia.org/wiki/Conical_pendulum

Derive the Equations of Motion
==============================

.. jupyter-execute::

   from sympy import symbols
   import sympy.physics.mechanics as me

The conical pendulum will have three links and three bobs.

.. jupyter-execute::

   n = 3

Each link's orientation is described by two spaced fixed angles: alpha and
beta.

.. jupyter-execute::

   # Generalized coordinates
   alpha = me.dynamicsymbols('alpha:{}'.format(n))
   beta = me.dynamicsymbols('beta:{}'.format(n))

   # Generalized speeds
   omega = me.dynamicsymbols('omega:{}'.format(n))
   delta = me.dynamicsymbols('delta:{}'.format(n))

At each joint there are point masses (i.e. the bobs).

.. jupyter-execute::

   m_bob = symbols('m:{}'.format(n))

Each link is modeled as a cylinder so it will have a length, mass, and a
symmetric inertia tensor.

.. jupyter-execute::

   l = symbols('l:{}'.format(n))
   m_link = symbols('M:{}'.format(n))
   Ixx = symbols('Ixx:{}'.format(n))
   Iyy = symbols('Iyy:{}'.format(n))
   Izz = symbols('Izz:{}'.format(n))

Acceleration due to gravity will be used when prescribing the forces acting on
the links and bobs.

.. jupyter-execute::

   g = symbols('g')

Now defining an inertial reference frame for the system to live in. The Y axis
of the frame will be aligned with, but opposite to, the gravity vector.

.. jupyter-execute::

   I = me.ReferenceFrame('I')

Three reference frames will track the orientation of the three links.

.. jupyter-execute::

   A = me.ReferenceFrame('A')
   A.orient(I, 'Space', [alpha[0], beta[0], 0], 'ZXY')

   B = me.ReferenceFrame('B')
   B.orient(A, 'Space', [alpha[1], beta[1], 0], 'ZXY')

   C = me.ReferenceFrame('C')
   C.orient(B, 'Space', [alpha[2], beta[2], 0], 'ZXY')

Define the kinematical differential equations such that the generalized speeds
equal the time derivative of the generalized coordinates.

.. jupyter-execute::

   kinematic_differentials = []
   for i in range(n):
      kinematic_differentials.append(omega[i] - alpha[i].diff())
      kinematic_differentials.append(delta[i] - beta[i].diff())

The angular velocities of the three frames can then be set.

.. jupyter-execute::

   A.set_ang_vel(I, omega[0] * I.z + delta[0] * I.x)
   B.set_ang_vel(I, omega[1] * I.z + delta[1] * I.x)
   C.set_ang_vel(I, omega[2] * I.z + delta[2] * I.x)

The base of the pendulum will be located at a point O which is stationary in
the inertial reference frame.

.. jupyter-execute::

   O = me.Point('O')
   O.set_vel(I, 0)

The location of the bobs (at the joints between the links) are created by
specifiying the vectors between the points.

.. jupyter-execute::

   P1 = O.locatenew('P1', -l[0] * A.y)
   P2 = P1.locatenew('P2', -l[1] * B.y)
   P3 = P2.locatenew('P3', -l[2] * C.y)

The velocities of the points can be computed by taking advantage that pairs of
points are fixed on the referene frames.

.. jupyter-execute::

   P1.v2pt_theory(O, I, A)
   P2.v2pt_theory(P1, I, B)
   P3.v2pt_theory(P2, I, C)
   points = [P1, P2, P3]

Now create a particle to represent each bob.

.. jupyter-execute::

   Pa1 = me.Particle('Pa1', points[0], m_bob[0])
   Pa2 = me.Particle('Pa2', points[1], m_bob[1])
   Pa3 = me.Particle('Pa3', points[2], m_bob[2])
   particles = [Pa1, Pa2, Pa3]

The mass centers of each link need to be specified and, assuming a constant
density cylinder, it is equidistance from each joint.

.. jupyter-execute::

   P_link1 = O.locatenew('P_link1', -l[0] / 2 * A.y)
   P_link2 = P1.locatenew('P_link2', -l[1] / 2 * B.y)
   P_link3 = P2.locatenew('P_link3', -l[2] / 2 * C.y)

The linear velocities can be specified the same way as the bob points.

.. jupyter-execute::

   P_link1.v2pt_theory(O, I, A)
   P_link2.v2pt_theory(P1, I, B)
   P_link3.v2pt_theory(P2, I, C)

   points_rigid_body = [P_link1, P_link2, P_link3]

The inertia tensors for the links are defined with respect to the mass center
of the link and the link's reference frame.

.. jupyter-execute::

   inertia_link1 = (me.inertia(A, Ixx[0], Iyy[0], Izz[0]), P_link1)
   inertia_link2 = (me.inertia(B, Ixx[1], Iyy[1], Izz[1]), P_link2)
   inertia_link3 = (me.inertia(C, Ixx[2], Iyy[2], Izz[2]), P_link3)

Now rigid bodies can be created for each link.

.. jupyter-execute::

   link1 = me.RigidBody('link1', P_link1, A, m_link[0], inertia_link1)
   link2 = me.RigidBody('link2', P_link2, B, m_link[1], inertia_link2)
   link3 = me.RigidBody('link3', P_link3, C, m_link[2], inertia_link3)
   links = [link1, link2, link3]

The only contributing forces to the system is the force due to gravity acting
on each particle and body.

.. jupyter-execute::

   forces = []

   for particle in particles:
      mass = particle.mass
      point = particle.point
      forces.append((point, -mass * g * I.y))

   for link in links:
      mass = link.mass
      point = link.masscenter
      forces.append((point, -mass * g * I.y))

Make a list of all the particles and bodies in the system.

.. jupyter-execute::

   total_system = links + particles

Lists of all generalized coordinates and speeds.

.. jupyter-execute::

   q = alpha + beta
   u = omega + delta

Now the equations of motion of the system can be formed.

.. jupyter-execute::

   kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kinematic_differentials)
   fr, frstar = kane.kanes_equations(total_system, loads=forces)

Simulate the System
===================

.. jupyter-execute::

   from numpy import radians, linspace, hstack, zeros, ones
   from scipy.integrate import odeint
   from pydy.codegen.ode_function_generators import generate_ode_function

   param_syms = []
   for par_seq in [l, m_bob, m_link, Ixx, Iyy, Izz, (g,)]:
      param_syms += list(par_seq)

All of the links and bobs will have the same numerical values for the
parameters.

.. jupyter-execute::

   link_length = 10.0  # meters
   link_mass = 10.0  # kg
   link_radius = 0.5  # meters
   link_ixx = 1.0 / 12.0 * link_mass * (3.0 * link_radius**2 + link_length**2)
   link_iyy = link_mass * link_radius**2
   link_izz = link_ixx

   particle_mass = 5.0  # kg
   particle_radius = 1.0  # meters

Create a list of the numerical values which have the same order as the list of
symbolic parameters.

.. jupyter-execute::

   param_vals = ([link_length for x in l] +
                 [particle_mass for x in m_bob] +
                 [link_mass for x in m_link] +
                 [link_ixx for x in list(Ixx)] +
                 [link_iyy for x in list(Iyy)] +
                 [link_izz for x in list(Izz)] +
                 [9.8])

A function that evaluates the right hand side of the set of first order ODEs
can be generated.

.. jupyter-execute::

   right_hand_side = generate_ode_function(kane.forcing_full, q, u, param_syms,
                                           mass_matrix=kane.mass_matrix_full,
                                           generator='cython',
                                           linear_sys_solver='sympy')

To simulate the system, a time vector and initial conditions for the
system's states is required.

.. jupyter-execute::

   duration = 10.0
   fps = 60.0
   t = linspace(0.0, duration, num=int(duration*fps))
   x0 = hstack((ones(6) * radians(10.0), zeros(6)))

   state_trajectories = odeint(right_hand_side, x0, t,
                               args=(dict(zip(param_syms, param_vals)),))

Visualize the System
====================

.. jupyter-execute::

   from pydy.viz.shapes import Cylinder, Sphere
   from pydy.viz.scene import Scene
   from pydy.viz.visualization_frame import VisualizationFrame

A cylinder will be attached to each link and a sphere to each bob for the
visualization.

.. jupyter-execute::

   viz_frames = []

   for i, (link, particle) in enumerate(zip(links, particles)):

      link_shape = Cylinder(name='cylinder{}'.format(i),
                           radius=link_radius,
                           length=link_length,
                           color='red')

      viz_frames.append(VisualizationFrame('link_frame{}'.format(i), link,
                                          link_shape))

      particle_shape = Sphere(name='sphere{}'.format(i),
                              radius=particle_radius,
                              color='blue')

      viz_frames.append(VisualizationFrame('particle_frame{}'.format(i),
                                          link.frame,
                                          particle,
                                          particle_shape))

Now the visualization frames can be passed in to create a scene.

.. jupyter-execute::

   scene = Scene(I, O, *viz_frames)

Provide the data to compute the trajectories of the visualization frames.

.. jupyter-execute::

   scene.times = t
   scene.constants = dict(zip(param_syms, param_vals))
   scene.states_symbols = q + u
   scene.states_trajectories = state_trajectories

   scene.display_jupyter()