## --- 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)))
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
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)