def test_linearize_pendulum_lagrange_minimal(): q1 = dynamicsymbols('q1') # angle of pendulum q1d = dynamicsymbols('q1', 1) # Angular velocity 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 A = N.orientnew('A', 'axis', [q1, N.z]) A.set_ang_vel(N, q1d*N.z) # Locate point P relative to the origin N* P = pN.locatenew('P', L*A.x) P.v2pt_theory(pN, N, A) pP = Particle('pP', P, m) # Solve for eom with Lagranges method Lag = Lagrangian(N, pP) LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N) LM.form_lagranges_equations() # Linearize A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True) assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]]) assert B == Matrix([])
def test_nonminimal_pendulum(): 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) # 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() # Check solution lam1 = LM.lam_vec[0, 0] eom_sol = Matrix([[m * Derivative(q1, t, t) - 9.8 * m + 2 * lam1 * q1], [m * Derivative(q2, t, t) + 2 * lam1 * q2]]) assert LM.eom == eom_sol # Check multiplier solution lam_sol = Matrix([ (19.6 * q1 + 2 * q1d**2 + 2 * q2d**2) / (4 * q1**2 / m + 4 * q2**2 / m) ]) assert LM.solve_multipliers(sol_type='Matrix') == lam_sol
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_dub_pen(): # The system considered is the double pendulum. Like in the # test of the simple pendulum above, we begin by creating the generalized # coordinates and the simple generalized speeds and accelerations which # will be used later. Following this we create frames and points necessary # for the kinematics. The procedure isn't explicitly explained as this is # similar to the simple pendulum. Also this is documented on the pydy.org # website. q1, q2 = dynamicsymbols("q1 q2") q1d, q2d = dynamicsymbols("q1 q2", 1) q1dd, q2dd = dynamicsymbols("q1 q2", 2) u1, u2 = dynamicsymbols("u1 u2") u1d, u2d = dynamicsymbols("u1 u2", 1) l, m, g = symbols("l m g") N = ReferenceFrame("N") A = N.orientnew("A", "Axis", [q1, N.z]) B = N.orientnew("B", "Axis", [q2, N.z]) A.set_ang_vel(N, q1d * A.z) B.set_ang_vel(N, q2d * A.z) O = Point("O") P = O.locatenew("P", l * A.x) R = P.locatenew("R", l * B.x) O.set_vel(N, 0) P.v2pt_theory(O, N, A) R.v2pt_theory(P, N, B) ParP = Particle("ParP", P, m) ParR = Particle("ParR", R, m) ParP.potential_energy = -m * g * l * cos(q1) ParR.potential_energy = -m * g * l * cos(q1) - m * g * l * cos(q2) L = Lagrangian(N, ParP, ParR) lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR]) lm.form_lagranges_equations() assert (simplify(l * m * (2 * g * sin(q1) + l * sin(q1) * sin(q2) * q2dd + l * sin(q1) * cos(q2) * q2d**2 - l * sin(q2) * cos(q1) * q2d**2 + l * cos(q1) * cos(q2) * q2dd + 2 * l * q1dd) - lm.eom[0]) == 0) assert (simplify(l * m * (g * sin(q2) + l * sin(q1) * sin(q2) * q1dd - l * sin(q1) * cos(q2) * q1d**2 + l * sin(q2) * cos(q1) * q1d**2 + l * cos(q1) * cos(q2) * q1dd + l * q2dd) - lm.eom[1]) == 0) assert lm.bodies == [ParP, ParR]
def test_dub_pen(): # The system considered is the double pendulum. Like in the # test of the simple pendulum above, we begin by creating the generalized # coordinates and the simple generalized speeds and accelerations which # will be used later. Following this we create frames and points necessary # for the kinematics. The procedure isn't explicitly explained as this is # similar to the simple pendulum. Also this is documented on the pydy.org # website. q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) q1dd, q2dd = dynamicsymbols('q1 q2', 2) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) l, m, g = symbols('l m g') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = N.orientnew('B', 'Axis', [q2, N.z]) A.set_ang_vel(N, q1d * A.z) B.set_ang_vel(N, q2d * A.z) O = Point('O') P = O.locatenew('P', l * A.x) R = P.locatenew('R', l * B.x) O.set_vel(N, 0) P.v2pt_theory(O, N, A) R.v2pt_theory(P, N, B) ParP = Particle('ParP', P, m) ParR = Particle('ParR', R, m) ParP.set_potential_energy(-m * g * l * cos(q1)) ParR.set_potential_energy(-m * g * l * cos(q1) - m * g * l * cos(q2)) L = Lagrangian(N, ParP, ParR) lm = LagrangesMethod(L, [q1, q2]) lm.form_lagranges_equations() assert expand(l * m * (2 * g * sin(q1) + l * sin(q1) * sin(q2) * q2dd + l * sin(q1) * cos(q2) * q2d**2 - l * sin(q2) * cos(q1) * q2d**2 + l * cos(q1) * cos(q2) * q2dd + 2 * l * q1dd) - (simplify(lm.eom[0]))) == 0 assert expand((l * m * (g * sin(q2) + l * sin(q1) * sin(q2) * q1dd - l * sin(q1) * cos(q2) * q1d**2 + l * sin(q2) * cos(q1) * q1d**2 + l * cos(q1) * cos(q2) * q1dd + l * q2dd)) - (simplify(lm.eom[1]))) == 0
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
def test_dub_pen(): # The system considered is the double pendulum. Like in the # test of the simple pendulum above, we begin by creating the generalized # coordinates and the simple generalized speeds and accelerations which # will be used later. Following this we create frames and points necessary # for the kinematics. The procedure isn't explicitly explained as this is # similar to the simple pendulum. Also this is documented on the pydy.org # website. q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) q1dd, q2dd = dynamicsymbols('q1 q2', 2) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) l, m, g = symbols('l m g') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = N.orientnew('B', 'Axis', [q2, N.z]) A.set_ang_vel(N, q1d * A.z) B.set_ang_vel(N, q2d * A.z) O = Point('O') P = O.locatenew('P', l * A.x) R = P.locatenew('R', l * B.x) O.set_vel(N, 0) P.v2pt_theory(O, N, A) R.v2pt_theory(P, N, B) ParP = Particle('ParP', P, m) ParR = Particle('ParR', R, m) ParP.potential_energy = - m * g * l * cos(q1) ParR.potential_energy = - m * g * l * cos(q1) - m * g * l * cos(q2) L = Lagrangian(N, ParP, ParR) lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR]) lm.form_lagranges_equations() assert simplify(l*m*(2*g*sin(q1) + l*sin(q1)*sin(q2)*q2dd + l*sin(q1)*cos(q2)*q2d**2 - l*sin(q2)*cos(q1)*q2d**2 + l*cos(q1)*cos(q2)*q2dd + 2*l*q1dd) - lm.eom[0]) == 0 assert simplify(l*m*(g*sin(q2) + l*sin(q1)*sin(q2)*q1dd - l*sin(q1)*cos(q2)*q1d**2 + l*sin(q2)*cos(q1)*q1d**2 + l*cos(q1)*cos(q2)*q1dd + l*q2dd) - lm.eom[1]) == 0 assert lm.bodies == [ParP, ParR]
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_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_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
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_lagrange_2forces(): ### Equations for two damped springs in serie with two forces ### generalized coordinates qs = q1, q2 = dynamicsymbols('q1, q2') ### generalized speeds qds = q1d, q2d = dynamicsymbols('q1, q2', 1) ### Mass, spring strength, friction coefficient m, k, nu = symbols('m, k, nu') N = ReferenceFrame('N') O = Point('O') ### Two points P1 = O.locatenew('P1', q1 * N.x) P1.set_vel(N, q1d * N.x) P2 = O.locatenew('P1', q2 * N.x) P2.set_vel(N, q2d * N.x) pP1 = Particle('pP1', P1, m) pP1.potential_energy = k * q1**2 / 2 pP2 = Particle('pP2', P2, m) pP2.potential_energy = k * (q1 - q2)**2 / 2 #### Friction forces forcelist = [(P1, - nu * q1d * N.x), (P2, - nu * q2d * N.x)] lag = Lagrangian(N, pP1, pP2) l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N) l_method.form_lagranges_equations() eq1 = l_method.eom[0] assert eq1.diff(q1d) == nu eq2 = l_method.eom[1] assert eq2.diff(q2d) == nu
def test_lagrange_2forces(): ### Equations for two damped springs in serie with two forces ### generalized coordinates qs = q1, q2 = dynamicsymbols('q1, q2') ### generalized speeds qds = q1d, q2d = dynamicsymbols('q1, q2', 1) ### Mass, spring strength, friction coefficient m, k, nu = symbols('m, k, nu') N = ReferenceFrame('N') O = Point('O') ### Two points P1 = O.locatenew('P1', q1 * N.x) P1.set_vel(N, q1d * N.x) P2 = O.locatenew('P1', q2 * N.x) P2.set_vel(N, q2d * N.x) pP1 = Particle('pP1', P1, m) pP1.set_potential_energy(k * q1**2 / 2) pP2 = Particle('pP2', P2, m) pP2.set_potential_energy(k * (q1 - q2)**2 / 2) #### Friction forces forcelist = [(P1, - nu * q1d * N.x), (P2, - nu * q2d * N.x)] lag = Lagrangian(N, pP1, pP2) l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N) l_method.form_lagranges_equations() eq1 = l_method.eom[0] assert eq1.diff(q1d) == nu eq2 = l_method.eom[1] assert eq2.diff(q2d) == nu
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
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())))
(link1_frame, -b_joint*2*dq[2]*N.z), (link2_frame, b_joint*2*dq[2]*N.z), (link1_frame, -b_joint*(beta_dot)*N.z), (link3_frame, b_joint*(beta_dot)*N.z), (link2_frame, b_joint*(beta_dot)*N.z), (link4_frame, -b_joint*(beta_dot)*N.z), (link3_frame, -b_joint*2*(beta_dot - dq[2])*N.z), (link4_frame, b_joint*2*(beta_dot - dq[2])*N.z)] forces = [(com_cart, -b_cart*dq[0]*N.x), # Rolling resistance of the cart (com_cart, F*N.x)] LM = LagrangesMethod(L, q, forcelist=torques + forces, frame=N) LM.form_lagranges_equations() M_symb = LM.mass_matrix_full F_symb = LM.forcing_full fun_args = [*q, *dq, F, *sym_pars] M_func = lambdify(fun_args, M_symb, modules="scipy") F_func = lambdify(fun_args, F_symb, modules="scipy") fun_args2 = [*q, *dq, *sym_pars] T_func = lambdify(fun_args2, T, modules="scipy") V_func = lambdify(fun_args2, V, modules="scipy") A_func = lambdify(fun_args2, A.pos_from(origin).to_matrix(N).simplify()[:2], modules="scipy")
# 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()
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()))
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()))
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)) p2.set_vel(N, p2.pos_from(O).dt(N))
#setting velocity for mass point P1.set_vel(N, vx1 * N.x + vy1 * N.y) P2.set_vel(N, vx2 * N.x + vy2 * N.y) # make mass point Pa1 = Particle("Pa_1", P1, m1) Pa2 = Particle("Pa_2", P2, m2) # Potential energy Pa1.potential_energy = m1 * g * y1 Pa2.potential_energy = m2 * g * y2 # list up force applied to each mass fl = [(P1, 0 * N.x), (P2, 0 * N.x)] # make lagrangian L = Lagrangian(N, Pa1, Pa2) #generate equations LM = LagrangesMethod(L, [theta1, theta2], forcelist=fl, frame=N) me = LM.form_lagranges_equations() #mprint( simplify(me) ) print(mlatex(simplify(me))) #print( mlatex(LM.rhs()) ) #simplyfied #eq1 = simplify( LM.rhs() ) #mprint( eq1 )
import math as m from sympy import Symbol from sympy.physics.mechanics import Lagrangian, LagrangesMethod, dynamicsymbols from math import sin, cos # Define the constants m = Symbol('m') g = Symbol('g') l = Symbol('l') # Define the symbols t = Symbol('t') the1, the2, phi1, phi2 = dynamicsymbols('the1 the2 ph1, ph2') the1d, the2d, phi1d, phi2d = dynamicsymbols('the1d the2d phi1d phi2d', 1) # Define the kinetic energies (T) and the potential (V) T1 = 2*the1d**2 + the2d**2 + 2*(phi1d*sin(the1))**2 + (phi2d*sin(the2))**2 + 2*the1d*the2d*sin(the1-the2) T2 = 2*the1d*the2d*cos(the1)*cos(the2)*cos(phi1-phi2) T3 = 2*the1d*phi2d*cos(the1)*sin(the2)*sin(phi1+phi2) T4 = -2*phi1d*the2d*sin(the1)*cos(the2)*sin(phi1+phi2) T5 = 2*phi1d*phi2d*sin(the1)*sin(the2)*cos(phi1-phi2) V1 = m*g*l*(2*cos(the1) + cos(the2)) # Define the Lagrangian L = 0.5*m*l*( T1+T2+T3+T4+T5 ) + V1 LM = LagrangesMethod(L, [the1, phi1, the2, phi2]) # The equations of motions mechanics_printing(pretty_print = False) LM.form_lagranges_equations()
# 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())
def LEq(L,a): LM = LagrangesMethod(L,[a]) return LM.form_lagranges_equations()
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)) p2.set_vel(N, p2.pos_from(O).dt(N))
# 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 )
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座標 """ x_t = x.subs({q: theta, t: t_i, length: 1}).evalf() y_t = y.subs({q: theta, t: t_i, length: 1}).evalf()
def test_nonminimal_pendulum(): 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) # 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() # Check solution lam1 = LM.lam_vec[0, 0] eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1], [m*Derivative(q2, t, t) + 2*lam1*q2]]) assert LM.eom == eom_sol # Check multiplier solution lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)]) assert LM.solve_multipliers(sol_type='Matrix') == lam_sol