p.set_vel(N, p.pos_from(pO).dt(N))
# kinematic differential equations
kde = [u1 - L*q1d, u2 - L*q2d, u3 - L*q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
# gravity forces
forces = [(pP1, 6*m*g*N.x),
(pP2, 5*m*g*N.x),
(pP3, 6*m*g*N.x),
(pP4, 5*m*g*N.x),
(pP5, 6*m*g*N.x),
(pP6, 5*m*g*N.x)]
# generalized active force contribution due to gravity
partials = partial_velocities(zip(*forces)[0], [u1, u2, u3], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print('Potential energy contribution of gravitational forces')
V = potential_energy(Fr, [q1, q2, q3], [u1, u2, u3], kde_map)
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, αi = π/2')
V = V.subs(dict(zip(symbols('C α1:4'), [0] + [pi/2]*3)))
print('V = {0}\n'.format(msprint(V)))
print('Generalized active force contributions from Vγ.')
Fr_V = generalized_active_forces_V(V, [q1, q2, q3], [u1, u2, u3], kde_map)
print('Frγ = {0}'.format(msprint(Fr_V)))
print('Fr = {0}'.format(msprint(Fr)))
from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame
from sympy.physics.mechanics import cross, dot, dynamicsymbols, inertia
from util import msprint
print("\n part a")
Ia, Ib, Ic, Iab, Ibc, Ica, t = symbols('Ia Ib Ic Iab Ibc Ica t')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
# I = (I11 * N.x + I12 * N.y + I13 * N.z) N.x +
# (I21 * N.x + I22 * N.y + I23 * N.z) N.y +
# (I31 * N.x + I32 * N.y + I33 * N.z) N.z
# definition of T* is:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
ang_vel = omega * N.x
I = inertia(N, Ia, Ib, Ic, Iab, Ibc, Ica)
T_star = -dot(ang_vel.diff(t, N), I) - dot(cross(ang_vel, I), ang_vel)
print(msprint(T_star))
print("\n part b")
I11, I22, I33, I12, I23, I31 = symbols('I11 I22 I33 I12 I23 I31')
omega1, omega2, omega3 = dynamicsymbols('omega1:4')
B = ReferenceFrame('B')
ang_vel = omega1 * B.x + omega2 * B.y + omega3 * B.z
I = inertia(B, I11, I22, I33, I12, I23, I31)
T_star = -dot(ang_vel.diff(t, B), I) - dot(cross(ang_vel, I), ang_vel)
print(msprint(T_star))
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2],
F,
kde_map,
vc_map,
express_frame=A)
Fr, _ = generalized_active_forces(partials, forces + torques)
q = [q1, q2, q3, q4, q5]
u = [u1, u2]
n = len(q)
p = len(u)
m = n - p
if vc_map is not None:
u += sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
dV_dq = symbols('∂V/∂q1:{0}'.format(n + 1))
dV_eq = Matrix(Fr).T
W_sr, _ = kde_matrix(u, kde_map)
if vc_map is not None:
A_kr, _ = vc_matrix(u, vc_map)
else:
A_kr = Matrix.zeros(m, p)
for s in range(W_sr.shape[0]):
dV_eq += dV_dq[s] * (W_sr[s, :p] + W_sr[s, p:] * A_kr[:, :p])
for elem in dV_eq:
print('{0} = 0'.format(msprint(elem)))
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde_map = solve(kde, [q1d, q2d, q3d])
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
forces = [(pB_star, -G * m * M / R**2 * A.x)]
torques = [(B, cross(3 * G * m / R**3 * A.x, dot(I, A.x)))]
partials = partial_velocities(
zip(*forces + torques)[0], [u1, u2, u3], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
print('part a')
V_gamma = potential_energy(Fr, [q1, q2, q3], [u1, u2, u3], kde_map)
print('V_γ = {0}'.format(msprint(V_gamma)))
print('Setting C = 0, α1, α2, α3 = 0')
V_gamma = V_gamma.subs(dict(zip(symbols('C α1 α2 α3'), [0] * 4)))
print('V_γ= {0}'.format(msprint(V_gamma)))
V_gamma_expected = (-3 * G * m / 2 / R**3 *
((I1 - I3) * sin(q2)**2 +
(I1 - I2) * cos(q2)**2 * sin(q3)**2))
assert expand(V_gamma) == expand(V_gamma_expected)
print('\npart b')
kde_b = [x - y for x, y in zip([u1, u2, u3], [q1d, q2d, q3d])]
kde_map_b = solve(kde_b, [q1d, q2d, q3d])
Fr_V_gamma = generalized_active_forces_V(V_gamma, [q1, q2, q3], [u1, u2, u3],
kde_map_b)
for i, fr in enumerate(Fr_V_gamma, 1):
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x)] # no friction at point Q
torques = [(A, -TB*A.z), (A, -TC*A.z), (B, TB*A.z), (C, TC*A.z)]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2],
F, kde_map, vc_map, express_frame=A)
Fr, _ = generalized_active_forces(partials, forces + torques)
q = [q1, q2, q3, q4, q5]
u = [u1, u2]
n = len(q)
p = len(u)
m = n - p
if vc_map is not None:
u += sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
dV_dq = symbols('∂V/∂q1:{0}'.format(n + 1))
dV_eq = Matrix(Fr).T
W_sr, _ = kde_matrix(u, kde_map)
if vc_map is not None:
A_kr, _ = vc_matrix(u, vc_map)
else:
A_kr = Matrix.zeros(m, p)
for s in range(W_sr.shape[0]):
dV_eq += dV_dq[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
for elem in dV_eq:
print('{0} = 0'.format(msprint(elem)))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
x1 = pC1.pos_from(pk1)
x2 = pC2.pos_from(pk2)
x3 = pC3.pos_from(pk3)
forces = [(pC1, -k * (x1.magnitude() - L_prime) * x1.normalize()),
(pC2, -k * (x2.magnitude() - L_prime) * x2.normalize()),
(pC3, -k * (x3.magnitude() - L_prime) * x3.normalize())]
partials = partial_velocities(zip(*forces)[0], u, X, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print('generalized active forces')
for i, fr in enumerate(Fr, 1):
print('\nF{0} = {1}'.format(i, msprint(fr)))
# use a dummy symbol since series() does not work with dynamicsymbols
_q = Dummy('q')
series_exp = (
lambda x, qi, n_: x.subs(qi, _q).series(_q, n=n_).removeO().subs(_q, qi))
# remove all terms order 3 or higher in qi
Fr_series = [reduce(lambda x, y: series_exp(x, y, 3), q, fr) for fr in Fr]
print('\nseries expansion of generalized active forces')
for i, fr in enumerate(Fr_series, 1):
print('\nF{0} = {1}'.format(i, msprint(fr)))
V = potential_energy(Fr_series, q, u, kde_map)
print('\nV = {0}'.format(msprint(V)))
print('Setting C = 0, α1, α2, α3, α4, α5, α6 = 0')
def print_fr(forces, ulist):
print("Generalized active forces:")
partials = partial_velocities(zip(*forces + torques)[0], ulist, N, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(trigsimp(f))))
pP1.v1pt_theory(pO, A, B)
pD_star.v2pt_theory(pP1, A, E)
## --- Expressions for generalized speeds u1, u2, u3 ---
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y),
u3 - dot(E.ang_vel_in(B), E.z)]
kde_map = solve(kde, [qd1, qd2, qd3])
## --- Velocity constraints ---
vc = [dot(pD_star.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
## --- Define forces on each point in the system ---
K = k*E.x - k/L*dot(pP1.pos_from(pO), E.y)*E.y
gravity = lambda m: -m*g*A.y
forces = [(pP1, K), (pP1, gravity(m1)), (pD_star, gravity(m2))]
## --- Calculate generalized active forces ---
partials = partial_velocities(zip(*forces)[0], [u1, u2], A,
kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials, forces)
Fr_tilde = map(expand, map(trigsimp, Fr_tilde))
print('Finding a potential energy function V.')
V = potential_energy(Fr_tilde, [q1, q2, q3], [u1, u2], kde_map, vc_map)
if V is not None:
print('V = {0}'.format(msprint(V)))
print('Substituting αi = 0, C = 0...')
zero_vars = dict(zip(symbols('C α1:4'), [0] * 4))
print('V = {0}'.format(msprint(V.subs(zero_vars))))
# Differentiate configuration constraints and treat as velocity constraints.
vc = map(lambda x: diff(x, symbols('t')), cc)
vc_map = solve(subs(vc, kde_map), [u2, u3])
forces = [(pP1, m1*g*A.x), (pP2, m2*g*A.x)]
partials = partial_velocities([pP1, pP2], [u1], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
assert (trigsimp(expand(Fr[0])) ==
trigsimp(expand(-g*L1*(m1*sin(q1) +
m2*sin(q3)*sin(q2 - q1)/sin(q2 - q3)))))
V_candidate = -g*(m1*L1*cos(q1) + m2*L3*cos(q3))
dV_dt = diff(V_candidate, symbols('t')).subs(kde_map).subs(vc_map)
Fr_ur = trigsimp(-Fr[0] * u1)
print('Show that {0} is a potential energy of the system.'.format(
msprint(V_candidate)))
print('dV/dt = {0}'.format(msprint(dV_dt)))
print('-F1*u1 = {0}'.format(msprint(Fr_ur)))
print('dV/dt == -sum(Fr*ur, (r, 1, p)) = -F1*u1? {0}'.format(
expand(dV_dt) == expand(Fr_ur)))
print('\nVerify Fr = {0} using V = {1}.'.format(msprint(Fr[0]),
msprint(V_candidate)))
Fr_V = generalized_active_forces_V(V_candidate, [q1, q2, q3], [u1],
kde_map, vc_map)
print('Fr obtained from V = {0}'.format(msprint(Fr_V)))
print('Fr == Fr_V? {0}'.format(
trigsimp(expand(Fr[0])) == trigsimp(expand(Fr_V[0]))))
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde_map = solve(kde, [q1d, q2d, q3d])
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
forces = [(pB_star, -G * m * M / R**2 * A.x)]
torques = [(B, cross(3 * G * m / R**3 * A.x, dot(I, A.x)))]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2, u3],
A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
print('part a')
V_gamma = potential_energy(Fr, [q1, q2, q3], [u1, u2, u3], kde_map)
print('V_γ = {0}'.format(msprint(V_gamma)))
print('Setting C = 0, α1, α2, α3 = 0')
V_gamma = V_gamma.subs(dict(zip(symbols('C α1 α2 α3'), [0] * 4)))
print('V_γ= {0}'.format(msprint(V_gamma)))
V_gamma_expected = (-3*G*m/2/R**3 * ((I1 - I3)*sin(q2)**2 +
(I1 - I2)*cos(q2)**2*sin(q3)**2))
assert expand(V_gamma) == expand(V_gamma_expected)
print('\npart b')
kde_b = [x - y for x, y in zip([u1, u2, u3], [q1d, q2d, q3d])]
kde_map_b = solve(kde_b, [q1d, q2d, q3d])
Fr_V_gamma = generalized_active_forces_V(V_gamma, [q1, q2, q3],
[u1, u2, u3], kde_map_b)
for i, fr in enumerate(Fr_V_gamma, 1):
print('(F{0})γ = {1}'.format(i, msprint(trigsimp(fr))))
F = ReferenceFrame('F')
# --- Define angular velocities of reference frames ---
B.set_ang_vel(A, u1 * A.x)
B.set_ang_vel(F, u2 * A.x)
CD.set_ang_vel(F, u3 * F.y)
CD_prime.set_ang_vel(F, u4 * -F.y)
E.set_ang_vel(F, u5 * -A.x)
# --- define velocity constraints ---
teeth = dict([('A', 60), ('B', 30), ('C', 30), ('D', 61), ('E', 20)])
vc = [
u2 * teeth['B'] - u3 * teeth['C'], # w_B_F * r_B = w_CD_F * r_C
u2 * teeth['B'] - u4 * teeth['C'], # w_B_F * r_B = w_CD'_F * r_C
u5 * teeth['E'] - u3 * teeth['D'], # w_E_F * r_E = w_CD_F * r_D
u5 * teeth['E'] - u4 * teeth['D'], # w_E_F * r_E = w_CD'_F * r_D;
(-u1 + u2) * teeth['A'] - u3 * teeth['D'], # w_A_F * r_A = w_CD_F * r_D;
(-u1 + u2) * teeth['A'] - u4 * teeth['D']
] # w_A_F * r_A = w_CD'_F * r_D;
vc_map = solve(vc, [u2, u3, u4, u5])
## --- Define torques ---
forces = []
torques = [(B, TB * A.x), (E, TE * A.x)]
partials = partial_velocities([B, E], [u1], A, constraint_map=vc_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(trigsimp(f))))
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
C11, C12, C13, C21, C22, C23, C31, C32, C33 = [dot(x, y)
for x in A for y in B]
f = 3/M/q4**2 * ((I1*(1 - 3*C11**2) + I2*(1 - 3*C12**2) +
I3*(1 - 3*C13**2))/2 * A.x +
(I1*C21*C11 + I2*C22*C12 + I3*C23*C13) * A.y +
(I1*C31*C11 + I2*C32*C12 + I3*C33*C13) * A.z)
forces = [(pB_star, -G * m * M / q4**2 * (A.x + f))]
torques = [(B, cross(3 * G * m / q4**3 * A.x, dot(I, A.x)))]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2, u3, u4],
A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V_gamma = potential_energy(Fr, [q1, q2, q3, q4], [u1, u2, u3, u4], kde_map)
print('V_γ = {0}'.format(msprint(V_gamma.subs(q4, R))))
print('Setting C = 0, α1, α2, α3 = 0, α4 = oo')
V_gamma = V_gamma.subs(dict(zip(symbols('C α1 α2 α3 α4'), [0]*4 + [oo] )))
print('V_γ= {0}'.format(msprint(V_gamma.subs(q4, R))))
V_gamma_expected = (-3*G*m/2/R**3 * ((I1 - I3)*sin(q2)**2 +
(I1 - I2)*cos(q2)**2*sin(q3)**2) +
G*m*M/R + G*m/2/R**3*(2*I1 - I2 + I3))
print('V_γ - V_γ_expected = {0}'.format(
msprint(trigsimp(expand(V_gamma.subs(q4, R)) -
expand(V_gamma_expected)))))
assert trigsimp(expand(V_gamma.subs(q4, R) - V_gamma_expected)) == 0
# calculate velocities in A
pCs.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pCs, A, C)
print("velocities of points R, C^, C* in rf A:")
print("v_R_A = {0}\nv_C^_A = {1}\nv_C*_A = {2}".format(pR.vel(A),
pC_hat.vel(A),
pCs.vel(A)))
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
print("using the following kinematic eqs:\n{0}".format(msprint(kde)))
## --- Define forces on each point in the system ---
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_Cs = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## --- Calculate generalized active forces ---
partials = partial_velocities([pC_hat, pCs], u, A, kde_map)
F, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(F, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
# Now impose the condition that disk C is rolling without slipping
u_indep = u[:3]
pP = pO.locatenew('P', q1*A.x + q2*A.y + q3*A.z)
pP.set_vel(A, pP.pos_from(pO).dt(A))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
forces = [(pP, -beta * pP.vel(A))]
torques = [(B, -alpha * B.ang_vel_in(A))]
partials_c = partial_velocities(zip(*forces + torques )[0], u, A, kde_map)
Fr_c, _ = generalized_active_forces(partials_c, forces + torques)
dissipation_function = function_from_partials(
map(lambda x: 0 if x == 0 else -x.subs(kde_map), Fr_c),
u,
zero_constants=True)
from sympy import simplify, trigsimp
dissipation_function = trigsimp(dissipation_function)
#print('ℱ = {0}'.format(msprint(dissipation_function)))
omega2 = trigsimp(dot(B.ang_vel_in(A), B.ang_vel_in(A)).subs(kde_map))
v2 = trigsimp(dot(pP.vel(A), pP.vel(A)).subs(kde_map))
sym_map = dict(zip([omega2, v2], map(lambda x: x**2, symbols('ω v'))))
#print('ω**2 = {0}'.format(msprint(omega2)))
#print('v**2 = {0}'.format(msprint(v2)))
print('ℱ = {0}'.format(msprint(dissipation_function.subs(sym_map))))
dissipation_function_expected = (alpha * omega2 + beta * v2) / 2
assert expand(dissipation_function - dissipation_function_expected) == 0
# calculate partials, generalized forces
partials = partial_velocities(points, [u1, u2, u3], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, point_masses, kde_map)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
partials_tilde = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
Fr_tilde_star, _ = generalized_inertia_forces(partials_tilde, point_masses,
kde_map, vc_map)
print("\nFor generalized speeds\n[u1, u2, u3] = {0}".format(msprint(u_expr)))
print("\nGeneralized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nGeneralized inertia forces:")
for i, f in enumerate(Fr_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized active forces:")
for i, f in enumerate(Fr_tilde, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized inertia forces:")
for i, f in enumerate(Fr_tilde_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
print("\nverify results")
A31, A32 = map(lambda x: diff(vc_map[u3], x), [u1, u2])
coord_map = dict([(x, x), (y, r * cos(theta)), (z, r * sin(theta))])
J = Matrix([coord_map.values()]).jacobian([x, theta, r])
dJ = trigsimp(J.det())
## --- define contact/distance forces ---
# force for a point on ring R1, R2, R3
n = alpha + beta * cos(theta / 2) # contact pressure
t = u_prime * n # kinetic friction
tau = -pQ.vel(C).subs(coord_map).normalize() # direction of friction
v = -P.y # direction of surface
point_force = sum(simplify(dot(n * v + t * tau, b)) * b for b in P)
# want to find gen. active forces for motions where u3 = 0
forces = [(pP_star, E * C.x + M * g * C.y),
(pQ, subs(point_force, u3, 0),
lambda i: integrate(i.subs(coord_map) * dJ,
(theta, -pi, pi)).subs(r, R))]
# 3 rings so repeat the last element twice more
forces += [forces[-1]] * 2
torques = []
## --- define partial velocities ---
partials = partial_velocities([f[0] for f in forces + torques], [u1, u2, u3],
C)
## -- calculate generalized active forces ---
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
# --- Define Points and set their velocities ---
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', L1 * (cos(q1) * A.x + sin(q1) * A.y))
pP1.set_vel(A, pP1.pos_from(pO).diff(t, A))
pP2 = pP1.locatenew('P2', L2 * (cos(q2) * A.x + sin(q2) * A.y))
pP2.set_vel(A, pP2.pos_from(pO).diff(t, A))
## --- configuration constraints ---
cc = [
L1 * cos(q1) + L2 * cos(q2) - L3 * cos(q3),
L1 * sin(q1) + L2 * sin(q2) - L3 * sin(q3) - L4
]
## --- Define kinematic differential equations/pseudo-generalized speeds ---
kde = [u1 - q1d, u2 - q2d, u3 - q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
# --- velocity constraints ---
vc = [c.diff(t) for c in cc]
vc_map = solve(subs(vc, kde_map), [u2, u3])
## --- Define gravitational forces ---
forces = [(pP1, m1 * g * A.x), (pP2, m2 * g * A.x)]
partials = partial_velocities([pP1, pP2], [u1], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0}_tilde = {1}".format(i, msprint(simplify(f))))
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
for p in [pS_star, pQ]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
## --- define partial velocities ---
partials = partial_velocities([pD, pS_star, pQ], [u1, u2, u3],
F, express_frame=A)
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
torques = []
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(f)))
F3 = symbols('F3')
fric_Q = Q2*A.y + Q3*A.z
# Q1 is the component of the contact force normal to plane P.
mag_friction_map = {fric_Q.magnitude() : u_prime * Q1}
# friction force points in opposite direction of velocity of Q
vel_Q_F = pQ.vel(F).subs(u3, 0)
eqs = subs([dot(fric_Q.normalize(), A.y) + dot(vel_Q_F.normalize(), A.y),
dot(fric_Q.normalize(), A.z) + dot(vel_Q_F.normalize(), A.z)],
mag_friction_map)
Qvals = solve(eqs, [Q2, Q3])
# solve for Q1 in terms of F3 and other variables
Q1_val = solve(F3 - Fr[2].subs(Qvals), Q1)[0]
f = (
3
/ M
/ q4 ** 2
* (
(I1 * (1 - 3 * C11 ** 2) + I2 * (1 - 3 * C12 ** 2) + I3 * (1 - 3 * C13 ** 2)) / 2 * A.x
+ (I1 * C21 * C11 + I2 * C22 * C12 + I3 * C23 * C13) * A.y
+ (I1 * C31 * C11 + I2 * C32 * C12 + I3 * C33 * C13) * A.z
)
)
forces = [(pB_star, -G * m * M / q4 ** 2 * (A.x + f))]
torques = [(B, cross(3 * G * m / q4 ** 3 * A.x, dot(I, A.x)))]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2, u3, u4], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V_gamma = potential_energy(Fr, [q1, q2, q3, q4], [u1, u2, u3, u4], kde_map)
print("V_γ = {0}".format(msprint(V_gamma.subs(q4, R))))
print("Setting C = 0, α1, α2, α3 = 0, α4 = oo")
V_gamma = V_gamma.subs(dict(zip(symbols("C α1 α2 α3 α4"), [0] * 4 + [oo])))
print("V_γ= {0}".format(msprint(V_gamma.subs(q4, R))))
V_gamma_expected = (
-3 * G * m / 2 / R ** 3 * ((I1 - I3) * sin(q2) ** 2 + (I1 - I2) * cos(q2) ** 2 * sin(q3) ** 2)
+ G * m * M / R
+ G * m / 2 / R ** 3 * (2 * I1 - I2 + I3)
)
print("V_γ - V_γ_expected = {0}".format(msprint(trigsimp(expand(V_gamma.subs(q4, R)) - expand(V_gamma_expected)))))
assert trigsimp(expand(V_gamma.subs(q4, R) - V_gamma_expected)) == 0
# calculate partials, generalized forces
partials = partial_velocities(points, [u1, u2, u3], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, point_masses, kde_map)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
partials_tilde = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
Fr_tilde_star, _ = generalized_inertia_forces(partials_tilde, point_masses,
kde_map, vc_map)
print("\nFor generalized speeds\n[u1, u2, u3] = {0}".format(msprint(u_expr)))
print("\nGeneralized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nGeneralized inertia forces:")
for i, f in enumerate(Fr_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized active forces:")
for i, f in enumerate(Fr_tilde, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized inertia forces:")
for i, f in enumerate(Fr_tilde_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
print("\nverify results")
A31, A32 = map(lambda x: diff(vc_map[u3], x), [u1, u2])
# Differentiate configuration constraints and treat as velocity constraints.
vc = map(lambda x: diff(x, symbols('t')), cc)
vc_map = solve(subs(vc, kde_map), [u2, u3])
forces = [(pP1, m1 * g * A.x), (pP2, m2 * g * A.x)]
partials = partial_velocities([pP1, pP2], [u1], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
assert (trigsimp(expand(Fr[0])) == trigsimp(
expand(-g * L1 *
(m1 * sin(q1) + m2 * sin(q3) * sin(q2 - q1) / sin(q2 - q3)))))
V_candidate = -g * (m1 * L1 * cos(q1) + m2 * L3 * cos(q3))
dV_dt = diff(V_candidate, symbols('t')).subs(kde_map).subs(vc_map)
Fr_ur = trigsimp(-Fr[0] * u1)
print('Show that {0} is a potential energy of the system.'.format(
msprint(V_candidate)))
print('dV/dt = {0}'.format(msprint(dV_dt)))
print('-F1*u1 = {0}'.format(msprint(Fr_ur)))
print('dV/dt == -sum(Fr*ur, (r, 1, p)) = -F1*u1? {0}'.format(
expand(dV_dt) == expand(Fr_ur)))
print('\nVerify Fr = {0} using V = {1}.'.format(msprint(Fr[0]),
msprint(V_candidate)))
Fr_V = generalized_active_forces_V(V_candidate, [q1, q2, q3], [u1], kde_map,
vc_map)
print('Fr obtained from V = {0}'.format(msprint(Fr_V)))
print('Fr == Fr_V? {0}'.format(
trigsimp(expand(Fr[0])) == trigsimp(expand(Fr_V[0]))))
pCs.set_vel(B, 0)
# calculate velocities in A
pCs.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pCs, A, C)
print("velocities of points R, C^, C* in rf A:")
print("v_R_A = {0}\nv_C^_A = {1}\nv_C*_A = {2}".format(
pR.vel(A), pC_hat.vel(A), pCs.vel(A)))
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
print("using the following kinematic eqs:\n{0}".format(msprint(kde)))
## --- Define forces on each point in the system ---
R_C_hat = Px*A.x + Py*A.y + Pz*A.z
R_Cs = -m*g*A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## --- Calculate generalized active forces ---
partials = partial_velocities([pC_hat, pCs], u, A, kde_map)
F, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(F, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
# Now impose the condition that disk C is rolling without slipping
u_indep = u[:3]
# forces
k = 5 * m * g / L
r = (L_prime + L * sin(q)) * N.y + (L - L * cos(q)) * N.z
forces = [(pP, -m * g * N.z),
(pP, -k * (r.magnitude() - L_prime) * r.normalize())]
partials = partial_velocities(zip(*forces)[0], [u], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
# use a dummy symbol since series() does not work with dynamicsymbols
print('part a')
_q = Dummy('q')
terms = Fr[0].subs(q, _q).series(_q, n=4).removeO().subs(_q, q)
print('Using a series approximation of order 4:')
print('F1 ≈ {0}'.format(msprint(collect(terms, m * g * L))))
V = potential_energy([terms], [q], [u], kde_map)
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, α1 = 0')
V = V.subs(dict(zip(symbols('C α1'), [0, 0])))
print('V = {0}'.format(msprint(collect(V, m * g * L))))
V_expected = m * g * L * (0 * q + 3 * q**2 + 0 * q**3 + -7 * q**4 / 8)
assert expand(V - V_expected) == 0
print('\npart b')
Fr_expected = m * g * L * (-6 * q + 0 * q**2 + 7 * q**3 / 2)
print('Fr using V')
Fr_V = generalized_active_forces_V(V, [q], [u], kde_map)
u_s1 = [dot(pP1.vel(A), A.x), dot(pP1.vel(A), A.y), q3d]
u_s2 = [dot(pP1.vel(A), E.x), dot(pP1.vel(A), E.y), q3d]
u_s3 = [q1d, q2d, q3d]
# f1, f2 are forces the panes of glass exert on P1, P2 respectively
R1 = f1 * B.z + C * E.x - m1 * g * B.y
R2 = f2 * B.z - C * E.x - m2 * g * B.y
forces = [(pP1, R1), (pP2, R2)]
point_masses = [Particle('P1', pP1, m1), Particle('P2', pP2, m2)]
torques = []
ulist = [u1, u2, u3]
for uset in [u_s1, u_s2, u_s3]:
print("\nFor generalized speeds:\n[u1, u2, u3] = {0}".format(
msprint(uset)))
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kde = [u_i - u_expr for u_i, u_expr in zip(ulist, uset)]
kde_map = solve(kde, [q1d, q2d, q3d])
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
partials = partial_velocities([pP1, pP2], ulist, A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, point_masses, kde_map)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("Generalized inertia forces:")
pB1 = pO.locatenew('B1', (L1 + q1)*N.x) # treat block 1 as a point mass
pB2 = pB1.locatenew('B2', (L2 + q2)*N.x) # treat block 2 as a point mass
pB1.set_vel(N, pB1.pos_from(pO).dt(N))
pB2.set_vel(N, pB2.pos_from(pO).dt(N))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
#spring_forces = [(pB1, -k1 * q1 * N.x),
# (pB1, k2 * q2 * N.x),
# (pB2, -k2 * q2 * N.x)]
dashpot_forces = [(pB1, beta * q2d * N.x),
(pB2, -beta * q2d * N.x),
(pB2, -alpha * (q1d + q2d) * N.x)]
#forces = spring_forces + dashpot_forces
partials_c = partial_velocities(zip(*dashpot_forces)[0], u, N, kde_map)
Fr_c, _ = generalized_active_forces(partials_c, dashpot_forces)
#print('generalized active forces due to dashpot forces')
#for i, fr in enumerate(Fr_c, 1):
# print('(F{0})c = {1} = -∂ℱ/∂u{0}'.format(i, msprint(fr)))
dissipation_function = function_from_partials(
map(lambda x: -x.subs(kde_map), Fr_c), u, zero_constants=True)
print('ℱ = {0}'.format(msprint(dissipation_function)))
dissipation_function_expected = (alpha*u1**2 + 2*alpha*u1*u2 +
(alpha + beta)*u2**2)/2
assert expand(dissipation_function - dissipation_function_expected) == 0
pQ = pD.locatenew('Q', f * A.y - R * A.x)
for p in [pS_star, pQ]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
## --- define partial velocities ---
partials = partial_velocities([pD, pS_star, pQ], [u1, u2, u3],
F,
express_frame=A)
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
torques = []
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(f)))
friction = -u_prime * Q1 * (pQ.vel(F).normalize().express(A)).subs(u3, 0)
Q_map = dict(zip([Q2, Q3], [dot(friction, x) for x in [A.y, A.z]]))
Q_map[Q1] = trigsimp(solve(F3 - Fr[-1].subs(Q_map), Q1)[0])
print('')
for x in [Q1, Q2, Q3]:
print('{0} = {1}'.format(x, msprint(Q_map[x])))
print("\nEx8.17")
### --- define new symbols ---
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
q2, q3 = dynamicsymbols('q2 q3')
q2d, q3d = dynamicsymbols('q2 q3', level=1)
# inertia[0] is defined to be the central inertia for each rigid body
rbA = RigidBody('rbA', pA_star, A, mA, (IA, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (IB, pB_star))
rbC = RigidBody('rbC', pC_star, C, mC, (IC, pC_star))
rbD = RigidBody('rbD', pD_star, D, mD, (ID, pD_star))
bodies = [rbA, rbB, rbC, rbD]
## --- generalized speeds ---
kde = [
u1 - dot(A.ang_vel_in(E), A.x), u2 - dot(B.ang_vel_in(A), B.y),
u3 - dot(pC_star.vel(B), B.z)
]
kde_map = solve(kde, [q0d, q1d, q2d])
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
print('\nEx8.20')
# inertia torque for a rigid body:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
T_star = lambda rb, F: (-dot(rb.frame.ang_acc_in(F), rb.inertia[0]) - dot(
cross(rb.frame.ang_vel_in(F), rb.inertia[0]), rb.frame.ang_vel_in(F)))
for rb in bodies:
print('\nT* ({0}) = {1}'.format(rb, msprint(T_star(rb, E).subs(kde_map))))
print('\nEx8.21')
system = [getattr(b, i) for b in bodies for i in ['frame', 'masscenter']]
partials = partial_velocities(system, [u1, u2, u3], E, kde_map)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
for i, f in enumerate(Fr_star, 1):
print("\nF*{0} = {1}".format(i, msprint(simplify(f))))
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
for p in [pS_star, pQ]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
## --- define partial velocities ---
partials = partial_velocities([pD, pS_star, pQ], [u1, u2, u3],
F, express_frame=A)
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
torques = []
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(f)))
friction = -u_prime*Q1*(pQ.vel(F).normalize().express(A)).subs(u3, 0)
Q_map = dict(zip([Q2, Q3], [dot(friction, x) for x in [A.y, A.z]]))
Q_map[Q1] = trigsimp(solve(F3 - Fr[-1].subs(Q_map), Q1)[0])
print('')
for x in [Q1, Q2, Q3]:
print('{0} = {1}'.format(x, msprint(Q_map[x])))
print("\nEx8.17")
### --- define new symbols ---
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
q2, q3 = dynamicsymbols('q2 q3')
q2d, q3d = dynamicsymbols('q2 q3', level=1)
forces = [(pP1, R1), (pDs, R2)]
system = [f[0] for f in forces]
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
partials = partial_velocities(system, [u1, u2, u3], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials_tilde = partial_velocities(system, [u1, u2], A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
print("\nFor generalized speeds {0}".format(msprint(solve(kde, [u1, u2, u3]))))
print("v_r_Pi = {0}".format(msprint(partials)))
print("\nGeneralized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized active forces:")
for i, f in enumerate(Fr_tilde, 1):
print("F{0}_tilde = {1}".format(i, msprint(simplify(f))))
print("\nverify results")
A31, A32 = map(lambda x: diff(vc_map[u3], x), [u1, u2])
print("F1_tilde = {0}".format(msprint(simplify(Fr[0] + A31*Fr[2]))))
print("F2_tilde = {0}".format(msprint(simplify(Fr[1] + A32*Fr[2]))))
u_s1 = [dot(pP1.vel(A), A.x), dot(pP1.vel(A), A.y), q3d]
u_s2 = [dot(pP1.vel(A), E.x), dot(pP1.vel(A), E.y), q3d]
u_s3 = [q1d, q2d, q3d]
# f1, f2 are forces the panes of glass exert on P1, P2 respectively
R1 = f1 * B.z + C * E.x - m1 * g * B.y
R2 = f2 * B.z - C * E.x - m2 * g * B.y
ulist = [u1, u2, u3]
for uset in [u_s1, u_s2, u_s3]:
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kinematic_eqs = [u_i - u_expr for u_i, u_expr in zip(ulist, uset)]
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
vlist = subs([pP1.vel(A), pP2.vel(A)], soln)
v_r_Pi = partial_velocity(vlist, ulist, A)
F1, F2, F3 = [
simplify(
factor(
sum(dot(v_Pi[r], R_i) for v_Pi, R_i in zip(v_r_Pi, [R1, R2]))))
for r in range(3)
]
print("\nFor generalized speeds [u1, u2, u3] = {0}".format(msprint(uset)))
print("v_P1_A = {0}".format(vlist[0]))
print("v_P2_A = {0}".format(vlist[1]))
print("v_r_Pi = {0}".format(v_r_Pi))
print("F1 = {0}".format(msprint(F1)))
print("F2 = {0}".format(msprint(F2)))
print("F3 = {0}".format(msprint(F3)))
# inertia[0] is defined to be the central inertia for each rigid body
rbA = RigidBody('rbA', pA_star, A, mA, (IA, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (IB, pB_star))
rbC = RigidBody('rbC', pC_star, C, mC, (IC, pC_star))
rbD = RigidBody('rbD', pD_star, D, mD, (ID, pD_star))
bodies = [rbA, rbB, rbC, rbD]
## --- generalized speeds ---
kde = [u1 - dot(A.ang_vel_in(E), A.x),
u2 - dot(B.ang_vel_in(A), B.y),
u3 - dot(pC_star.vel(B), B.z)]
kde_map = solve(kde, [q0d, q1d, q2d])
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
print('\nEx8.20')
# inertia torque for a rigid body:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
T_star = lambda rb, F: (-dot(rb.frame.ang_acc_in(F), rb.inertia[0]) -
dot(cross(rb.frame.ang_vel_in(F), rb.inertia[0]),
rb.frame.ang_vel_in(F)))
for rb in bodies:
print('\nT* ({0}) = {1}'.format(rb, msprint(T_star(rb, E).subs(kde_map))))
print('\nEx8.21')
system = [getattr(b, i) for b in bodies for i in ['frame', 'masscenter']]
partials = partial_velocities(system, [u1, u2, u3], E, kde_map)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
for i, f in enumerate(Fr_star, 1):
print("\nF*{0} = {1}".format(i, msprint(simplify(f))))
pP1 = pO.locatenew('P1', -q1 * N.x - b * N.z)
pP2 = pO.locatenew('P2', -q2 * N.x + b * N.z)
for p in [pB_star, pP1, pP2]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
# kinematic differential equations
kde = [u1 - q1d, u2 - q2d]
kde_map = solve(kde, [q1d, q2d])
# contact/distance forces
M = lambda qi, qj: 12 * E * I / (L**2) * (L / 3 * (qj - qi) / (2 * b) - qi / 2)
V = lambda qi, qj: 12 * E * I / (L**3) * (qi - L / 2 * (qj - qi) / (2 * b))
forces = [(pP1, V(q1, q2) * N.x), (pB_star, -m * g * N.x),
(pP2, V(q2, q1) * N.x)]
# M2 torque is applied in the opposite direction
torques = [(B, (M(q1, q2) - M(q2, q1)) * N.y)]
partials = partial_velocities([pP1, pP2, pB_star, B], [u1, u2], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V = simplify(potential_energy(Fr, [q1, q2], [u1, u2], kde_map))
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, αi = 0')
V = V.subs(dict(zip(symbols('C α1:3'), [0] * 3)))
print('V = {0}\n'.format(msprint(V)))
assert (expand(V) == expand(6 * E * I / L**3 *
((1 + L / 2 / b + L**2 / 6 / b**2) *
(q1**2 + q2**2) - q1 * q2 * L / b *
(1 + L / 3 / b)) - m * g / 2 * (q1 + q2)))
def print_fr(forces, ulist):
print("Generalized active forces:")
partials = partial_velocities(zip(*forces + torques)[0], ulist, N, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(trigsimp(f))))
partials_tilde = partial_velocities([pC_hat, pCs], u_indep, A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
Fr_tilde = map(expand, Fr_tilde)
# solve for ∂V/∂qs using 5.1.9
V_gamma = m * g * R * cos(q[1])
print(('\nVerify V_γ = {0} is a potential energy '.format(V_gamma) +
'contribution of γ for C.'))
V_gamma_dot = -sum(
fr * ur
for fr, ur in zip(*generalized_active_forces(partials_tilde, forces[1:])))
if V_gamma_dot == V_gamma.diff(t).subs(kde_map):
print('d/dt(V_γ) == -sum(Fr_γ * ur).')
else:
print('d/dt(V_γ) != -sum(Fr_γ * ur).')
print('d/dt(V_γ) = {0}'.format(msprint(V_gamma.diff(t))))
print('-sum(Fr_γ * ur) = {0}'.format(msprint(V_gamma_dot)))
#print('\nFinding a potential energy function V while C is rolling '
# 'without slip.')
#V = potential_energy(Fr_tilde, q, u_indep, kde_map, vc_map)
#if V is not None:
# print('V = {0}'.format(V))
print('\nFinding a potential energy function V while C is rolling with slip.')
V = potential_energy(Fr, q, u, kde_map)
if V is not None:
print('V = {0}'.format(V))
print('\nFinding a potential energy function V while C is rolling with slip '
'without friction.')
pD_star.v2pt_theory(pP1, A, E)
## --- Expressions for generalized speeds u1, u2, u3 ---
kde = [
u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y),
u3 - dot(E.ang_vel_in(B), E.z)
]
kde_map = solve(kde, [qd1, qd2, qd3])
## --- Velocity constraints ---
vc = [dot(pD_star.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
## --- Define forces on each point in the system ---
K = k * E.x - k / L * dot(pP1.pos_from(pO), E.y) * E.y
gravity = lambda m: -m * g * A.y
forces = [(pP1, K), (pP1, gravity(m1)), (pD_star, gravity(m2))]
## --- Calculate generalized active forces ---
partials = partial_velocities(zip(*forces)[0], [u1, u2], A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials, forces)
Fr_tilde = map(expand, map(trigsimp, Fr_tilde))
print('Finding a potential energy function V.')
V = potential_energy(Fr_tilde, [q1, q2, q3], [u1, u2], kde_map, vc_map)
if V is not None:
print('V = {0}'.format(msprint(V)))
print('Substituting αi = 0, C = 0...')
zero_vars = dict(zip(symbols('C α1:4'), [0] * 4))
print('V = {0}'.format(msprint(V.subs(zero_vars))))
def define_forces(c, exert_by, exert_on, express):
return sum(x * y
for x, y in zip(symbols('{0}_{1}/{2}_1:4'.format(
c, exert_by, exert_on)),
express))
T_EA = define_forces('T', E, A, A)
K_EA = define_forces('K', E, A, A)
T_AB = define_forces('T', A, B, B)
K_AB = define_forces('K', A, B, B)
T_BC = define_forces('T', B, C, B)
K_BC = define_forces('K', B, C, B)
# K_AB will be applied from A onto B and -K_AB will be applied from B onto A
# at point P so these internal forces will cancel. Note point P is fixed in
# both A and B.
forces = [(pO, K_EA), (pC_star, K_BC), (pB_hat, -K_BC),
(pA_star, -mA*g*E.x), (pB_star, -mB*g*E.x),
(pC_star, -mC*g*E.x), (pD_star, -mD*g*E.x)]
torques = [(A, T_EA - T_AB), (B, T_AB - T_BC), (C, T_BC)]
## --- define partial velocities ---
partials = partial_velocities([f[0] for f in forces + torques],
[u1, u2, u3], E, kde_map)
## -- calculate generalized active forces ---
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
E = ReferenceFrame('E')
F = ReferenceFrame('F')
# --- Define angular velocities of reference frames ---
B.set_ang_vel(A, u1 * A.x)
B.set_ang_vel(F, u2 * A.x)
CD.set_ang_vel(F, u3 * F.y)
CD_prime.set_ang_vel(F, u4 * -F.y)
E.set_ang_vel(F, u5 * -A.x)
# --- define velocity constraints ---
teeth = dict([('A', 60), ('B', 30), ('C', 30),
('D', 61), ('E', 20)])
vc = [u2*teeth['B'] - u3*teeth['C'], # w_B_F * r_B = w_CD_F * r_C
u2*teeth['B'] - u4*teeth['C'], # w_B_F * r_B = w_CD'_F * r_C
u5*teeth['E'] - u3*teeth['D'], # w_E_F * r_E = w_CD_F * r_D
u5*teeth['E'] - u4*teeth['D'], # w_E_F * r_E = w_CD'_F * r_D;
(-u1 + u2)*teeth['A'] - u3*teeth['D'], # w_A_F * r_A = w_CD_F * r_D;
(-u1 + u2)*teeth['A'] - u4*teeth['D']] # w_A_F * r_A = w_CD'_F * r_D;
vc_map = solve(vc, [u2, u3, u4, u5])
## --- Define torques ---
forces = []
torques = [(B, TB*A.x), (E, TE*A.x)]
partials = partial_velocities([B, E], [u1], A, constraint_map = vc_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(trigsimp(f))))
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
C11, C12, C13, C21, C22, C23, C31, C32, C33 = [dot(x, y) for x in A for y in B]
f = 3 / M / q4**2 * ((I1 * (1 - 3 * C11**2) + I2 * (1 - 3 * C12**2) + I3 *
(1 - 3 * C13**2)) / 2 * A.x +
(I1 * C21 * C11 + I2 * C22 * C12 + I3 * C23 * C13) * A.y +
(I1 * C31 * C11 + I2 * C32 * C12 + I3 * C33 * C13) * A.z)
forces = [(pB_star, -G * m * M / q4**2 * (A.x + f))]
torques = [(B, cross(3 * G * m / q4**3 * A.x, dot(I, A.x)))]
partials = partial_velocities(
zip(*forces + torques)[0], [u1, u2, u3, u4], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V_gamma = potential_energy(Fr, [q1, q2, q3, q4], [u1, u2, u3, u4], kde_map)
print('V_γ = {0}'.format(msprint(V_gamma.subs(q4, R))))
print('Setting C = 0, α1, α2, α3 = 0, α4 = oo')
V_gamma = V_gamma.subs(dict(zip(symbols('C α1 α2 α3 α4'), [0] * 4 + [oo])))
print('V_γ= {0}'.format(msprint(V_gamma.subs(q4, R))))
V_gamma_expected = (-3 * G * m / 2 / R**3 *
((I1 - I3) * sin(q2)**2 +
(I1 - I2) * cos(q2)**2 * sin(q3)**2) + G * m * M / R +
G * m / 2 / R**3 * (2 * I1 - I2 + I3))
print('V_γ - V_γ_expected = {0}'.format(
msprint(trigsimp(expand(V_gamma.subs(q4, R)) - expand(V_gamma_expected)))))
assert trigsimp(expand(V_gamma.subs(q4, R) - V_gamma_expected)) == 0
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), u_dep)
partials_tilde = partial_velocities([pC_hat, pCs], u_indep, A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
Fr_tilde = map(expand, Fr_tilde)
# solve for ∂V/∂qs using 5.1.9
V_gamma = m * g * R * cos(q[1])
print(("\nVerify V_γ = {0} is a potential energy ".format(V_gamma) + "contribution of γ for C."))
V_gamma_dot = -sum(fr * ur for fr, ur in zip(*generalized_active_forces(partials_tilde, forces[1:])))
if V_gamma_dot == V_gamma.diff(t).subs(kde_map):
print("d/dt(V_γ) == -sum(Fr_γ * ur).")
else:
print("d/dt(V_γ) != -sum(Fr_γ * ur).")
print("d/dt(V_γ) = {0}".format(msprint(V_gamma.diff(t))))
print("-sum(Fr_γ * ur) = {0}".format(msprint(V_gamma_dot)))
# print('\nFinding a potential energy function V while C is rolling '
# 'without slip.')
# V = potential_energy(Fr_tilde, q, u_indep, kde_map, vc_map)
# if V is not None:
# print('V = {0}'.format(V))
print("\nFinding a potential energy function V while C is rolling with slip.")
V = potential_energy(Fr, q, u, kde_map)
if V is not None:
print("V = {0}".format(V))
print("\nFinding a potential energy function V while C is rolling with slip " "without friction.")
V = potential_energy(subs(Fr, {Px: 0, Py: 0}), q, u, kde_map)
kde_map = {qd: u}
# forces
k = 5*m*g/L
r = (L_prime + L*sin(q))*N.y + (L - L*cos(q))*N.z
forces = [(pP, -m*g*N.z), (pP, -k*(r.magnitude() - L_prime)*r.normalize())]
partials = partial_velocities(zip(*forces)[0], [u], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
# use a dummy symbol since series() does not work with dynamicsymbols
print('part a')
_q = Dummy('q')
terms = Fr[0].subs(q, _q).series(_q, n=4).removeO().subs(_q, q)
print('Using a series approximation of order 4:')
print('F1 ≈ {0}'.format(msprint(collect(terms, m*g*L))))
V = potential_energy([terms], [q], [u], kde_map)
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, α1 = 0')
V = V.subs(dict(zip(symbols('C α1'), [0, 0])))
print('V = {0}'.format(msprint(collect(V, m*g*L))))
V_expected = m*g*L*(0*q + 3*q**2 + 0*q**3 + -7*q**4/8)
assert expand(V - V_expected) == 0
print('\npart b')
Fr_expected = m*g*L*(-6*q + 0*q**2 + 7*q**3/2)
print('Fr using V')
# --- Define Points and set their velocities ---
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', L1*(cos(q1)*A.x + sin(q1)*A.y))
pP1.set_vel(A, pP1.pos_from(pO).diff(t, A))
pP2 = pP1.locatenew('P2', L2*(cos(q2)*A.x + sin(q2)*A.y))
pP2.set_vel(A, pP2.pos_from(pO).diff(t, A))
## --- configuration constraints ---
cc = [L1*cos(q1) + L2*cos(q2) - L3*cos(q3),
L1*sin(q1) + L2*sin(q2) - L3*sin(q3) - L4]
## --- Define kinematic differential equations/pseudo-generalized speeds ---
kde = [u1 - q1d, u2 - q2d, u3 - q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
# --- velocity constraints ---
vc = [c.diff(t) for c in cc]
vc_map = solve(subs(vc, kde_map), [u2, u3])
## --- Define gravitational forces ---
forces = [(pP1, m1*g*A.x), (pP2, m2*g*A.x)]
partials = partial_velocities([pP1, pP2], [u1], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0}_tilde = {1}".format(i, msprint(simplify(f))))
partials_tilde = partial_velocities([pC_hat, pCs], u_indep, A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
Fr_tilde = map(expand, Fr_tilde)
# solve for ∂V/∂qs using 5.1.9
V_gamma = m * g * R * cos(q[1])
print(('\nVerify V_γ = {0} is a potential energy '.format(V_gamma) +
'contribution of γ for C.'))
V_gamma_dot = -sum(fr * ur for fr, ur in
zip(*generalized_active_forces(partials_tilde,
forces[1:])))
if V_gamma_dot == V_gamma.diff(t).subs(kde_map):
print('d/dt(V_γ) == -sum(Fr_γ * ur).')
else:
print('d/dt(V_γ) != -sum(Fr_γ * ur).')
print('d/dt(V_γ) = {0}'.format(msprint(V_gamma.diff(t))))
print('-sum(Fr_γ * ur) = {0}'.format(msprint(V_gamma_dot)))
#print('\nFinding a potential energy function V while C is rolling '
# 'without slip.')
#V = potential_energy(Fr_tilde, q, u_indep, kde_map, vc_map)
#if V is not None:
# print('V = {0}'.format(V))
print('\nFinding a potential energy function V while C is rolling with slip.')
V = potential_energy(Fr, q, u, kde_map)
if V is not None:
print('V = {0}'.format(V))
print('\nFinding a potential energy function V while C is rolling with slip '
'without friction.')
forces = [(pP1, R1), (pDs, R2)]
system = [f[0] for f in forces]
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
partials = partial_velocities(system, [u1, u2, u3], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials_tilde = partial_velocities(system, [u1, u2], A, kde_map, vc_map)
Fr_tilde, _ = generalized_active_forces(partials_tilde, forces)
print("\nFor generalized speeds {0}".format(msprint(solve(kde, [u1, u2, u3]))))
print("v_r_Pi = {0}".format(msprint(partials)))
print("\nGeneralized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("\nNonholonomic generalized active forces:")
for i, f in enumerate(Fr_tilde, 1):
print("F{0}_tilde = {1}".format(i, msprint(simplify(f))))
print("\nverify results")
A31, A32 = map(lambda x: diff(vc_map[u3], x), [u1, u2])
print("F1_tilde = {0}".format(msprint(simplify(Fr[0] + A31 * Fr[2]))))
print("F2_tilde = {0}".format(msprint(simplify(Fr[1] + A32 * Fr[2]))))
pP = pO.locatenew('P', q1 * A.x + q2 * A.y + q3 * A.z)
pP.set_vel(A, pP.pos_from(pO).dt(A))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
forces = [(pP, -beta * pP.vel(A))]
torques = [(B, -alpha * B.ang_vel_in(A))]
partials_c = partial_velocities(zip(*forces + torques)[0], u, A, kde_map)
Fr_c, _ = generalized_active_forces(partials_c, forces + torques)
dissipation_function = function_from_partials(map(
lambda x: 0 if x == 0 else -x.subs(kde_map), Fr_c),
u,
zero_constants=True)
from sympy import simplify, trigsimp
dissipation_function = trigsimp(dissipation_function)
#print('ℱ = {0}'.format(msprint(dissipation_function)))
omega2 = trigsimp(dot(B.ang_vel_in(A), B.ang_vel_in(A)).subs(kde_map))
v2 = trigsimp(dot(pP.vel(A), pP.vel(A)).subs(kde_map))
sym_map = dict(zip([omega2, v2], map(lambda x: x**2, symbols('ω v'))))
#print('ω**2 = {0}'.format(msprint(omega2)))
#print('v**2 = {0}'.format(msprint(v2)))
print('ℱ = {0}'.format(msprint(dissipation_function.subs(sym_map))))
dissipation_function_expected = (alpha * omega2 + beta * v2) / 2
assert expand(dissipation_function - dissipation_function_expected) == 0
# three sets of generalized speeds
u_s1 = [dot(pP1.vel(A), A.x), dot(pP1.vel(A), A.y), q3d]
u_s2 = [dot(pP1.vel(A), E.x), dot(pP1.vel(A), E.y), q3d]
u_s3 = [q1d, q2d, q3d]
# f1, f2 are forces the panes of glass exert on P1, P2 respectively
R1 = f1*B.z + C*E.x - m1*g*B.y
R2 = f2*B.z - C*E.x - m2*g*B.y
forces = [(pP1, R1), (pP2, R2)]
point_masses = [Particle('P1', pP1, m1), Particle('P2', pP2, m2)]
torques = []
ulist = [u1, u2, u3]
for uset in [u_s1, u_s2, u_s3]:
print("\nFor generalized speeds:\n[u1, u2, u3] = {0}".format(msprint(uset)))
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kde = [u_i - u_expr for u_i, u_expr in zip(ulist, uset)]
kde_map = solve(kde, [q1d, q2d, q3d])
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
partials = partial_velocities([pP1, pP2], ulist, A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, point_masses, kde_map)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
print("Generalized inertia forces:")
for p in [pP1, pP2, pP3, pP4, pP5, pP6]:
p.set_vel(N, p.pos_from(pO).dt(N))
# kinematic differential equations
kde = [u1 - L*q1d, u2 - L*q2d, u3 - L*q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
# gravity forces
forces = [(pP1, 6*m*g*N.x),
(pP2, 5*m*g*N.x),
(pP3, 6*m*g*N.x),
(pP4, 5*m*g*N.x),
(pP5, 6*m*g*N.x),
(pP6, 5*m*g*N.x)]
# generalized active force contribution due to gravity
partials = partial_velocities(zip(*forces)[0], [u1, u2, u3], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print('Potential energy contribution of gravitational forces')
V = potential_energy(Fr, [q1, q2, q3], [u1, u2, u3], kde_map)
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, αi = π/2')
V = V.subs(dict(zip(symbols('C α1:4'), [0] + [pi/2]*3)))
print('V = {0}\n'.format(msprint(V)))
print('Generalized active force contributions from Vγ.')
Fr_V = generalized_active_forces_V(V, [q1, q2, q3], [u1, u2, u3], kde_map)
print('Frγ = {0}'.format(msprint(Fr_V)))
print('Fr = {0}'.format(msprint(Fr)))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
x1 = pC1.pos_from(pk1)
x2 = pC2.pos_from(pk2)
x3 = pC3.pos_from(pk3)
forces = [(pC1, -k * (x1.magnitude() - L_prime) * x1.normalize()),
(pC2, -k * (x2.magnitude() - L_prime) * x2.normalize()),
(pC3, -k * (x3.magnitude() - L_prime) * x3.normalize())]
partials = partial_velocities(zip(*forces)[0], u, X, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print('generalized active forces')
for i, fr in enumerate(Fr, 1):
print('\nF{0} = {1}'.format(i, msprint(fr)))
# use a dummy symbol since series() does not work with dynamicsymbols
_q = Dummy('q')
series_exp = (
lambda x, qi, n_: x.subs(qi, _q).series(_q, n=n_).removeO().subs(_q, qi))
# remove all terms order 3 or higher in qi
Fr_series = [reduce(lambda x, y: series_exp(x, y, 3), q, fr) for fr in Fr]
print('\nseries expansion of generalized active forces')
for i, fr in enumerate(Fr_series, 1):
print('\nF{0} = {1}'.format(i, msprint(fr)))
V = potential_energy(Fr_series, q, u, kde_map)
print('\nV = {0}'.format(msprint(V)))
print('Setting C = 0, α1, α2, α3, α4, α5, α6 = 0')
pP2 = pO.locatenew('P2', -q2*N.x + b*N.z)
for p in [pB_star, pP1, pP2]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
# kinematic differential equations
kde = [u1 - q1d, u2 - q2d]
kde_map = solve(kde, [q1d, q2d])
# contact/distance forces
M = lambda qi, qj: 12*E*I/(L**2) * (L/3 * (qj - qi)/(2*b) - qi/2)
V = lambda qi, qj: 12*E*I/(L**3) * (qi - L/2 * (qj - qi)/(2*b))
forces = [(pP1, V(q1, q2)*N.x),
(pB_star, -m*g*N.x),
(pP2, V(q2, q1)*N.x)]
# M2 torque is applied in the opposite direction
torques = [(B, (M(q1, q2) - M(q2, q1))*N.y)]
partials = partial_velocities([pP1, pP2, pB_star, B], [u1, u2], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V = simplify(potential_energy(Fr, [q1, q2], [u1, u2], kde_map))
print('V = {0}'.format(msprint(V)))
print('Setting C = 0, αi = 0')
V = V.subs(dict(zip(symbols('C α1:3'), [0] * 3)))
print('V = {0}\n'.format(msprint(V)))
assert (expand(V) ==
expand(6*E*I/L**3 * ((1 + L/2/b + L**2/6/b**2)*(q1**2 + q2**2) -
q1*q2*L/b * (1 + L/3/b)) -
m*g/2 * (q1 + q2)))
pP1.vel(A), pP2.vel(A)))
# three sets of generalized speeds
u_s1 = [dot(pP1.vel(A), A.x), dot(pP1.vel(A), A.y), q3d]
u_s2 = [dot(pP1.vel(A), E.x), dot(pP1.vel(A), E.y), q3d]
u_s3 = [q1d, q2d, q3d]
# f1, f2 are forces the panes of glass exert on P1, P2 respectively
R1 = f1*B.z + C*E.x - m1*g*B.y
R2 = f2*B.z - C*E.x - m2*g*B.y
ulist = [u1, u2, u3]
for uset in [u_s1, u_s2, u_s3]:
# solve for u1, u2, u3 in terms of q1d, q2d, q3d and substitute
kinematic_eqs = [u_i - u_expr for u_i, u_expr in zip(ulist, uset)]
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
vlist = subs([pP1.vel(A), pP2.vel(A)], soln)
v_r_Pi = partial_velocity(vlist, ulist, A)
F1, F2, F3 = [simplify(factor(
sum(dot(v_Pi[r], R_i) for v_Pi, R_i in zip(v_r_Pi, [R1, R2]))))
for r in range(3)]
print("\nFor generalized speeds [u1, u2, u3] = {0}".format(msprint(uset)))
print("v_P1_A = {0}".format(vlist[0]))
print("v_P2_A = {0}".format(vlist[1]))
print("v_r_Pi = {0}".format(v_r_Pi))
print("F1 = {0}".format(msprint(F1)))
print("F2 = {0}".format(msprint(F2)))
print("F3 = {0}".format(msprint(F3)))
