File: C:/Users/fred/anaconda3/Lib/site-packages/sympy/physics/mechanics/tests/test_system_class.py
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)