def coupled_speeds(ic_matrix, partials): ulist = partials.ulist print('dynamically coupled speeds:') found = False for i, r in enumerate(ulist): for j, s in enumerate(ulist[i + 1:], i + 1): if trigsimp(ic_matrix[i, j]) != 0: found = True print('{0} and {1}'.format(msprint(r), msprint(s))) if not found: print('None')
# define euler angle symbols and reference frames q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) theta, r, R = symbols('θ r R', real=True, positive=True) theta_val = pi/3 N = ReferenceFrame('N') #B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz') F1 = N.orientnew('F1', 'axis', [q1, N.z]) F2 = F1.orientnew('F2', 'axis', [theta, F1.x]) B = F2.orientnew('B', 'axis', [q2, F2.z]) print('rotation matrix from frame N to frame B:') C = B.dcm(N).subs(theta, theta_val) print(msprint(C)) # velocity of the disk at the point of contact with the ground is not moving # since the disk rolls without slipping. pA = Point('pA') # ball bearing A pB = pA.locatenew('pB', -R*F1.y) # ball bearing B pC = pB.locatenew('pC', -r*F2.y) # disk ground contact point ##pO.set_vel(N, 0) pA.set_vel(N, 0) pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1)
from sympy.physics.vector import ReferenceFrame from sympy.physics.mechanics import inertia, msprint from sympy.physics.mechanics import Point, RigidBody from sympy.physics.mechanics import Lagrangian, LagrangesMethod from sympy import symbols q = q1, q2, q3, q4, q5, q6 = dynamicsymbols("q1:7") m, g, k, px, Ip = symbols("m g k px Ip") N = ReferenceFrame("N") B = N.orientnew("B", "body", [q4, q5, q6], "zyx") A = Point("A") S = A.locatenew("S", q1 * N.x + q2 * N.y + q3 * N.z) P = S.locatenew("P", px * B.x) A.set_vel(N, 0) S.set_vel(N, S.pos_from(A).dt(N)) P.v2pt_theory(S, N, B) Ixx = Ip / 2 Iyy = Ip / 2 Izz = Ip I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb = RigidBody("rb", S, B, m, (I, S)) rb.set_potential_energy(-m * g * (rb.masscenter.pos_from(A) & N.z) + k / 2 * (P.pos_from(A)).magnitude() ** 2) L = Lagrangian(N, rb) print("{} = {}\n".format("L", msprint(L))) lm = LagrangesMethod(L, q) print(msprint(lm.form_lagranges_equations()))
v0: 20} N = ReferenceFrame('N') B = N.orientnew('B', 'axis', [q3, N.z]) O = Point('O') S = O.locatenew('S', q1*N.x + q2*N.y) S.set_vel(N, S.pos_from(O).dt(N)) #Is = m/12*(l**2 + w**2) Is = symbols('Is') I = inertia(B, 0, 0, Is, 0, 0, 0) rb = RigidBody('rb', S, B, m, (I, S)) rb.set_potential_energy(0) L = Lagrangian(N, rb) lm = LagrangesMethod( L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d]) lm.form_lagranges_equations() rhs = lm.rhs() print('{} = {}'.format(msprint(q1d.diff(t)), msprint(rhs[3].simplify()))) print('{} = {}'.format(msprint(q2d.diff(t)), msprint(rhs[4].simplify()))) print('{} = {}'.format(msprint(q3d.diff(t)), msprint(rhs[5].simplify()))) print('{} = {}'.format('λ', msprint(rhs[6].simplify())))
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 Fr, _ = generalized_active_forces(partials, forces + torques) Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map) print('Fr') for i, fr in enumerate(Fr, 1): print('{0}: {1}'.format(i, msprint(fr))) print('Fr_star') for i, fr in enumerate(Fr_star, 1): fr = trigsimp(expand(expand_trig(fr)), deep=True, recursive=True) print('{0}: {1}'.format(i, msprint(fr))) # The dynamical equations would be of the form Fr = Fr* (r = 1, 2, 3). Fr_expected = [ alpha2 + a*(R1*cos(q1) - R3*sin(q1)), b*(R1*cos(q2) + R3*sin(q2)), Q3 - R3] Fr_star_expected = [ -mA*(a_star**2 + kA**2)*u1d, -mB*((b_star**2 + kB**2)*u2d - b_star*sin(q2)*u3d), -1*((mB + mC)*u3d - mB*b_star*sin(q2)*u2d + mB*b_star*cos(q2)*u2**2)] for x, y in zip(Fr, Fr_expected):
# 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 assert expand(trigsimp(dyn_eq_map[u2d] - u2d_expected)) == 0
N = ReferenceFrame('N') F1 = N.orientnew('F1', 'axis', [q1, N.z]) F2 = F1.orientnew('F2', 'axis', [theta, F1.x]) B = F2.orientnew('B', 'axis', [q2, F2.z]) # bearing A pA = Point('A') pA.set_vel(N, 0) pA.set_vel(F1, 0) # bearing B, center of mass of disc pB = pA.locatenew('pB', -R * F1.y) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) print('v_B_N = {}'.format(msprint(pB.vel(N)))) Ixx = m * r**2 / 4 Iyy = m * r**2 / 4 Izz = m * r**2 / 2 I_disc = inertia(F2, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('disc', pB, B, m, (I_disc, pB)) H = rb_disc.angular_momentum(pB, N).subs(vals).express(F2).simplify() print("H about B in frame N = {}".format(msprint(H))) #2b # disc/ground contact point pC = pB.locatenew('pC', -r * F2.y) fAx, fAy, fAz, fBx, fBy, fBz = symbols('fAx fAy fAz fBx fBy fBz') fCx, fCy, fCz = symbols('fCx fCy fCz')
# kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])] # kde += [x - y for x, y in zip(qd[3:], u[3:])] # kde_map = solve(kde, qd) kde = [x - y for x, y in zip(u, qd)] kde_map = solve(kde, qd) 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 IC = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2) rbC = RigidBody("rbC", pC_star, C, m, (IC, pC_star)) rbC.set_potential_energy(m * g * dot(pC_star.pos_from(pR), A.z)) # potential energy V = rbC.potential_energy print("V = {0}".format(msprint(V))) # kinetic energy K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)) print("K = {0}".format(msprint(K))) u_indep = [u1, u2, u3] Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map) # Fr + Fr* = 0 but the dynamical equations cannot be formulated by only # kinetic energy as Fr = -Fr* for r = 1, ..., p print("\ngeneralized active forces, Fr") for i, x in enumerate(Fr, 1): print("F{0} = {1}".format(i, msprint(x))) L = K - V le = lagrange_equations(L, q, u, kde_map)
(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 Fr, _ = generalized_active_forces(partials, forces + torques) Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map) print('Fr') for i, fr in enumerate(Fr, 1): print('{0}: {1}'.format(i, msprint(fr))) print('Fr_star') for i, fr in enumerate(Fr_star, 1): fr = trigsimp(expand(expand_trig(fr)), deep=True, recursive=True) print('{0}: {1}'.format(i, msprint(fr))) # The dynamical equations would be of the form Fr = Fr* (r = 1, 2, 3). Fr_expected = [ alpha2 + a * (R1 * cos(q1) - R3 * sin(q1)), b * (R1 * cos(q2) + R3 * sin(q2)), Q3 - R3 ] Fr_star_expected = [ -mA * (a_star**2 + kA**2) * u1d, -mB * ((b_star**2 + kB**2) * u2d - b_star * sin(q2) * u3d), -1 * ((mB + mC) * u3d - mB * b_star * sin(q2) * u2d + mB * b_star * cos(q2) * u2**2)
pS_star = pO.locatenew('S*', b * A.y) pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone pS1 = pS_star.locatenew('S1', -r * A.z) # S1 touches horizontal wall of the race pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) # Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) # Since S is rolling against C, v_S^_C = 0. # Cone has only angular velocity in R.z direction. vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A] vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc_map = solve(vc, u) # Pure rolling between S and C, dot(ω_C_S, B.y) = 0. b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0] print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r)))) b_expected = r * (1 + sin(theta)) / (cos(theta) - sin(theta)) assert trigsimp(b_val - b_expected) == 0
# since the disk rolls without slipping. pA = Point('pA') # ball bearing A pB = pA.locatenew('pB', -R*F1.y) # ball bearing B pA.set_vel(N, 0) pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) #pC.v2pt_theory(pB, N, B) #print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ') #print(pC.vel(N).express(N).subs(q2d, q2d_val)) Ixx = m*r**2/4 Iyy = m*r**2/4 Izz = m*r**2/2 I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB)) #T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val}) T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}) print('T = {}'.format(msprint(simplify(T)))) values = {R: 1, r: 1, m: 0.5, theta: theta_val} q1d_val = solve([-1 - q2d_val], q1d)[q1d] #print(msprint(q1d_val)) print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
O = Point('O') p1 = O.locatenew('p1', r1) p2 = O.locatenew('p2', r2) O.set_vel(N, 0) p1.set_vel(N, p1.pos_from(O).dt(N)) p2.set_vel(N, p2.pos_from(O).dt(N)) P1 = Particle('P1', p1, 2 * m) P2 = Particle('P2', p2, m) P1.set_potential_energy(0) P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y)) L1 = Lagrangian(N, P1, P2) print('{} = {}'.format('L1', msprint(L1))) lm1 = LagrangesMethod(L1, [s, theta]) lm1.form_lagranges_equations() rhs = lm1.rhs() t = symbols('t') print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify()))) print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify()))) # part b r1 = s * N.x + h * N.y r2 = (s + l * cos(theta)) * N.x + (h + l * sin(theta)) * N.y p1 = O.locatenew('p1', r1) p2 = O.locatenew('p2', r2) p1.set_vel(N, p1.pos_from(O).dt(N))
# 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
from sympy.physics.mechanics import inertia, msprint from sympy.physics.mechanics import Point, RigidBody from sympy import pi, solve, symbols, simplify from sympy import acos, sin, cos # 2a q1 = dynamicsymbols('q1') px, py, pz = symbols('px py pz', real=True, positive=True) sx, sy, sz = symbols('sx sy sz', real=True, sositive=True) m, g, l0, k = symbols('m g l0 k', real=True, positive=True) Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True) N = ReferenceFrame('N') B = N.orientnew('B', 'axis', [q1, N.z]) pA = Point('A') pA.set_vel(N, 0) pP = pA.locatenew('P', l0 * N.y - 2 * l0 * N.z) pS = pP.locatenew('S', -px * B.x - pz * B.z) I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb = RigidBody('plane', pS, B, m, (I, pS)) H = rb.angular_momentum(pS, N) print('H about S in frame N = {}'.format(msprint(H))) print('dH/dt = {}'.format(msprint(H.dt(N)))) print('a_S_N = {}'.format(pS.acc(N)))
pRh = pRs.locatenew('R^', cR * B.x) # orthogonal projection of front wheel center on steer axis pFh = pRh.locatenew('S^', ls * B.z) # front wheel center point pFs = pFh.locatenew('S*', cF * H.x) # front wheel/ground contact point pQ = pFs.locatenew('Q', rF * F_z) # N.z component of vector to pQ from pP # this is our configuration constraint f = simplify(pQ.pos_from(pP) & N.z) print("f = {}\n".format(msprint(f))) # calculate the derivative of f for use with Newton-Raphson df = f.diff(theta) print("df/dθ = {}\n".format(msprint(df))) # constraint function for zero steer/lean configuration and # using the benchmark parameters f0 = lambdify(theta, f.subs({phi: 0, delta: 0}).subs(benchmark_parameters)) df0 = lambdify(theta, df.subs({phi: 0, delta: 0}).subs(benchmark_parameters)) print("verifying constraint equations are correct") print("for zero steer/lean, pitch should be pi/10") findroot(f0, 0.3, solver="newton", tol=1e-8, verbose=True, df=df0) # convert to moore parameters
# define points pO = Point('O') pO.set_vel(A, 0) pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y) pP2 = pP1.locatenew('P2', L * R.x) pP1.set_vel(B, pP1.pos_from(pO).dt(B)) pP2.v2pt_theory(pP1, B, R) pP1.v1pt_theory(pO, A, B) pP2.v2pt_theory(pP1, A, R) # define particles paP1 = Particle('P1', pP1, m1) paP2 = Particle('P2', pP2, m2) # kinetic energy K_S = lambda rf: sum(pa.kinetic_energy(rf) for pa in [paP1, paP2]) K_S_A = trigsimp(K_S(A)) K_S_B = trigsimp(K_S(B)) print('K_S_A = {0}'.format(msprint(K_S_A))) print('K_S_B = {0}'.format(msprint(K_S_B))) K_S_B_expected = ((m1 + m2) * (q1d**2 + q2d**2) / 2 - m2 * L * (q1d * sin(q3) - q2d * cos(q3) - L * q3d / 2) * q3d) K_S_A_expected = K_S_B + omega**2 / 2 * (m1 * q1**2 + m2 * (q1 + L * cos(q3))**2) assert expand(K_S_A - K_S_A_expected) == 0 assert expand(K_S_B - K_S_B_expected) == 0
from sympy.physics.mechanics import dynamicsymbols, inertia, msprint m, a, b = symbols('m a b') q1, q2 = dynamicsymbols('q1, q2') q1d, q2d = dynamicsymbols('q1, q2', level=1) # reference frames # N.x parallel to horizontal line, N.y parallel to line AC N = ReferenceFrame('N') A = N.orientnew('A', 'axis', [-q1, N.y]) B = A.orientnew('B', 'axis', [-q2, A.x]) # points B*, O pO = Point('O') pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y)) pO.set_vel(N, 0) pB_star.v2pt_theory(pO, N, B) # rigidbody B I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18) rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star)) # kinetic energy K = rbB.kinetic_energy(N) print('K = {0}'.format(msprint(trigsimp(K)))) K_expected = m / 4 * ((a**2 + b**2 * sin(q2)**2 / 3) * q1d**2 + a * b * cos(q2) * q1d * q2d + b**2 * q2d**2 / 3) print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected))))) assert expand(trigsimp(K - K_expected)) == 0
N = ReferenceFrame('N') F1 = N.orientnew('F1', 'axis', [q1, N.z]) F2 = F1.orientnew('F2', 'axis', [theta, F1.x]) B = F2.orientnew('B', 'axis', [q2, F2.z]) # bearing A pA = Point('A') pA.set_vel(N, 0) pA.set_vel(F1, 0) # bearing B, center of mass of disc pB = pA.locatenew('pB', -R*F1.y) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) print('v_B_N = {}'.format(msprint(pB.vel(N)))) Ixx = m*r**2/4 Iyy = m*r**2/4 Izz = m*r**2/2 I_disc = inertia(F2, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('disc', pB, B, m, (I_disc, pB)) H = rb_disc.angular_momentum(pB, N).subs(vals).express(F2).simplify() print("H about B in frame N = {}".format(msprint(H))) #2b # disc/ground contact point pC = pB.locatenew('pC', -r*F2.y) fAx, fAy, fAz, fBx, fBy, fBz = symbols('fAx fAy fAz fBx fBy fBz') fCx, fCy, fCz = symbols('fCx fCy fCz')
pDs.set_vel(E, 0) pDs.v2pt_theory(pP1, B, E) pDs.v2pt_theory(pP1, A, E) # define generalized speeds and constraints kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d] kde_map = solve(kde, qd) # include second derivatives in kde map for k, v in kde_map.items(): kde_map[k.diff(t)] = v.diff(t) vc = [dot(pDs.vel(B), E.y)] vc_map = solve(subs(vc, kde_map), [u3]) # define system of particles system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)] # calculate kinetic energy, generalized inertia forces K = sum(map(lambda x: x.kinetic_energy(A), system)) Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map) for i, f in enumerate(Fr_tilde_star, 1): print("F{0}* = {1}".format(i, msprint(simplify(f)))) Fr_tilde_star_expected = [((m1 + m2)*(omega**2*q1*cos(q3) - u1.diff(t)) - m1*u2**2/L + m2*L*omega**2*cos(q3)**2), (-m1*(u2.diff(t) + omega**2*q1*sin(q3) - u1*u2/L))] for x, y in zip(Fr_tilde_star, Fr_tilde_star_expected): assert simplify(x - y) == 0
pDs.v2pt_theory(pP1, B, E) pDs.v2pt_theory(pP1, A, E) # define generalized speeds and constraints kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d] kde_map = solve(kde, qd) # include second derivatives in kde map for k, v in kde_map.items(): kde_map[k.diff(t)] = v.diff(t) vc = [dot(pDs.vel(B), E.y)] vc_map = solve(subs(vc, kde_map), [u3]) # define system of particles system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)] # calculate kinetic energy, generalized inertia forces K = sum(map(lambda x: x.kinetic_energy(A), system)) Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map) for i, f in enumerate(Fr_tilde_star, 1): print("F{0}* = {1}".format(i, msprint(simplify(f)))) Fr_tilde_star_expected = [ ((m1 + m2) * (omega**2 * q1 * cos(q3) - u1.diff(t)) - m1 * u2**2 / L + m2 * L * omega**2 * cos(q3)**2), (-m1 * (u2.diff(t) + omega**2 * q1 * sin(q3) - u1 * u2 / L)) ] for x, y in zip(Fr_tilde_star, Fr_tilde_star_expected): assert simplify(x - y) == 0
from sympy import pi, solve, symbols, simplify from sympy import acos, sin, cos # 2a q1 = dynamicsymbols('q1') px, py, pz = symbols('px py pz', real=True, positive=True) sx, sy, sz = symbols('sx sy sz', real=True, sositive=True) m, g, l0, k = symbols('m g l0 k', real=True, positive=True) Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True) N = ReferenceFrame('N') B = N.orientnew('B', 'axis', [q1, N.z]) pA = Point('A') pA.set_vel(N, 0) pP = pA.locatenew('P', l0*N.y - 2*l0*N.z) pS = pP.locatenew('S', -px*B.x - pz*B.z) I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb = RigidBody('plane', pS, B, m, (I, pS)) H = rb.angular_momentum(pS, N) print('H about S in frame N = {}'.format(msprint(H))) print('dH/dt = {}'.format(msprint(H.dt(N)))) print('a_S_N = {}'.format(pS.acc(N)))
# velocity of the disk at the point of contact with the ground is not moving # since the disk rolls without slipping. pA = Point('pA') # ball bearing A pB = pA.locatenew('pB', -R * F1.y) # ball bearing B pA.set_vel(N, 0) pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) #pC.v2pt_theory(pB, N, B) #print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ') #print(pC.vel(N).express(N).subs(q2d, q2d_val)) Ixx = m * r**2 / 4 Iyy = m * r**2 / 4 Izz = m * r**2 / 2 I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB)) #T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val}) T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}) print('T = {}'.format(msprint(simplify(T)))) values = {R: 1, r: 1, m: 0.5, theta: theta_val} q1d_val = solve([-1 - q2d_val], q1d)[q1d] #print(msprint(q1d_val)) print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
# 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) # 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)))
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) u3d_expected = 2 * u1 * u2 / 3 assert trigsimp(expand(dyn_eq_map[u1d] - u1d_expected)) == 0 assert trigsimp(expand(dyn_eq_map[u2d] - u2d_expected)) == 0 assert trigsimp(expand(dyn_eq_map[u3d] - u3d_expected)) == 0
from sympy.physics.mechanics import Point, RigidBody from sympy.physics.mechanics import Lagrangian, LagrangesMethod from sympy import symbols q = q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7') m, g, k, px, Ip = symbols('m g k px Ip') N = ReferenceFrame('N') B = N.orientnew('B', 'body', [q4, q5, q6], 'zyx') A = Point('A') S = A.locatenew('S', q1*N.x + q2*N.y + q3*N.z) P = S.locatenew('P', px*B.x) A.set_vel(N, 0) S.set_vel(N, S.pos_from(A).dt(N)) P.v2pt_theory(S, N, B) Ixx = Ip/2 Iyy = Ip/2 Izz = Ip I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb = RigidBody('rb', S, B, m, (I, S)) rb.set_potential_energy( -m*g*(rb.masscenter.pos_from(A) & N.z) + k/2*(P.pos_from(A)).magnitude()**2) L = Lagrangian(N, rb) print('{} = {}\n'.format('L', msprint(L))) lm = LagrangesMethod(L, q) print(msprint(lm.form_lagranges_equations()))
# define euler angle symbols and reference frames q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) theta, r, R = symbols('θ r R', real=True, positive=True) theta_val = pi / 3 N = ReferenceFrame('N') #B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz') F1 = N.orientnew('F1', 'axis', [q1, N.z]) F2 = F1.orientnew('F2', 'axis', [theta, F1.x]) B = F2.orientnew('B', 'axis', [q2, F2.z]) print('rotation matrix from frame N to frame B:') C = B.dcm(N).subs(theta, theta_val) print(msprint(C)) # velocity of the disk at the point of contact with the ground is not moving # since the disk rolls without slipping. pA = Point('pA') # ball bearing A pB = pA.locatenew('pB', -R * F1.y) # ball bearing B pC = pB.locatenew('pC', -r * F2.y) # disk ground contact point ##pO.set_vel(N, 0) pA.set_vel(N, 0) pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1)
pC_star.set_vel(N, v * C.y) pA_star.v2pt_theory(pC_star, N, C) # pA* and pC* are both fixed in frame C pB_star.v2pt_theory(pC_star, N, C) # pB* and pC* are both fixed in frame C pA_hat.v2pt_theory(pA_star, N, A) # pA* and pA^ are both fixed in frame A pB_hat.v2pt_theory(pB_star, N, B) # pB* and pB^ are both fixed in frame B I_rod = inertia(C, 0, m0 * L**2 / 12, m0 * L**2 / 12, 0, 0, 0) rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star)) I_discA = inertia(A, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0) rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star)) I_discB = inertia(B, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0) rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star)) print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C)))) print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N)))) qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)], [q2d, q3d]) print(msprint(qd_val)) print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val))))) print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val))))) T = expand( simplify( rbA.kinetic_energy(N).subs(qd_val) + rbB.kinetic_energy(N).subs(qd_val) + rbC.kinetic_energy(N))) print('T = {}'.format(msprint(T))) #T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
I = inertia(F, Ia, It, It, 0, 0, 0) rb = RigidBody('flywheel', P, B, m, (I, P)) H = rb.angular_momentum(P, N) print('H_P_N = {}\n = {}'.format(H.express(N), H.express(F))) dH = H.dt(N) print('d^N(H_P_N)/dt = {}'.format(dH.express(F).subs(q2dd, 0))) print('\ndH/dt = M') print('M = {}'.format(dH.express(F).subs(q2dd, 0))) print('\ncalculation using euler angles') t = symbols('t') omega = F.ang_vel_in(N) wx = omega & F.x wy = omega & F.y wz = omega & F.z s = B.ang_vel_in(F) & F.x Mx = Ia*(wx + s).diff(t) My = It*(wy).diff(t) - (It - Ia)*wz*wx + Ia*s*wz Mz = It*(wz).diff(t) - (It - Ia)*wx*wy + Ia*s*wy M = Mx*F.x + My*F.y + Mz*F.z print('M = {}'.format(M.subs(q2dd, 0))) Fr, a = symbols('Fr a') f = Fr*F.x soln = solve([(((2*a*N.z)^f) - M) & F.y], Fr) print('\nFr = {}'.format(msprint(soln[Fr])))
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
A = ReferenceFrame('A') A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x]) A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y]) B = A_2.orientnew('B', 'axis', [ea[2], A_2.z]) # display the rotation matrix C from frame A to frame B print('Rotation matrix C:') mprint(B.dcm(A)) # define angular velocity vector w = dynamicsymbols('ω_x ω_y ω_z') omega_A = sum((a*uv for a, uv in zip(w, A)), Vector(0)) omega_B = sum((a*uv for a, uv in zip(w, B)), Vector(0)) # display angular velocity vector omega print('\nangular velocity:') omega = B.ang_vel_in(A) print('ω = {}'.format(msprint(omega))) print('ω_A = {}'.format(msprint(omega.express(A)))) print('ω_B = {}'.format(msprint(omega.express(B)))) # solve for alpha, beta, gamma in terms of angular velocity components dea = [a.diff() for a in ea] for omega_fr, frame in [(omega_A, A), (omega_B, B)]: eqs = [dot(omega - omega_fr, uv) for uv in frame] soln = solve(eqs, dea) print('\nif angular velocity components are taken to be in frame ' '{}'.format(frame)) for a in dea: print('{} = {}'.format(msprint(a), msprint(soln[a])))
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) # 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)))
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)
pA_star.v2pt_theory(pC_star, N, C) # pA* and pC* are both fixed in frame C pB_star.v2pt_theory(pC_star, N, C) # pB* and pC* are both fixed in frame C pA_hat.v2pt_theory(pA_star, N, A) # pA* and pA^ are both fixed in frame A pB_hat.v2pt_theory(pB_star, N, B) # pB* and pB^ are both fixed in frame B I_rod = inertia(C, 0, m0*L**2/12, m0*L**2/12, 0, 0, 0) rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star)) I_discA = inertia(A, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0) rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star)) I_discB = inertia(B, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0) rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star)) print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C)))) print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N)))) qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)], [q2d, q3d]) print(msprint(qd_val)) print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val))))) print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val))))) T = expand(simplify( rbA.kinetic_energy(N).subs(qd_val) + rbB.kinetic_energy(N).subs(qd_val) + rbC.kinetic_energy(N))) print('T = {}'.format(msprint(T)))
pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) #pC.v2pt_theory(pB, N, B) #print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ') #print(pC.vel(N).express(N).subs(q2d, q2d_val)) Ixx = m * r**2 / 4 Iyy = m * r**2 / 4 Izz = m * r**2 / 2 I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB)) T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val}) print('T = {}'.format(msprint(simplify(T)))) from sympy import S #T2 = q1d**2*(m*r**2/4*(S(5)/8 - (2*R + r)/(2*r) + (2*R + r)**2/(2*r)**2) + m*R**2/2) T2 = m * q1d**2 * R**2 / 2 + m * r**2 / 2 * ( (2 * R + r)**2 / (2 * r)**2 * q1d**2 / 2 - (2 * R + r)**2 / (2 * r) * q1d**2 / 2 + 5 * q1d**2 / 16) from sympy import expand print('T - T2 = {}'.format(msprint(expand(T - T2).simplify()))) t = symbols('t') dT = T.diff(symbols('t')) print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
def print_fr_star(fr_star): for i, f in enumerate(fr_star, 1): print("F{0}* = {1}".format(i, msprint(trigsimp(together(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)
pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pC_star = pC_hat.locatenew('C*', R*B.y) pC_star.set_vel(C, 0) pC_star.set_vel(B, 0) # calculate velocities in A pC_star.v2pt_theory(pR, A, B) pC_hat.v2pt_theory(pC_star, A, C) ## --- 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) 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)) # kinetic energy K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)), m*R**2/8) print('K = {0}'.format(msprint(K))) K_expected = (m*R**2/8) * (5*u1**2 + u2**2 + 6*u3**2) assert expand(K - K_expected) == 0
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] # Since S is rolling against C, v_S^_C = 0. pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) vc += [dot(pS_hat.vel(C), basis) for basis in A] # Cone has only angular velocity ω in R.z direction. vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc += [omega - dot(C.ang_vel_in(R), R.z)] vc_map = solve(vc, u) # cone rigidbody I_C = inertia(A, I11, I22, J) rbC = RigidBody('rbC', pO, C, M, (I_C, pO)) # sphere rigidbody I_S = inertia(A, 2 * m * r**2 / 5, 2 * m * r**2 / 5, 2 * m * r**2 / 5) rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star)) # kinetic energy K = radsimp( expand((rbC.kinetic_energy(R) + 4 * rbS.kinetic_energy(R)).subs(vc_map))) print('K = {0}'.format(msprint(collect(K, omega**2 / 2)))) K_expected = (J + 18 * m * r**2 * (2 + sqrt(3)) / 5) * omega**2 / 2 #print('K_expected = {0}'.format(msprint(collect(expand(K_expected), # omega**2/2)))) assert expand(K - K_expected) == 0
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] # Since S is rolling against C, v_S^_C = 0. pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) vc += [dot(pS_hat.vel(C), basis) for basis in A] # Cone has only angular velocity ω in R.z direction. vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc += [omega - dot(C.ang_vel_in(R), R.z)] vc_map = solve(vc, u) # cone rigidbody I_C = inertia(A, I11, I22, J) rbC = RigidBody('rbC', pO, C, M, (I_C, pO)) # sphere rigidbody I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5) rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star)) # kinetic energy K = radsimp(expand((rbC.kinetic_energy(R) + 4*rbS.kinetic_energy(R)).subs(vc_map))) print('K = {0}'.format(msprint(collect(K, omega**2/2)))) K_expected = (J + 18*m*r**2*(2 + sqrt(3))/5) * omega**2/2 #print('K_expected = {0}'.format(msprint(collect(expand(K_expected), # omega**2/2)))) assert expand(K - K_expected) == 0
# reference frames A = ReferenceFrame('A') B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz') # points B*, O pB_star = Point('B*') pB_star.set_vel(A, 0) # rigidbody B I_B_Bs = inertia(B, I11, I22, I33) rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star)) # kinetic energy K = rbB.kinetic_energy(A) # velocity of point B* is zero print('K_ω = {0}'.format(msprint(K))) print('\nSince I11, I22, I33 are the central principal moments of inertia') print('let I_min = I11, I_max = I33') I_min = I11 I_max = I33 H = rbB.angular_momentum(pB_star, A) K_min = dot(H, H) / I_max / 2 K_max = dot(H, H) / I_min / 2 print('K_ω_min = {0}'.format(msprint(K_min))) print('K_ω_max = {0}'.format(msprint(K_max))) print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω') print('Similarly, I22/I11, I33/I11 >= 1, ' 'since I11 <= I22, I33, so K_ω_max >= K_ω')
O = Point('O') p1 = O.locatenew('p1', r1) p2 = O.locatenew('p2', r2) O.set_vel(N, 0) p1.set_vel(N, p1.pos_from(O).dt(N)) p2.set_vel(N, p2.pos_from(O).dt(N)) P1 = Particle('P1', p1, 2*m) P2 = Particle('P2', p2, m) P1.set_potential_energy(0) P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y)) L1 = Lagrangian(N, P1, P2) print('{} = {}'.format('L1', msprint(L1))) lm1 = LagrangesMethod(L1, [s, theta]) lm1.form_lagranges_equations() rhs = lm1.rhs() t = symbols('t') print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify()))) print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify()))) # part b r1 = s*N.x + h*N.y r2 = (s + l*cos(theta))*N.x + (h + l*sin(theta))*N.y p1 = O.locatenew('p1', r1) p2 = O.locatenew('p2', r2) p1.set_vel(N, p1.pos_from(O).dt(N))
A = ReferenceFrame('A') A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x]) A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y]) B = A_2.orientnew('B', 'axis', [ea[2], A_2.z]) # display the rotation matrix C from frame A to frame B print('Rotation matrix C:') mprint(B.dcm(A)) # define angular velocity vector w = dynamicsymbols('ω_x ω_y ω_z') omega_A = sum((a * uv for a, uv in zip(w, A)), Vector(0)) omega_B = sum((a * uv for a, uv in zip(w, B)), Vector(0)) # display angular velocity vector omega print('\nangular velocity:') omega = B.ang_vel_in(A) print('ω = {}'.format(msprint(omega))) print('ω_A = {}'.format(msprint(omega.express(A)))) print('ω_B = {}'.format(msprint(omega.express(B)))) # solve for alpha, beta, gamma in terms of angular velocity components dea = [a.diff() for a in ea] for omega_fr, frame in [(omega_A, A), (omega_B, B)]: eqs = [dot(omega - omega_fr, uv) for uv in frame] soln = solve(eqs, dea) print('\nif angular velocity components are taken to be in frame ' '{}'.format(frame)) for a in dea: print('{} = {}'.format(msprint(a), msprint(soln[a])))
pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) #pC.v2pt_theory(pB, N, B) #print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ') #print(pC.vel(N).express(N).subs(q2d, q2d_val)) Ixx = m*r**2/4 Iyy = m*r**2/4 Izz = m*r**2/2 I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB)) T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val}) print('T = {}'.format(msprint(simplify(T)))) from sympy import S #T2 = q1d**2*(m*r**2/4*(S(5)/8 - (2*R + r)/(2*r) + (2*R + r)**2/(2*r)**2) + m*R**2/2) T2 = m*q1d**2*R**2/2 + m*r**2/2 * ( (2*R + r)**2/(2*r)**2 * q1d**2 / 2 - (2*R + r)**2/(2*r) * q1d**2 / 2 + 5*q1d**2/16) from sympy import expand print('T - T2 = {}'.format(msprint(expand(T - T2).simplify()))) t = symbols('t') dT = T.diff(symbols('t')) print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
#kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])] #kde += [x - y for x, y in zip(qd[3:], u[3:])] #kde_map = solve(kde, qd) kde = [x - y for x, y in zip(u, qd)] kde_map = solve(kde, qd) 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 IC = inertia(C, m*R**2/4, m*R**2/4, m*R**2/2) rbC = RigidBody('rbC', pC_star, C, m, (IC, pC_star)) rbC.set_potential_energy(m*g*dot(pC_star.pos_from(pR), A.z)) # potential energy V = rbC.potential_energy print('V = {0}'.format(msprint(V))) # kinetic energy K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)) print('K = {0}'.format(msprint(K))) u_indep = [u1, u2, u3] Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map) # Fr + Fr* = 0 but the dynamical equations cannot be formulated by only # kinetic energy as Fr = -Fr* for r = 1, ..., p print('\ngeneralized active forces, Fr') for i, x in enumerate(Fr, 1): print('F{0} = {1}'.format(i, msprint(x))) L = K - V le = lagrange_equations(L, q, u, kde_map)
pRh = pRs.locatenew('R^', cR*B.x) # orthogonal projection of front wheel center on steer axis pFh = pRh.locatenew('S^', ls*B.z) # front wheel center point pFs = pFh.locatenew('S*', cF*H.x) # front wheel/ground contact point pQ = pFs.locatenew('Q', rF*F_z) # N.z component of vector to pQ from pP # this is our configuration constraint f = simplify(pQ.pos_from(pP) & N.z) print("f = {}\n".format(msprint(f))) # calculate the derivative of f for use with Newton-Raphson df = f.diff(theta) print("df/dθ = {}\n".format(msprint(df))) # constraint function for zero steer/lean configuration and # using the benchmark parameters f0 = lambdify(theta, f.subs({phi: 0, delta: 0}).subs(benchmark_parameters)) df0 = lambdify(theta, df.subs({phi: 0, delta: 0}).subs(benchmark_parameters)) print("verifying constraint equations are correct") print("for zero steer/lean, pitch should be pi/10") findroot(f0, 0.3, solver="newton", tol=1e-8, verbose=True, df=df0) # convert to moore parameters
m, a, b = symbols('m a b') q1, q2 = dynamicsymbols('q1, q2') q1d, q2d = dynamicsymbols('q1, q2', level=1) # reference frames # N.x parallel to horizontal line, N.y parallel to line AC N = ReferenceFrame('N') A = N.orientnew('A', 'axis', [-q1, N.y]) B = A.orientnew('B', 'axis', [-q2, A.x]) # points B*, O pO = Point('O') pB_star = pO.locatenew('B*', S(1)/3*(2*a*B.x - b*B.y)) pO.set_vel(N, 0) pB_star.v2pt_theory(pO, N, B) # rigidbody B I_B_Bs = inertia(B, m*b**2/18, m*a**2/18, m*(a**2 + b**2)/18) rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star)) # kinetic energy K = rbB.kinetic_energy(N) print('K = {0}'.format(msprint(trigsimp(K)))) K_expected = m/4*((a**2 + b**2*sin(q2)**2/3)*q1d**2 + a*b*cos(q2)*q1d*q2d + b**2*q2d**2/3) print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected))))) assert expand(trigsimp(K - K_expected)) == 0
pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pC_star = pC_hat.locatenew('C*', R * B.y) pC_star.set_vel(C, 0) pC_star.set_vel(B, 0) # calculate velocities in A pC_star.v2pt_theory(pR, A, B) pC_hat.v2pt_theory(pC_star, A, C) ## --- 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) 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)) # kinetic energy K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)), m * R**2 / 8) print('K = {0}'.format(msprint(K))) K_expected = (m * R**2 / 8) * (5 * u1**2 + u2**2 + 6 * u3**2) assert expand(K - K_expected) == 0
pO = Point('O') pS_star = pO.locatenew('S*', b*A.y) pS_hat = pS_star.locatenew('S^', -r*B.y) # S^ touches the cone pS1 = pS_star.locatenew('S1', -r*A.z) # S1 touches horizontal wall of the race pS2 = pS_star.locatenew('S2', r*A.y) # S2 touches vertical wall of the race pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) # Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) # Since S is rolling against C, v_S^_C = 0. # Cone has only angular velocity in R.z direction. vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A] vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc_map = solve(vc, u) # Pure rolling between S and C, dot(ω_C_S, B.y) = 0. b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0] print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r)))) b_expected = r*(1 + sin(theta))/(cos(theta) - sin(theta)) assert trigsimp(b_val - b_expected) == 0
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
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 assert expand(trigsimp(dyn_eq_map[u2d] - u2d_expected)) == 0
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint m, B11, B22, B33, B12, B23, B31 = symbols('m B11 B22 B33 B12 B23 B31') q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7') # reference frames A = ReferenceFrame('A') B = A.orientnew('B', 'body', [q4, q5, q6], 'xyz') omega = B.ang_vel_in(A) # points B*, O pB_star = Point('B*') pO = pB_star.locatenew('O', q1*B.x + q2*B.y + q3*B.z) pO.set_vel(A, 0) pO.set_vel(B, 0) pB_star.v2pt_theory(pO, A, B) # rigidbody B I_B_O = inertia(B, B11, B22, B33, B12, B23, B31) rbB = RigidBody('rbB', pB_star, B, m, (I_B_O, pO)) # kinetic energy K = rbB.kinetic_energy(A) print('K = {0}'.format(msprint(trigsimp(K)))) K_expected = dot(dot(omega, I_B_O), omega)/2 print('K_expected = 1/2*omega*I*omega = {0}'.format( msprint(trigsimp(K_expected)))) assert expand(expand_trig(K - K_expected)) == 0