def test_particle(): m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h') P = Point('P') P2 = Point('P2') p = Particle('pa', P, m) assert p.mass == m assert p.point == P # Test the mass setter p.mass = m2 assert p.mass == m2 # Test the point setter p.point = P2 assert p.point == P2 # Test the linear momentum function N = ReferenceFrame('N') O = Point('O') P2.set_pos(O, r * N.y) P2.set_vel(N, v1 * N.x) assert p.linear_momentum(N) == m2 * v1 * N.x assert p.angular_momentum(O, N) == -m2 * r *v1 * N.z P2.set_vel(N, v2 * N.y) assert p.linear_momentum(N) == m2 * v2 * N.y assert p.angular_momentum(O, N) == 0 P2.set_vel(N, v3 * N.z) assert p.linear_momentum(N) == m2 * v3 * N.z assert p.angular_momentum(O, N) == m2 * r * v3 * N.x P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z) assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z) p.set_potential_energy(m * g * h) assert p.potential_energy == m * g * h # TODO make the result not be system-dependent assert p.kinetic_energy( N) in [m2*(v1**2 + v2**2 + v3**2)/2, m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2]
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_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_potential_energy(): m, M, l1, g, h, H = symbols('m M l1 g h H') omega = dynamicsymbols('omega') N = ReferenceFrame('N') O = Point('O') O.set_vel(N, 0 * N.x) Ac = O.locatenew('Ac', l1 * N.x) P = Ac.locatenew('P', l1 * N.x) a = ReferenceFrame('a') a.set_ang_vel(N, omega * N.z) Ac.v2pt_theory(O, N, a) P.v2pt_theory(O, N, a) Pa = Particle('Pa', P, m) I = outer(N.z, N.z) A = RigidBody('A', Ac, a, M, (I, Ac)) Pa.set_potential_energy(m * g * h) A.set_potential_energy(M * g * H) assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_potential_energy(): m, M, l1, g, h, H = symbols('m M l1 g h H') omega = dynamicsymbols('omega') N = ReferenceFrame('N') O = Point('O') O.set_vel(N, 0 * N.x) Ac = O.locatenew('Ac', l1 * N.x) P = Ac.locatenew('P', l1 * N.x) a = ReferenceFrame('a') a.set_ang_vel(N, omega * N.z) Ac.v2pt_theory(O, N, a) P.v2pt_theory(O, N, a) Pa = Particle('Pa', P, m) I = outer(N.z, N.z) A = RigidBody('A', Ac, a, M, (I, Ac)) Pa.set_potential_energy(m * g * h) A.set_potential_energy(M * g * H) assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_potential_energy(): m, M, l1, g, h, H = symbols("m M l1 g h H") omega = dynamicsymbols("omega") N = ReferenceFrame("N") O = Point("O") O.set_vel(N, 0 * N.x) Ac = O.locatenew("Ac", l1 * N.x) P = Ac.locatenew("P", l1 * N.x) a = ReferenceFrame("a") a.set_ang_vel(N, omega * N.z) Ac.v2pt_theory(O, N, a) P.v2pt_theory(O, N, a) Pa = Particle("Pa", P, m) I = outer(N.z, N.z) A = RigidBody("A", Ac, a, M, (I, Ac)) Pa.set_potential_energy(m * g * h) A.set_potential_energy(M * g * H) assert potential_energy(A, Pa) == m * g * h + M * g * H
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.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_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
g] #Specified contains the matrix for the input torques specified = [ankle_torque, waist_torque] #Specifies numerical constants for inertial/mass properties #Robot Params #numerical_constants = array([1.035, # leg_length[m] # 36.754, # leg_mass[kg] # 0.85, # body_length[m] # 91.61, # body_mass[kg] # 9.81] # acceleration due to gravity [m/s^2] # ) numerical_constants = array([0.75, 7.0, 0.5, 8.0, 9.81]) #Set input torques to 0 numerical_specified = zeros(2) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, waistP, bodyP).subs(parameter_dict)) waistP.set_potential_energy(leg_mass*g*leg_length*cos(theta1)) bodyP.set_potential_energy(body_mass*g*(leg_length*cos(theta1)+body_length*cos(theta1+theta2))) pe_energy = simplify(potential_energy(waistP, bodyP).subs(parameter_dict))
numerical_constants = array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.81]) #Set input torques to 0 numerical_specified = zeros(3) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, bP, cP, dP)) bP.set_potential_energy(a_mass*g*a_length*cos(theta_a)) cP.set_potential_energy(b_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b))) dP.set_potential_energy(c_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b)+c_length*cos(theta_a+theta_b+theta_c))) pe_energy = simplify(potential_energy(bP, cP, dP).subs(parameter_dict)) forcing = simplify(kane.forcing) mass_matrix = simplify(kane.mass_matrix) zero_omega = dict(zip(speeds, zeros(3))) torques = Matrix(specified)
# 9.81] # acceleration due to gravity [m/s^2] # ) numerical_constants = array([0.75, 7.0, 0.5, 8.0, 9.81]) #Set input torques to 0 numerical_specified = zeros(2) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, twoP, threeP)) twoP.set_potential_energy(one_mass*g*one_length*cos(theta1)) threeP.set_potential_energy(two_mass*g*(one_length*cos(theta1)+two_length*cos(theta1+theta2))) pe_energy = simplify(potential_energy(twoP, threeP)) forcing = simplify(kane.forcing) mass_matrix = simplify(kane.mass_matrix) torques = Matrix(specified) zero_omega = dict(zip(speeds, zeros(2))) com = simplify(forcing.subs(zero_omega) - torques)
def test_deprecated_set_potential_energy(): m, g, h = symbols('m g h') P = Point('P') p = Particle('pa', P, m) with warns_deprecated_sympy(): p.set_potential_energy(m*g*h)
# part a r1 = s*N.x r2 = (s + l*cos(theta))*N.x + l*sin(theta)*N.y 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
# part a r1 = s * N.x r2 = (s + l * cos(theta)) * N.x + l * sin(theta) * N.y 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