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)))
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)))
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)
print('F1_V = {0}'.format(msprint(collect(Fr_V[0], m*g*L))))
assert expand(Fr_V[0] - Fr_expected) == 0
print('Fr not using V, as calculated in part a')
print('F1 = {0}'.format(msprint(collect(terms, m*g*L))))
assert expand(terms - Fr_expected) == 0
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))))
Fr_V_gamma_expected = [
0, 3 * G * m / R**3 * sin(q2) * cos(q2) *
(I1 * cos(q3)**2 + I2 * sin(q3)**2 - I3),
3 * G * m / R**3 * (I1 - I2) * cos(q2)**2 * sin(q3) * cos(q3)
]
for x, y in zip(Fr_V_gamma, Fr_V_gamma_expected):
assert expand(trigsimp(x - y)) == 0
print('\npart c')
q4 = dynamicsymbols('q4')
q4d = dynamicsymbols('q4', level=1)
u4 = dynamicsymbols('u4')
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)
print('F1_V = {0}'.format(msprint(collect(Fr_V[0], m * g * L))))
assert expand(Fr_V[0] - Fr_expected) == 0
print('Fr not using V, as calculated in part a')
print('F1 = {0}'.format(msprint(collect(terms, m * g * L))))
assert expand(terms - Fr_expected) == 0
# 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]))))
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))))
Fr_V_gamma_expected = [0,
3*G*m/R**3 * sin(q2)*cos(q2) * (I1*cos(q3)**2 +
I2*sin(q3)**2 - I3),
3*G*m/R**3 * (I1 - I2) * cos(q2)**2*sin(q3)*cos(q3)]
for x, y in zip(Fr_V_gamma, Fr_V_gamma_expected):
assert expand(trigsimp(x - y)) == 0
print('\npart c')
q4 = dynamicsymbols('q4')
q4d = dynamicsymbols('q4', level=1)
u4 = dynamicsymbols('u4')
# 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]))))
