pP1.set_vel(E, 0)
pP1.set_vel(B, pP1.pos_from(pO).dt(B))
pP1.v1pt_theory(pO, A, B)
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# define generalized speeds and constraints
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
# define system of particles
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# calculate kinetic energy, generalized inertia forces
K = sum(map(lambda x: x.kinetic_energy(A), system))
Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map)
for i, f in enumerate(Fr_tilde_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
Fr_tilde_star_expected = [
((m1 + m2) * (omega**2 * q1 * cos(q3) - u1.diff(t)) - m1 * u2**2 / L +
m2 * L * omega**2 * cos(q3)**2),
(-m1 * (u2.diff(t) + omega**2 * q1 * sin(q3) - u1 * u2 / L))
kde_map = solve(kde, qd)
## --- 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)
Fr, _ = generalized_active_forces(partials, forces)
# Impose the condition that disk C is rolling without slipping
u_indep = u[:3]
u_dep = u[3:]
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:
## --- 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]
Qvals[Q1] = Q1_val
print("Contact force components:")
for k in sorted(Qvals.keys(), cmp=lambda x, y: x.compare(y)):
print("{0} = {1}".format(k, msprint(Qvals[k])))
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.set_vel(B, 0)
pC_hat.set_vel(C, 0)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# kinematic differential equations and velocity constraints
kde = [u1 - dot(A.ang_vel_in(F), A.x),
u2 - dot(pD.vel(F), A.y),
u3 - q3d,
u4 - q4d,
u5 - q5d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
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))
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]
u_dep = u[3:]
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)
F_tilde, _ = generalized_active_forces(partials_tilde, forces)
print("Nonholonomic generalized active forces:")
for i, f in enumerate(F_tilde, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
#print("\nUsing KanesMethod...")
#KM = KanesMethod(A, q, u, kde)
#fr, _ = KM.kanes_equations(forces, [])
#print("Generalized active forces:\n{0}".format(simplify(fr)))
#
#KM_tilde = KanesMethod(A, q, u_indep, kde,
# u_dependent=u_dep,
# velocity_constraints=vc)
pD_star = pP1.locatenew('D*', L*E.x)
pP1.set_vel(B, pP1.pos_from(pO).dt(B))
pD_star.v2pt_theory(pP1, B, E)
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:
(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)]
# partial velocities
system = [x for b in bodies for x in [b.masscenter, b.frame]]
system += [f[0] for f in forces + torques]
partials = partial_velocities(system, u, E, kde_map)
# generalized active forces
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
# dynamical equations
dyn_eq = subs([x + y for x, y in zip(Fr, Fr_star)], kde_map)
ud = [x.diff(t) for x in u]
# rewrite in the form:
# summation(X_sr * u'_r, (r, 1, 3)) = Y_s for s = 1, 2, 3
DE = Matrix(dyn_eq)
X_rs = Matrix(map(lambda x: DE.T.diff(x), ud)).T
Y_s = -expand(DE - X_rs * Matrix(ud))
Z1 = u1 * cos(q1)
Z2 = u1 * sin(q1)
Z3 = -Z2 * u2
Z4 = Z1 * u2
Z5 = -LA * u1
Z6 = -(LP + LB * cos(q1))
Z7 = u2 * LB
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]
u_dep = u[3:]
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)
F_tilde, _ = generalized_active_forces(partials_tilde, forces)
print("Nonholonomic generalized active forces:")
for i, f in enumerate(F_tilde, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
#print("\nUsing KanesMethod...")
#KM = KanesMethod(A, q, u, kde)
#fr, _ = KM.kanes_equations(forces, [])
#print("Generalized active forces:\n{0}".format(simplify(fr)))
#
#KM_tilde = KanesMethod(A, q, u_indep, kde,
# u_dependent=u_dep,
# velocity_constraints=vc)
(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))))
# 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)))
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)]
# partial velocities
system = [x for b in bodies for x in [b.masscenter, b.frame]]
system += [f[0] for f in forces + torques]
partials = partial_velocities(system, u, E, kde_map)
# generalized active forces
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
# dynamical equations
dyn_eq = subs([x + y for x, y in zip(Fr, Fr_star)], kde_map)
ud = [x.diff(t) for x in u]
# rewrite in the form:
# summation(X_sr * u'_r, (r, 1, 3)) = Y_s for s = 1, 2, 3
DE = Matrix(dyn_eq)
X_rs = Matrix(map(lambda x: DE.T.diff(x), ud)).T
Y_s = -expand(DE - X_rs*Matrix(ud))
Z1 = u1 * cos(q1)
Z2 = u1 * sin(q1)
Z3 = -Z2 * u2
Z4 = Z1 * u2
Z5 = -LA * u1
Z6 = -(LP + LB*cos(q1))
Z7 = u2 * LB
pQ.v2pt_theory(pP, N, B)
# Define the distance between points Q, C* as c.
pC_star = pQ.locatenew('C*', c_star * C.z)
pC_star.v2pt_theory(pQ, N, C)
# configuration constraint for q2.
cc = [dot(pC_star.pos_from(pO), N.x)]
cc_map = solve(cc, q2)[1]
# kinematic differential equations
kde = [u1 - q1d, u2 - dot(B.ang_vel_in(N), N.y)]
kde_map = solve(kde, [q1d, q2d])
# velocity constraints
vc = subs([u3 - dot(pC_star.vel(N), N.z), cc[0].diff(t)], kde_map)
vc_map = solve(vc, [u2, u3])
# verify motion constraint equation match text
u2_expected = -a * cos(q1) / (b * cos(q2)) * u1
u3_expected = -a / cos(q2) * (sin(q1) * cos(q2) + cos(q1) * sin(q2)) * u1
assert trigsimp(vc_map[u2] - u2_expected) == 0
assert trigsimp(vc_map[u3] - u3_expected) == 0
# add the term to get u3 from u1 to kde_map
kde_map[dot(pC_star.vel(N), N.z)] = u3
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# central inertias, rigid bodies
IA = inertia(A, A1, mA * kA**2, A3)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
# kinematic differential equations
kde = [
x - y
for x, y in zip([dot(C.ang_vel_in(A), basis) for basis in B] + qd[3:], u)
]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m * R**2 / 4, m * R**2 / 4, m * R**2 / 2)
rbC = RigidBody('rbC', pC_star, C, m, (I_C, pC_star))
# forces
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_C_star = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pC_star, R_C_star)]
# partial velocities
bodies = [rbC]
system = ([i.masscenter
for i in bodies] + [i.frame
for i in bodies] + list(zip(*forces)[0]))
pQ.v2pt_theory(pP, N, B)
# Define the distance between points Q, C* as c.
pC_star = pQ.locatenew('C*', c_star*C.z)
pC_star.v2pt_theory(pQ, N, C)
# configuration constraint for q2.
cc = [dot(pC_star.pos_from(pO), N.x)]
cc_map = solve(cc, q2)[1]
# kinematic differential equations
kde = [u1 - q1d, u2 - dot(B.ang_vel_in(N), N.y)]
kde_map = solve(kde, [q1d, q2d])
# velocity constraints
vc = subs([u3 - dot(pC_star.vel(N), N.z), cc[0].diff(t)], kde_map)
vc_map = solve(vc, [u2, u3])
# verify motion constraint equation match text
u2_expected = -a*cos(q1)/(b*cos(q2))*u1
u3_expected = -a/cos(q2)*(sin(q1)*cos(q2) + cos(q1)*sin(q2))*u1
assert trigsimp(vc_map[u2] - u2_expected) == 0
assert trigsimp(vc_map[u3] - u3_expected) == 0
# add the term to get u3 from u1 to kde_map
kde_map[dot(pC_star.vel(N), N.z)] = u3
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# central inertias, rigid bodies
IA = inertia(A, A1, mA*kA**2, A3)
coord_pairs = [(x, x), (y, r * cos(theta)), (z, r * sin(theta))]
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))))
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
# kinematic differential equations
kde = [x - y for x, y in zip([dot(C.ang_vel_in(A), basis) for basis in B] + qd[3:], u)]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (I_C, pC_star))
# forces
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_C_star = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pC_star, R_C_star)]
# partial velocities
bodies = [rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies] + list(zip(*forces)[0])
partials = partial_velocities(system, [u1, u2, u3], A, kde_map, vc_map)
kde_map = solve(kde, qd)
## --- 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)
Fr, _ = generalized_active_forces(partials, forces)
# Impose the condition that disk C is rolling without slipping
u_indep = u[:3]
u_dep = u[3:]
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:
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.set_vel(B, 0)
pC_hat.set_vel(C, 0)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# kinematic differential equations and velocity constraints
kde = [
u1 - dot(A.ang_vel_in(F), A.x), u2 - dot(pD.vel(F), A.y), u3 - q3d,
u4 - q4d, u5 - q5d
]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
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
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)))
