In this notebook, we are going to develop the EoMs for Double Pendulum system. We are going to derive EoMs, simulate them and finally visualize them.
The code is loaded in the notebook using %load IPython magic, to avoid duplication of the code.
In [1]:
#import the code from double_pendulum.py
%load double_pendulum.py
In [ ]:
from sympy import symbols
from sympy.physics.mechanics import *
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
l, m, g = symbols('l m g')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = N.orientnew('B', 'Axis', [q2, N.z])
A.set_ang_vel(N, u1 * N.z)
B.set_ang_vel(N, u2 * N.z)
O = Point('O')
P = O.locatenew('P', l * A.x)
R = P.locatenew('R', l * B.x)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)
ParP = Particle('ParP', P, m)
ParR = Particle('ParR', R, m)
kd = [q1d - u1, q2d - u2]
FL = [(P, m * g * N.x), (R, m * g * N.x)]
BL = [ParP, ParR]
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
KM.kanes_equations(FL, BL)
#kdd = KM.kindiffdict()
#mass_matrix = KM.mass_matrix_full
#forcing_vector = KM.forcing_full
#qudots = mass_matrix.inv() * forcing_vector
#qudots = qudots.subs(kdd)
#qudots.simplify()
#
In [2]:
#import the code from simulate.py
%load simulate.py
In [ ]:
"""
This file will use pydy.codegen to simulate the double pendulum.
"""
from numpy import concatenate, array, linspace
from pydy.system import System
from scipy.integrate import odeint
from double_pendulum import *
constants = {l: 10.0, m: 10.0, g: 9.81}
initial_conditions = {q1: 1.0, q2: 0.0, u1: 0.0, u2: 0.0}
sys = System(KM, constants=constants,
initial_conditions=initial_conditions)
frames_per_sec = 60
final_time = 5.0
times = linspace(0.0, final_time, final_time * frames_per_sec)
sys.times = times
x = sys.integrate()
In [3]:
#Import the code from visualize.py
%load visualize.py
In [ ]:
"""
This file will use pydy.viz to visualize the double pendulum. Run this script
via a command line:
$ python visualization.py
"""
from numpy import pi
from pydy.viz.shapes import Cylinder, Sphere
from pydy.viz.scene import Scene
from pydy.viz.visualization_frame import VisualizationFrame
from simulate import *
# Create geometry
# ===============
# Each link in the pendulum is visualized with a cylinder, and a sphere at its
# far end.
link = Cylinder(name='link', radius=0.5, length=l, color='red')
sphere = Sphere(name='sphere', radius=1.0)
# By default, Cylinders are drawn so that their center is at the origin of the
# VisualizationFrame, and their axis is the y axis of the VisualizationFrame.
# We want the end of the Cylinder to be at the origin of the
# VisualizationFrame, and we want the Cylinder's axis to be aligned with the x
# axis of the VisualizationFrame. For these reasons, we must use the
# 'orientnew' and 'locatenew' methods to create new frames/points.
linkP_frame = A.orientnew('frameP', 'Axis', [0.5 * pi, N.z])
linkP_origin = O.locatenew('originP', 0.5 * l * A.x)
linkP_viz_frame = VisualizationFrame('linkP', linkP_frame, linkP_origin, link)
linkR_frame = B.orientnew('frameR', 'Axis', [0.5 * pi, N.z])
linkR_origin = P.locatenew('originP', 0.5 * l * B.x)
linkR_viz_frame = VisualizationFrame('linkR', linkR_frame, linkR_origin, link)
sphereP_viz_frame = VisualizationFrame('sphereP', N, P, sphere)
sphereR_viz_frame = VisualizationFrame('sphereR', N, R, sphere)
# Construct the scene
# ===================
# We want gravity to be directed downwards in the visualization. Gravity is in
# the -x direction. By default, the visualization uses the xz plane as the
# ground plane. Thus, gravity is contained in the ground plane. However, we
# want gravity to point in the -y direction in the visualization. To achieve
# this, we create a world frame that is rotated +90 degrees about the N frame's
# z direction.
world_frame = N.orientnew('world', 'Axis', [0.5 * pi, N.z])
scene = Scene(world_frame, O,
linkP_viz_frame, linkR_viz_frame, sphereP_viz_frame, sphereR_viz_frame)
# Create the visualization
# ========================
scene.generate_visualization_json_system(sys)
if __name__ == "__main__":
try: #If called from inside notebook,
scene.display_ipython()
except:#If called from interpreter
scene.display()