def test_apply_loads_on_multi_degree_freedom_holonomic_system(): """Example based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.html""" W = Body('W') #Wall B = Body('B') #Block P = Body('P') #Pendulum b = Body('b') #bob q1, q2 = dynamicsymbols('q1 q2') #generalized coordinates k, c, g, kT = symbols('k c g kT') #constants F, T = dynamicsymbols('F T') #Specified forces #Applying forces B.apply_force(F * W.x) W.apply_force(k * q1 * W.x, reaction_body=B) #Spring force W.apply_force(c * q1.diff() * W.x, reaction_body=B) #dampner P.apply_force(P.mass * g * W.y) b.apply_force(b.mass * g * W.y) #Applying torques P.apply_torque(kT * q2 * W.z, reaction_body=b) P.apply_torque(T * W.z) assert B.loads == [(B.masscenter, (F - k * q1 - c * q1.diff()) * W.x)] assert P.loads == [(P.masscenter, P.mass * g * W.y), (P.frame, (T + kT * q2) * W.z)] assert b.loads == [(b.masscenter, b.mass * g * W.y), (b.frame, -kT * q2 * W.z)] assert W.loads == [(W.masscenter, (c * q1.diff() + k * q1) * W.x)]
def test_complete_simple_double_pendulum(): q1, q2 = dynamicsymbols('q1 q2') u1, u2 = dynamicsymbols('u1 u2') m, l, g = symbols('m l g') C = Body('C') # ceiling PartP = Body('P', mass=m) PartR = Body('R', mass=m) J1 = PinJoint('J1', C, PartP, speeds=u1, coordinates=q1, child_joint_pos=-l*PartP.x, parent_axis=C.z, child_axis=PartP.z) J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2, child_joint_pos=-l*PartR.x, parent_axis=PartP.z, child_axis=PartR.z) PartP.apply_force(m*g*C.x) PartR.apply_force(m*g*C.x) method = JointsMethod(C, J1, J2) method.form_eoms() assert expand(method.mass_matrix_full) == Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 2*l**2*m*cos(q2) + 3*l**2*m, l**2*m*cos(q2) + l**2*m], [0, 0, l**2*m*cos(q2) + l**2*m, l**2*m]]) assert trigsimp(method.forcing_full) == trigsimp(Matrix([[u1], [u2], [-g*l*m*(sin(q1 + q2) + sin(q1)) - g*l*m*sin(q1) + l**2*m*(2*u1 + u2)*u2*sin(q2)], [-g*l*m*sin(q1 + q2) - l**2*m*u1**2*sin(q2)]]))
def test_chaos_pendulum(): #https://www.pydy.org/examples/chaos_pendulum.html mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g = symbols('mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g') theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha') A = ReferenceFrame('A') B = ReferenceFrame('B') rod = Body('rod', mass=mA, frame=A, central_inertia=inertia(A, IAxx, IAxx, 0)) plate = Body('plate', mass=mB, frame=B, central_inertia=inertia(B, IBxx, IByy, IBzz)) C = Body('C') J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega, child_joint_pos=-lA*rod.z, parent_axis=C.y, child_axis=rod.y) J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha, parent_joint_pos=(lB-lA)*rod.z, parent_axis=rod.z, child_axis=plate.z) rod.apply_force(mA*g*C.z) plate.apply_force(mB*g*C.z) method = JointsMethod(C, J1, J2) method.form_eoms() MM = method.mass_matrix forcing = method.forcing rhs = MM.LUsolve(forcing) xd = (-2 * IBxx * alpha * omega * sin(phi) * cos(phi) + 2 * IByy * alpha * omega * sin(phi) * cos(phi) - g * lA * mA * sin(theta) - g * lB * mB * sin(theta)) / (IAxx + IBxx * sin(phi)**2 + IByy * cos(phi)**2 + lA**2 * mA + lB**2 * mB) assert (rhs[0] - xd).simplify() == 0 xd = (IBxx - IByy) * omega**2 * sin(phi) * cos(phi) / IBzz assert (rhs[1] - xd).simplify() == 0
def test_clear_load(): a = symbols('a') P = Point('P') B = Body('B') force = a * B.z B.apply_force(force, P) assert B.loads == [(P, force)] B.clear_loads() assert B.loads == []
def test_apply_force_multiple_one_point(): a, b = symbols('a b') P = Point('P') B = Body('B') f1 = a * B.x f2 = b * B.y B.apply_force(f1, P) assert B.loads == [(P, f1)] B.apply_force(f2, P) assert B.loads == [(P, f1 + f2)]
def test_apply_force(): f, g = symbols('f g') q, x, v1, v2 = dynamicsymbols('q x v1 v2') P1 = Point('P1') P2 = Point('P2') B1 = Body('B1') B2 = Body('B2') N = ReferenceFrame('N') P1.set_vel(B1.frame, v1 * B1.x) P2.set_vel(B2.frame, v2 * B2.x) force = f * q * N.z # time varying force B1.apply_force(force, P1, B2, P2) #applying equal and opposite force on moving points assert B1.loads == [(P1, force)] assert B2.loads == [(P2, -force)] g1 = B1.mass * g * N.y g2 = B2.mass * g * N.y B1.apply_force(g1) #applying gravity on B1 masscenter B2.apply_force(g2) #applying gravity on B2 masscenter assert B1.loads == [(P1, force), (B1.masscenter, g1)] assert B2.loads == [(P2, -force), (B2.masscenter, g2)] force2 = x * N.x B1.apply_force( force2, reaction_body=B2) #Applying time varying force on masscenter assert B1.loads == [(P1, force), (B1.masscenter, force2 + g1)] assert B2.loads == [(P2, -force), (B2.masscenter, -force2 + g2)]
def test_remove_load(): P1 = Point('P1') P2 = Point('P2') B = Body('B') f1 = B.x f2 = B.y B.apply_force(f1, P1) B.apply_force(f2, P2) B.loads == [(P1, f1), (P2, f2)] B.remove_load(P2) B.loads == [(P1, f1)] B.apply_torque(f1.cross(f2)) B.loads == [(P1, f1), (B.frame, f1.cross(f2))] B.remove_load() B.loads == [(P1, f1)]
def test_jointsmethod(): P = Body('P') C = Body('C') Pin = PinJoint('P1', P, C) C_ixx, g = symbols('C_ixx g') theta, omega = dynamicsymbols('theta_P1, omega_P1') P.apply_force(g * P.y) method = JointsMethod(P, Pin) assert method.frame == P.frame assert method.bodies == [C, P] assert method.loads == [(P.masscenter, g * P.frame.y)] assert method.q == [theta] assert method.u == [omega] assert method.kdes == [omega - theta.diff()] soln = method.form_eoms() assert soln == Matrix([[-C_ixx * omega.diff()]]) assert method.forcing_full == Matrix([[omega], [0]]) assert method.mass_matrix_full == Matrix([[1, 0], [0, C_ixx]]) assert isinstance(method.method, KanesMethod)
def test_two_dof_joints(): q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2') m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2') W = Body('W') B1 = Body('B1', mass=m) B2 = Body('B2', mass=m) J1 = PrismaticJoint('J1', W, B1, coordinates=q1, speeds=u1) J2 = PrismaticJoint('J2', B1, B2, coordinates=q2, speeds=u2) W.apply_force(k1*q1*W.x, reaction_body=B1) W.apply_force(c1*u1*W.x, reaction_body=B1) B1.apply_force(k2*q2*W.x, reaction_body=B2) B1.apply_force(c2*u2*W.x, reaction_body=B2) method = JointsMethod(W, J1, J2) method.form_eoms() MM = method.mass_matrix forcing = method.forcing rhs = MM.LUsolve(forcing) assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m) assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_body_add_force(): # Body with RigidBody. rigidbody_masscenter = Point('rigidbody_masscenter') rigidbody_mass = Symbol('rigidbody_mass') rigidbody_frame = ReferenceFrame('rigidbody_frame') body_inertia = inertia(rigidbody_frame, 1, 0, 0) rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass, rigidbody_frame, body_inertia) l = Symbol('l') Fa = Symbol('Fa') point = rigid_body.masscenter.locatenew('rigidbody_body_point0', l * rigid_body.frame.x) point.set_vel(rigid_body.frame, 0) force_vector = Fa * rigid_body.frame.z # apply_force with point rigid_body.apply_force(force_vector, point) assert len(rigid_body.loads) == 1 force_point = rigid_body.loads[0][0] frame = rigid_body.frame assert force_point.vel(frame) == point.vel(frame) assert force_point.pos_from(force_point) == point.pos_from(force_point) assert rigid_body.loads[0][1] == force_vector # apply_force without point rigid_body.apply_force(force_vector) assert len(rigid_body.loads) == 2 assert rigid_body.loads[1][1] == force_vector # passing something else than point raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0)) raises(TypeError, lambda: rigid_body.apply_force(0))
def test_body_add_force(): # Body with RigidBody. rigidbody_masscenter = Point('rigidbody_masscenter') rigidbody_mass = Symbol('rigidbody_mass') rigidbody_frame = ReferenceFrame('rigidbody_frame') body_inertia = inertia(rigidbody_frame, 1, 0, 0) rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass, rigidbody_frame, body_inertia) l = Symbol('l') Fa = Symbol('Fa') point = rigid_body.masscenter.locatenew( 'rigidbody_body_point0', l * rigid_body.frame.x) point.set_vel(rigid_body.frame, 0) force_vector = Fa * rigid_body.frame.z # apply_force with point rigid_body.apply_force(force_vector, point) assert len(rigid_body.loads) == 1 force_point = rigid_body.loads[0][0] frame = rigid_body.frame assert force_point.vel(frame) == point.vel(frame) assert force_point.pos_from(force_point) == point.pos_from(force_point) assert rigid_body.loads[0][1] == force_vector # apply_force without point rigid_body.apply_force(force_vector) assert len(rigid_body.loads) == 2 assert rigid_body.loads[1][1] == force_vector # passing something else than point raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0)) raises(TypeError, lambda: rigid_body.apply_force(0))
def test_particle_body_add_force(): # Body with Particle particle_masscenter = Point('particle_masscenter') particle_mass = Symbol('particle_mass') particle_frame = ReferenceFrame('particle_frame') particle_body = Body('particle_body', particle_masscenter, particle_mass, particle_frame) a = Symbol('a') force_vector = a * particle_body.frame.x particle_body.apply_force(force_vector, particle_body.masscenter) assert len(particle_body.loads) == 1 point = particle_body.masscenter.locatenew(particle_body._name + '_point0', 0) point.set_vel(particle_body.frame, 0) force_point = particle_body.loads[0][0] frame = particle_body.frame assert force_point.vel(frame) == point.vel(frame) assert force_point.pos_from(force_point) == point.pos_from(force_point) assert particle_body.loads[0][1] == force_vector
def test_particle_body_add_force(): # Body with Particle particle_masscenter = Point('particle_masscenter') particle_mass = Symbol('particle_mass') particle_frame = ReferenceFrame('particle_frame') particle_body = Body('particle_body', particle_masscenter, particle_mass, particle_frame) a = Symbol('a') force_vector = a * particle_body.frame.x particle_body.apply_force(force_vector, particle_body.masscenter) assert len(particle_body.loads) == 1 point = particle_body.masscenter.locatenew( particle_body._name + '_point0', 0) point.set_vel(particle_body.frame, 0) force_point = particle_body.loads[0][0] frame = particle_body.frame assert force_point.vel(frame) == point.vel(frame) assert force_point.pos_from(force_point) == point.pos_from(force_point) assert particle_body.loads[0][1] == force_vector