def print_equations(model: Model, pretty_print: bool) -> None: mass_mat = model.kanes.mass_matrix forcing_mat = model.kanes.forcing rhs = mass_mat.inv() * forcing_mat kdd = model.kanes.kindiffdict() rhs = rhs.subs(kdd) rhs.simplify() if pretty_print: mechanics.mpprint(rhs) else: mechanics.mprint(rhs)
B_prime = A.orientnew('B_prime', 'Axis', [q1, A.z]) B = B_prime.orientnew('B', 'Axis', [pi/2 - q2, B_prime.x]) C = B.orientnew('C', 'Axis', [q3, B.z]) # Angular velocity based on coordinate time derivatives w_C_in_A_qd = C.ang_vel_in(A) # First definition of Angular velocity w_C_in_A_uxuyuz = ux * A.x + uy * A.y + uz * A.z print("Using w_C_A as") print(w_C_in_A_uxuyuz) kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_uxuyuz) & uv for uv in A] print("The kinematic equations are:") soln = solve(kinematic_eqs, [q1d, q2d, q3d]) for qd in [q1d, q2d, q3d]: mprint(qd) print("=") mprint(soln[qd]) # Second definition of Angular velocity w_C_in_A_u1u2u3 = u1 * B.x + u2 * B.y + u3 * B.z print("Using w_C_A as") print(w_C_in_A_u1u2u3) kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_u1u2u3) & uv for uv in A] print("The kinematic equations are:") soln = solve(kinematic_eqs, [q1d, q2d, q3d]) for qd in [q1d, q2d, q3d]: mprint(qd) print("=") mprint(soln[qd])
from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import dot, dynamicsymbols from sympy.physics.mechanics import mprint, msprint from sympy import solve # define euler angle symbols and reference frames ea = dynamicsymbols('α β γ') 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]
# forces # # add the gravitional force to each body rodGravity = (Ao, N.z * gravity * mA) plateGravity = (Bo, N.z * gravity * mB) ## equations of motion with Kane's method ## # make a list of the bodies and forces bodyList = [rod, plate] forceList = [rodGravity, plateGravity] # create a Kane object with respect to the Newtonian reference frame kane = me.KanesMethod(N, q_ind=[theta, phi], u_ind=[omega, alpha], kd_eqs=kinDiffs) # calculate Kane's equations fr, frstar = kane.kanes_equations(forceList, bodyList) zero = fr + frstar # solve Kane's equations for the derivatives of the speeds eom = sym.solvers.solve(zero, omegad, alphad) # add the kinematical differential equations to get the equations of motion eom.update(kane.kindiffdict()) # print the results me.mprint(eom)
B_prime = A.orientnew('B_prime', 'Axis', [q1, A.z]) B = B_prime.orientnew('B', 'Axis', [pi / 2 - q2, B_prime.x]) C = B.orientnew('C', 'Axis', [q3, B.z]) # Angular velocity based on coordinate time derivatives w_C_in_A_qd = C.ang_vel_in(A) # First definition of Angular velocity w_C_in_A_uxuyuz = ux * A.x + uy * A.y + uz * A.z print("Using w_C_A as") print(w_C_in_A_uxuyuz) kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_uxuyuz) & uv for uv in A] print("The kinematic equations are:") soln = solve(kinematic_eqs, [q1d, q2d, q3d]) for qd in [q1d, q2d, q3d]: mprint(qd) print("=") mprint(soln[qd]) # Second definition of Angular velocity w_C_in_A_u1u2u3 = u1 * B.x + u2 * B.y + u3 * B.z print("Using w_C_A as") print(w_C_in_A_u1u2u3) kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_u1u2u3) & uv for uv in A] print("The kinematic equations are:") soln = solve(kinematic_eqs, [q1d, q2d, q3d]) for qd in [q1d, q2d, q3d]: mprint(qd) print("=") mprint(soln[qd])
from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import dot, dynamicsymbols from sympy.physics.mechanics import mprint, msprint from sympy import pi # define euler angle symbols and reference frames ea = dynamicsymbols('α β γ') # zyx s_i = dynamicsymbols('s_x s_y s_z') p_i = dynamicsymbols('p_x p_y p_z') A = ReferenceFrame('A') B = A.orientnew('B', 'body', ea, 'zyx') # display the rotation matrix C from frame A to frame B print('Rotation matrix C:') mprint(B.dcm(A)) vals = [pi, pi/3, -pi/4] print('\nsubstitute in euler angle values') print(' α = {}'.format(vals[0])) print(' β = {}'.format(vals[1])) print(' γ = {}'.format(vals[2])) ea_dict = dict(zip(ea, vals)) R = B.dcm(A).subs(ea_dict) print('R = ') mprint(R) p = s_i[0]*A.x + s_i[1]*A.y + s_i[2]*A.z p += p_i[0]*B.x + p_i[2]*B.z print('\nvector p = ') mprint(p.express(A).subs(ea_dict))
from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import dot, dynamicsymbols from sympy.physics.mechanics import mprint, msprint from sympy import solve # define euler angle symbols and reference frames ea = dynamicsymbols('α β γ') 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]
# Alternatively, f_a can be formed as #v_co_n = cross(C.ang_vel_in(N), CO.pos_from(P)) #a_co_n = v_co_n.dt(B) + cross(B.ang_vel_in(N), v_co_n) #f_a = Matrix([((a_co_n - CO.acc(N)) & uv).expand() for uv in B]) # Kane's dynamic equations via elbow grease # Partial angular velocities and velocities partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u] partial_v_CO = [CO.vel(N).diff(ui, N) for ui in u] # Active forces F_CO = m*g*A.z # Generalized active forces (unconstrained) gaf = [dot(F_CO, pv) for pv in partial_v_CO] print("Generalized active forces (unconstrained)") mprint(gaf) # Inertia force R_star_CO = -m*CO.acc(N) I = (m * r**2) / 4 J = (m * r**2) / 2 # Inertia torque I_C_CO = inertia(C, I, J, I) # Inertia of disc C about point CO T_star_C = -(dot(I_C_CO, C.ang_acc_in(N)) + cross(C.ang_vel_in(N), dot(I_C_CO, C.ang_vel_in(N)))) # Generalized inertia forces (unconstrained) gif = [dot(R_star_CO, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_CO, partial_w_C)]
from sympy import symbols, simplify, latex from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint, mlatex, Point, ReferenceFrame) Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J") mA, mB, lx, lz = symbols("m_A m_B l_x l_z") A = ReferenceFrame("A") IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz) IB_BO = inertia(A, I, J, I) BO = Point('BO') AO = BO.locatenew('AO', lx*A.x + lz*A.z) CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO)) print(mlatex(CO.pos_from(BO))) IC_CO = IA_AO + IB_BO +\ inertia_of_point_mass(mA, AO.pos_from(CO), A) +\ inertia_of_point_mass(mB, BO.pos_from(CO), A) mprint((A.x & IC_CO & A.x).expand()) mprint((A.y & IC_CO & A.y).expand()) mprint((A.z & IC_CO & A.z).expand()) mprint((A.x & IC_CO & A.z).expand())
# Making a particle from point mass Pa_1 = Particle("P_1",P_1,m_theta) Pa_2 = Particle("P_2",P_2,m_l) # Potential energy of system Pa_1.potential_energy = m_theta * g * y1 Pa_2.potential_energy = m_l * g * y2 # Non-restorative forces f_theta = (P_1, torque*( -sin(theta) * N.x + cos(theta) * N.y)/lg_1) f_sliding = (P_2, sliding_force * (cos(theta) * N.x + sin(theta) * N.y)) fl = [f_theta, f_sliding] # Setting-up Lagrangian L = Lagrangian(N,Pa_1,Pa_2) # Generation Equation of Motion LM = LagrangesMethod(L,var,forcelist = fl,frame=N) EOM = LM.form_lagranges_equations() """ Printing results """ mprint( simplify(EOM) ) #print( mlatex(simplify(me)) ) #print( mlatex(LM.rhs()) ) #simplyfied #eq1 = simplify( LM.rhs() ) #mprint( eq1 )
# Form equations of motion KM = KanesMethod(A, q, u, kindiffs) fr, frstar = KM.kanes_equations(torque_force_list, body_list) # Simplifying assumptions simplifications = {IFxx: symbols('I'), # Flywheel transverse inertias IFyy: symbols('I'), # assumed to be equal IFzz: symbols('J'), Ixx: 0, Iyy: 0, Izz: 0} frstar = frstar.subs(simplifications) MM = -frstar.jacobian(ud) MM.simplify() forcing = fr + frstar.subs({ud[0]: 0, ud[1]:0, ud[2]:0}).expand() forcing.simplify() print("Mass matrix:") mprint(MM) print("forcing vector:") mprint(forcing) eqns = MM * Matrix(ud) - forcing udots = solve(eqns, ud) for udi in ud: mprint(udi) print(":") mprint(udots[udi])
rod = me.RigidBody("rod", Ao, A, mA, (me.inertia(A, IAxx, IAxx, 0.0), Ao)) # create the empty plate object plate = me.RigidBody("plate", Bo, B, mB, (me.inertia(B, IBxx, IByy, IBzz), Bo)) # forces # # add the gravitional force to each body rodGravity = (Ao, N.z * gravity * mA) plateGravity = (Bo, N.z * gravity * mB) ## equations of motion with Kane's method ## # make a list of the bodies and forces bodyList = [rod, plate] forceList = [rodGravity, plateGravity] # create a Kane object with respect to the Newtonian reference frame kane = me.KanesMethod(N, q_ind=[theta, phi], u_ind=[omega, alpha], kd_eqs=kinDiffs) # calculate Kane's equations fr, frstar = kane.kanes_equations(forceList, bodyList) zero = fr + frstar # solve Kane's equations for the derivatives of the speeds eom = sym.solvers.solve(zero, omegad, alphad) # add the kinematical differential equations to get the equations of motion eom.update(kane.kindiffdict()) # print the results me.mprint(eom)
from sympy import symbols, simplify, latex from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint, mlatex, Point, ReferenceFrame) Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J") mA, mB, lx, lz = symbols("m_A m_B l_x l_z") A = ReferenceFrame("A") IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz) IB_BO = inertia(A, I, J, I) BO = Point('BO') AO = BO.locatenew('AO', lx * A.x + lz * A.z) CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO)) print(mlatex(CO.pos_from(BO))) IC_CO = IA_AO + IB_BO +\ inertia_of_point_mass(mA, AO.pos_from(CO), A) +\ inertia_of_point_mass(mB, BO.pos_from(CO), A) mprint((A.x & IC_CO & A.x).expand()) mprint((A.y & IC_CO & A.y).expand()) mprint((A.z & IC_CO & A.z).expand()) mprint((A.x & IC_CO & A.z).expand())
Pa1 = Particle('Pa1', P1, m1) Pa1.potential_energy = m1 * g * y1 # 質点2(振子)の質点、位置 P2 = Point('P2') x2 = p - l * sin(theta) y2 = cos(theta) vx2 = diff(x2, t) vy2 = diff(y2, t) P2.set_vel(N, vx2 * N.x + vy2 * N.y) Pa2 = Particle('Pa2', P2, m2) Pa2.potential_energy = m1 * g * y2 # 外力 fl = [(P1, F * N.x), (P2, 0 * N.x)] # 運動方程式 LL = Lagrangian(N, Pa1, Pa2) LM = LagrangesMethod(LL, q, forcelist=fl, frame=N) eom = LM.form_lagranges_equations().simplify() f = LM.rhs().simplify() # 線形化 linearlizer = LM.to_linearizer(q_ind=q, qd_ind=qd) op_point = {p: 0, theta: 0, p.diff(t): 0, theta.diff(t): 0} A, B = linearlizer.linearize(A_and_B=True, op_point=op_point) mprint(A) mprint(B)
# Define rigid body objects Enclosure = RigidBody('Enclosure', O, B, 0, (I_enclosure, O)) Flywheel = RigidBody('Flywheel', O, C, 0, (I_flywheel, O)) body_list = [Enclosure, Flywheel] # List of forces and torques tau = symbols('tau') # Gimbal torque scalar torque_force_list = [(O, m*g*A.z), # Gravitational force (B, tau*A.y)] # Gimbal torque vector # Form equations of motion KM = KanesMethod(A, q, u, kindiffs) fr, frstar = KM.kanes_equations(torque_force_list, body_list) frstar.simplify() print("Pre simplification") mprint(fr) mprint(frstar) # Simplifying assumptions simplifications = {IFxx: symbols('I'), # Flywheel transverse inertias IFyy: symbols('I')} # assumed to be equal frstar = frstar.subs(simplifications) frstar.simplify() print("Post simplification") mprint(fr) mprint(frstar)