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_rigidbody(): m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega') A = ReferenceFrame('A') A2 = ReferenceFrame('A2') P = Point('P') P2 = Point('P2') I = Dyadic(0) I2 = Dyadic(0) B = RigidBody('B', P, A, m, (I, P)) assert B.mass == m assert B.frame == A assert B.masscenter == P assert B.inertia == (I, B.masscenter) B.mass = m2 B.frame = A2 B.masscenter = P2 B.inertia = (I2, B.masscenter) raises(TypeError, lambda: RigidBody(P, P, A, m, (I, P))) raises(TypeError, lambda: RigidBody('B', P, P, m, (I, P))) raises(TypeError, lambda: RigidBody('B', P, A, m, (P, P))) raises(TypeError, lambda: RigidBody('B', P, A, m, (I, I))) assert B.__str__() == 'B' assert B.mass == m2 assert B.frame == A2 assert B.masscenter == P2 assert B.inertia == (I2, B.masscenter) assert B.masscenter == P2 assert B.inertia == (I2, B.masscenter) # Testing linear momentum function assuming A2 is the inertial frame N = ReferenceFrame('N') P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
def test_rigidbody(): m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega') A = ReferenceFrame('A') A2 = ReferenceFrame('A2') P = Point('P') P2 = Point('P2') I = Dyadic([]) I2 = Dyadic([]) B = RigidBody('B', P, A, m, (I, P)) assert B.mass == m assert B.frame == A assert B.masscenter == P assert B.inertia == (I, B.masscenter) B.mass = m2 B.frame = A2 B.masscenter = P2 B.inertia = (I2, B.masscenter) assert B.mass == m2 assert B.frame == A2 assert B.masscenter == P2 assert B.inertia == (I2, B.masscenter) assert B.masscenter == P2 assert B.inertia == (I2, B.masscenter) # Testing linear momentum function assuming A2 is the inertial frame N = ReferenceFrame('N') P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
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_rigidbody(): m = Symbol('m') A = ReferenceFrame('A') P = Point('P') I = Dyadic([]) B = RigidBody() assert B.mass == None assert B.mc == None assert B.inertia == (None, None) assert B.frame == None B.mass = m B.frame = A B.cm = P B.inertia = (I, B.cm) assert B.mass == m assert B.frame == A assert B.cm == P assert B.inertia == (I, B.cm)
def test_rigidbody(): m, m2 = symbols('m m2') A = ReferenceFrame('A') A2 = ReferenceFrame('A2') P = Point('P') P2 = Point('P2') I = Dyadic([]) I2 = Dyadic([]) B = RigidBody('B', P, A, m, (I, P)) assert B.mass == m assert B.frame == A assert B.mc == P assert B.inertia == (I, B.mc) B.mass = m2 B.frame = A2 B.mc = P2 B.inertia = (I2, B.mc) assert B.mass == m2 assert B.frame == A2 assert B.mc == P2 assert B.inertia == (I2, B.mc)
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.mc = Dmc BodyD.inertia = (I, Dmc) BodyD.frame = R BodyD.mass = m 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_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.mc = Dmc BodyD.inertia = (I, Dmc) BodyD.frame = R BodyD.mass = m 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()