def test_assemble(self):
# Test system:
#
# /-- m2
# m1 -----conn----|
# \-- m3
s = System()
m1 = RigidBody('m1', 1.3)
m2 = RigidBody('m2', 3.4)
m3 = RigidBody('m3', 7.5)
conn = RigidConnection('conn')
s.add_leaf(conn)
s.add_leaf(m1)
conn.add_leaf(m2)
conn.add_leaf(m3)
s.setup()
# Check starting mass matrices of elements are as expected
assert_aae(np.diag(m1.mass_vv[:3, :3]), 1.3)
assert_aae(np.diag(m2.mass_vv[:3, :3]), 3.4)
assert_aae(np.diag(m3.mass_vv[:3, :3]), 7.5)
# Initially make system matrix empty for testing
s.lhs[:, :] = 0
assert_aae(s.lhs, 0)
# After assembly, the mass matrices are put in the correct places:
# 0:6 -> m1 node
# 6:12 -> conn constraints
# 12:18 -> m2 node
# 12:18 -> m3 node
s.assemble()
M = s.lhs.copy()
# Subtract expected mass
M[0:6, 0:6] -= m1.mass_vv
M[12:18, 12:18] -= m2.mass_vv + m3.mass_vv
# Subtract expected constraints
M[0:6, 6:12] -= conn.F_vp
M[6:12, 0:6] -= conn.F_vp
M[12:18, 6:12] -= conn.F_vd
M[6:12, 12:18] -= conn.F_vd
assert_aae(M, 0)
