In order to change the number of suspended particles, modify $n$. The total number of particles is $n+1$. We apply the input force to the first one (Equations of Motion [2]).
We can change a few other values, stored in $parameter\_vals$ (Simulation [1]).
To change the input force (Simulation [4]), we must return a function $f$, that is functor (lambda t: input_function(t)). The example of input function models a step function, whose amplitude is $weigts$ and duration is $dt = 1s$.
Then it is possible to change the initial state $x0$ (that contains both positions and speeds of each points) and the time vector $l0$ (Simulation [5]).
The Plotting section allows us to plot the position or the speed.
The Animation section allows us to generate the complete movement.
In the following, "I" refers to Jason K. Moore, author of the initiale version of this notebook.
I used these software versions for the following computations:
We'll start by generating the equations of motion for the system with SymPy mechanics. The functionality that mechanics provides is much more in depth than Mathematica's functionality. In the Mathematica example, Lagrangian mechanics were implemented manually with Mathematica's symbolic functionality. mechanics provides an assortment of functions and classes to derive the equations of motion for arbitrarily complex (i.e. configuration constraints, nonholonomic motion constraints, etc) multibody systems in a very natural way. First we import the necessary functionality from SymPy.
In [1]:
from sympy import symbols
from sympy.physics.mechanics import *
from sympy import Dummy, lambdify
from numpy import array, hstack, zeros, linspace, pi,floor
from numpy.linalg import solve
from scipy.integrate import odeint
from numpy import zeros, cos, sin, arange, around
from matplotlib import pyplot as plt
from matplotlib import animation
from matplotlib.patches import Rectangle
%pylab
Now specify the number of links, $n$. I'll start with 5 since the Wolfram folks only showed four.
In [ ]:
n = 5
mechanics will need the generalized coordinates, generalized speeds, and the input force which are all time dependent variables and the bob masses, link lengths, and acceleration due to gravity which are all constants. Time, $t$, is also made available because we will need to differentiate with respect to time.
Now we can create and inertial reference frame $I$ and define the point, $O$, as the origin.
Secondly, we define the define the first point of the pendulum as a particle which has mass. This point can only move laterally and represents the motion of the "cart".
Now we can define the $n$ reference frames, particles, gravitational forces, and kinematical differential equations for each of the pendulum links. This is easily done with a loop.
In [ ]:
q = dynamicsymbols('q:' + str(n + 1)) # Generalized coordinates
u = dynamicsymbols('u:' + str(n + 1)) # Generalized speeds
f = dynamicsymbols('f') # Force applied to the cart
m = symbols('m:' + str(n + 1)) # Mass of each bob
l = symbols('l:' + str(n)) # Length of each link
g, t = symbols('g t') # Gravity and time
I = ReferenceFrame('I') # Inertial reference frame
O = Point('O') # Origin point
O.set_vel(I, 0) # Origin's velocity is zero
P0 = Point('P0') # Hinge point of top link
P0.set_pos(O, q[0] * I.x) # Set the position of P0
P0.set_vel(I, u[0] * I.x) # Set the velocity of P0
Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0
frames = [I] # List to hold the n + 1 frames
points = [P0] # List to hold the n + 1 points
particles = [Pa0] # List to hold the n + 1 particles
forces = [(P0, f * I.x - m[0] * g * I.y)] # List to hold the n + 1 applied forces, including the input force, f
kindiffs = [q[0].diff(t) - u[0]] # List to hold kinematic ODE's
for i in range(n):
Bi = I.orientnew('B' + str(i), 'Axis', [q[i + 1], I.z]) # Create a new frame
Bi.set_ang_vel(I, u[i + 1] * I.z) # Set angular velocity
frames.append(Bi) # Add it to the frames list
Pi = points[-1].locatenew('P' + str(i + 1), l[i] * Bi.x) # Create a new point
Pi.v2pt_theory(points[-1], I, Bi) # Set the velocity
points.append(Pi) # Add it to the points list
Pai = Particle('Pa' + str(i + 1), Pi, m[i + 1]) # Create a new particle
particles.append(Pai) # Add it to the particles list
forces.append((Pi, -m[i + 1] * g * I.y)) # Set the force applied at the point
kindiffs.append(q[i + 1].diff(t) - u[i + 1]) # Define the kinematic ODE: dq_i / dt - u_i = 0
With all of the necessary point velocities and particle masses defined, the KanesMethod class can be used to derive the equations of motion of the system automatically.
The equations of motion are quite long. This is the general nature of most non-simple mutlibody problems. That is why a SymPy is so useful; no more mistakes in algegra, differentiation, or copying in hand written equations.
In [ ]:
kane = KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs) # Initialize the object
fr, frstar = kane.kanes_equations(forces, particles) # Generate EoM's fr + frstar = 0
In [33]:
arm_length = 1. / n # The maximum length of the pendulum is 1 meter
bob_mass = 0.01 / n # The maximum mass of the bobs is 10 grams
parameters = [g, m[0]] # Parameter definitions starting with gravity and the first bob
parameter_vals = [9.81, 0.01 / n] # Numerical values for the first two
for i in range(n): # Then each mass and length
parameters += [l[i], m[i + 1]]
parameter_vals += [arm_length, bob_mass]
Mathematica has a really nice NDSolve function for quickly integrating their symbolic differential equations. We have plans to develop something similar for SymPy but haven't found the development time yet to do it properly. So the next bit isn't as clean as we'd like but you can make use of SymPy's lambdify function to create functions that will evaluate the mass matrix, $M$, and forcing vector, $\bar{f}$ from $M\dot{u} = \bar{f}(q, \dot{q}, u, t)$ as a NumPy function. We make use of dummy symbols to replace the time varying functions in the SymPy equations a simple dummy symbol.
In [34]:
dynamic = q + u # Make a list of the states
dynamic.append(f) # Add the input force
dummy_symbols = [Dummy() for i in dynamic] # Create a dummy symbol for each variable
dummy_dict = dict(zip(dynamic, dummy_symbols))
kindiff_dict = kane.kindiffdict() # Get the solved kinematical differential equations
M = kane.mass_matrix_full.subs(kindiff_dict).subs(dummy_dict) # Substitute into the mass matrix
F = kane.forcing_full.subs(kindiff_dict).subs(dummy_dict) # Substitute into the forcing vector
M_func = lambdify(dummy_symbols + parameters, M) # Create a callable function to evaluate the mass matrix
F_func = lambdify(dummy_symbols + parameters, F) # Create a callable function to evaluate the forcing vector
To integrate the ODE's we need to define a function that returns the derivatives of the states given the current state and time.
In [35]:
def functor(f):
def right_hand_side(x, t, args):
arguments = hstack((x, f(t), args)) # States, input, and parameters
dx = array(solve(M_func(*arguments), # Solving for the derivatives
F_func(*arguments))).T[0]
return dx
return right_hand_side
In [36]:
weights = [0.01,0.06,-0.05,0.01,0.,0.,0.,0.,-0.1,0.01]
dt = 1
def activate (t, dt):
return 0 <= t < dt
f = functor (lambda t: sum([w * activate(t - i * dt, dt) for i, w in enumerate(weights)]))
Now that we have the right hand side function, the initial conditions are set such that the pendulum is in the vertical equilibrium and a slight initial rate is set for each speed to ensure the pendulum falls. The equations can then be integrated with SciPy's odeint function given a time series.
In [37]:
x0 = hstack(( 0, -pi / 2 * ones(len(q) - 1), 0 * ones(len(u)) )) # Initial conditions, q and u
t = linspace(0, 10, 1000) # Time vector
y = odeint(f, x0, t, args=(parameter_vals,)) # Actual integration
y[3, 0]
Out[37]:
In [38]:
lines = plot(t, y[:, :y.shape[1] / 2])
lab = xlabel('Time [sec]')
leg = legend(dynamic[:y.shape[1] / 2])
In [18]:
lines = plot(t, y[:, y.shape[1] / 2:])
lab = xlabel('Time [sec]')
leg = legend(dynamic[y.shape[1] / 2:])
The following function was modeled from Jake Vanderplas's post on matplotlib animations.
In [39]:
def animate_pendulum(t, states, length, filename=None):
"""Animates the n-pendulum and optionally saves it to file.
Parameters
----------
t : ndarray, shape(m)
Time array.
states: ndarray, shape(m,p)
State time history.
length: float
The length of the pendulum links.
filename: string or None, optional
If true a movie file will be saved of the animation. This may take some time.
Returns
-------
fig : matplotlib.Figure
The figure.
anim : matplotlib.FuncAnimation
The animation.
"""
# the number of pendulum bobs
numpoints = states.shape[1] / 2
# first set up the figure, the axis, and the plot elements we want to animate
fig = plt.figure()
# some dimesions
cart_width = 0.4
cart_height = 0.2
# set the limits based on the motion
xmin = around(states[:, 0].min() - cart_width / 2.0, 1)
xmax = around(states[:, 0].max() + cart_width / 2.0, 1)
# create the axes
ax = plt.axes(xlim=(xmin, xmax), ylim=(-1.1, 1.1))
# display the current time
time_text = ax.text(0.04, 0.9, '', transform=ax.transAxes)
# create a rectangular cart
rect = Rectangle([states[0, 0] - cart_width / 2.0, -cart_height / 2],
cart_width, cart_height, fill=True, color='red', ec='black')
ax.add_patch(rect)
# blank line for the pendulum
line, = ax.plot([], [], lw=2, marker='o', markersize=6)
# initialization function: plot the background of each frame
def init():
time_text.set_text('')
rect.set_xy((0.0, 0.0))
line.set_data([], [])
return time_text, rect, line,
# animation function: update the objects
def animate(i):
time_text.set_text('time = {:2.2f}'.format(t[i]))
rect.set_xy((states[i, 0] - cart_width / 2.0, -cart_height / 2))
x = hstack((states[i, 0], zeros((numpoints - 1))))
y = zeros((numpoints))
for j in arange(1, numpoints):
x[j] = x[j - 1] + length * cos(states[i, j])
y[j] = y[j - 1] + length * sin(states[i, j])
line.set_data(x, y)
return time_text, rect, line,
# call the animator function
anim = animation.FuncAnimation(fig, animate, frames=len(t), init_func=init,
interval=t[-1] / len(t) * 1000, blit=True, repeat=False)
# save the animation if a filename is given
if filename is not None:
anim.save(filename, fps=30, codec='libx264')
Now we can create the animation of the pendulum. This animation will show the open loop dynamics.
In [40]:
animate_pendulum(t, y, arm_length, filename="open-loop.mp4")
y[-1,0]
Out[40]:
In [41]:
from IPython.display import HTML
h = \
"""
<video width="640" height="480" controls>
<source src="files/open-loop.mp4" type="video/mp4">
Your browser does not support the video tag, check out the YouTuve version instead: http://youtu.be/Nj3_npq7MZI.
</video>
"""
HTML(h)
Out[41]:
In [ ]: