def test_rolling_disc(): # Rolling Disc Example # Here the rolling disc is formed from the contact point up, removing the # need to introduce generalized speeds. Only 3 configuration and 3 # speed variables are need to describe this system, along with the # disc's mass and radius, and the local gravity. q1, q2, q3 = dynamicsymbols('q1 q2 q3') q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1) r, m, g = symbols('r m g') # The kinematics are formed by a series of simple rotations. Each simple # rotation creates a new frame, and the next rotation is defined by the new # frame's basis vectors. This example uses a 3-1-2 series of rotations, or # Z, X, Y series of rotations. Angular velocity for this is defined using # the second frame's basis (the lean frame). 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]) # This is the translational kinematics. We create a point with no velocity # in N; this is the contact point between the disc and ground. Next we form # the position vector from the contact point to the disc's center of mass. # Finally we form the velocity and acceleration of the disc. C = Point('C') C.set_vel(N, 0) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) # Forming the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) # Finally we form the equations of motion, using the same steps we did # before. Supply the Lagrangian, the generalized speeds. BodyD.potential_energy = -m * g * r * cos(q2) Lag = Lagrangian(N, BodyD) q = [q1, q2, q3] q1 = Function('q1') q2 = Function('q2') q3 = Function('q3') l = LagrangesMethod(Lag, q) l.form_lagranges_equations() RHS = l.rhs().as_mutable() RHS.simplify() t = symbols('t') assert tuple(l.mass_matrix[3:6]) == (0, 5 * m * r**2 / 4, 0) assert RHS[4].simplify() == ( (-8 * g * sin(q2(t)) + r * (5 * sin(2 * q2(t)) * Derivative(q1(t), t) + 12 * cos(q2(t)) * Derivative(q3(t), t)) * Derivative(q1(t), t)) / (10 * r)) assert RHS[5] == (-5 * cos(q2(t)) * Derivative(q1(t), t) + 6 * tan(q2(t)) * Derivative(q3(t), t) + 4 * Derivative(q1(t), t) / cos(q2(t))) * Derivative( q2(t), t)
def test_rolling_disc(): # Rolling Disc Example # Here the rolling disc is formed from the contact point up, removing the # need to introduce generalized speeds. Only 3 configuration and 3 # speed variables are need to describe this system, along with the # disc's mass and radius, and the local gravity. q1, q2, q3 = dynamicsymbols('q1 q2 q3') q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1) r, m, g = symbols('r m g') # The kinematics are formed by a series of simple rotations. Each simple # rotation creates a new frame, and the next rotation is defined by the new # frame's basis vectors. This example uses a 3-1-2 series of rotations, or # Z, X, Y series of rotations. Angular velocity for this is defined using # the second frame's basis (the lean frame). 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]) # This is the translational kinematics. We create a point with no velocity # in N; this is the contact point between the disc and ground. Next we form # the position vector from the contact point to the disc's center of mass. # Finally we form the velocity and acceleration of the disc. C = Point('C') C.set_vel(N, 0) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) # Forming the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) # Finally we form the equations of motion, using the same steps we did # before. Supply the Lagrangian, the generalized speeds. BodyD.set_potential_energy(- m * g * r * cos(q2)) Lag = Lagrangian(N, BodyD) q = [q1, q2, q3] q1 = Function('q1') q2 = Function('q2') q3 = Function('q3') l = LagrangesMethod(Lag, q) l.form_lagranges_equations() RHS = l.rhs() RHS.simplify() t = symbols('t') assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0]) assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t) )*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t )*Derivative(q3(t), t))/(10*r) assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t) )*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t)) )*Derivative(q2(t), t)
def test_disc_on_an_incline_plane(): # Disc rolling on an inclined plane # First the generalized coordinates are created. The mass center of the # disc is located from top vertex of the inclined plane by the generalized # coordinate 'y'. The orientation of the disc is defined by the angle # 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of # the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the # gravitational constant. y, theta = dynamicsymbols('y theta') yd, thetad = dynamicsymbols('y theta', 1) m, g, R, l, alpha = symbols('m g R l alpha') # Next, we create the inertial reference frame 'N'. A reference frame 'A' # is attached to the inclined plane. Finally a frame is created which is attached to the disk. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z]) B = A.orientnew('B', 'Axis', [-theta, A.z]) # Creating the disc 'D'; we create the point that represents the mass # center of the disc and set its velocity. The inertia dyadic of the disc # is created. Finally, we create the disc. Do = Point('Do') Do.set_vel(N, yd * A.x) I = m * R**2 / 2 * B.z | B.z D = RigidBody('D', Do, B, m, (I, Do)) # To construct the Lagrangian, 'L', of the disc, we determine its kinetic # and potential energies, T and U, respectively. L is defined as the # difference between T and U. D.set_potential_energy(m * g * (l - y) * sin(alpha)) L = Lagrangian(N, D) # We then create the list of generalized coordinates and constraint # equations. The constraint arises due to the disc rolling without slip on # on the inclined path. Also, the constraint is holonomic but we supply the # differentiated holonomic equation as the 'LagrangesMethod' class requires # that. We then invoke the 'LagrangesMethod' class and supply it the # necessary arguments and generate the equations of motion. The'rhs' method # solves for the q_double_dots (i.e. the second derivative with respect to # time of the generalized coordinates and the lagrange multiplers. q = [y, theta] coneq = [yd - R * thetad] m = LagrangesMethod(L, q, coneq) m.form_lagranges_equations() rhs = m.rhs() rhs.simplify() assert rhs[2] == 2*g*sin(alpha)/3
def test_disc_on_an_incline_plane(): # Disc rolling on an inclined plane # First the generalized coordinates are created. The mass center of the # disc is located from top vertex of the inclined plane by the generalized # coordinate 'y'. The orientation of the disc is defined by the angle # 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of # the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the # gravitational constant. y, theta = dynamicsymbols('y theta') yd, thetad = dynamicsymbols('y theta', 1) m, g, R, l, alpha = symbols('m g R l alpha') # Next, we create the inertial reference frame 'N'. A reference frame 'A' # is attached to the inclined plane. Finally a frame is created which is attached to the disk. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [pi / 2 - alpha, N.z]) B = A.orientnew('B', 'Axis', [-theta, A.z]) # Creating the disc 'D'; we create the point that represents the mass # center of the disc and set its velocity. The inertia dyadic of the disc # is created. Finally, we create the disc. Do = Point('Do') Do.set_vel(N, yd * A.x) I = m * R**2 / 2 * B.z | B.z D = RigidBody('D', Do, B, m, (I, Do)) # To construct the Lagrangian, 'L', of the disc, we determine its kinetic # and potential energies, T and U, respectively. L is defined as the # difference between T and U. D.potential_energy = m * g * (l - y) * sin(alpha) L = Lagrangian(N, D) # We then create the list of generalized coordinates and constraint # equations. The constraint arises due to the disc rolling without slip on # on the inclined path. We then invoke the 'LagrangesMethod' class and # supply it the necessary arguments and generate the equations of motion. # The'rhs' method solves for the q_double_dots (i.e. the second derivative # with respect to time of the generalized coordinates and the lagrange # multipliers. q = [y, theta] hol_coneqs = [y - R * theta] m = LagrangesMethod(L, q, hol_coneqs=hol_coneqs) m.form_lagranges_equations() rhs = m.rhs() rhs.simplify() assert rhs[2] == 2 * g * sin(alpha) / 3
def test_simp_pen(): # This tests that the equations generated by LagrangesMethod are identical # to those obtained by hand calculations. The system under consideration is # the simple pendulum. # We begin by creating the generalized coordinates as per the requirements # of LagrangesMethod. Also we created the associate symbols # that characterize the system: 'm' is the mass of the bob, l is the length # of the massless rigid rod connecting the bob to a point O fixed in the # inertial frame. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u ', 1) l, m, g = symbols('l m g') # We then create the inertial frame and a frame attached to the massless # string following which we define the inertial angular velocity of the # string. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q, N.z]) A.set_ang_vel(N, qd * N.z) # Next, we create the point O and fix it in the inertial frame. We then # locate the point P to which the bob is attached. Its corresponding # velocity is then determined by the 'two point formula'. O = Point('O') O.set_vel(N, 0) P = O.locatenew('P', l * A.x) P.v2pt_theory(O, N, A) # The 'Particle' which represents the bob is then created and its # Lagrangian generated. Pa = Particle('Pa', P, m) Pa.potential_energy = -m * g * l * cos(q) L = Lagrangian(N, Pa) # The 'LagrangesMethod' class is invoked to obtain equations of motion. lm = LagrangesMethod(L, [q]) lm.form_lagranges_equations() RHS = lm.rhs() assert RHS[1] == -g * sin(q) / l
def test_simp_pen(): # This tests that the equations generated by LagrangesMethod are identical # to those obtained by hand calculations. The system under consideration is # the simple pendulum. # We begin by creating the generalized coordinates as per the requirements # of LagrangesMethod. Also we created the associate symbols # that characterize the system: 'm' is the mass of the bob, l is the length # of the massless rigid rod connecting the bob to a point O fixed in the # inertial frame. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u ', 1) l, m, g = symbols('l m g') # We then create the inertial frame and a frame attached to the massless # string following which we define the inertial angular velocity of the # string. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q, N.z]) A.set_ang_vel(N, qd * N.z) # Next, we create the point O and fix it in the inertial frame. We then # locate the point P to which the bob is attached. Its corresponding # velocity is then determined by the 'two point formula'. O = Point('O') O.set_vel(N, 0) P = O.locatenew('P', l * A.x) P.v2pt_theory(O, N, A) # The 'Particle' which represents the bob is then created and its # Lagrangian generated. Pa = Particle('Pa', P, m) Pa.set_potential_energy(- m * g * l * cos(q)) L = Lagrangian(N, Pa) # The 'LagrangesMethod' class is invoked to obtain equations of motion. lm = LagrangesMethod(L, [q]) lm.form_lagranges_equations() RHS = lm.rhs() assert RHS[1] == -g*sin(q)/l
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)
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)) p2.set_vel(N, p2.pos_from(O).dt(N)) P1 = Particle('P1', p1, 2*m) P2 = Particle('P2', p2, m)
# 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()
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)
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)) p2.set_vel(N, p2.pos_from(O).dt(N)) P1 = Particle('P1', p1, 2 * m) P2 = Particle('P2', p2, m)
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座標 """ x_t = x.subs({q: theta, t: t_i, length: 1}).evalf() y_t = y.subs({q: theta, t: t_i, length: 1}).evalf() return x_t, y_t