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_parallel_axis(): # This is for a 2 dof inverted pendulum on a cart. # This tests the parallel axis code in Kane. The inertia of the pendulum is # defined about the hinge, not about the center of mass. # Defining the constants and knowns of the system gravity = symbols('g') k, ls = symbols('k ls') a, mA, mC = symbols('a mA mC') F = dynamicsymbols('F') Ix, Iy, Iz = symbols('Ix Iy Iz') # Declaring the Generalized coordinates and speeds q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) # Creating reference frames N = ReferenceFrame('N') A = ReferenceFrame('A') A.orient(N, 'Axis', [-q2, N.z]) A.set_ang_vel(N, -u2 * N.z) # Origin of Newtonian reference frame O = Point('O') # Creating and Locating the positions of the cart, C, and the # center of mass of the pendulum, A C = O.locatenew('C', q1 * N.x) Ao = C.locatenew('Ao', a * A.y) # Defining velocities of the points O.set_vel(N, 0) C.set_vel(N, u1 * N.x) Ao.v2pt_theory(C, N, A) Cart = Particle('Cart', C, mC) Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C)) # kinematical differential equations kindiffs = [q1d - u1, q2d - u2] bodyList = [Cart, Pendulum] forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)] km=Kane(N) km.coords([q1, q2]) km.speeds([u1, u2]) km.kindiffeq(kindiffs) (fr,frstar) = km.kanes_equations(forceList, bodyList) mm = km.mass_matrix_full assert mm[3, 3] == -Iz
def test_aux(): # Same as above, except we have 2 auxiliary speeds for the ground contact # point, which is known to be zero. In one case, we go through then # substitute the aux. speeds in at the end (they are zero, as well as their # derivative), in the other case, we use the built-in auxiliary speed part # of Kane. The equations from each should be the same. q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2') u4d, u5d = dynamicsymbols('u4, u5', 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]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N))) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) Dmc.a2pt_theory(C, N, R) I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)] ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))] BodyD = RigidBody() BodyD.mc = Dmc BodyD.inertia = (I, Dmc) BodyD.frame = R BodyD.mass = m BodyList = [BodyD] KM = Kane(N) KM.coords([q1, q2, q3]) KM.speeds([u1, u2, u3, u4, u5]) KM.kindiffeq(kd) kdd = KM.kindiffdict() (fr, frstar) = KM.kanes_equations(ForceList, BodyList) fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0}) frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0}) KM2 = Kane(N) KM2.coords([q1, q2, q3]) KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5]) KM2.kindiffeq(kd) (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList) fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0}) frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0}) assert fr.expand() == fr2.expand() assert frstar.expand() == frstar2.expand()
def test_pendulum_angular_momentum(): """Consider a pendulum of length OA = 2a, of mass m as a rigid body of center of mass G (OG = a) which turn around (O,z). The angle between the reference frame R and the rod is q. The inertia of the body is I = (G,0,ma^2/3,ma^2/3). """ m, a = symbols('m, a') q = dynamicsymbols('q') R = ReferenceFrame('R') R1 = R.orientnew('R1', 'Axis', [q, R.z]) R1.set_ang_vel(R, q.diff() * R.z) I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3) O = Point('O') A = O.locatenew('A', 2*a * R1.x) G = O.locatenew('G', a * R1.x) S = RigidBody('S', G, R1, m, (I, G)) O.set_vel(R, 0) A.v2pt_theory(O, R, R1) G.v2pt_theory(O, R, R1) assert (4 * m * a**2 / 3 * q.diff() * R.z - S.angular_momentum(O, R).express(R)) == 0
def test_point_v2pt_theorys(): q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 0) O.set_vel(N, 0) assert P.v2pt_theory(O, N, B) == 0 P = O.locatenew('P', B.x) assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x) O.set_vel(N, N.x) assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
def test_aux(): # Same as above, except we have 2 auxiliary speeds for the ground contact # point, which is known to be zero. In one case, we go through then # substitute the aux. speeds in at the end (they are zero, as well as their # derivative), in the other case, we use the built-in auxiliary speed part # of KanesMethod. The equations from each should be the same. q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2') u4d, u5d = dynamicsymbols('u4, u5', 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]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) Dmc.a2pt_theory(C, N, R) I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5], kd_eqs=kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations(ForceList, BodyList) fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd, u_auxiliary=[u4, u5]) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList) fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar.simplify() frstar2.simplify() assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0]) assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_point_pos(): q = dynamicsymbols('q') N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 10 * N.x + 5 * B.x) assert P.pos_from(O) == 10 * N.x + 5 * B.x Q = P.locatenew('Q', 10 * N.y + 5 * B.y) assert Q.pos_from(P) == 10 * N.y + 5 * B.y assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.y
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_parallel_axis(): # This is for a 2 dof inverted pendulum on a cart. # This tests the parallel axis code in KanesMethod. The inertia of the # pendulum is defined about the hinge, not about the center of mass. # Defining the constants and knowns of the system gravity = symbols("g") k, ls = symbols("k ls") a, mA, mC = symbols("a mA mC") F = dynamicsymbols("F") Ix, Iy, Iz = symbols("Ix Iy Iz") # Declaring the Generalized coordinates and speeds q1, q2 = dynamicsymbols("q1 q2") q1d, q2d = dynamicsymbols("q1 q2", 1) u1, u2 = dynamicsymbols("u1 u2") u1d, u2d = dynamicsymbols("u1 u2", 1) # Creating reference frames N = ReferenceFrame("N") A = ReferenceFrame("A") A.orient(N, "Axis", [-q2, N.z]) A.set_ang_vel(N, -u2 * N.z) # Origin of Newtonian reference frame O = Point("O") # Creating and Locating the positions of the cart, C, and the # center of mass of the pendulum, A C = O.locatenew("C", q1 * N.x) Ao = C.locatenew("Ao", a * A.y) # Defining velocities of the points O.set_vel(N, 0) C.set_vel(N, u1 * N.x) Ao.v2pt_theory(C, N, A) Cart = Particle("Cart", C, mC) Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C)) # kinematical differential equations kindiffs = [q1d - u1, q2d - u2] bodyList = [Cart, Pendulum] forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)] km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = km.kanes_equations(forceList, bodyList) mm = km.mass_matrix_full assert mm[3, 3] == Iz
def test_point_a2pt_theorys(): q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) qdd = dynamicsymbols('q', 2) N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 0) O.set_vel(N, 0) assert P.a2pt_theory(O, N, B) == 0 P.set_pos(O, B.x) assert P.a2pt_theory(O, N, B) == (-qd**2) * B.x + (qdd) * B.y
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 get_equations(m_val, g_val, l_val): # This function body is copyied from: # http://www.pydy.org/examples/double_pendulum.html # Retrieved 2015-09-29 from sympy import symbols from sympy.physics.mechanics import ( dynamicsymbols, ReferenceFrame, Point, Particle, KanesMethod ) q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) 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, u1 * N.z) B.set_ang_vel(N, u2 * N.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) kd = [q1d - u1, q2d - u2] FL = [(P, m * g * N.x), (R, m * g * N.x)] BL = [ParP, ParR] KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) (fr, frstar) = KM.kanes_equations(FL, BL) kdd = KM.kindiffdict() mm = KM.mass_matrix_full fo = KM.forcing_full qudots = mm.inv() * fo qudots = qudots.subs(kdd) qudots.simplify() # Edit: depv = [q1, q2, u1, u2] subs = list(zip([m, g, l], [m_val, g_val, l_val])) return zip(depv, [expr.subs(subs) for expr in qudots])
def test_point_v1pt_theorys(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, qd * B.z) O = Point('O') P = O.locatenew('P', B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.y O.set_vel(N, N.x) assert P.v1pt_theory(O, N, B) == N.x + qd * B.y P.set_vel(B, B.z) assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
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_point_a1pt_theorys(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, qd * B.z) O = Point('O') P = O.locatenew('P', B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y P.set_vel(B, q2d * B.z) assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z O.set_vel(N, q2d * B.x) assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x + (q2d * qd + qdd) * B.y + q2dd * B.z)
def test_angular_momentum_and_linear_momentum(): m, M, l1 = symbols('m M l1') q1d = dynamicsymbols('q1d') 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, q1d * 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)) assert linear_momentum(N, A, Pa) == 2 * m * q1d* l1 * N.y + M * l1 * q1d * N.y assert angular_momentum(O, N, A, Pa) == 4 * m * q1d * l1**2 * N.z + q1d * N.z
def test_angular_momentum_and_linear_momentum(): m, M, l1 = symbols("m M l1") q1d = dynamicsymbols("q1d") 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, q1d * 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)) assert linear_momentum(N, A, Pa) == 2 * m * q1d * l1 * N.y + M * l1 * q1d * N.y assert angular_momentum(O, N, A, Pa) == 4 * m * q1d * l1 ** 2 * N.z + q1d * N.z
def test_kinetic_energy(): m, M, l1 = symbols('m M l1') 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)) assert 0 == kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2 + 2*l1**2*m*omega**2 + omega**2/2)
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.potential_energy = m * g * h A.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_kinetic_energy(): m, M, l1 = symbols("m M l1") 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)) assert 0 == kinetic_energy(N, Pa, A) - ( M * l1 ** 2 * omega ** 2 / 2 + 2 * l1 ** 2 * m * omega ** 2 + omega ** 2 / 2 )
def test_rigidbody3(): q1, q2, q3, q4 = dynamicsymbols('q1:5') p1, p2, p3 = symbols('p1:4') m = symbols('m') A = ReferenceFrame('A') B = A.orientnew('B', 'axis', [q1, A.x]) O = Point('O') O.set_vel(A, q2*A.x + q3*A.y + q4*A.z) P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z) I = outer(B.x, B.x) rb1 = RigidBody('rb1', P, B, m, (I, P)) # I_S/O = I_S/S* + I_S*/O rb2 = RigidBody('rb2', P, B, m, (I + inertia_of_point_mass(m, P.pos_from(O), B), O)) assert rb1.central_inertia == rb2.central_inertia assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') 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]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list) == [[- r*L.y, 0, L.x], [r*L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0], [cos(q2)*L.y - sin(q2)*L.z, cos(q2)*L.y - sin(q2)*L.z, 0]])
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_angular_momentum_and_linear_momentum(): """A rod with length 2l, centroidal inertia I, and mass M along with a particle of mass m fixed to the end of the rod rotate with an angular rate of omega about point O which is fixed to the non-particle end of the rod. The rod's reference frame is A and the inertial frame is N.""" m, M, l, I = symbols('m, M, l, I') omega = dynamicsymbols('omega') N = ReferenceFrame('N') a = ReferenceFrame('a') O = Point('O') Ac = O.locatenew('Ac', l * N.x) P = Ac.locatenew('P', l * N.x) O.set_vel(N, 0 * N.x) 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) A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac)) expected = 2 * m * omega * l * N.y + M * l * omega * N.y assert linear_momentum(N, A, Pa) == expected expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z assert angular_momentum(O, N, A, Pa) == expected
def test_angular_momentum_and_linear_momentum(): """A rod with length 2l, centroidal inertia I, and mass M along with a particle of mass m fixed to the end of the rod rotate with an angular rate of omega about point O which is fixed to the non-particle end of the rod. The rod's reference frame is A and the inertial frame is N.""" m, M, l, I = symbols("m, M, l, I") omega = dynamicsymbols("omega") N = ReferenceFrame("N") a = ReferenceFrame("a") O = Point("O") Ac = O.locatenew("Ac", l * N.x) P = Ac.locatenew("P", l * N.x) O.set_vel(N, 0 * N.x) 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) A = RigidBody("A", Ac, a, M, (I * outer(N.z, N.z), Ac)) expected = 2 * m * omega * l * N.y + M * l * omega * N.y assert (linear_momentum(N, A, Pa) - expected) == Vector(0) expected = (I + M * l ** 2 + 4 * m * l ** 2) * omega * N.z assert (angular_momentum(O, N, A, Pa) - expected).simplify() == Vector(0)
q0d, q1d, q2d = dynamicsymbols('q0 q1 q2', level=1) u1, u2, u3 = dynamicsymbols('u1 u2 u3') LA, LB, LP = symbols('LA LB LP') p1, p2, p3 = symbols('p1 p2 p3') g, mA, mB, mC, mD, t = symbols('g mA mB mC mD t') ## --- reference frames --- E = ReferenceFrame('E') A = E.orientnew('A', 'Axis', [q0, E.x]) B = A.orientnew('B', 'Axis', [q1, A.y]) C = B.orientnew('C', 'Axis', [0, B.x]) D = C.orientnew('D', 'Axis', [0, C.x]) ## --- points and their velocities --- pO = Point('O') pA_star = pO.locatenew('A*', LA * A.z) pP = pO.locatenew('P', LP * A.z) pB_star = pP.locatenew('B*', LB * B.z) pC_star = pB_star.locatenew('C*', q2 * B.z) pD_star = pC_star.locatenew('D*', p1 * B.x + p2 * B.y + p3 * B.z) pO.set_vel(E, 0) # Point O is fixed in Reference Frame E pA_star.v2pt_theory(pO, E, A) # Point A* is fixed in Reference Frame A pP.v2pt_theory(pO, E, A) # Point P is fixed in Reference Frame A pB_star.v2pt_theory(pP, E, B) # Point B* is fixed in Reference Frame B # Point C* is moving in Reference Frame B pC_star.set_vel(B, pC_star.pos_from(pB_star).diff(t, B)) pC_star.v1pt_theory(pB_star, E, B) pD_star.set_vel(B, pC_star.vel(B)) # Point D* is fixed rel to Point C* in B pD_star.v1pt_theory(pB_star, E, B) # Point D* is moving in Reference Frame B
IA22, IA23, IA33 = symbols('IA22 IA23 IA33') R, M, g, e, f, theta, t = symbols('R M g e f θ t') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # reference frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) # define frames for wheels B = A.orientnew('B', 'axis', [q4, A.z]) C = A.orientnew('C', 'axis', [q5, A.z]) # define points pO = Point('O') pO.set_vel(F, 0) pD = pO.locatenew('D', q2 * P.y + q3 * P.z) pD.set_vel(A, 0) pD.set_vel(F, pD.pos_from(pO).dt(F)) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) for p in [pS_star, pQ]: p.set_vel(A, 0) p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', -b * A.z) pC_star = pD.locatenew('C*', +b * A.z) for p in [pA_star, pB_star, pC_star]: p.set_vel(A, 0)
from sympy.simplify.simplify import trigsimp def msprint(expr): pr = MechanicsStrPrinter() return pr.doprint(expr) theta = symbols('theta:3') x = symbols('x:3') q = symbols('q') A = ReferenceFrame('A') B = A.orientnew('B', 'SPACE', theta, 'xyz') O = Point('O') P = O.locatenew('P', x[0] * A.x + x[1] * A.y + x[2] * A.z) p = P.pos_from(O) # From problem, point P is on L (span(B.x)) when: constraint_eqs = { x[0]: q * cos(theta[1]) * cos(theta[2]), x[1]: q * cos(theta[1]) * sin(theta[2]), x[2]: -q * sin(theta[1]) } # If point P is on line L then r^{P/O} will have no components in the B.y or # B.z directions since point O is also on line L and B.x is parallel to L. assert (trigsimp(dot(P.pos_from(O), B.y).subs(constraint_eqs)) == 0) assert (trigsimp(dot(P.pos_from(O), B.z).subs(constraint_eqs)) == 0)
theta_val = pi / 3 N = ReferenceFrame('N') #B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz') F1 = N.orientnew('F1', 'axis', [q1, N.z]) F2 = F1.orientnew('F2', 'axis', [theta, F1.x]) B = F2.orientnew('B', 'axis', [q2, F2.z]) print('rotation matrix from frame N to frame B:') C = B.dcm(N).subs(theta, theta_val) print(msprint(C)) # velocity of the disk at the point of contact with the ground is not moving # since the disk rolls without slipping. pA = Point('pA') # ball bearing A pB = pA.locatenew('pB', -R * F1.y) # ball bearing B pC = pB.locatenew('pC', -r * F2.y) # disk ground contact point ##pO.set_vel(N, 0) pA.set_vel(N, 0) pA.set_vel(F1, 0) pB.set_vel(F1, 0) pB.set_vel(B, 0) pB.v2pt_theory(pA, N, F1) pC.v2pt_theory(pB, N, B) print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ') print(pC.vel(N).express(N)) print('\ndisc is rolling without slip so v_C_N = 0')
def eval_vec(v): vs = v.subs(ab_val) return sum(dot(vs, n).evalf() * n for n in N) # For each part A, B, C, D, define an origin for that part such that the # centers of mass of each part of the component have positive N.x, N.y, # and N.z values. ## FIND CENTER OF MASS OF A vol_A_1 = pi * a**2 * b / 4 vol_A_2 = pi * a**2 * a / 4 / 2 vol_A = vol_A_1 + vol_A_2 pA_O = Point('A_O') pAs_1 = pA_O.locatenew( 'A*_1', (b / 2 * N.z + centroid_sector.subs(theta_pi_4) * sin(pi / 4) * (N.x + N.y))) pAs_2 = pA_O.locatenew('A*_2', (b * N.z + N.z * integrate( (theta * R**2 * (z)).subs(R_theta_val), (z, 0, a)) / vol_A_2 + N.x * integrate( (theta * R**2 * cos(theta) * centroid_sector).subs(R_theta_val), (z, 0, a)) / vol_A_2 + N.y * integrate( (2 * R**3 / 3 * 4 * a / pi * sin(theta)**2).subs(R, a), (theta, 0, pi / 4)) / vol_A_2)) pAs = pA_O.locatenew( 'A*', ((pAs_1.pos_from(pA_O) * vol_A_1 + pAs_2.pos_from(pA_O) * vol_A_2) / vol_A)) print('A* = {0}'.format(pAs.pos_from(pA_O))) print('A* = {0}'.format(eval_vec(pAs.pos_from(pA_O)))) ## FIND CENTER OF MASS OF B
q = dynamicsymbols('q1:6') qd = dynamicsymbols('q1:6', level=1) u = dynamicsymbols('u1:6') ## --- Define ReferenceFrames --- A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## --- Define Points and their velocities --- pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R * B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocities in A pCs.v2pt_theory(pR, A, B) pC_hat.v2pt_theory(pCs, A, C)
# Reference frames N = ReferenceFrame("N") A = N.orientnew("A", "Body", [pi, 0, q1], "123") B = A.orientnew("B", "Body", [pi / 2, 0, q2], "123") C = B.orientnew("C", "Body", [-pi / 2, 0, q3], "123") D = C.orientnew("D", "Body", [pi / 2, 0, q4], "123") E = D.orientnew("E", "Body", [-pi / 2, 0, q5], "123") F = E.orientnew("F", "Body", [pi / 2, 0, q6], "123") G = F.orientnew("G", "Body", [-pi / 2, 0, q7], "123") # The interface module's reference frame H = G.orientnew("H", "Body", [pi, 0, 0], "123") # Unit for distance: meter P0 = Point("O") P1 = P0.locatenew("P1", 0.1564 * N.z) P2 = P1.locatenew("P2", 0.0054 * A.y - 0.1284 * A.z) P3 = P2.locatenew("P3", -0.2104 * B.y - 0.0064 * B.z) P4 = P3.locatenew("P4", 0.0064 * C.y - 0.2104 * C.z) P5 = P4.locatenew("P5", -0.2084 * D.y - 0.0064 * D.z) P6 = P5.locatenew("P6", -0.1059 * E.z) P7 = P6.locatenew("P7", -0.1059 * F.y) P8 = P7.locatenew("P8", -0.0615 * G.z) # Mid-point of each link Ao_half = P1.locatenew("P1_half", 0.0054 / 2 * A.y - 0.1284 / 2 * A.z) Bo_half = P2.locatenew("P2_half", -0.2014 / 2 * B.y - 0.0064 / 2 * B.z) Co_half = P3.locatenew("P3_half", 0.0064 / 2 * C.y - 0.2104 / 2 * C.z) Do_half = P4.locatenew("P4_half", -0.2084 / 2 * D.y - 0.0064 / 2 * D.z) Eo_half = P5.locatenew("P5_half", -0.1059 / 2 * E.z) Fo_half = P6.locatenew("P6_half", -0.1059 / 2 * F.y)
def test_replace_qdots_in_force(): # Test PR 16700 "Replaces qdots with us in force-list in kanes.py" # The new functionality allows one to specify forces in qdots which will # automatically be replaced with u:s which are defined by the kde supplied # to KanesMethod. The test case is the double pendulum with interacting # forces in the example of chapter 4.7 "CONTRIBUTING INTERACTION FORCES" # in Ref. [1]. Reference list at end test function. q1, q2 = dynamicsymbols('q1, q2') qd1, qd2 = dynamicsymbols('q1, q2', level=1) u1, u2 = dynamicsymbols('u1, u2') l, m = symbols('l, m') N = ReferenceFrame('N') # Inertial frame A = N.orientnew('A', 'Axis', (q1, N.z)) # Rod A frame B = A.orientnew('B', 'Axis', (q2, N.z)) # Rod B frame O = Point('O') # Origo O.set_vel(N, 0) P = O.locatenew('P', (l * A.x)) # Point @ end of rod A P.v2pt_theory(O, N, A) Q = P.locatenew('Q', (l * B.x)) # Point @ end of rod B Q.v2pt_theory(P, N, B) Ap = Particle('Ap', P, m) Bp = Particle('Bp', Q, m) # The forces are specified below. sigma is the torsional spring stiffness # and delta is the viscous damping coefficient acting between the two # bodies. Here, we specify the viscous damper as function of qdots prior # forming the kde. In more complex systems it not might be obvious which # kde is most efficient, why it is convenient to specify viscous forces in # qdots independently of the kde. sig, delta = symbols('sigma, delta') Ta = (sig * q2 + delta * qd2) * N.z forces = [(A, Ta), (B, -Ta)] # Try different kdes. kde1 = [u1 - qd1, u2 - qd2] kde2 = [u1 - qd1, u2 - (qd1 + qd2)] KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1) fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces) KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2) fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces) # Check EOM for KM2: # Mass and force matrix from p.6 in Ref. [2] with added forces from # example of chapter 4.7 in [1] and without gravity. forcing_matrix_expected = Matrix( [[m * l**2 * sin(q2) * u2**2 + sig * q2 + delta * (u2 - u1)], [m * l**2 * sin(q2) * -u1**2 - sig * q2 - delta * (u2 - u1)]]) mass_matrix_expected = Matrix([[2 * m * l**2, m * l**2 * cos(q2)], [m * l**2 * cos(q2), m * l**2]]) assert (KM2.mass_matrix.expand() == mass_matrix_expected.expand()) assert (KM2.forcing.expand() == forcing_matrix_expected.expand()) # Check fr1 with reference fr_expected from [1] with u:s instead of qdots. fr1_expected = Matrix([0, -(sig * q2 + delta * u2)]) assert fr1.expand() == fr1_expected.expand() # Check fr2 fr2_expected = Matrix( [sig * q2 + delta * (u2 - u1), -sig * q2 - delta * (u2 - u1)]) assert fr2.expand() == fr2_expected.expand() # Specifying forces in u:s should stay the same: Ta = (sig * q2 + delta * u2) * N.z forces = [(A, Ta), (B, -Ta)] KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1) fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces) assert fr1.expand() == fr1_expected.expand() Ta = (sig * q2 + delta * (u2 - u1)) * N.z forces = [(A, Ta), (B, -Ta)] KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2) fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces) assert fr2.expand() == fr2_expected.expand() # Test if we have a qubic qdot force: Ta = (sig * q2 + delta * qd2**3) * N.z forces = [(A, Ta), (B, -Ta)] KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1) fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces) fr1_cubic_expected = Matrix([0, -(sig * q2 + delta * u2**3)]) assert fr1.expand() == fr1_cubic_expected.expand() KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2) fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces) fr2_cubic_expected = Matrix( [sig * q2 + delta * (u2 - u1)**3, -sig * q2 - delta * (u2 - u1)**3]) assert fr2.expand() == fr2_cubic_expected.expand()
def test_sub_qdot(): # This test solves exercises 8.12, 8.17 from Kane 1985 and defines # some velocities in terms of q, qdot. ## --- Declare symbols --- q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # --- Reference Frames --- F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) ## --- define points D, S*, Q on frame A and their velocities --- pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll w/o slip pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pS_star, pQ, pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # --- relate qdot, u --- # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] kde += [u1 - q1d] kde_map = solve(kde, [q1d, q2d, q3d]) for k, v in list(kde_map.items()): kde_map[k.diff(t)] = v.diff(t) # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) ## --- use kanes method --- km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2) # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2 fr_expected = Matrix([ f * Q3 + M * g * e * sin(theta) * cos(q1), Q2 + M * g * sin(theta) * sin(q1), e * M * g * cos(theta) - Q1 * f - Q2 * R ]) #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))]) fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) fr, fr_star = km.kanes_equations(forces, bodies) assert (fr.expand() == fr_expected.expand()) assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
def test_non_central_inertia(): # This tests that the calculation of Fr* does not depend the point # about which the inertia of a rigid body is defined. This test solves # exercises 8.12, 8.17 from Kane 1985. # Declare symbols q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3, u4, u5 = dynamicsymbols('u1:6') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') Q1, Q2, Q3 = symbols('Q1, Q2 Q3') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') # Reference Frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) B.set_ang_vel(A, u4 * A.z) C.set_ang_vel(A, u5 * A.z) # define points D, S*, Q on frame A and their velocities pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll without slip. pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) for p in [pS_star, pQ]: p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # the velocities of B^, C^ are zero since B, C are assumed to roll without slip #kde = [dot(p.vel(F), b) for b in A for p in [pB_hat, pC_hat]] kde = [q1d - u1, q2d - u4, q3d - u5] vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde, u_dependent=[u4, u5], velocity_constraints=vc, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] fr, fr_star = km.kanes_equations(forces, bodies) vc_map = solve(vc, [u4, u5]) # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985. fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) assert (trigsimp(fr_star.subs(vc_map).subs( u3, 0)).doit().expand() == fr_star_expected.expand()) # define inertias of rigid bodies A, B, C about point D # I_S/O = I_S/S* + I_S*/O bodies2 = [] for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]): I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD), rb.frame) bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass, (I, pD))) fr2, fr_star2 = km.kanes_equations(forces, bodies2) assert (trigsimp(fr_star2.subs(vc_map).subs( u3, 0)).doit().expand() == fr_star_expected.expand())
from sympy.physics.mechanics import msprint from sympy.physics.mechanics import Point, Particle from sympy.physics.mechanics import Lagrangian, LagrangesMethod from sympy import symbols, sin, cos s, theta, h = dynamicsymbols('s θ h') # s, theta, h sd, thetad, hd = dynamicsymbols('s θ h', 1) m, g, l = symbols('m g l') N = ReferenceFrame('N') # 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)))
comb_implicit_mat = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 1, 0, -x / m], [0, 0, 0, 1, -y / m], [0, 0, 0, 0, l**2 / m]]) comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g * y]) kin_explicit_rhs = Matrix([u, v]) comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs) # Set up a body and load to pass into the system theta = atan(x / y) N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [theta, N.z]) O = Point('O') P = O.locatenew('P', l * A.x) Pa = Particle('Pa', P, m) bodies = [Pa] loads = [(P, g * m * N.x)] # Set up some output equations to be given to SymbolicSystem # Change to make these fit the pendulum PE = symbols("PE") out_eqns = {PE: m * g * (l + y)} # Set up remaining arguments that can be passed to SymbolicSystem alg_con = [2] alg_con_full = [4] coordinates = (x, y, lam)
q1, q2, q3, q4, q5 = q = dynamicsymbols('q1:6') qd = dynamicsymbols('q1:6', level=1) u1, u2, u3, u4, u5 = u = dynamicsymbols('u1:6') # reference frames A = ReferenceFrame('A') 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]) # points, velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q4 * A.x + q5 * A.y) pR.set_vel(A, pR.pos_from(pO).dt(A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pC_star = pC_hat.locatenew('C*', R * B.y) pC_star.set_vel(C, 0) pC_star.set_vel(B, 0) # calculate velocities in A pC_star.v2pt_theory(pR, A, B) pC_hat.v2pt_theory(pC_star, A, C)
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint m, a, b = symbols('m a b') q1, q2 = dynamicsymbols('q1, q2') q1d, q2d = dynamicsymbols('q1, q2', level=1) # reference frames # N.x parallel to horizontal line, N.y parallel to line AC N = ReferenceFrame('N') A = N.orientnew('A', 'axis', [-q1, N.y]) B = A.orientnew('B', 'axis', [-q2, A.x]) # points B*, O pO = Point('O') pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y)) pO.set_vel(N, 0) pB_star.v2pt_theory(pO, N, B) # rigidbody B I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18) rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star)) # kinetic energy K = rbB.kinetic_energy(N) print('K = {0}'.format(msprint(trigsimp(K)))) K_expected = m / 4 * ((a**2 + b**2 * sin(q2)**2 / 3) * q1d**2 + a * b * cos(q2) * q1d * q2d + b**2 * q2d**2 / 3) print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected))))) assert expand(trigsimp(K - K_expected)) == 0
# reference frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) # points D, S*, Q on frame A and their velocities pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll without slip. pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) for p in [pS_star, pQ]: p.set_vel(A, 0) p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pA_star, pB_star, pC_star]: p.set_vel(A, 0) p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x)
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 three # speed variables are need to describe this system, along with the disc's # mass and radius, and the local graivty (note that mass will drop out). q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 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]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N))) # 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 mass center. # 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) Dmc.a2pt_theory(C, N, R) # This is a simple way to form the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) # Kinematic differential equations; how the generalized coordinate time # derivatives relate to generalized speeds. kd = [q1d - u3 / cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)] # Creation of the force list; it is the gravitational force at the mass # center of the disc. Then we create the disc by assigning a Point to the # mass center attribute, a ReferenceFrame to the frame attribute, and mass # and inertia. Then we form the body list. ForceList = [(Dmc, -m * g * Y.z)] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] # Finally we form the equations of motion, using the same steps we did # before. Specify inertial frame, supply generalized speeds, supply # kinematic differential equation dictionary, compute Fr from the force # list and Fr* fromt the body list, compute the mass matrix and forcing # terms, then solve for the u dots (time derivatives of the generalized # speeds). KM = Kane(N) KM.coords([q1, q2, q3]) KM.speeds([u1, u2, u3]) KM.kindiffeq(kd) KM.kanes_equations(ForceList, BodyList) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing kdd = KM.kindiffdict() rhs = rhs.subs(kdd) assert rhs.expand() == Matrix([ (10 * u2 * u3 * r - 5 * u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2)) ]).expand()
def test_sub_qdot2(): # This test solves exercises 8.3 from Kane 1985 and defines # all velocities in terms of q, qdot. We check that the generalized active # forces are correctly computed if u terms are only defined in the # kinematic differential equations. # # This functionality was added in PR 8948. Without qdot/u substitution, the # KanesMethod constructor will fail during the constraint initialization as # the B matrix will be poorly formed and inversion of the dependent part # will fail. g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t') q = dynamicsymbols('q:5') qd = dynamicsymbols('q:5', level=1) u = dynamicsymbols('u:5') ## Define inertial, intermediate, and rigid body reference frames A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## Define points of interest and their velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R * B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocites of points C* and C^ in frame A pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C ## Define forces on each point of the system R_C_hat = Px * A.x + Py * A.y + Pz * A.z R_Cs = -m * g * A.z forces = [(pC_hat, R_C_hat), (pCs, R_Cs)] ## Define kinematic differential equations # let ui = omega_C_A & bi (i = 1, 2, 3) # u4 = qd4, u5 = qd5 u_expr = [C.ang_vel_in(A) & uv for uv in B] u_expr += qd[3:] kde = [ui - e for ui, e in zip(u, u_expr)] km1 = KanesMethod(A, q, u, kde) fr1, _ = km1.kanes_equations([], forces) ## Calculate generalized active forces if we impose the condition that the # disk C is rolling without slipping u_indep = u[:3] u_dep = list(set(u) - set(u_indep)) vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]] km2 = KanesMethod(A, q, u_indep, kde, u_dependent=u_dep, velocity_constraints=vc) fr2, _ = km2.kanes_equations([], forces) fr1_expected = Matrix([ -R * g * m * sin(q[1]), -R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]), R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py ]) fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0]) assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand())) assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
class TestVisualizationFrameScene(object): def __init__(self): #We define some quantities required for tests here.. self.p = dynamicsymbols('p:3') self.q = dynamicsymbols('q:3') self.dynamic = list(self.p) + list(self.q) self.states = [radians(45) for x in self.p] + \ [radians(30) for x in self.q] self.I = ReferenceFrame('I') self.A = self.I.orientnew('A', 'space', self.p, 'XYZ') self.B = self.A.orientnew('B', 'space', self.q, 'XYZ') self.O = Point('O') self.P1 = self.O.locatenew('P1', 10 * self.I.x + \ 10 * self.I.y + 10 * self.I.z) self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \ 10 * self.I.y + 10 * self.I.z) self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]] self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]] self.shape1 = Cylinder() self.shape2 = Cylinder() self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz') self.mass = symbols('mass') self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass] self.param_vals = [0, 0, 0, 0] self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz) self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \ self.mass, (self.inertia, self.P1)) self.global_frame1 = VisualizationFrame('global_frame1', \ self.A, self.P1, self.shape1) self.global_frame2 = VisualizationFrame('global_frame2', \ self.B, self.P2, self.shape2) self.scene1 = Scene(self.I, self.O, \ (self.global_frame1, self.global_frame2), \ name='scene') self.particle = Particle('particle1', self.P1, self.mass) #To make it more readable p = self.p q = self.q #Here is the dragon .. self.transformation_matrix = \ [[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \ [sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \ sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \ sin(p[0])*cos(p[1]), 0], \ [sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \ -sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \ cos(p[0])*cos(p[1]), 0], \ [10, 10, 10, 1]] def test_vframe_with_rframe(self): self.frame1 = VisualizationFrame('frame1', self.I, self.O, \ self.shape1) assert self.frame1.name == 'frame1' assert self.frame1.reference_frame == self.I assert self.frame1.origin == self.O assert self.frame1.shape is self.shape1 self.frame1.name = 'frame1_' assert self.frame1.name == 'frame1_' self.frame1.reference_frame = self.A assert self.frame1.reference_frame == self.A self.frame1.origin = self.P1 assert self.frame1.origin == self.P1 self.frame1.shape = self.shape2 assert self.frame1.shape is self.shape2 assert self.frame1.generate_transformation_matrix(self.I, self.O).tolist() == \ self.transformation_matrix def test_vframe_with_rbody(self): self.frame2 = VisualizationFrame('frame2', self.rigid_body, \ self.shape1) assert self.frame2.name == 'frame2' assert self.frame2.reference_frame == self.A assert self.frame2.origin == self.P1 assert self.frame2.shape == self.shape1 self.frame2.name = 'frame2_' assert self.frame2.name == 'frame2_' self.frame2.reference_frame = self.B assert self.frame2.reference_frame == self.B self.frame2.origin = self.P2 assert self.frame2.origin == self.P2 self.frame2.shape = self.shape2 assert self.frame2.shape is self.shape2 self.frame2.reference_frame = self.A self.frame2.origin = self.P1 assert self.frame2.generate_transformation_matrix(self.I, self.O).tolist() == \ self.transformation_matrix def test_vframe_with_particle(self): self.frame3 = VisualizationFrame('frame3', \ self.A, self.particle, \ self.shape1) assert self.frame3.name == 'frame3' assert self.frame3.reference_frame == self.A assert self.frame3.origin == self.P1 assert self.frame3.shape is self.shape1 self.frame3.name = 'frame3_' assert self.frame3.name == 'frame3_' self.frame3.reference_frame = self.B assert self.frame3.reference_frame == self.B self.frame3.origin = self.P2 assert self.frame3.origin == self.P2 self.frame3.shape = self.shape2 assert self.frame3.shape is self.shape2 self.frame3.reference_frame = self.A self.frame3.origin = self.P1 assert self.frame3.generate_transformation_matrix(self.I, self.O).tolist() == \ self.transformation_matrix def test_vframe_without_name(self): self.frame4 = VisualizationFrame(self.I, self.O, \ self.shape1) assert self.frame4.name == 'unnamed' #To check if referenceframe and origin are defined #properly without name arg assert self.frame4.reference_frame == self.I assert self.frame4.origin == self.O assert self.frame4.shape is self.shape1 self.frame4.name = 'frame1_' assert self.frame4.name == 'frame1_' def test_numeric_transform(self): self.list1 = [[0.5000000000000001, 0.5, \ -0.7071067811865475, 0.0, \ -0.14644660940672627, 0.8535533905932737, \ 0.5, 0.0, \ 0.8535533905932737, -0.14644660940672627, \ 0.5000000000000001, 0.0, \ 10.0, 10.0, 10.0, 1.0]] self.list2 = [[-0.11518993731879767, 0.8178227645734215, \ -0.563823734943801, 0.0, \ 0.1332055011661179, 0.5751927992738988, \ 0.8070994598700584, 0.0, \ 0.984371663956036, 0.017865313009926137, \ -0.17519491371464685, 0.0, \ 20.0, 20.0, 20.0, 1.0]] self.global_frame1.generate_transformation_matrix(self.I, self.O) self.global_frame1.generate_numeric_transform_function(self.dynamic, \ self.parameters) assert_allclose(self.global_frame1.\ evaluate_transformation_matrix(self.states, \ self.param_vals).tolist(), self.list1) self.global_frame2.generate_transformation_matrix(self.I, self.O) self.global_frame2.generate_numeric_transform_function(self.dynamic, \ self.parameters) assert_allclose(self.global_frame2.\ evaluate_transformation_matrix(self.states, \ self.param_vals).tolist(), self.list2) def test_perspective_camera(self): #Camera is a subclass of VisualizationFrame, but without any #specific shape attached. We supply only ReferenceFrame,Point #to camera. and it inherits methods from VisualizationFrame #Testing with rigid-body .. camera = PerspectiveCamera('camera', self.rigid_body, fov=45, \ near=1, far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.A assert camera.origin == self.P1 assert camera.fov == 45 assert camera.near == 1 assert camera.far == 1000 #Testing with reference_frame, particle .. camera = PerspectiveCamera('camera', self.I, self.particle, \ fov=45, near=1, far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.I assert camera.origin == self.P1 assert camera.fov == 45 assert camera.near == 1 assert camera.far == 1000 #Testing with reference_frame, point .. camera = PerspectiveCamera('camera', self.I, self.O, \ fov=45, near=1, far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.I assert camera.origin == self.O assert camera.fov == 45 assert camera.near == 1 assert camera.far == 1000 camera.name = 'camera1' assert camera.name == 'camera1' assert camera.__str__() == 'PerspectiveCamera: camera1' assert camera.__repr__() == 'PerspectiveCamera' camera.reference_frame = self.A assert camera.reference_frame == self.A camera.origin = self.P1 assert camera.origin == self.P1 camera.fov = 30 assert camera.fov == 30 camera.near = 10 assert camera.near == 10 camera.far = 500 assert camera.far == 500 #Test unnamed camera1 = PerspectiveCamera(self.I, self.O) assert camera1.name == 'unnamed' assert camera1.reference_frame == self.I assert camera1.origin == self.O assert camera1.fov == 45 assert camera1.near == 1 assert camera1.far == 1000 def test_orthographic_camera(self): #As compared to Perspective Camera, Orthographic Camera doesnt #have fov, instead the left,right,top and bottom faces are #adjusted by the Scene width, and height #Testing with rigid_body camera = OrthoGraphicCamera('camera', self.rigid_body, \ near=1, far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.A assert camera.origin == self.P1 assert camera.near == 1 assert camera.far == 1000 #Testing with reference_frame, particle camera = OrthoGraphicCamera('camera', self.I, self.particle, \ near=1, far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.I assert camera.origin == self.P1 assert camera.near == 1 assert camera.far == 1000 #Testing with reference_frame, point ... camera = OrthoGraphicCamera('camera', self.I, self.O, near=1, \ far=1000) assert camera.name == 'camera' assert camera.reference_frame == self.I assert camera.origin == self.O assert camera.near == 1 assert camera.far == 1000 camera.name = 'camera1' assert camera.name == 'camera1' assert camera.__str__() == 'OrthoGraphicCamera: camera1' assert camera.__repr__() == 'OrthoGraphicCamera' camera.reference_frame = self.A assert camera.reference_frame == self.A camera.origin = self.P1 assert camera.origin == self.P1 camera.near = 10 assert camera.near == 10 camera.far = 500 assert camera.far == 500 camera1 = OrthoGraphicCamera(self.I, self.O) assert camera1.name == 'unnamed' assert camera1.reference_frame == self.I assert camera1.origin == self.O assert camera1.near == 1 assert camera1.far == 1000 def test_point_light(self): #Testing with rigid-body .. light = PointLight('light', self.rigid_body, color='blue') assert light.name == 'light' assert light.reference_frame == self.A assert light.origin == self.P1 assert light.color == 'blue' #Testing with reference_frame, particle .. light = PointLight('light', self.I, self.particle, color='blue') assert light.name == 'light' assert light.reference_frame == self.I assert light.origin == self.P1 assert light.color == 'blue' #Testing with reference_frame, point .. light = PointLight('light', self.I, self.O, color='blue') assert light.name == 'light' assert light.reference_frame == self.I assert light.origin == self.O assert light.color == 'blue' light.name = 'light1' assert light.name == 'light1' assert light.__str__() == 'PointLight: light1' assert light.__repr__() == 'PointLight' light.reference_frame = self.A assert light.reference_frame == self.A light.origin = self.P1 assert light.origin == self.P1 light.color = 'red' assert light.color == 'red' #Test unnamed light1 = PointLight(self.I, self.O) assert light1.name == 'unnamed' assert light1.reference_frame == self.I assert light1.origin == self.O assert light1.color == 'white' def test_scene_init(self): self.scene2 = Scene(self.I, self.O, \ self.global_frame1, self.global_frame2, \ name='scene') assert self.scene2.name == 'scene' assert self.scene2.reference_frame == self.I assert self.scene2.origin == self.O assert self.scene2.visualization_frames[0] is self.global_frame1 assert self.scene2.visualization_frames[1] is self.global_frame2 self.scene2.name = 'scene1' assert self.scene2.name == 'scene1' self.scene2.reference_frame = self.A assert self.scene2.reference_frame == self.A
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols("q1:3") q1d, q2d = dynamicsymbols("q1:3", level=1) u1, u2 = dynamicsymbols("u1:3") u1d, u2d = dynamicsymbols("u1: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]) # Locate the pendulum mass P = pN.locatenew("P1", q1 * N.x + q2 * N.y) pP = Particle("pP", P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m * g * N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod( N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde, ) with warns_deprecated_sympy(): (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize( op_point=[q_op, u_op, ud_op], A_and_B=True, simplify=True ) assert A.expand() == Matrix([[0, 1], [-9.8 / L, 0]]) assert B == Matrix([])
q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') g, m, L, t = symbols('g m L t') Q, R, S = symbols('Q R S') # --- ReferenceFrames --- N = ReferenceFrame('N') # --- Define Points and set their velocities --- # Simplify the system to 7 points, where each point is the aggregations of # rods that are parallel horizontally. pO = Point('O') pO.set_vel(N, 0) pP1 = pO.locatenew('P1', L / 2 * (cos(q1) * N.x + sin(q1) * N.y)) pP2 = pP1.locatenew('P2', L / 2 * (cos(q1) * N.x + sin(q1) * N.y)) pP3 = pP2.locatenew('P3', L / 2 * (cos(q2) * N.x - sin(q2) * N.y)) pP4 = pP3.locatenew('P4', L / 2 * (cos(q2) * N.x - sin(q2) * N.y)) pP5 = pP4.locatenew('P5', L / 2 * (cos(q3) * N.x + sin(q3) * N.y)) pP6 = pP5.locatenew('P6', L / 2 * (cos(q3) * N.x + sin(q3) * N.y)) for p in [pP1, pP2, pP3, pP4, pP5, pP6]: p.set_vel(N, p.pos_from(pO).diff(t, N)) ## --- Define kinematic differential equations/pseudo-generalized speeds --- kde = [u1 - L * q1d, u2 - L * q2d, u3 - L * q3d] kde_map = solve(kde, [q1d, q2d, q3d]) ## --- Define contact/distance forces --- forces = [(pP1, 6 * m * g * N.x), (pP2, S * N.y + 5 * m * g * N.x),
def test_linearize_rolling_disc_kane(): # Symbols for time and constant parameters t, r, m, g, v = symbols("t r m g v") # Configuration variables and their time derivatives q1, q2, q3, q4, q5, q6 = q = dynamicsymbols("q1:7") q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q] # Generalized speeds and their time derivatives u = dynamicsymbols("u:6") u1, u2, u3, u4, u5, u6 = u = dynamicsymbols("u1:7") u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u] # Reference frames N = ReferenceFrame("N") # Inertial frame NO = Point("NO") # Inertial origin A = N.orientnew("A", "Axis", [q1, N.z]) # Yaw intermediate frame B = A.orientnew("B", "Axis", [q2, A.x]) # Lean intermediate frame C = B.orientnew("C", "Axis", [q3, B.y]) # Disc fixed frame CO = NO.locatenew("CO", q4 * N.x + q5 * N.y + q6 * N.z) # Disc center # Disc angular velocity in N expressed using time derivatives of coordinates w_c_n_qd = C.ang_vel_in(N) w_b_n_qd = B.ang_vel_in(N) # Inertial angular velocity and angular acceleration of disc fixed frame C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z) # Disc center velocity in N expressed using time derivatives of coordinates v_co_n_qd = CO.pos_from(NO).dt(N) # Disc center velocity in N expressed using generalized speeds CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z) # Disc Ground Contact Point P = CO.locatenew("P", r * B.z) P.v2pt_theory(CO, N, C) # Configuration constraint f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)]) # Velocity level constraints f_v = Matrix([dot(P.vel(N), uv) for uv in C]) # Kinematic differential equations kindiffs = Matrix( [dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_co_n_qd - CO.vel(N), uv) for uv in N] ) qdots = solve(kindiffs, qd) # Set angular velocity of remaining frames B.set_ang_vel(N, w_b_n_qd.subs(qdots)) C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Active forces F_CO = m * g * A.z # Create inertia dyadic of disc C about point CO I = (m * r ** 2) / 4 J = (m * r ** 2) / 2 I_C_CO = inertia(C, I, J, I) Disc = RigidBody("Disc", CO, C, m, (I_C_CO, CO)) BL = [Disc] FL = [(CO, F_CO)] KM = KanesMethod( N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs, q_dependent=[q6], configuration_constraints=f_c, u_dependent=[u4, u5, u6], velocity_constraints=f_v, ) with warns_deprecated_sympy(): (fr, fr_star) = KM.kanes_equations(FL, BL) # Test generalized form equations linearizer = KM.to_linearizer() assert linearizer.f_c == f_c assert linearizer.f_v == f_v assert linearizer.f_a == f_v.diff(t).subs(KM.kindiffdict()) sol = solve(linearizer.f_0 + linearizer.f_1, qd) for qi in qdots.keys(): assert sol[qi] == qdots[qi] assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0]) # Perform the linearization # Precomputed operating point q_op = {q6: -r * cos(q2)} u_op = { u1: 0, u2: sin(q2) * q1d + q3d, u3: cos(q2) * q1d, u4: -r * (sin(q2) * q1d + q3d) * cos(q3), u5: 0, u6: -r * (sin(q2) * q1d + q3d) * sin(q3), } qd_op = { q2d: 0, q4d: -r * (sin(q2) * q1d + q3d) * cos(q1), q5d: -r * (sin(q2) * q1d + q3d) * sin(q1), q6d: 0, } ud_op = { u1d: 4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d ** 2 / 2 + 6 * cos(q2) * q1d * q3d / 5, u2d: 0, u3d: 0, u4d: r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d ** 2), u5d: r * ( 4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d ** 2 / 2 + 6 * cos(q2) * q1d * q3d / 5 ), u6d: -r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d ** 2), } A, B = linearizer.linearize( op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True ) upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1} # Precomputed solution A_sol = Matrix( [ [0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0], [-cos(q1) * q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0], [0, Rational(4, 5), 0, 0, 0, 0, 0, 6 * q3d / 5], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0], ] ) B_sol = Matrix([]) # Check that linearization is correct assert A.subs(upright_nominal) == A_sol assert B.subs(upright_nominal) == B_sol # Check eigenvalues at critical speed are all zero: assert sympify(A.subs(upright_nominal).subs(q3d, 1 / sqrt(3))).eigenvals() == {0: 8}
#!/usr/bin/env python # -*- coding: utf-8 -*- """Exercise 7.8 from Kane 1985.""" from __future__ import division from sympy import acos, pi, symbols from sympy.physics.mechanics import ReferenceFrame, Point from sympy.physics.mechanics import cross, dot # vectors A, B have equal magnitude 10 N alpha = 10 # [N] N = ReferenceFrame('N') pO = Point('O') pS = pO.locatenew('S', 10 * N.x + 5 * N.z) pR = pO.locatenew('R', 10 * N.x + 12 * N.y) pQ = pO.locatenew('Q', 12 * N.y + 10 * N.z) pP = pO.locatenew('P', 4 * N.x + 7 * N.z) A = alpha * pQ.pos_from(pP).normalize() B = alpha * pS.pos_from(pR).normalize() R = A + B M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B) Ms = dot(R, M) * R / dot(R, R) ps = cross(R, M) / dot(R, R) # Replace set S (vectors A, B) with a wrench consisting of the resultant of S, # placed on the central axis of S and a couple whose torque is equal to S's # moment of minimum magnitude. r = R C = Ms
u1 = dynamicsymbols('u1') g, m, r, theta = symbols('g m r θ') omega, t = symbols('ω t') # reference frames M = ReferenceFrame('M') # Plane containing W rotates about a vertical line through center of W. N = M.orientnew('N', 'Axis', [omega * t, M.z]) C = N.orientnew('C', 'Axis', [q1, N.x]) R = C # R is fixed relative to C A = C.orientnew('A', 'Axis', [-theta, R.x]) B = C.orientnew('B', 'Axis', [theta, R.x]) # points, velocities pO = Point('O') # Point O is at the center of the circular wire pA = pO.locatenew('A', -r * A.z) pB = pO.locatenew('B', -r * B.z) pR_star = pO.locatenew('R*', 1 / S(2) * (pA.pos_from(pO) + pB.pos_from(pO))) pO.set_vel(N, 0) pO.set_vel(C, 0) for p in [pA, pB, pR_star]: p.set_vel(C, 0) p.v1pt_theory(pO, N, C) # kinematic differential equations kde = [u1 - q1d] kde_map = solve(kde, [q1d]) # contact/distance forces forces = [(pR_star, -m * g * N.z)]
from sympy.physics.mechanics import inertia, msprint from sympy.physics.mechanics import Point, RigidBody from sympy import pi, solve, symbols, simplify from sympy import acos, sin, cos # 2a q1 = dynamicsymbols('q1') px, py, pz = symbols('px py pz', real=True, positive=True) sx, sy, sz = symbols('sx sy sz', real=True, sositive=True) m, g, l0, k = symbols('m g l0 k', real=True, positive=True) Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True) N = ReferenceFrame('N') B = N.orientnew('B', 'axis', [q1, N.z]) pA = Point('A') pA.set_vel(N, 0) pP = pA.locatenew('P', l0 * N.y - 2 * l0 * N.z) pS = pP.locatenew('S', -px * B.x - pz * B.z) I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0) rb = RigidBody('plane', pS, B, m, (I, pS)) H = rb.angular_momentum(pS, N) print('H about S in frame N = {}'.format(msprint(H))) print('dH/dt = {}'.format(msprint(H.dt(N)))) print('a_S_N = {}'.format(pS.acc(N)))
g, m1, m2, k, L, omega, t = symbols('g m1 m2 k L ω t') q1, q2, q3 = dynamicsymbols('q1:4') qd1, qd2, qd3 = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') ## --- Define ReferenceFrames --- A = ReferenceFrame('A') B = A.orientnew('B', 'Axis', [omega * t, A.y]) E = B.orientnew('E', 'Axis', [q3, B.z]) ## --- Define Points and their velocities --- pO = Point('O') pO.set_vel(A, 0) pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y) pD_star = pP1.locatenew('D*', L * E.x) pP1.set_vel(B, pP1.pos_from(pO).dt(B)) pD_star.v2pt_theory(pP1, B, E) pP1.v1pt_theory(pO, A, B) pD_star.v2pt_theory(pP1, A, E) ## --- Expressions for generalized speeds u1, u2, u3 --- kde = [ u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - dot(E.ang_vel_in(B), E.z) ] kde_map = solve(kde, [qd1, qd2, qd3])
r, theta, b = symbols('r θ b', real=True, positive=True) # define reference frames R = ReferenceFrame('R') # fixed race rf, let R.z point upwards A = R.orientnew('A', 'axis', [q7, R.z]) # rf that rotates with S* about R.z # B.x, B.z are parallel with face of cone, B.y is perpendicular B = A.orientnew('B', 'axis', [-theta, A.x]) S = ReferenceFrame('S') S.set_ang_vel(A, u1 * A.x + u2 * A.y + u3 * A.z) C = ReferenceFrame('C') C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z) # define points pO = Point('O') pS_star = pO.locatenew('S*', b * A.y) pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone pS1 = pS_star.locatenew('S1', -r * A.z) # S1 touches horizontal wall of the race pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) # Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A)
def test_bicycle(): if ON_TRAVIS: skip("Too slow for travis.") # Code to get equations of motion for a bicycle modeled as in: # J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized # dynamics equations for the balance and steer of a bicycle: a benchmark # and review. Proceedings of The Royal Society (2007) 463, 1955-1982 # doi: 10.1098/rspa.2007.1857 # Note that this code has been crudely ported from Autolev, which is the # reason for some of the unusual naming conventions. It was purposefully as # similar as possible in order to aide debugging. # Declare Coordinates & Speeds # Simple definitions for qdots - qd = u # Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame # ang. rate (spinning motion), frame ang. rate (pitching motion), steering # frame ang. rate, and front wheel ang. rate (spinning motion). # Wheel positions are ignorable coordinates, so they are not introduced. q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5') q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1) u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6') u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1) # Declare System's Parameters WFrad, WRrad, htangle, forkoffset = symbols( 'WFrad WRrad htangle forkoffset') forklength, framelength, forkcg1 = symbols( 'forklength framelength forkcg1') forkcg3, framecg1, framecg3, Iwr11 = symbols( 'forkcg3 framecg1 framecg3 Iwr11') Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11') Iframe22, Iframe33, Iframe31, Ifork11 = symbols( 'Iframe22 Iframe33 Iframe31 Ifork11') Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g') mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr') # Set up reference frames for the system # N - inertial # Y - yaw # R - roll # WR - rear wheel, rotation angle is ignorable coordinate so not oriented # Frame - bicycle frame # TempFrame - statically rotated frame for easier reference inertia definition # Fork - bicycle fork # TempFork - statically rotated frame for easier reference inertia definition # WF - front wheel, again posses a ignorable coordinate N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) R = Y.orientnew('R', 'Axis', [q2, Y.x]) Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y]) WR = ReferenceFrame('WR') TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y]) Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x]) TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y]) WF = ReferenceFrame('WF') # Kinematics of the Bicycle First block of code is forming the positions of # the relevant points # rear wheel contact -> rear wheel mass center -> frame mass center + # frame/fork connection -> fork mass center + front wheel mass center -> # front wheel contact point WR_cont = Point('WR_cont') WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z) Steer = WR_mc.locatenew('Steer', framelength * Frame.z) Frame_mc = WR_mc.locatenew('Frame_mc', -framecg1 * Frame.x + framecg3 * Frame.z) Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z) WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z) WF_cont = WF_mc.locatenew( 'WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize()) # Set the angular velocity of each frame. # Angular accelerations end up being calculated automatically by # differentiating the angular velocities when first needed. # u1 is yaw rate # u2 is roll rate # u3 is rear wheel rate # u4 is frame pitch rate # u5 is fork steer rate # u6 is front wheel rate Y.set_ang_vel(N, u1 * Y.z) R.set_ang_vel(Y, u2 * R.x) WR.set_ang_vel(Frame, u3 * Frame.y) Frame.set_ang_vel(R, u4 * Frame.y) Fork.set_ang_vel(Frame, u5 * Fork.x) WF.set_ang_vel(Fork, u6 * Fork.y) # Form the velocities of the previously defined points, using the 2 - point # theorem (written out by hand here). Accelerations again are calculated # automatically when first needed. WR_cont.set_vel(N, 0) WR_mc.v2pt_theory(WR_cont, N, WR) Steer.v2pt_theory(WR_mc, N, Frame) Frame_mc.v2pt_theory(WR_mc, N, Frame) Fork_mc.v2pt_theory(Steer, N, Fork) WF_mc.v2pt_theory(Steer, N, Fork) WF_cont.v2pt_theory(WF_mc, N, WF) # Sets the inertias of each body. Uses the inertia frame to construct the # inertia dyadics. Wheel inertias are only defined by principle moments of # inertia, and are in fact constant in the frame and fork reference frames; # it is for this reason that the orientations of the wheels does not need # to be defined. The frame and fork inertias are defined in the 'Temp' # frames which are fixed to the appropriate body frames; this is to allow # easier input of the reference values of the benchmark paper. Note that # due to slightly different orientations, the products of inertia need to # have their signs flipped; this is done later when entering the numerical # value. Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc) Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc) WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc) WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc) # Declaration of the RigidBody containers. :: BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I) BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I) BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I) BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I) # The kinematic differential equations; they are defined quite simply. Each # entry in this list is equal to zero. kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5] # The nonholonomic constraints are the velocity of the front wheel contact # point dotted into the X, Y, and Z directions; the yaw frame is used as it # is "closer" to the front wheel (1 less DCM connecting them). These # constraints force the velocity of the front wheel contact point to be 0 # in the inertial frame; the X and Y direction constraints enforce a # "no-slip" condition, and the Z direction constraint forces the front # wheel contact point to not move away from the ground frame, essentially # replicating the holonomic constraint which does not allow the frame pitch # to change in an invalid fashion. conlist_speed = [ WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z ] # The holonomic constraint is that the position from the rear wheel contact # point to the front wheel contact point when dotted into the # normal-to-ground plane direction must be zero; effectively that the front # and rear wheel contact points are always touching the ground plane. This # is actually not part of the dynamic equations, but instead is necessary # for the lineraization process. conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z] # The force list; each body has the appropriate gravitational force applied # at its mass center. FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z), (WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)] BL = [BodyFrame, BodyFork, BodyWR, BodyWF] # The N frame is the inertial frame, coordinates are supplied in the order # of independent, dependent coordinates, as are the speeds. The kinematic # differential equation are also entered here. Here the dependent speeds # are specified, in the same order they were provided in earlier, along # with the non-holonomic constraints. The dependent coordinate is also # provided, with the holonomic constraint. Again, this is only provided # for the linearization process. KM = KanesMethod(N, q_ind=[q1, q2, q5], q_dependent=[q4], configuration_constraints=conlist_coord, u_ind=[u2, u3, u5], u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed, kd_eqs=kd) (fr, frstar) = KM.kanes_equations(FL, BL) # This is the start of entering in the numerical values from the benchmark # paper to validate the eigen values of the linearized equations from this # model to the reference eigen values. Look at the aforementioned paper for # more information. Some of these are intermediate values, used to # transform values from the paper into the coordinate systems used in this # model. PaperRadRear = 0.3 PaperRadFront = 0.35 HTA = evalf.N(pi / 2 - pi / 10) TrailPaper = 0.08 rake = evalf.N(-(TrailPaper * sin(HTA) - (PaperRadFront * cos(HTA)))) PaperWb = 1.02 PaperFrameCgX = 0.3 PaperFrameCgZ = 0.9 PaperForkCgX = 0.9 PaperForkCgZ = 0.7 FrameLength = evalf.N(PaperWb * sin(HTA) - (rake - (PaperRadFront - PaperRadRear) * cos(HTA))) FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear - (PaperFrameCgX / sin(HTA)) * cos(HTA)) * sin(HTA)) FrameCGPar = evalf.N( (PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA))) tempa = evalf.N((PaperForkCgZ - PaperRadFront)) tempb = evalf.N((PaperWb - PaperForkCgX)) tempc = evalf.N(sqrt(tempa**2 + tempb**2)) PaperForkL = evalf.N( (PaperWb * cos(HTA) - (PaperRadFront - PaperRadRear) * sin(HTA))) ForkCGNorm = evalf.N(rake + (tempc * sin(pi / 2 - HTA - acos(tempa / tempc)))) ForkCGPar = evalf.N(tempc * cos((pi / 2 - HTA) - acos(tempa / tempc)) - PaperForkL) # Here is the final assembly of the numerical values. The symbol 'v' is the # forward speed of the bicycle (a concept which only makes sense in the # upright, static equilibrium case?). These are in a dictionary which will # later be substituted in. Again the sign on the *product* of inertia # values is flipped here, due to different orientations of coordinate # systems. v = symbols('v') val_dict = { WFrad: PaperRadFront, WRrad: PaperRadRear, htangle: HTA, forkoffset: rake, forklength: PaperForkL, framelength: FrameLength, forkcg1: ForkCGPar, forkcg3: ForkCGNorm, framecg1: FrameCGNorm, framecg3: FrameCGPar, Iwr11: 0.0603, Iwr22: 0.12, Iwf11: 0.1405, Iwf22: 0.28, Ifork11: 0.05892, Ifork22: 0.06, Ifork33: 0.00708, Ifork31: 0.00756, Iframe11: 9.2, Iframe22: 11, Iframe33: 2.8, Iframe31: -2.4, mfork: 4, mframe: 85, mwf: 3, mwr: 2, g: 9.81, q1: 0, q2: 0, q4: 0, q5: 0, u1: 0, u2: 0, u3: v / PaperRadRear, u4: 0, u5: 0, u6: v / PaperRadFront } # Linearizes the forcing vector; the equations are set up as MM udot = # forcing, where MM is the mass matrix, udot is the vector representing the # time derivatives of the generalized speeds, and forcing is a vector which # contains both external forcing terms and internal forcing terms, such as # centripital or coriolis forces. This actually returns a matrix with as # many rows as *total* coordinates and speeds, but only as many columns as # independent coordinates and speeds. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) forcing_lin = KM.linearize()[0] # As mentioned above, the size of the linearized forcing terms is expanded # to include both q's and u's, so the mass matrix must have this done as # well. This will likely be changed to be part of the linearized process, # for future reference. MM_full = KM.mass_matrix_full MM_full_s = MM_full.subs(val_dict) forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict) MM_full_s = MM_full_s.evalf() forcing_lin_s = forcing_lin_s.evalf() # Finally, we construct an "A" matrix for the form xdot = A x (x being the # state vector, although in this case, the sizes are a little off). The # following line extracts only the minimum entries required for eigenvalue # analysis, which correspond to rows and columns for lean, steer, lean # rate, and steer rate. Amat = MM_full_s.inv() * forcing_lin_s A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5]) # Precomputed for comparison Res = Matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0], [ 9.48977444677355, -0.891197738059089 * v**2 - 0.571523173729245, -0.105522449805691 * v, -0.330515398992311 * v ], [ 11.7194768719633, -1.97171508499972 * v**2 + 30.9087533932407, 3.67680523332152 * v, -3.08486552743311 * v ]]) # Actual eigenvalue comparison eps = 1.e-12 for i in range(6): error = Res.subs(v, i) - A.subs(v, i) assert all(abs(x) < eps for x in error)
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 three # speed variables are need to describe this system, along with the disc's # mass and radius, and the local gravity (note that mass will drop out). q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 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]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) # 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) # This is a simple way to form the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) # Kinematic differential equations; how the generalized coordinate time # derivatives relate to generalized speeds. kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] # Creation of the force list; it is the gravitational force at the mass # center of the disc. Then we create the disc by assigning a Point to the # center of mass attribute, a ReferenceFrame to the frame attribute, and mass # and inertia. Then we form the body list. ForceList = [(Dmc, - m * g * Y.z)] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] # Finally we form the equations of motion, using the same steps we did # before. Specify inertial frame, supply generalized speeds, supply # kinematic differential equation dictionary, compute Fr from the force # list and Fr* from the body list, compute the mass matrix and forcing # terms, then solve for the u dots (time derivatives of the generalized # speeds). KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(ForceList, BodyList) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing kdd = KM.kindiffdict() rhs = rhs.subs(kdd) rhs.simplify() assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) + 4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand() assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(6, 1) # This code tests our output vs. benchmark values. When r=g=m=1, the # critical speed (where all eigenvalues of the linearized equations are 0) # is 1 / sqrt(3) for the upright case. A = KM.linearize(A_and_B=True, new_method=True)[0] A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0}) assert A_upright.subs(u2, 1 / sqrt(3)).eigenvals() == {S(0): 6}
def test_aux_dep(): # This test is about rolling disc dynamics, comparing the results found # with KanesMethod to those found when deriving the equations "manually" # with SymPy. # The terms Fr, Fr*, and Fr*_steady are all compared between the two # methods. Here, Fr*_steady refers to the generalized inertia forces for an # equilibrium configuration. # Note: comparing to the test of test_rolling_disc() in test_kane.py, this # test also tests auxiliary speeds and configuration and motion constraints #, seen in the generalized dependent coordinates q[3], and depend speeds # u[3], u[4] and u[5]. # First, mannual derivation of Fr, Fr_star, Fr_star_steady. # Symbols for time and constant parameters. # Symbols for contact forces: Fx, Fy, Fz. t, r, m, g, I, J = symbols('t r m g I J') Fx, Fy, Fz = symbols('Fx Fy Fz') # Configuration variables and their time derivatives: # q[0] -- yaw # q[1] -- lean # q[2] -- spin # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in # A.z direction # Generalized speeds and their time derivatives: # u[0] -- disc angular velocity component, disc fixed x direction # u[1] -- disc angular velocity component, disc fixed y direction # u[2] -- disc angular velocity component, disc fixed z direction # u[3] -- disc velocity component, A.x direction # u[4] -- disc velocity component, A.y direction # u[5] -- disc velocity component, A.z direction # Auxiliary generalized speeds: # ua[0] -- contact point auxiliary generalized speed, A.x direction # ua[1] -- contact point auxiliary generalized speed, A.y direction # ua[2] -- contact point auxiliary generalized speed, A.z direction q = dynamicsymbols('q:4') qd = [qi.diff(t) for qi in q] u = dynamicsymbols('u:6') ud = [ui.diff(t) for ui in u] #ud_zero = {udi : 0 for udi in ud} ud_zero = dict(zip(ud, [0.] * len(ud))) ua = dynamicsymbols('ua:3') #ua_zero = {uai : 0 for uai in ua} ua_zero = dict(zip(ua, [0.] * len(ua))) # Reference frames: # Yaw intermediate frame: A. # Lean intermediate frame: B. # Disc fixed frame: C. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q[0], N.z]) B = A.orientnew('B', 'Axis', [q[1], A.x]) C = B.orientnew('C', 'Axis', [q[2], B.y]) # Angular velocity and angular acceleration of disc fixed frame # u[0], u[1] and u[2] are generalized independent speeds. C.set_ang_vel(N, u[0] * B.x + u[1] * B.y + u[2] * B.z) C.set_ang_acc( N, C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Velocity and acceleration of points: # Disc-ground contact point: P. # Center of disc: O, defined from point P with depend coordinate: q[3] # u[3], u[4] and u[5] are generalized dependent speeds. P = Point('P') P.set_vel(N, ua[0] * A.x + ua[1] * A.y + ua[2] * A.z) O = P.locatenew('O', q[3] * A.z + r * sin(q[1]) * A.y) O.set_vel(N, u[3] * A.x + u[4] * A.y + u[5] * A.z) O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N))) # Kinematic differential equations: # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates # directions of B, for qd0, qd1 and qd2. # the other is v_o_n_qd = O.vel(N) in A.z direction for qd3. # Then, solve for dq/dt's in terms of u's: qd_kd. w_c_n_qd = qd[0] * A.z + qd[1] * B.x + qd[2] * B.y v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P)) kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)]) qd_kd = solve(kindiffs, qd) # Values of generalized speeds during a steady turn for later substitution # into the Fr_star_steady. steady_conditions = solve(kindiffs.subs({qd[1]: 0, qd[3]: 0}), u) steady_conditions.update({qd[1]: 0, qd[3]: 0}) # Partial angular velocities and velocities. partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] # Configuration constraint: f_c, the projection of radius r in A.z direction # is q[3]. # Velocity constraints: f_v, for u3, u4 and u5. # Acceleration constraints: f_a. f_c = Matrix([dot(-r * B.z, A.z) - q[3]]) f_v = Matrix([ dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))), ai).expand() for ai in A ]) v_o_n = cross(C.ang_vel_in(N), O.pos_from(P)) a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n) f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # Solve for constraint equations in the form of # u_dependent = A_rs * [u_i; u_aux]. # First, obtain constraint coefficient matrix: M_v * [u; ua] = 0; # Second, taking u[0], u[1], u[2] as independent, # taking u[3], u[4], u[5] as dependent, # rearranging the matrix of M_v to be A_rs for u_dependent. # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict. M_v = zeros(3, 9) for i in range(3): for j, ui in enumerate(u + ua): M_v[i, j] = f_v[i].diff(ui) M_v_i = M_v[:, :3] M_v_d = M_v[:, 3:6] M_v_aux = M_v[:, 6:] M_v_i_aux = M_v_i.row_join(M_v_aux) A_rs = -M_v_d.inv() * M_v_i_aux u_dep = A_rs[:, :3] * Matrix(u[:3]) u_dep_dict = dict(zip(u[3:], u_dep)) #u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())} # Active forces: F_O acting on point O; F_P acting on point P. # Generalized active forces (unconstrained): Fr_u = F_point * pv_point. F_O = m * g * A.z F_P = Fx * A.x + Fy * A.y + Fz * A.z Fr_u = Matrix([ dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in zip(partial_v_O, partial_v_P) ]) # Inertia force: R_star_O. # Inertia of disc: I_C_O, where J is a inertia component about principal axis. # Inertia torque: T_star_C. # Generalized inertia forces (unconstrained): Fr_star_u. R_star_O = -m * O.acc(N) I_C_O = inertia(B, I, J, I) T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \ + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N)))) Fr_star_u = Matrix([ dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_O, partial_w_C) ]) # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c. # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady. Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :] Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\ + A_rs.T * Fr_star_u[3:6, :] Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\ .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand() # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady. # Rigid Bodies: disc, with inertia I_C_O. iner_tuple = (I_C_O, O) disc = RigidBody('disc', O, C, m, iner_tuple) bodyList = [disc] # Generalized forces: Gravity: F_o; Auxiliary forces: F_p. F_o = (O, F_O) F_p = (P, F_P) forceList = [F_o, F_p] # KanesMethod. kane = KanesMethod(N, q_ind=q[:3], u_ind=u[:3], kd_eqs=kindiffs, q_dependent=q[3:], configuration_constraints=f_c, u_dependent=u[3:], velocity_constraints=f_v, u_auxiliary=ua) # fr, frstar, frstar_steady and kdd(kinematic differential equations). (fr, frstar) = kane.kanes_equations(forceList, bodyList) frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\ .subs({q[3]: -r*cos(q[1])}).expand() kdd = kane.kindiffdict() assert Matrix(Fr_c).expand() == fr.expand() assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand() assert (simplify(Matrix(Fr_star_steady).expand()) == simplify( frstar_steady.expand()))
from util import msprint, partial_velocities from util import generalized_active_forces, potential_energy q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7') u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7') # L' is the natural length of the springs a, k, L_prime = symbols('a k L\'', real=True, positive=True) # reference frames X = ReferenceFrame('X') C = X.orientnew('C', 'body', [q4, q5, q6], 'xyz') # define points pO = Point('O') # point O is fixed in X pC_star = pO.locatenew('C*', a*(q1*X.x + q2*X.y + q3*X.z)) # define points of the cube connected to springs pC1 = pC_star.locatenew('C1', a*(C.x + C.y - C.z)) pC2 = pC_star.locatenew('C2', a*(C.y + C.z - C.x)) pC3 = pC_star.locatenew('C3', a*(C.z + C.x - C.y)) # define fixed spring points pk1 = pO.locatenew('k1', L_prime * X.x + a*(X.x + X.y - X.z)) pk2 = pO.locatenew('k2', L_prime * X.y + a*(X.y + X.z - X.x)) pk3 = pO.locatenew('k3', L_prime * X.z + a*(X.z + X.x - X.y)) pC_star.set_vel(X, pC_star.pos_from(pO).dt(X)) pC1.v2pt_theory(pC_star, X, C) pC2.v2pt_theory(pC_star, X, C) pC3.v2pt_theory(pC_star, X, C) # kinematic differential equations
def test_point_funcs(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) assert P.pos_from(O) == q * B.x P.set_vel(B, qd * B.x + q2d * B.y) assert P.vel(B) == qd * B.x + q2d * B.y O.set_vel(N, 0) assert O.vel(N) == 0 assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y + (-10 * qd) * B.z) B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 10 * B.x) O.set_vel(N, 5 * N.x) assert O.vel(N) == 5 * N.x assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) P.set_vel(B, qd * B.x + q2d * B.y) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
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_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_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3") u4, u5 = dynamicsymbols("u4, u5") r = symbols("r") 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]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point("C") C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew("Dmc", r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert partial_velocity(vel_list, u_list) == [ [-r * L.y, 0, L.x], [r * L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0], [cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0], ]