def test_linearize_pendulum_lagrange_nonminimal(): q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose World Frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2 / q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Create point P, the pendulum mass P = pN.locatenew('P1', q1 * N.x + q2 * N.y) P.set_vel(N, P.pos_from(pN).dt(N)) pP = Particle('pP', P, m) # Constraint Equations f_c = Matrix([q1**2 + q2**2 - L**2]) # Calculate the lagrangian, and form the equations of motion Lag = Lagrangian(N, pP) LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m * g * N.x)], frame=N) LM.form_lagranges_equations() # Compose operating point op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0} # Solve for multiplier operating point lam_op = LM.solve_multipliers(op_point=op_point) op_point.update(lam_op) # Perform the Linearization A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d], op_point=op_point, A_and_B=True) assert A == Matrix([[0, 1], [-9.8 / L, 0]]) assert B == Matrix([])
def test_linearize_rolling_disc_lagrange(): q1, q2, q3 = q = dynamicsymbols('q1 q2 q3') q1d, q2d, q3d = qd = dynamicsymbols('q1 q2 q3', 1) r, m, g = symbols('r m g') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) C = Point('C') C.set_vel(N, 0) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyD.potential_energy = - m * g * r * cos(q2) Lag = Lagrangian(N, BodyD) l = LagrangesMethod(Lag, q) l.form_lagranges_equations() # Linearize about steady-state upright rolling op_point = {q1: 0, q2: 0, q3: 0, q1d: 0, q2d: 0, q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0} A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0] sol = Matrix([[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -6*q3d, 0], [0, -4*g/(5*r), 0, 6*q3d/5, 0, 0], [0, 0, 0, 0, 0, 0]]) assert A == sol
# Other system variables m, l, g = symbols('m l g') # Set up the reference frames # Reference frame A set up in the plane perpendicular to the page containing # segment OP N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [theta, N.z]) # Set up the points and particles O = Point('O') P = O.locatenew('P', l * A.x) Pa = Particle('Pa', P, m) # Set up velocities A.set_ang_vel(N, thetad * N.z) O.set_vel(N, 0) P.v2pt_theory(O, N, A) # Set up the lagrangian L = Lagrangian(N, Pa) # Create the list of forces acting on the system fl = [(P, g * m * N.x)] # Create the equations of motion using lagranges method l = LagrangesMethod(L, [theta], forcelist=fl, frame=N) pprint(l.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())))
# Definition of mass and velocity Pa1 = Particle('Pa1', P1, m1) Pa2 = Particle('Pa2', P2, m2) vx1 = diff(x1, t) vy1 = diff(y1, t) vx2 = diff(x2, t) vy2 = diff(y2, t) P1.set_vel(N, vx1 * N.x + vy1 * N.y) P2.set_vel(N, vx2 * N.x + vy2 * N.y) # Potential Energy Pa1.potential_energy = m1 * g * y1 Pa2.potential_energy = m2 * g * y2 # External Force fl = [(P1, F * N.x), (P2, 0 * N.x)] # Lagrangian LL = Lagrangian(N, Pa1, Pa2) LM = LagrangesMethod(LL, q, forcelist=fl, frame=N) eom = LM.form_lagranges_equations() f = LM.rhs() # Linearization linearizer = 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 = linearizer.linearize(A_and_B=True, op_point=op_point)
# 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 )
# Corpos Rígidos I_1 = inertia(B1, 0, 0, I_1_ZZ) # Elo 1 E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1)) I_2 = inertia(B1, 0, 0, I_1_ZZ) # Elo 2 E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2)) # Energia Potencial P_1 = -M_1 * G * B0.z R_1_CM = CM_1.pos_from(O).express(B0) E_1.potential_energy = R_1_CM.dot(P_1) P_2 = -M_2 * G * B0.z R_2_CM = CM_2.pos_from(O).express(B0).simplify() E_2.potential_energy = R_2_CM.dot(P_2) # Forças/Momentos Generalizados FL = [(B1, TAU_1 * B0.z), (B2, TAU_2 * B0.z)] # Método de Lagrange L = Lagrangian(B0, E_1, E_2) L = L.simplify() pprint(L) LM = LagrangesMethod(L, [THETA_1, THETA_2], frame=B0, forcelist=FL) # Equações do Movimento L_EQ = LM.form_lagranges_equations() pprint(L_EQ) # Equações Prontas para Solução Numérica RHS = LM.rhs()
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)
# v_x = symbols('v_x') # x方向の速度 # v_y = symbols('v_y') # y方向の速度 v_x = x.diff(t) v_y = y.diff(t) N = ReferenceFrame('N') # 参照座標系 pt = Point("P") pt.set_vel(N, v_x * N.x + v_y * N.y) m = symbols("m") # 質量 pcl = Particle("mass", pt, m) f_x, f_y = dynamicsymbols("f_x f_y") LL = Lagrangian(N, pcl) LM = LagrangesMethod(LL, [q]) eom = LM.form_lagranges_equations().simplify() rhs = LM.rhs() import pdb pdb.set_trace() def get_path(t_i): """ :param t_i: 時間 :return: 時間t_iでのx, y座標