import pytest from sympy.core.symbol import symbols from sympy.core.sympify import sympify from sympy.functions.elementary.trigonometric import cos, sin from sympy.matrices.dense import eye, zeros from sympy.matrices.immutable import ImmutableMatrix from sympy.physics.mechanics import ( Force, KanesMethod, LagrangesMethod, Particle, PinJoint, Point, PrismaticJoint, ReferenceFrame, RigidBody, Torque, TorqueActuator, System, dynamicsymbols) from sympy.simplify.simplify import simplify from sympy.solvers.solvers import solve t = dynamicsymbols._t # type: ignore q = dynamicsymbols('q:6') # type: ignore qd = dynamicsymbols('q:6', 1) # type: ignore u = dynamicsymbols('u:6') # type: ignore ua = dynamicsymbols('ua:3') # type: ignore class TestSystemBase: @pytest.fixture() def _empty_system_setup(self): self.system = System(ReferenceFrame('frame'), Point('fixed_point')) def _empty_system_check(self, exclude=()): matrices = ('q_ind', 'q_dep', 'q', 'u_ind', 'u_dep', 'u', 'u_aux', 'kdes', 'holonomic_constraints', 'nonholonomic_constraints') tuples = ('loads', 'bodies', 'joints', 'actuators') for attr in matrices: if attr not in exclude: assert getattr(self.system, attr)[:] == [] for attr in tuples: if attr not in exclude: assert getattr(self.system, attr) == () if 'eom_method' not in exclude: assert self.system.eom_method is None def _create_filled_system(self, with_speeds=True): self.system = System(ReferenceFrame('frame'), Point('fixed_point')) u = dynamicsymbols('u:6') if with_speeds else qd self.bodies = symbols('rb1:5', cls=RigidBody) self.joints = ( PinJoint('J1', self.bodies[0], self.bodies[1], q[0], u[0]), PrismaticJoint('J2', self.bodies[1], self.bodies[2], q[1], u[1]), PinJoint('J3', self.bodies[2], self.bodies[3], q[2], u[2]) ) self.system.add_joints(*self.joints) self.system.add_coordinates(q[3], independent=[False]) self.system.add_speeds(u[3], independent=False) if with_speeds: self.system.add_kdes(u[3] - qd[3]) self.system.add_auxiliary_speeds(ua[0], ua[1]) self.system.add_holonomic_constraints(q[2] - q[0] + q[1]) self.system.add_nonholonomic_constraints(u[3] - qd[1] + u[2]) self.system.u_ind = u[:2] self.system.u_dep = u[2:4] self.q_ind, self.q_dep = self.system.q_ind[:], self.system.q_dep[:] self.u_ind, self.u_dep = self.system.u_ind[:], self.system.u_dep[:] self.kdes = self.system.kdes[:] self.hc = self.system.holonomic_constraints[:] self.vc = self.system.velocity_constraints[:] self.nhc = self.system.nonholonomic_constraints[:] @pytest.fixture() def _filled_system_setup(self): self._create_filled_system(with_speeds=True) @pytest.fixture() def _filled_system_setup_no_speeds(self): self._create_filled_system(with_speeds=False) def _filled_system_check(self, exclude=()): assert 'q_ind' in exclude or self.system.q_ind[:] == q[:3] assert 'q_dep' in exclude or self.system.q_dep[:] == [q[3]] assert 'q' in exclude or self.system.q[:] == q[:4] assert 'u_ind' in exclude or self.system.u_ind[:] == u[:2] assert 'u_dep' in exclude or self.system.u_dep[:] == u[2:4] assert 'u' in exclude or self.system.u[:] == u[:4] assert 'u_aux' in exclude or self.system.u_aux[:] == ua[:2] assert 'kdes' in exclude or self.system.kdes[:] == [ ui - qdi for ui, qdi in zip(u[:4], qd[:4])] assert ('holonomic_constraints' in exclude or self.system.holonomic_constraints[:] == [q[2] - q[0] + q[1]]) assert ('nonholonomic_constraints' in exclude or self.system.nonholonomic_constraints[:] == [u[3] - qd[1] + u[2]] ) assert ('velocity_constraints' in exclude or self.system.velocity_constraints[:] == [ qd[2] - qd[0] + qd[1], u[3] - qd[1] + u[2]]) assert ('bodies' in exclude or self.system.bodies == tuple(self.bodies)) assert ('joints' in exclude or self.system.joints == tuple(self.joints)) @pytest.fixture() def _moving_point_mass(self, _empty_system_setup): self.system.q_ind = q[0] self.system.u_ind = u[0] self.system.kdes = u[0] - q[0].diff(t) p = Particle('p', mass=symbols('m')) self.system.add_bodies(p) p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x) class TestSystem(TestSystemBase): def test_empty_system(self, _empty_system_setup): self._empty_system_check() self.system.validate_system() def test_filled_system(self, _filled_system_setup): self._filled_system_check() self.system.validate_system() @pytest.mark.parametrize('frame', [None, ReferenceFrame('frame')]) @pytest.mark.parametrize('fixed_point', [None, Point('fixed_point')]) def test_init(self, frame, fixed_point): if fixed_point is None and frame is None: self.system = System() else: self.system = System(frame, fixed_point) if fixed_point is None: assert self.system.fixed_point.name == 'inertial_point' else: assert self.system.fixed_point == fixed_point if frame is None: assert self.system.frame.name == 'inertial_frame' else: assert self.system.frame == frame self._empty_system_check() assert isinstance(self.system.q_ind, ImmutableMatrix) assert isinstance(self.system.q_dep, ImmutableMatrix) assert isinstance(self.system.q, ImmutableMatrix) assert isinstance(self.system.u_ind, ImmutableMatrix) assert isinstance(self.system.u_dep, ImmutableMatrix) assert isinstance(self.system.u, ImmutableMatrix) assert isinstance(self.system.kdes, ImmutableMatrix) assert isinstance(self.system.holonomic_constraints, ImmutableMatrix) assert isinstance(self.system.nonholonomic_constraints, ImmutableMatrix) def test_from_newtonian_rigid_body(self): rb = RigidBody('body') self.system = System.from_newtonian(rb) assert self.system.fixed_point == rb.masscenter assert self.system.frame == rb.frame self._empty_system_check(exclude=('bodies',)) self.system.bodies = (rb,) def test_from_newtonian_particle(self): pt = Particle('particle') with pytest.raises(TypeError): System.from_newtonian(pt) @pytest.mark.parametrize('args, kwargs, exp_q_ind, exp_q_dep, exp_q', [ (q[:3], {}, q[:3], [], q[:3]), (q[:3], {'independent': True}, q[:3], [], q[:3]), (q[:3], {'independent': False}, [], q[:3], q[:3]), (q[:3], {'independent': [True, False, True]}, [q[0], q[2]], [q[1]], [q[0], q[2], q[1]]), ]) def test_coordinates(self, _empty_system_setup, args, kwargs, exp_q_ind, exp_q_dep, exp_q): # Test add_coordinates self.system.add_coordinates(*args, **kwargs) assert self.system.q_ind[:] == exp_q_ind assert self.system.q_dep[:] == exp_q_dep assert self.system.q[:] == exp_q self._empty_system_check(exclude=('q_ind', 'q_dep', 'q')) # Test setter for q_ind and q_dep self.system.q_ind = exp_q_ind self.system.q_dep = exp_q_dep assert self.system.q_ind[:] == exp_q_ind assert self.system.q_dep[:] == exp_q_dep assert self.system.q[:] == exp_q self._empty_system_check(exclude=('q_ind', 'q_dep', 'q')) @pytest.mark.parametrize('func', ['add_coordinates', 'add_speeds']) @pytest.mark.parametrize('args, kwargs', [ ((q[0], q[5]), {}), ((u[0], u[5]), {}), ((q[0],), {'independent': False}), ((u[0],), {'independent': False}), ((u[0], q[5]), {}), ((symbols('a'), q[5]), {}), ]) def test_coordinates_speeds_invalid(self, _filled_system_setup, func, args, kwargs): with pytest.raises(ValueError): getattr(self.system, func)(*args, **kwargs) self._filled_system_check() @pytest.mark.parametrize('args, kwargs, exp_u_ind, exp_u_dep, exp_u', [ (u[:3], {}, u[:3], [], u[:3]), (u[:3], {'independent': True}, u[:3], [], u[:3]), (u[:3], {'independent': False}, [], u[:3], u[:3]), (u[:3], {'independent': [True, False, True]}, [u[0], u[2]], [u[1]], [u[0], u[2], u[1]]), ]) def test_speeds(self, _empty_system_setup, args, kwargs, exp_u_ind, exp_u_dep, exp_u): # Test add_speeds self.system.add_speeds(*args, **kwargs) assert self.system.u_ind[:] == exp_u_ind assert self.system.u_dep[:] == exp_u_dep assert self.system.u[:] == exp_u self._empty_system_check(exclude=('u_ind', 'u_dep', 'u')) # Test setter for u_ind and u_dep self.system.u_ind = exp_u_ind self.system.u_dep = exp_u_dep assert self.system.u_ind[:] == exp_u_ind assert self.system.u_dep[:] == exp_u_dep assert self.system.u[:] == exp_u self._empty_system_check(exclude=('u_ind', 'u_dep', 'u')) @pytest.mark.parametrize('args, kwargs, exp_u_aux', [ (ua[:3], {}, ua[:3]), ]) def test_auxiliary_speeds(self, _empty_system_setup, args, kwargs, exp_u_aux): # Test add_speeds self.system.add_auxiliary_speeds(*args, **kwargs) assert self.system.u_aux[:] == exp_u_aux self._empty_system_check(exclude=('u_aux',)) # Test setter for u_ind and u_dep self.system.u_aux = exp_u_aux assert self.system.u_aux[:] == exp_u_aux self._empty_system_check(exclude=('u_aux',)) @pytest.mark.parametrize('args, kwargs', [ ((ua[2], q[0]), {}), ((ua[2], u[1]), {}), ((ua[0], ua[2]), {}), ((symbols('a'), ua[2]), {}), ]) def test_auxiliary_invalid(self, _filled_system_setup, args, kwargs): with pytest.raises(ValueError): self.system.add_auxiliary_speeds(*args, **kwargs) self._filled_system_check() @pytest.mark.parametrize('prop, add_func, args, kwargs', [ ('q_ind', 'add_coordinates', (q[0],), {}), ('q_dep', 'add_coordinates', (q[3],), {'independent': False}), ('u_ind', 'add_speeds', (u[0],), {}), ('u_dep', 'add_speeds', (u[3],), {'independent': False}), ('u_aux', 'add_auxiliary_speeds', (ua[2],), {}), ('kdes', 'add_kdes', (qd[0] - u[0],), {}), ('holonomic_constraints', 'add_holonomic_constraints', (q[0] - q[1],), {}), ('nonholonomic_constraints', 'add_nonholonomic_constraints', (u[0] - u[1],), {}), ('bodies', 'add_bodies', (RigidBody('body'),), {}), ('loads', 'add_loads', (Force(Point('P'), ReferenceFrame('N').x),), {}), ('actuators', 'add_actuators', (TorqueActuator( symbols('T'), ReferenceFrame('N').x, ReferenceFrame('A')),), {}), ]) def test_add_after_reset(self, _filled_system_setup, prop, add_func, args, kwargs): setattr(self.system, prop, ()) exclude = (prop, 'q', 'u') if prop in ('holonomic_constraints', 'nonholonomic_constraints'): exclude += ('velocity_constraints',) self._filled_system_check(exclude=exclude) assert list(getattr(self.system, prop)[:]) == [] getattr(self.system, add_func)(*args, **kwargs) assert list(getattr(self.system, prop)[:]) == list(args) @pytest.mark.parametrize('prop, add_func, value, error', [ ('q_ind', 'add_coordinates', symbols('a'), ValueError), ('q_dep', 'add_coordinates', symbols('a'), ValueError), ('u_ind', 'add_speeds', symbols('a'), ValueError), ('u_dep', 'add_speeds', symbols('a'), ValueError), ('u_aux', 'add_auxiliary_speeds', symbols('a'), ValueError), ('kdes', 'add_kdes', 7, TypeError), ('holonomic_constraints', 'add_holonomic_constraints', 7, TypeError), ('nonholonomic_constraints', 'add_nonholonomic_constraints', 7, TypeError), ('bodies', 'add_bodies', symbols('a'), TypeError), ('loads', 'add_loads', symbols('a'), TypeError), ('actuators', 'add_actuators', symbols('a'), TypeError), ]) def test_type_error(self, _filled_system_setup, prop, add_func, value, error): with pytest.raises(error): getattr(self.system, add_func)(value) with pytest.raises(error): setattr(self.system, prop, value) self._filled_system_check() @pytest.mark.parametrize('args, kwargs, exp_kdes', [ ((), {}, [ui - qdi for ui, qdi in zip(u[:4], qd[:4])]), ((u[4] - qd[4], u[5] - qd[5]), {}, [ui - qdi for ui, qdi in zip(u[:6], qd[:6])]), ]) def test_kdes(self, _filled_system_setup, args, kwargs, exp_kdes): # Test add_speeds self.system.add_kdes(*args, **kwargs) self._filled_system_check(exclude=('kdes',)) assert self.system.kdes[:] == exp_kdes # Test setter for kdes self.system.kdes = exp_kdes self._filled_system_check(exclude=('kdes',)) assert self.system.kdes[:] == exp_kdes @pytest.mark.parametrize('args, kwargs', [ ((u[0] - qd[0], u[4] - qd[4]), {}), ((-(u[0] - qd[0]), u[4] - qd[4]), {}), (([u[0] - u[0], u[4] - qd[4]]), {}), ]) def test_kdes_invalid(self, _filled_system_setup, args, kwargs): with pytest.raises(ValueError): self.system.add_kdes(*args, **kwargs) self._filled_system_check() @pytest.mark.parametrize('args, kwargs, exp_con', [ ((), {}, [q[2] - q[0] + q[1]]), ((q[4] - q[5], q[5] + q[3]), {}, [q[2] - q[0] + q[1], q[4] - q[5], q[5] + q[3]]), ]) def test_holonomic_constraints(self, _filled_system_setup, args, kwargs, exp_con): exclude = ('holonomic_constraints', 'velocity_constraints') exp_vel_con = [c.diff(t) for c in exp_con] + self.nhc # Test add_holonomic_constraints self.system.add_holonomic_constraints(*args, **kwargs) self._filled_system_check(exclude=exclude) assert self.system.holonomic_constraints[:] == exp_con assert self.system.velocity_constraints[:] == exp_vel_con # Test setter for holonomic_constraints self.system.holonomic_constraints = exp_con self._filled_system_check(exclude=exclude) assert self.system.holonomic_constraints[:] == exp_con assert self.system.velocity_constraints[:] == exp_vel_con @pytest.mark.parametrize('args, kwargs', [ ((q[2] - q[0] + q[1], q[4] - q[3]), {}), ((-(q[2] - q[0] + q[1]), q[4] - q[3]), {}), ((q[0] - q[0], q[4] - q[3]), {}), ]) def test_holonomic_constraints_invalid(self, _filled_system_setup, args, kwargs): with pytest.raises(ValueError): self.system.add_holonomic_constraints(*args, **kwargs) self._filled_system_check() @pytest.mark.parametrize('args, kwargs, exp_con', [ ((), {}, [u[3] - qd[1] + u[2]]), ((u[4] - u[5], u[5] + u[3]), {}, [u[3] - qd[1] + u[2], u[4] - u[5], u[5] + u[3]]), ]) def test_nonholonomic_constraints(self, _filled_system_setup, args, kwargs, exp_con): exclude = ('nonholonomic_constraints', 'velocity_constraints') exp_vel_con = self.vc[:len(self.hc)] + exp_con # Test add_nonholonomic_constraints self.system.add_nonholonomic_constraints(*args, **kwargs) self._filled_system_check(exclude=exclude) assert self.system.nonholonomic_constraints[:] == exp_con assert self.system.velocity_constraints[:] == exp_vel_con # Test setter for nonholonomic_constraints self.system.nonholonomic_constraints = exp_con self._filled_system_check(exclude=exclude) assert self.system.nonholonomic_constraints[:] == exp_con assert self.system.velocity_constraints[:] == exp_vel_con @pytest.mark.parametrize('args, kwargs', [ ((u[3] - qd[1] + u[2], u[4] - u[3]), {}), ((-(u[3] - qd[1] + u[2]), u[4] - u[3]), {}), ((u[0] - u[0], u[4] - u[3]), {}), (([u[0] - u[0], u[4] - u[3]]), {}), ]) def test_nonholonomic_constraints_invalid(self, _filled_system_setup, args, kwargs): with pytest.raises(ValueError): self.system.add_nonholonomic_constraints(*args, **kwargs) self._filled_system_check() @pytest.mark.parametrize('constraints, expected', [ ([], []), (qd[2] - qd[0] + qd[1], [qd[2] - qd[0] + qd[1]]), ([qd[2] + qd[1], u[2] - u[1]], [qd[2] + qd[1], u[2] - u[1]]), ]) def test_velocity_constraints_overwrite(self, _filled_system_setup, constraints, expected): self.system.velocity_constraints = constraints self._filled_system_check(exclude=('velocity_constraints',)) assert self.system.velocity_constraints[:] == expected def test_velocity_constraints_back_to_auto(self, _filled_system_setup): self.system.velocity_constraints = qd[3] - qd[2] self._filled_system_check(exclude=('velocity_constraints',)) assert self.system.velocity_constraints[:] == [qd[3] - qd[2]] self.system.velocity_constraints = None self._filled_system_check() def test_bodies(self, _filled_system_setup): rb1, rb2 = RigidBody('rb1'), RigidBody('rb2') p1, p2 = Particle('p1'), Particle('p2') self.system.add_bodies(rb1, p1) assert self.system.bodies == (*self.bodies, rb1, p1) self.system.add_bodies(p2) assert self.system.bodies == (*self.bodies, rb1, p1, p2) self.system.bodies = [] assert self.system.bodies == () self.system.bodies = p2 assert self.system.bodies == (p2,) symb = symbols('symb') pytest.raises(TypeError, lambda: self.system.add_bodies(symb)) pytest.raises(ValueError, lambda: self.system.add_bodies(p2)) with pytest.raises(TypeError): self.system.bodies = (rb1, rb2, p1, p2, symb) assert self.system.bodies == (p2,) def test_add_loads(self): system = System() N, A = ReferenceFrame('N'), ReferenceFrame('A') rb1 = RigidBody('rb1', frame=N) mc1 = Point('mc1') p1 = Particle('p1', mc1) system.add_loads(Torque(rb1, N.x), (mc1, A.x), Force(p1, A.x)) assert system.loads == ((N, N.x), (mc1, A.x), (mc1, A.x)) system.loads = [(A, A.x)] assert system.loads == ((A, A.x),) pytest.raises(ValueError, lambda: system.add_loads((N, N.x, N.y))) with pytest.raises(TypeError): system.loads = (N, N.x) assert system.loads == ((A, A.x),) def test_add_actuators(self): system = System() N, A = ReferenceFrame('N'), ReferenceFrame('A') act1 = TorqueActuator(symbols('T1'), N.x, N) act2 = TorqueActuator(symbols('T2'), N.y, N, A) system.add_actuators(act1) assert system.actuators == (act1,) assert system.loads == () system.actuators = (act2,) assert system.actuators == (act2,) def test_add_joints(self): q1, q2, q3, q4, u1, u2, u3 = dynamicsymbols('q1:5 u1:4') rb1, rb2, rb3, rb4, rb5 = symbols('rb1:6', cls=RigidBody) J1 = PinJoint('J1', rb1, rb2, q1, u1) J2 = PrismaticJoint('J2', rb2, rb3, q2, u2) J3 = PinJoint('J3', rb3, rb4, q3, u3) J_lag = PinJoint('J_lag', rb4, rb5, q4, q4.diff(t)) system = System() system.add_joints(J1) assert system.joints == (J1,) assert system.bodies == (rb1, rb2) assert system.q_ind == ImmutableMatrix([q1]) assert system.u_ind == ImmutableMatrix([u1]) assert system.kdes == ImmutableMatrix([u1 - q1.diff(t)]) system.add_bodies(rb4) system.add_coordinates(q3) system.add_kdes(u3 - q3.diff(t)) system.add_joints(J3) assert system.joints == (J1, J3) assert system.bodies == (rb1, rb2, rb4, rb3) assert system.q_ind == ImmutableMatrix([q1, q3]) assert system.u_ind == ImmutableMatrix([u1, u3]) assert system.kdes == ImmutableMatrix( [u1 - q1.diff(t), u3 - q3.diff(t)]) system.add_kdes(-(u2 - q2.diff(t))) system.add_joints(J2) assert system.joints == (J1, J3, J2) assert system.bodies == (rb1, rb2, rb4, rb3) assert system.q_ind == ImmutableMatrix([q1, q3, q2]) assert system.u_ind == ImmutableMatrix([u1, u3, u2]) assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t), -(u2 - q2.diff(t))]) system.add_joints(J_lag) assert system.joints == (J1, J3, J2, J_lag) assert system.bodies == (rb1, rb2, rb4, rb3, rb5) assert system.q_ind == ImmutableMatrix([q1, q3, q2, q4]) assert system.u_ind == ImmutableMatrix([u1, u3, u2, q4.diff(t)]) assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t), -(u2 - q2.diff(t))]) assert system.q_dep[:] == [] assert system.u_dep[:] == [] pytest.raises(ValueError, lambda: system.add_joints(J2)) pytest.raises(TypeError, lambda: system.add_joints(rb1)) def test_joints_setter(self, _filled_system_setup): self.system.joints = self.joints[1:] assert self.system.joints == self.joints[1:] self._filled_system_check(exclude=('joints',)) self.system.q_ind = () self.system.u_ind = () self.system.joints = self.joints self._filled_system_check() @pytest.mark.parametrize('name, joint_index', [ ('J1', 0), ('J2', 1), ('not_existing', None), ]) def test_get_joint(self, _filled_system_setup, name, joint_index): joint = self.system.get_joint(name) if joint_index is None: assert joint is None else: assert joint == self.joints[joint_index] @pytest.mark.parametrize('name, body_index', [ ('rb1', 0), ('rb3', 2), ('not_existing', None), ]) def test_get_body(self, _filled_system_setup, name, body_index): body = self.system.get_body(name) if body_index is None: assert body is None else: assert body == self.bodies[body_index] @pytest.mark.parametrize('eom_method', [KanesMethod, LagrangesMethod]) def test_form_eoms_calls_subclass(self, _moving_point_mass, eom_method): class MyMethod(eom_method): pass self.system.form_eoms(eom_method=MyMethod) assert isinstance(self.system.eom_method, MyMethod) @pytest.mark.parametrize('kwargs, expected', [ ({}, ImmutableMatrix([[-1, 0], [0, symbols('m')]])), ({'explicit_kinematics': True}, ImmutableMatrix([[1, 0], [0, symbols('m')]])), ]) def test_system_kane_form_eoms_kwargs(self, _moving_point_mass, kwargs, expected): self.system.form_eoms(**kwargs) assert self.system.mass_matrix_full == expected @pytest.mark.parametrize('kwargs, mm, gm', [ ({}, ImmutableMatrix([[1, 0], [0, symbols('m')]]), ImmutableMatrix([q[0].diff(t), 0])), ]) def test_system_lagrange_form_eoms_kwargs(self, _moving_point_mass, kwargs, mm, gm): self.system.form_eoms(eom_method=LagrangesMethod, **kwargs) assert self.system.mass_matrix_full == mm assert self.system.forcing_full == gm @pytest.mark.parametrize('eom_method, kwargs, error', [ (KanesMethod, {'non_existing_kwarg': 1}, TypeError), (LagrangesMethod, {'non_existing_kwarg': 1}, TypeError), (KanesMethod, {'bodies': []}, ValueError), (KanesMethod, {'kd_eqs': []}, ValueError), (LagrangesMethod, {'bodies': []}, ValueError), (LagrangesMethod, {'Lagrangian': 1}, ValueError), ]) def test_form_eoms_kwargs_errors(self, _empty_system_setup, eom_method, kwargs, error): self.system.q_ind = q[0] p = Particle('p', mass=symbols('m')) self.system.add_bodies(p) p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x) with pytest.raises(error): self.system.form_eoms(eom_method=eom_method, **kwargs) class TestValidateSystem(TestSystemBase): @pytest.mark.parametrize('valid_method, invalid_method, with_speeds', [ (KanesMethod, LagrangesMethod, True), (LagrangesMethod, KanesMethod, False) ]) def test_only_valid(self, valid_method, invalid_method, with_speeds): self._create_filled_system(with_speeds=with_speeds) self.system.validate_system(valid_method) # Test Lagrange should fail due to the usage of generalized speeds with pytest.raises(ValueError): self.system.validate_system(invalid_method) @pytest.mark.parametrize('method, with_speeds', [ (KanesMethod, True), (LagrangesMethod, False)]) def test_missing_joint_coordinate(self, method, with_speeds): self._create_filled_system(with_speeds=with_speeds) self.system.q_ind = self.q_ind[1:] self.system.u_ind = self.u_ind[:-1] self.system.kdes = self.kdes[:-1] pytest.raises(ValueError, lambda: self.system.validate_system(method)) def test_missing_joint_speed(self, _filled_system_setup): self.system.q_ind = self.q_ind[:-1] self.system.u_ind = self.u_ind[1:] self.system.kdes = self.kdes[:-1] pytest.raises(ValueError, lambda: self.system.validate_system()) def test_missing_joint_kdes(self, _filled_system_setup): self.system.kdes = self.kdes[1:] pytest.raises(ValueError, lambda: self.system.validate_system()) def test_negative_joint_kdes(self, _filled_system_setup): self.system.kdes = [-self.kdes[0]] + self.kdes[1:] self.system.validate_system() @pytest.mark.parametrize('method, with_speeds', [ (KanesMethod, True), (LagrangesMethod, False)]) def test_missing_holonomic_constraint(self, method, with_speeds): self._create_filled_system(with_speeds=with_speeds) self.system.holonomic_constraints = [] self.system.nonholonomic_constraints = self.nhc + [ self.u_ind[1] - self.u_dep[0] + self.u_ind[0]] pytest.raises(ValueError, lambda: self.system.validate_system(method)) self.system.q_dep = [] self.system.q_ind = self.q_ind + self.q_dep self.system.validate_system(method) def test_missing_nonholonomic_constraint(self, _filled_system_setup): self.system.nonholonomic_constraints = [] pytest.raises(ValueError, lambda: self.system.validate_system()) self.system.u_dep = self.u_dep[1] self.system.u_ind = self.u_ind + [self.u_dep[0]] self.system.validate_system() def test_number_of_coordinates_speeds(self, _filled_system_setup): # Test more speeds than coordinates self.system.u_ind = self.u_ind + [u[5]] self.system.kdes = self.kdes + [u[5] - qd[5]] self.system.validate_system() # Test more coordinates than speeds self.system.q_ind = self.q_ind self.system.u_ind = self.u_ind[:-1] self.system.kdes = self.kdes[:-1] pytest.raises(ValueError, lambda: self.system.validate_system()) def test_number_of_kdes(self, _filled_system_setup): # Test wrong number of kdes self.system.kdes = self.kdes[:-1] pytest.raises(ValueError, lambda: self.system.validate_system()) self.system.kdes = self.kdes + [u[2] + u[1] - qd[2]] pytest.raises(ValueError, lambda: self.system.validate_system()) def test_duplicates(self, _filled_system_setup): # This is basically a redundant feature, which should never fail self.system.validate_system(check_duplicates=True) def test_speeds_in_lagrange(self, _filled_system_setup_no_speeds): self.system.u_ind = u[:len(self.u_ind)] with pytest.raises(ValueError): self.system.validate_system(LagrangesMethod) self.system.u_ind = [] self.system.validate_system(LagrangesMethod) self.system.u_aux = ua with pytest.raises(ValueError): self.system.validate_system(LagrangesMethod) self.system.u_aux = [] self.system.validate_system(LagrangesMethod) self.system.add_joints( PinJoint('Ju', RigidBody('rbu1'), RigidBody('rbu2'))) self.system.u_ind = [] with pytest.raises(ValueError): self.system.validate_system(LagrangesMethod) class TestSystemExamples: def test_cart_pendulum_kanes(self): # This example is the same as in the top documentation of System # Added a spring to the cart g, l, mc, mp, k = symbols('g l mc mp k') F, qp, qc, up, uc = dynamicsymbols('F qp qc up uc') rail = RigidBody('rail') cart = RigidBody('cart', mass=mc) bob = Particle('bob', mass=mp) bob_frame = ReferenceFrame('bob_frame') system = System.from_newtonian(rail) assert system.bodies == (rail,) assert system.frame == rail.frame assert system.fixed_point == rail.masscenter slider = PrismaticJoint('slider', rail, cart, qc, uc, joint_axis=rail.x) pin = PinJoint('pin', cart, bob, qp, up, joint_axis=cart.z, child_interframe=bob_frame, child_point=l * bob_frame.y) system.add_joints(slider, pin) assert system.joints == (slider, pin) assert system.get_joint('slider') == slider assert system.get_body('bob') == bob system.apply_uniform_gravity(-g * system.y) system.add_loads((cart.masscenter, F * rail.x)) system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart)) system.validate_system() system.form_eoms() assert isinstance(system.eom_method, KanesMethod) assert (simplify(system.mass_matrix - ImmutableMatrix( [[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]])) == zeros(2, 2)) assert (simplify(system.forcing - ImmutableMatrix([ [mp * l * up ** 2 * sin(qp) + F], [-mp * g * l * sin(qp) + k * qp]])) == zeros(2, 1)) system.add_holonomic_constraints( sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x))) assert system.eom_method is None system.q_ind, system.q_dep = qp, qc system.u_ind, system.u_dep = up, uc system.validate_system() # Computed solution based on manually solving the constraints subs = {qc: -l * sin(qp), uc: -l * cos(qp) * up, uc.diff(t): l * (up ** 2 * sin(qp) - up.diff(t) * cos(qp))} upd_expected = ( (-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * up ** 2 / 2 - l * mp * sin(2 * qp) * up ** 2 / 2 - F * cos(qp)) / (l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2))) upd_sol = tuple(solve(system.form_eoms().xreplace(subs), up.diff(t)).values())[0] assert simplify(upd_sol - upd_expected) == 0 assert isinstance(system.eom_method, KanesMethod) # Test other output Mk = -ImmutableMatrix([[0, 1], [1, 0]]) gk = -ImmutableMatrix([uc, up]) Md = ImmutableMatrix([[-l ** 2 * mp * cos(qp) ** 2 + l ** 2 * mp, l * mp * cos(qp) - l * (mc + mp) * cos(qp)], [l * cos(qp), 1]]) gd = ImmutableMatrix( [[-g * l * mp * sin(qp) + k * qp - l ** 2 * mp * up ** 2 * sin(qp) * cos(qp) - l * F * cos(qp)], [l * up ** 2 * sin(qp)]]) Mm = (Mk.row_join(zeros(2, 2))).col_join(zeros(2, 2).row_join(Md)) gm = gk.col_join(gd) assert simplify(system.mass_matrix - Md) == zeros(2, 2) assert simplify(system.forcing - gd) == zeros(2, 1) assert simplify(system.mass_matrix_full - Mm) == zeros(4, 4) assert simplify(system.forcing_full - gm) == zeros(4, 1) def test_cart_pendulum_lagrange(self): # Lagrange version of test_cart_pendulus_kanes # Added a spring to the cart g, l, mc, mp, k = symbols('g l mc mp k') F, qp, qc = dynamicsymbols('F qp qc') qpd, qcd = dynamicsymbols('qp qc', 1) rail = RigidBody('rail') cart = RigidBody('cart', mass=mc) bob = Particle('bob', mass=mp) bob_frame = ReferenceFrame('bob_frame') system = System.from_newtonian(rail) assert system.bodies == (rail,) assert system.frame == rail.frame assert system.fixed_point == rail.masscenter slider = PrismaticJoint('slider', rail, cart, qc, qcd, joint_axis=rail.x) pin = PinJoint('pin', cart, bob, qp, qpd, joint_axis=cart.z, child_interframe=bob_frame, child_point=l * bob_frame.y) system.add_joints(slider, pin) assert system.joints == (slider, pin) assert system.get_joint('slider') == slider assert system.get_body('bob') == bob for body in system.bodies: body.potential_energy = body.mass * g * body.masscenter.pos_from( system.fixed_point).dot(system.y) system.add_loads((cart.masscenter, F * rail.x)) system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart)) system.validate_system(LagrangesMethod) system.form_eoms(LagrangesMethod) assert (simplify(system.mass_matrix - ImmutableMatrix( [[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]])) == zeros(2, 2)) assert (simplify(system.forcing - ImmutableMatrix([ [mp * l * qpd ** 2 * sin(qp) + F], [-mp * g * l * sin(qp) + k * qp]] )) == zeros(2, 1)) system.add_holonomic_constraints( sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x))) assert system.eom_method is None system.q_ind, system.q_dep = qp, qc # Computed solution based on manually solving the constraints subs = {qc: -l * sin(qp), qcd: -l * cos(qp) * qpd, qcd.diff(t): l * (qpd ** 2 * sin(qp) - qpd.diff(t) * cos(qp))} qpdd_expected = ( (-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * qpd ** 2 / 2 - l * mp * sin(2 * qp) * qpd ** 2 / 2 - F * cos(qp)) / (l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2))) eoms = system.form_eoms(LagrangesMethod) lam1 = system.eom_method.lam_vec[0] lam1_sol = system.eom_method.solve_multipliers()[lam1] qpdd_sol = solve(eoms[0].xreplace({lam1: lam1_sol}).xreplace(subs), qpd.diff(t))[0] assert simplify(qpdd_sol - qpdd_expected) == 0 assert isinstance(system.eom_method, LagrangesMethod) # Test other output Md = ImmutableMatrix([[l ** 2 * mp, l * mp * cos(qp), -l * cos(qp)], [l * mp * cos(qp), mc + mp, -1]]) gd = ImmutableMatrix( [[-g * l * mp * sin(qp) + k * qp], [l * mp * sin(qp) * qpd ** 2 + F]]) Mm = (eye(2).row_join(zeros(2, 3))).col_join(zeros(3, 2).row_join( Md.col_join(ImmutableMatrix([l * cos(qp), 1, 0]).T))) gm = ImmutableMatrix([qpd, qcd] + gd[:] + [l * sin(qp) * qpd ** 2]) assert simplify(system.mass_matrix - Md) == zeros(2, 3) assert simplify(system.forcing - gd) == zeros(2, 1) assert simplify(system.mass_matrix_full - Mm) == zeros(5, 5) assert simplify(system.forcing_full - gm) == zeros(5, 1) def test_box_on_ground(self): # Particle sliding on ground with friction. The applied force is assumed # to be positive and to be higher than the friction force. g, m, mu = symbols('g m mu') q, u, ua = dynamicsymbols('q u ua') N, F = dynamicsymbols('N F', positive=True) P = Particle("P", mass=m) system = System() system.add_bodies(P) P.masscenter.set_pos(system.fixed_point, q * system.x) P.masscenter.set_vel(system.frame, u * system.x + ua * system.y) system.q_ind, system.u_ind, system.u_aux = [q], [u], [ua] system.kdes = [q.diff(t) - u] system.apply_uniform_gravity(-g * system.y) system.add_loads( Force(P, N * system.y), Force(P, F * system.x - mu * N * system.x)) system.validate_system() system.form_eoms() # Test other output Mk = ImmutableMatrix([1]) gk = ImmutableMatrix([u]) Md = ImmutableMatrix([m]) gd = ImmutableMatrix([F - mu * N]) Mm = (Mk.row_join(zeros(1, 1))).col_join(zeros(1, 1).row_join(Md)) gm = gk.col_join(gd) aux_eqs = ImmutableMatrix([N - m * g]) assert simplify(system.mass_matrix - Md) == zeros(1, 1) assert simplify(system.forcing - gd) == zeros(1, 1) assert simplify(system.mass_matrix_full - Mm) == zeros(2, 2) assert simplify(system.forcing_full - gm) == zeros(2, 1) assert simplify(system.eom_method.auxiliary_eqs - aux_eqs ) == zeros(1, 1)