This is example is the classic 4 bar linkage. In this case there are two particles at P2 and P3 with mass.
import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting()
We have three non-minimal coordinates but only one is needed to specify the configuration.
q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
u1 = me.dynamicsymbols('u1')
T = me.dynamicsymbols('T')
l1, l2, l3, l4 = sm.symbols('l1:5')
m2, m3, g = sm.symbols('m2, m3, g')
N = me.ReferenceFrame('N')
A = N.orientnew('A', 'Axis', (q1, N.z))
B = A.orientnew('B', 'Axis', (q2, -A.z))
C = B.orientnew('C', 'Axis', (q3, -B.z))
P1 = me.Point('P1')
P2 = P1.locatenew('P2', l1*A.x)
P3 = P2.locatenew('P3', l2*B.x)
P4 = P3.locatenew('P4', l3*C.x)
This is the vector equation that will be used to create the configuration constraints.
loop = P4.pos_from(P1) - l4*N.x
loop
There are two configuration constraints and thus only one of the configuration variables is necessary for speciying the complete coniguration of the system. In this case it may be possible to solve these two nonlinear equations for q2 and q3, but that is often not the case.
config_con1 = loop.dot(N.x).simplify()
config_con1
config_con2 = loop.dot(N.y).simplify()
config_con2
Time differentiate the two configuation constraints. This effectively provides two motion constraints which can be used to elminate all time derivatives related to $q_2$ and $q_3$.
t = me.dynamicsymbols._t
config_con1_dot = config_con1.diff(t).subs({q1.diff(): u1})
config_con2_dot = config_con2.diff(t).subs({q1.diff(): u1})
These motion constraints are linear in the $\dot{q}$'s and thus can be solved. I went ahead a plugged in $u_1$ for $\dot{q}_1$.
qdots_nonmin = sm.solve([config_con1_dot, config_con2_dot], q2.diff(), q3.diff())
qdots_nonmin
Now set the velocities and angular velocities to be only in terms of the three non-minimal q's and the single actual u.
A.set_ang_vel(N, u1*N.z)
B.set_ang_vel(A, -qdots_nonmin[q2.diff()]*A.z)
C.set_ang_vel(B, -qdots_nonmin[q3.diff()]*B.z)
P1.set_vel(N, 0)
P2.v2pt_theory(P1, N, A)
P3.v2pt_theory(P2, N, B)
P2.acc(N)
P3.acc(N)
P3.set_acc(N, P3.acc(N).subs(qdots_nonmin))
P3.acc(N)
Apply gravity to both particles and also add a driving torque to the $l_1$ bar.
RP2 = -m2*g*N.y + T/l1*A.y
RP3 = -m3*g*N.y
F1 = P2.vel(N).diff(u1, N).dot(RP2) + P3.vel(N).diff(u1, N).dot(RP3)
F1
F1star = P2.vel(N).diff(u1, N).dot(-m2*P2.acc(N)) + P3.vel(N).diff(u1, N).dot(-m3*P3.acc(N))
F1star
Make sure these only contain $q_1,q_2,q_3$, $u_1,\dot{u}_1$, and $T$. There should be no u's related to the non-minimal coordinates.
kanes = F1 + F1star
me.find_dynamicsymbols(kanes)
kanes.free_symbols
M = kanes.diff(u1.diff())
M
G = (M*u1.diff() - kanes).simplify()
me.find_dynamicsymbols(G)
from pydy.codegen.ode_function_generators import generate_ode_function
eval_rhs = generate_ode_function(sm.Matrix([G]), # right hand side of M*u1'=G
(q1, q2, q3), # all three coordinates
(u1,), # the single generalized speed
(g, l1, l2, l3, m2, m3), # constants
mass_matrix=sm.Matrix([M]), # M of M*u1'=G
coordinate_derivatives=sm.Matrix([u1, # kinematical differential equations: H of q'= H
qdots_nonmin[q2.diff()],
qdots_nonmin[q3.diff()]]),
specifieds=(T,)) # the specified torque
This creates a function that is useable with scipy.integrate.odeint
or other ODE integrators.
eval_rhs?
Signature: eval_rhs(*args) Docstring: Returns the derivatives of the states, i.e. numerically evaluates the right hand side of the first order differential equation. x' = f(x, t, r, p) Parameters ========== x : ndarray, shape(4,) The state vector is ordered as such: - q1(t) - q2(t) - q3(t) - u1(t) t : float The current time. r : dictionary; ndarray, shape(1,); function There are three options for this argument. (1) is more flexible but (2) and (3) are much more efficient. (1) A dictionary that maps the specified functions of time to floats, ndarrays, or functions that produce ndarrays. The keys can be a single specified symbolic function of time or a tuple of symbols. The total number of symbols must be equal to 1. If the value is a function it must be of the form g(x, t), where x is the current state vector ndarray and t is the current time float and it must return an ndarray of the correct shape. For example:: r = {a: 1.0, (d, b) : np.array([1.0, 2.0]), (e, f) : lambda x, t: np.array(x[0], x[1]), c: lambda x, t: np.array(x[2])} (2) A ndarray with the specified values in the correct order and of the correct shape. (3) A function that must be of the form g(x, t), where x is the current state vector and t is the current time and it must return an ndarray of the correct shape. The specified inputs are, in order: - T(t) p : dictionary len(6) or ndarray shape(6,) Either a dictionary that maps the constants symbols to their numerical values or an array with the constants in the following order: - g - l1 - l2 - l3 - m2 - m3 Returns ======= dx : ndarray, shape(4,) The derivative of the state vector. File: ~/miniconda3/lib/python3.6/site-packages/pydy/codegen/ode_function_generators.py Type: function
Check to see that it works as expected:
import numpy as np
rnd = np.random.random
eval_rhs(rnd(4), 0.3, rnd(1), rnd(6))
array([ 8.30493986e-01, 6.24870903e+02, -1.02042869e+04, -1.31920777e+06])
from scipy.integrate import odeint
Create a parameter vector:
g_val = 9.81
l1_val = 0.8
l2_val = 2.0
l3_val = 1.0
l4_val = 2.0
m2_val = 1.0
m3_val = 1.0
p = np.array([g_val, l1_val, l2_val, l3_val, m2_val, m3_val])
Create an initial condition vector. Since we have configuration constraints we can only specify $q_1$ and we must calculate what $q_2$ and $q_3$ are using the configuration constraints.
q1_0 = np.deg2rad(15.0)
These two lines create a function that evaluates the configuration constraints and that will work with scipy.optimize.fsolve
so that we can numerically solve the constraints.
eval_config_con = sm.lambdify((q1, q2, q3, l1, l2, l3, l4), sm.Matrix([config_con1, config_con2]))
eval_config_con_fsolve = lambda x, q1, l1, l2, l3, l4: np.squeeze(eval_config_con(q1, x[0], x[1], l1, l2, l3, l4))
from scipy.optimize import fsolve
q2_0, q3_0 = fsolve(eval_config_con_fsolve, np.ones(2), args=(q1_0, l1_val, l2_val, l3_val, l4_val))
Integrate the equations with a constant $T$.
times = np.linspace(0.0, 10.0, num=60*10)
x0 = [q1_0, q2_0, q3_0, 0.0]
x0
Create a function that turns on the torque for 1 second then turns it off.
def step_pulse(x, t):
if t < 2.0:
T = 10.0
else:
T = 0.0
return np.array([T])
x = odeint(eval_rhs, x0, times, args=({T: step_pulse}, p))
import matplotlib.pyplot as plt
fig, ax = plt.subplots()
lines = ax.plot(times, np.rad2deg(x))
ax.legend([sm.latex(v, mode='inline') for v in (q1, q2, q3, u1)])
ax.set_xlabel('Time [s]')
ax.set_ylabel('[deg] or [deg/s]')
Text(0, 0.5, '[deg] or [deg/s]')
p2_xy = np.array([l1_val*np.cos(x[:, 0]),
l1_val*np.sin(x[:, 0])])
p3_xy = p2_xy + np.array([l2_val*np.cos(x[:, 0] - x[:, 1]),
l2_val*np.sin(x[:, 0] - x[:, 1])])
fig, ax = plt.subplots()
line, = ax.plot([0.0, p2_xy[0, 0], p3_xy[0, 0], l4_val],
[0.0, p2_xy[1, 0], p3_xy[1, 0], 0.0])
title = 'Time = {:0.1f} seconds'
ax.set_title(title.format(0.0))
ax.set_ylim((-1.0, 3.0))
ax.set_xlim((-1.0, 3.0))
ax.set_aspect('equal')
from matplotlib.animation import FuncAnimation
def update(i):
xdata = [0.0, p2_xy[0, i], p3_xy[0, i], l4_val]
ydata = [0.0, p2_xy[1, i], p3_xy[1, i], 0.0]
line.set_data(xdata, ydata)
ax.set_title(title.format(times[i]))
return line,
ani = FuncAnimation(fig, update, save_count=len(times))
from IPython.display import HTML
HTML(ani.to_jshtml())
ani.save('four-bar-linkage.mp4', fps=60)