So lets model this…
I’ll begin with importing all the necessary libraries.
from sympy import symbols, init_printing
import sympy
import sympy.physics.mechanics as me
from pydy.system import System
import matplotlib as mpl
import matplotlib.pyplot as plt
import numpy as np
%matplotlib inline
init_printing()
I am going to make all the masses and pendulum lengths the same. The cart_mass is actually the connection point from \(L_1\) to the rigid ceiling. The connection point will be free to move in the \(Y\) direction. However, a very strong spring force will hold the point. This spring connection will be key to make the pendulum oscillate.
# Constants
Length1 = 0.5
Length2 = 0.5
Length3 = 0.5
point1_mass = 0.01
point2_mass = 0.01
point3_mass = 0.01
cart_mass = 10.0
# Create the Main Frame
A = me.ReferenceFrame('A')
# Create the symbols
x, y, beta, theta, gamma, f = me.dynamicsymbols('x y beta theta gamma f')
x_dot, y_dot, gamma_dot = me.dynamicsymbols('x_dot y_dot gamma_dot')
beta_dot, theta_dot = me.dynamicsymbols('beta_dot theta_dot')
M, m1, m2, m3, g, t = sympy.symbols('M m1 m2 m3 g t')
L1, L2, L3 = sympy.symbols('L1 L2 L3')
# Orient the other frames
B = A.orientnew('B', 'Axis', [beta, A.z])
C = A.orientnew('C', 'Axis', [theta, A.z])
D = A.orientnew('D', 'Axis', [gamma, A.z])
# Create the Origin
Origin = me.Point('Origin')
Origin.set_pos(Origin, 0)
Origin.set_vel(A, 0)
# Create the Rod Points
P0 = me.Point('P0')
P1 = me.Point('P1')
P2 = me.Point('P2')
P3 = me.Point('P3')
# Set the Rod End Points Positions
P0.set_pos(Origin, x * A.x + y * A.y)
P1.set_pos(P0, -L1 * B.y)
P2.set_pos(P1, -L2 * C.y)
P3.set_pos(P2, -L3 * D.y)
Now we can set the velocities and export the velocity of P3.
# Set the Velocities of the Points
P0.set_vel(A, x_dot * A.x + y_dot * A.y)
P1.v2pt_theory(P0, A, B)
P2.v2pt_theory(P1, A, C)
P3.v2pt_theory(P2, A, D)
\[ x_{dot}\mathbf{\hat{a}_x} + y_{dot}\mathbf{\hat{a}_y} + L_{1} \dot{\beta}\mathbf{\hat{b}_x} + L_{2} \dot{\theta}\mathbf{\hat{c}_x} + L_{3} \dot{\Gamma\left(t\right)}\mathbf{\hat{d}_x} \]
# Set up the kinematic differential equations
kde = [x_dot - x.diff(t),
y_dot - y.diff(t),
beta_dot - beta.diff(t),
theta_dot - theta.diff(t),
gamma_dot - gamma.diff(t)]
# Create the Particles
Pa0 = me.Particle('Pa0', P0, M)
Pa1 = me.Particle('Pa1', P1, m1)
Pa2 = me.Particle('Pa2', P2, m2)
Pa3 = me.Particle('Pa3', P3, m3)
We can now add the external forces acting on the bodies. I added a slight rotational \(k\) force on all the connection points so they will eventually stop rotating. The horizontal variable is the huge spring that will make the device vibrate. The forcing variable is a “push” on the last mass. We are going to push it to see how well it can stabilize.
# Creating the forces acting on the bodies
grav_force_0 = (P0, -M * g * A.y)
grav_force_1 = (P1, -m1 * g * A.y)
grav_force_2 = (P2, -m2 * g * A.y)
grav_force_3 = (P3, -m3 * g * A.y)
static_force = (P0, (m1 + m2 + m3 + M) * g * A.y)
rot1_k = (B, -beta_dot * 0.001 * A.z)
rot2_k = (C, -theta_dot * 0.001 * A.z)
rot3_k = (D, -gamma_dot * 0.001 * A.z)
horizontal = (P0, -y * 1000000 * A.y)
forcing = (P3, f * A.x)
loads = [grav_force_0,
grav_force_1,
grav_force_2,
grav_force_3,
static_force,
rot1_k,
rot2_k,
rot3_k,
horizontal,
forcing]
This is our “pushing” force.
def bang(amp,begin,end,t):
duration = end - begin
return amp * (t >= begin) + -2*amp * (t >= end) + amp * (t>= end + duration)
# Setting up the coordinates, speeds, and creating KanesMethod
coordinates = [x, y, beta, theta, gamma]
speeds = [x_dot, y_dot, beta_dot, theta_dot, gamma_dot]
kane = me.KanesMethod(A, coordinates, speeds, kde)
# Creating Fr and Fr_star
fr, frstar = kane.kanes_equations(loads, [Pa0, Pa1, Pa2, Pa3])
# Creating the PyDy System
sys = System(kane)
# Assigning all the constants
runtime = 15
sys.constants = {m1: point1_mass,
m2: point2_mass,
m3: point3_mass,
M: cart_mass,
g: 9.81,
L1: Length1,
L2: Length2,
L3: Length3}
# Y at -0.025 for proper oscillation
sys.initial_conditions = {x:0, y:-0.025, beta:3.2, theta:3.2, gamma:3.2}
# sys.specifieds={f:lambda y, t:bang(4,2,4,t)}
sys.specifieds={f:lambda y, t:bang(0.5,2,2.2,t)}
sys.times = np.linspace(0.0, runtime, runtime*30)
sys.generate_ode_function(generator='cython')
resp = sys.integrate()
Let’s plot the response as if we were looking straight on.
x_resp = resp[:,0]
y_resp = resp[:,1]
beta_resp = resp[:,2]
theta_resp = resp[:,3]
gamma_resp = resp[:,4]
ax = plt.gca()
ax.set_aspect('equal')
# plt.subplot(131)
plt.plot(x_resp, y_resp)
pen1 =Length1*np.sin(beta_resp) + x_resp
pen2 = -Length1*np.cos(beta_resp) + y_resp
pen3 = (Length1*np.sin(beta_resp) + Length2*np.sin(theta_resp) + x_resp)
pen4 = (-Length1*np.cos(beta_resp) - Length2*np.cos(theta_resp) + y_resp)
pen5 = (Length1*np.sin(beta_resp) + Length2*np.sin(theta_resp) + Length3*np.sin(gamma_resp) + x_resp)
pen6 = (-Length1*np.cos(beta_resp) - Length2*np.cos(theta_resp) - Length3*np.cos(gamma_resp) + y_resp)
plt.plot(pen1, pen2)
plt.plot(pen3, pen4)
plt.plot(pen5, pen6)
plt.xlim(-2,3)
plt.ylim(-2,3)
plt.title('Front Facing Response')
So why does this happen?
… to be continued
Created: 15 Jun 2019