## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# point on B
pP_prime = pP.locatenew("P'", 0)
pP_prime.set_vel(B, 0)
pP_prime.v1pt_theory(pC_star, A, B)
## --- Define forces ---
cart_sph_map = dict([(z, r * sin(phi)), (y, r * cos(phi) * sin(theta)),
(x, r * cos(phi) * cos(theta))])
J = Matrix([cart_sph_map.values()]).jacobian([r, phi, theta])
dJ = simplify(J.det())
dtheta = -c * pP.vel(B) * dJ
integral = lambda i: integrate(
integrate(i.subs(cart_sph_map), (theta, 0, 2 * pi)),
(phi, -pi / 2, pi / 2)).subs(r, R)
forces = [(pP, dtheta, integral), (pP_prime, -dtheta, integral)]
partials = partial_velocities([pP, pP_prime], [u2, u4], A, express_frame=B)
Flist, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for f, i in zip(Flist, [2, 4]):
print("F{0} = {1}".format(i, msprint(simplify(f))))
points = [pP1, pDs]
forces = [(pP1, R1), (pDs, R2)]
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
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)
# 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 = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
u1d, u2d = ud = [x.diff(t) for x in [u1, u2]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print('{0} = {1}'.format(msprint(x), msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (-g * sin(q3) + omega**2 * q1 * cos(q3) +
(m2 * L * omega**2 * cos(q3)**2 - m1 * u2**2 / L) / (m1 + m2))
u2d_expected = -g * cos(q3) - omega**2 * q1 * sin(q3) + u1 * u2 / L
assert expand(trigsimp(dyn_eq_map[u1d] - u1d_expected)) == 0
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])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB*R**2/4
J = mB*R**2/2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print('inertia_coefficient_matrix = {0}'.format(msprint(M)))
M_expected = Matrix([[IA + mA*a**2 + mB*(R**2/2 + 3*b**2), 0],
[0, mA + 3*mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
pC1.v2pt_theory(pC_star, X, C)
pC2.v2pt_theory(pC_star, X, C)
pC3.v2pt_theory(pC_star, X, C)
# 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)))
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]
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):
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))))
V = A * q1 / cos(q2)
beta = V * rho * g * N.z
# The buoyancy force acts through the center of buoyancy.
pO = Point('pO') # define point O to be at the surface of the fluid
p1 = pO.locatenew('p1', -q1 * N.z)
p2 = p1.locatenew('p2', q1 / cos(q2) / 2 * R.z)
p1.set_vel(N, p1.pos_from(pO).dt(N))
p2.v2pt_theory(p1, N, R)
forces = [(p2, beta)]
## --- find V ---
# since qid = ui, Fr = -dV/dqr
q = [q1, q2]
partials = partial_velocities([p2], map(diff, q), N)
Fr, _ = generalized_active_forces(partials, forces)
# check if dFr/dqs = dFs/dqr for all r, s = 1, ..., n
for r in range(len(q)):
for s in range(r + 1, len(q)):
if Fr[r].diff(q[s]) != Fr[s].diff(q[r]):
print('∂F{0}/∂q{1} != ∂F{1}/∂q{0}. V does not exist.'.format(r, s))
print('∂F{0}/∂q{1} = {2}'.format(r, s, Fr[r].diff(q[s])))
print('∂F{1}/∂q{0} = {2}'.format(r, s, Fr[s].diff(q[r])))
sys.exit(0)
# form V using 5.1.6
alpha = symbols('α1:{0}'.format(len(q) + 1))
q_alpha = zip(q, alpha)
V = C
# 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)))))
# kinematic differential equations
kde = [u1 - q1d, u2 - q2d, u3 - q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
# configuration constraints
cc = [
L1 * cos(q1) + L2 * cos(q2) - L3 * cos(q3),
L1 * sin(q1) + L2 * sin(q2) - L3 * sin(q3) - L4
]
# 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)))
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))))
R2 = Y*E.y + Z*E.z - C*E.x - m2*g*B.y
resultants = [R1, R2]
forces = [(pP1, R1), (pDs, R2)]
point_masses = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
points = [f[0] for f in forces]
# define generalized speeds
kde = [u_i - u_ex for u_i, u_ex in zip(ulist, u_expr)]
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)
# 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):
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
system += [x[0] for x in forces + torques]
# partial velocities
partials = partial_velocities(system, [u1, u2, u3],
F,
kde_map,
express_frame=A)
# Fr, Fr*
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, uaux=[u3])
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])
#F3 + F3* = 0
Q_map[Q1] = Q_map[Q1].subs(F3, -Fr_star[2])
print('Q1 = {0}'.format(msprint(Q_map[Q1])))
Q1_expected = e * M * g * cos(theta) / (
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))))
# Simplify the system to 7 points, where each point is the aggregations of
# rods that are parallel horizontally.
pO = Point('O')
pO.set_vel(N, 0)
pP1 = pO.locatenew('P1', L / 2 * (cos(q1) * N.x + sin(q1) * N.y))
pP2 = pP1.locatenew('P2', L / 2 * (cos(q1) * N.x + sin(q1) * N.y))
pP3 = pP2.locatenew('P3', L / 2 * (cos(q2) * N.x - sin(q2) * N.y))
pP4 = pP3.locatenew('P4', L / 2 * (cos(q2) * N.x - sin(q2) * N.y))
pP5 = pP4.locatenew('P5', L / 2 * (cos(q3) * N.x + sin(q3) * N.y))
pP6 = pP5.locatenew('P6', L / 2 * (cos(q3) * N.x + sin(q3) * N.y))
for p in [pP1, pP2, pP3, pP4, pP5, pP6]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
## --- Define kinematic differential equations/pseudo-generalized speeds ---
kde = [u1 - L * q1d, u2 - L * q2d, u3 - L * q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
## --- Define contact/distance forces ---
forces = [(pP1, 6 * m * g * N.x), (pP2, S * N.y + 5 * m * g * N.x),
(pP3, 6 * m * g * N.x), (pP4, -Q * N.y + 5 * m * g * N.x),
(pP5, 6 * m * g * N.x), (pP6, R * N.y + 5 * m * g * N.x)]
partials = partial_velocities([pP1, pP2, pP3, pP4, pP5, pP6], [u1, u2, u3], N,
kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{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)))
pO.set_vel(N, 0)
pP1 = pO.locatenew('P1', L/2*(cos(q1)*N.x + sin(q1)*N.y))
pP2 = pP1.locatenew('P2', L/2*(cos(q1)*N.x + sin(q1)*N.y))
pP3 = pP2.locatenew('P3', L/2*(cos(q2)*N.x - sin(q2)*N.y))
pP4 = pP3.locatenew('P4', L/2*(cos(q2)*N.x - sin(q2)*N.y))
pP5 = pP4.locatenew('P5', L/2*(cos(q3)*N.x + sin(q3)*N.y))
pP6 = pP5.locatenew('P6', L/2*(cos(q3)*N.x + sin(q3)*N.y))
for p in [pP1, pP2, pP3, pP4, pP5, pP6]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
## --- Define kinematic differential equations/pseudo-generalized speeds ---
kde = [u1 - L*q1d, u2 - L*q2d, u3 - L*q3d]
kde_map = solve(kde, [q1d, q2d, q3d])
## --- Define contact/distance forces ---
forces = [(pP1, 6*m*g*N.x),
(pP2, S*N.y + 5*m*g*N.x),
(pP3, 6*m*g*N.x),
(pP4, -Q*N.y + 5*m*g*N.x),
(pP5, 6*m*g*N.x),
(pP6, R*N.y + 5*m*g*N.x)]
partials = partial_velocities([pP1, pP2, pP3, pP4, pP5, pP6],
[u1, u2, u3], N, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(simplify(f))))
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])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB * R ** 2 / 4
J = mB * R ** 2 / 2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody("rbA", pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody("rbB", pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody("rbC", pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print("inertia_coefficient_matrix = {0}".format(msprint(M)))
M_expected = Matrix([[IA + mA * a ** 2 + mB * (R ** 2 / 2 + 3 * b ** 2), 0], [0, mA + 3 * mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
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
A.set_ang_vel(F, u1*A.x + u3*A.z)
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
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)
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)]
# 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))
R2 = Y * E.y + Z * E.z - C * E.x - m2 * g * B.y
resultants = [R1, R2]
forces = [(pP1, R1), (pDs, R2)]
point_masses = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
points = [f[0] for f in forces]
# define generalized speeds
kde = [u_i - u_ex for u_i, u_ex in zip(ulist, u_expr)]
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)
# 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):
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)
# kinetic energy of robot arm E
K = sum(rb.kinetic_energy(E) for rb in bodies).subs(kde_map)
print('K = {0}'.format(msprint(K)))
# find potential energy contribution of the set of gravitational forces
forces = [(pA_star, -mA * g * E.x), (pB_star, -mB * g * E.x),
(pC_star, -mC * g * E.x), (pD_star, -mD * g * E.x)]
## --- define partial velocities ---
partials = partial_velocities([f[0] for f in forces], [u1, u2, u3], E, kde_map)
## -- calculate generalized active forces ---
Fr, _ = generalized_active_forces(partials, forces)
V = potential_energy(Fr, [q0, q1, q2], [u1, u2, u3], kde_map)
#print('V = {0}'.format(msprint(V)))
print('\nSetting C = g*mD*p1, α1, α2, α3 = 0')
V = V.subs(dict(zip(symbols('C α1 α2 α3'), [g * mD * p1, 0, 0, 0])))
print('V = {0}'.format(msprint(V)))
Z1 = u1 * cos(q1)
Z2 = u1 * sin(q1)
Z3 = -Z2 * u2
Z4 = Z1 * u2
Z5 = -LA * u1
Z6 = -(LP + LB * cos(q1))
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# define points
pO = Point('O')
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)))
for p in points:
p.set_vel(N, p.pos_from(pO).diff(t, N))
# kinematic differential equations
kde = [u1 - L * q1d, u2 - L * q2d, u3 - L * 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)
# contact/distance forces
forces = [(pP1, 6 * m * g * N.x), (pP2, S * N.y + 5 * m * g * N.x),
(pP3, 6 * m * g * N.x), (pP4, -Q * N.y + 5 * m * g * N.x),
(pP5, 6 * m * g * N.x), (pP6, R * N.y + 5 * m * g * N.x)]
partials = partial_velocities(points, u, N, kde_map)
system = [
Particle('P{0}'.format(i), p, x * m * g)
for i, p, x in zip(range(1, 7), points, [6, 5] * 3)
]
# part a
Fr_star_a, _ = generalized_inertia_forces(partials, system, kde_map)
# part b
K = sum(map(lambda x: x.kinetic_energy(N), system))
Fr_star_b = generalized_inertia_forces_K(K, q, u, kde_map)
# part c
G = sum(P.mass * dot(P.point.acc(N), P.point.acc(N))
for P in system).subs(kde_map) / 2
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 = [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)
# 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)]
system = [Particle('P1', pP1, m1), Particle('P2', pP2, m2)]
partials = partial_velocities([pP1, pP2], u, A, kde_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
u1d, u2d, u3d = ud = [x.diff(t) for x in u]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print('{0} = {1}'.format(msprint(x),
msprint(cancel(trigsimp(dyn_eq_map[x])))))
u1d_expected = (-g*sin(q3) + omega**2*q1*cos(q3) + u2*u3 +
(omega**2*cos(q3)**2 + u3**2)*L*m2/(m1 + m2))
u2d_expected = -g*cos(q3) - (omega**2*q1*sin(q3) + u3*u1)
r_ = q1*N.x + q2*N.y + q3*N.z
v = v1*N.x + v2*N.y + v3*N.z
p1 = Point('p1')
p1.set_vel(N, v)
p2 = p1.locatenew('p2', r_)
p2.set_vel(N, v + r_.dt(N))
## --- define gravitational forces ---
F1 = (G * m1 * m2 / dot(r_, r_))*(p2.pos_from(p1).normalize())
F2 = -F1
forces = [(p1, F1), (p2, F2)]
## --- find V ---
# since qid = ui, Fr = -dV/dqr
q = [q1, q2, q3]
partials = partial_velocities([p1, p2], map(diff, q), N)
Fr, _ = generalized_active_forces(partials, forces)
# check if dFr/dqs = dFs/dqr for all r, s = 1, ..., n
for r in range(len(q)):
for s in range(len(q)):
if Fr[r].diff(q[s]) != Fr[s].diff(q[r]):
print('∂Fr/∂qs != ∂Fs/∂qr. V does not exist.')
# form V using 5.1.6
alpha = symbols('α1:{0}'.format(len(q) + 1))
q_alpha = zip(q, alpha)
V = C
for i, dV_dqr in enumerate(map(lambda x: -x, Fr)):
V += integrate(dV_dqr.subs(dict(q_alpha[i + 1:])).subs(q[i], zeta),
(zeta, alpha[i], q[i]))
# 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))))
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# point on B
pP_prime = pP.locatenew("P'", 0)
pP_prime.set_vel(B, 0)
pP_prime.v1pt_theory(pC_star, A, B)
## --- Define forces ---
cart_sph_map = dict([(z, r*sin(phi)),
(y, r*cos(phi)*sin(theta)),
(x, r*cos(phi)*cos(theta))])
J = Matrix([cart_sph_map.values()]).jacobian([r, phi, theta])
dJ = simplify(J.det())
dtheta = -c * pP.vel(B) * dJ
integral = lambda i: integrate(integrate(i.subs(cart_sph_map),
(theta, 0, 2*pi)),
(phi, -pi/2, pi/2)).subs(r, R)
forces = [(pP, dtheta, integral), (pP_prime, -dtheta, integral)]
partials = partial_velocities([pP, pP_prime], [u2, u4], A, express_frame=B)
Flist, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for f, i in zip(Flist, [2, 4]):
print("F{0} = {1}".format(i, msprint(simplify(f))))
def find_coupled_speeds(kde_map):
partials = partial_velocities([rb.frame, rb.masscenter], [u1, u2, u3],
A, kde_map)
M = trigsimp(inertia_coefficient_matrix([rb], partials))
coupled_speeds(M, partials)
return M
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# define points
pO = Point('O')
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)))
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))))
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))))
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))))
pCs.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pCs, A, C)
## --- 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)
## --- 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) +
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))))
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))))
# --- 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))))
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde += [u4 - q4d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d])
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)))))
# kinematic differential equations
kde = [u1 - L*q1d, u2 - L*q2d, u3 - L*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)
# contact/distance forces
forces = [(pP1, 6*m*g*N.x),
(pP2, S*N.y + 5*m*g*N.x),
(pP3, 6*m*g*N.x),
(pP4, -Q*N.y + 5*m*g*N.x),
(pP5, 6*m*g*N.x),
(pP6, R*N.y + 5*m*g*N.x)]
partials = partial_velocities(points, u, N, kde_map)
system = [Particle('P{0}'.format(i), p, x*m*g)
for i, p, x in zip(range(1, 7), points, [6, 5] * 3)]
# part a
Fr_star_a, _ = generalized_inertia_forces(partials, system, kde_map)
# part b
K = sum(map(lambda x: x.kinetic_energy(N), system))
Fr_star_b = generalized_inertia_forces_K(K, q, u, kde_map)
# part c
G = sum(P.mass * dot(P.point.acc(N), P.point.acc(N))
for P in system).subs(kde_map) / 2
Fr_star_c = map(lambda u_r: -G.diff(u_r.diff(t)), u)
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))
dV_dq = symbols('∂V/∂q1:{0}'.format(n + 1))
dV_eq = Matrix(Fr).T
W_sr, _ = kde_matrix(u, kde_map)
"velocities of points P1, D* in rf B:\nv_P1_B = {0}\nv_D*_B = {1}".format(
pP1.vel(B),
pDs.vel(B).express(E)))
# X*B.z, (Y*E.y + Z*E.z) are forces the panes of glass
# exert on P1, D* respectively
R1 = X * B.z + C * E.x - m1 * g * B.y
R2 = Y * E.y + Z * E.z - C * E.x - m2 * g * B.y
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:")
points = [pP1, pDs]
forces = [(pP1, R1), (pDs, R2)]
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
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)
# 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 = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
u1d, u2d = ud = [x.diff(t) for x in [u1, u2]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print('{0} = {1}'.format(msprint(x),
msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (-g*sin(q3) + omega**2*q1*cos(q3) +
(m2*L*omega**2*cos(q3)**2 - m1*u2**2/L)/(m1 + m2))
u2d_expected = -g*cos(q3) - omega**2*q1*sin(q3) + u1*u2/L
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 i, f in enumerate(Fr_star, 1):
sub_map = {}
if uset == u_s1: # make the results easier to read
if i == 1 or i == 3:
sub_map = solve([u1 - u_s1[0]], [omega*q1*sin(omega*t)])
print("F{0}* = {1}".format(i, msprint(simplify(f.subs(sub_map)))))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pO, P1*N.x + P2*N.y + P3*N.z),
(pP, R1*N.x + R2*N.y + R3*N.z),
(pP_prime, -(R1*N.x + R2*N.y + R3*N.z)),
(pC_star, Q1*N.x + Q2*N.y + Q3*N.z)]
torques = [(A, alpha1*N.x + alpha2*N.y + alpha3*N.z),
(A, gamma1*N.x + 0*N.y + gamma3*N.z),
(B, -(gamma1*N.x + 0*N.y + gamma3*N.z)),
(C, beta1*N.x + beta2*N.y + beta3*N.z)]
# partial velocities
system = [x for y in bodies for x in [y.masscenter, y.frame]]
system += [f[0] for f in forces + torques]
partials = partial_velocities(system, u, N, kde_map)
# Rewrite the partial velocities of points B*, C*, P'
eq_gen_speed_map = {
a*u1*cos(q1): -b*u2*cos(q2),
a*u1*sin(q1): -u3 - a*u1*sin(q2)*cos(q1)/cos(q2),
(b - b_star)*u2*sin(q2): u3 + a*u1*sin(q1) - b_star*u2*sin(q2)}
for p in [pB_star, pC_star, pP_prime]:
v = p.vel(N).express(N).subs(kde_map).subs(eq_gen_speed_map)
partials[p] = dict(zip(u, map(lambda x: v.diff(x, N), u)))
u1d, u2d, u3d = ud = [x.diff(t) for x in u]
for k, v in vc_map.items():
vc_map[k.diff(t)] = v.diff(t).subs(kde_map).subs(vc_map)
# generalized active/inertia forces
pC1.v2pt_theory(pC_star, X, C)
pC2.v2pt_theory(pC_star, X, C)
pC3.v2pt_theory(pC_star, X, C)
# 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)))
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)
# generalized active forces
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, vc_map)
# dynamical equations
dyn_eq = subs([x + y for x, y in zip(Fr, Fr_star)], kde_map)
u1d, u2d, u3d = ud = [x.diff(t) for x in [u1, u2, u3]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print("{0} = {1}".format(msprint(x), msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (u2 ** 2 * tan(q2) - 6 * u2 * u3 - 4 * g * sin(q2) / R) / 5
u2d_expected = 2 * u3 * u1 - u1 * u2 * tan(q2)
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
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])))
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
system += [x[0] for x in forces + torques]
# partial velocities
partials = partial_velocities(system, [u1, u2, u3], F,
kde_map, express_frame=A)
# Fr, Fr*
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, uaux=[u3])
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])
#F3 + F3* = 0
Q_map[Q1] = Q_map[Q1].subs(F3, -Fr_star[2])
print('Q1 = {0}'.format(msprint(Q_map[Q1])))
Q1_expected = e*M*g*cos(theta)/(f - u_prime*R*u2/sqrt(u2**2 + f**2*u1**2))
assert expand(radsimp(Q_map[Q1] - Q1_expected)) == 0
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)))
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))
dV_dq = symbols('∂V/∂q1:{0}'.format(n + 1))
dV_eq = Matrix(Fr).T
A.set_ang_vel(F, u1*A.x + u3*A.z)
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
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])))
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)))
# --- velocity constraints ---
# Assume C1, C2 roll without slip so velocity at C1^ and C2^ is zero.
vc = [dot(p.vel(A).subs(kde_map), b) for b in S for p in [pC1_hat, pC2_hat]]
vc_map = solve(vc, [u4, u5, u6])
## --- Define contact forces between S and C1, C2 ---
M1 = alpha1*N.x + alpha2*N.y + alpha3*N.z # torques
M2 = beta1*N.x + beta2*N.y + beta3*N.z
K1 = gamma1*N.x + gamma2*N.y + gamma3*N.z # forces
K2 = delta1*N.x + delta2*N.y + delta3*N.z
forces = [(pS1, -K1), (pS2, -K2), (pC1_star, K1), (pC2_star, K2)]
torques = [(S, -M1 - M2), (C1, M1), (C2, M2)]
system = zip(*forces + torques)[0]
partials = partial_velocities(system, [u1, u2, u3], A, kde_map, vc_map, N)
Fr, _ = generalized_active_forces(partials, forces + torques)
print("Exercise 8.4")
print("Generalized active forces:")
for i, f in enumerate(Fr, 1):
print("F{0}_tilde = {1}".format(i, factor(simplify(f))))
#For Exercise 8.5, define 2 new symbols and add 2 new velocity constraints.
omega1, omega2 = symbols('omega1 omega2')
# w_C1_S, w_C2_S are defined in terms of q1d, q2d. We want to substitute in the
# values of ui using the kinematic DEs so that u1 determined in terms of
# u2, u3, u4, u5, u6.
vc += [dot(C1.ang_vel_in(S).subs(kde_map), N.z) - omega1,
dot(C2.ang_vel_in(S).subs(kde_map), N.z) - omega2]
vc_map = solve(vc, [u1, u2, u4, u5, u6])
partials = partial_velocities(system, [u3], A, kde_map, vc_map, N)
