def test_rotmats(self):
assert_array_equal(rotmat_x(0), eye(3))
assert_array_equal(rotmat_y(0), eye(3))
assert_array_equal(rotmat_z(0), eye(3))
# Rotate 45 deg about each axis
assert_aae(dot(rotmat_x(pi / 4), [1, 1, 1]), [1, 0, sqrt(2)])
assert_aae(dot(rotmat_y(pi / 4), [1, 1, 1]), [sqrt(2), 1, 0])
assert_aae(dot(rotmat_z(pi / 4), [1, 1, 1]), [0, sqrt(2), 1])
def test_rotmats(self):
assert_array_equal(rotmat_x(0), eye(3))
assert_array_equal(rotmat_y(0), eye(3))
assert_array_equal(rotmat_z(0), eye(3))
# Rotate 45 deg about each axis
assert_aae(dot(rotmat_x(pi / 4), [1, 1, 1]), [1, 0, sqrt(2)])
assert_aae(dot(rotmat_y(pi / 4), [1, 1, 1]), [sqrt(2), 1, 0])
assert_aae(dot(rotmat_z(pi / 4), [1, 1, 1]), [0, sqrt(2), 1])
def test_element_mass_when_rotated_and_offset(self):
Rp = dot(rotmat_y(pi / 5), dot(rotmat_z(-pi / 7),
rotmat_x(3 * pi / 4)))
self.element.rp[:] = [304.3, 12.3, -402.0]
self.element.Rp[:, :] = Rp
self.element.calc_mass()
# Expected values: rod along x axis
m = self.density * self.length
Iy = m * self.length**2 / 3
expected_mass = m * eye(3)
expected_inertia = diag([0, Iy, Iy])
expected_offdiag = zeros((3, 3))
# Y accel -> positive moment about Z
# Z accel -> negative moment about Y
expected_offdiag[2, 1] = m * self.length / 2
expected_offdiag[1, 2] = -m * self.length / 2
transform = lambda A: dot(Rp, dot(A, Rp.T))
assert_aae(expected_mass, transform(expected_mass))
elmass = self.element.mass_vv
assert_aae(elmass[:3, :3], transform(expected_mass))
assert_aae(elmass[3:, 3:], transform(expected_inertia))
assert_aae(elmass[3:, :3], transform(expected_offdiag))
assert_aae(elmass[:3, 3:], transform(expected_offdiag).T)
def test_element_mass_when_rotated_and_offset(self):
Rp = dot(rotmat_y(pi/5), dot(rotmat_z(-pi/7), rotmat_x(3*pi/4)))
self.element.rp[:] = [304.3, 12.3, -402.0]
self.element.Rp[:, :] = Rp
self.element.calc_mass()
# Expected values: rod along x axis
m = self.density * self.length
Iy = m * self.length**2 / 3
expected_mass = m * eye(3)
expected_inertia = diag([0, Iy, Iy])
expected_offdiag = zeros((3, 3))
# Y accel -> positive moment about Z
# Z accel -> negative moment about Y
expected_offdiag[2, 1] = m * self.length / 2
expected_offdiag[1, 2] = -m * self.length / 2
transform = lambda A: dot(Rp, dot(A, Rp.T))
assert_aae(expected_mass, transform(expected_mass))
elmass = self.element.mass_vv
assert_aae(elmass[:3, :3], transform(expected_mass))
assert_aae(elmass[3:, 3:], transform(expected_inertia))
assert_aae(elmass[3:, :3], transform(expected_offdiag))
assert_aae(elmass[:3, 3:], transform(expected_offdiag).T)
def test_calculates_mass_and_inertia_in_global_coordinates(self):
# simple rigid body at origin: this just checks the mass and
# inertia carry directly through to element mass matrix
II = diag([4.2, 6.7, 11.7])
b = RigidBody('body', mass=4.3, inertia=II)
b.calc_mass()
assert_array_equal(b.mass_vv[:3, :3], 4.3 * eye(3))
assert_array_equal(b.mass_vv[:3, 3:], 0)
assert_array_equal(b.mass_vv[3:, :3], 0)
assert_array_equal(b.mass_vv[3:, 3:], II)
assert_array_equal(b.quad_forces, 0)
# now rotate node by 90 deg about z axis, swapping x & y. This
# confirms that the mass part doesn't change, but the inertia
# matrix is updated.
b.Rp = rotmat_z(pi / 2)
b.calc_mass()
assert_array_equal(b.mass_vv[:3, :3], 4.3 * eye(3))
assert_array_equal(b.mass_vv[:3, 3:], 0)
assert_array_equal(b.mass_vv[3:, :3], 0)
assert_array_almost_equal(b.mass_vv[3:, 3:], diag([6.7, 4.2, 11.7]))
assert_array_equal(b.quad_forces, 0)
def test_calculates_mass_and_inertia_in_global_coordinates(self):
# simple rigid body at origin: this just checks the mass and
# inertia carry directly through to element mass matrix
II = diag([4.2, 6.7, 11.7])
b = RigidBody("body", mass=4.3, inertia=II)
b.calc_mass()
assert_array_equal(b.mass_vv[:3, :3], 4.3 * eye(3))
assert_array_equal(b.mass_vv[:3, 3:], 0)
assert_array_equal(b.mass_vv[3:, :3], 0)
assert_array_equal(b.mass_vv[3:, 3:], II)
assert_array_equal(b.quad_forces, 0)
# now rotate node by 90 deg about z axis, swapping x & y. This
# confirms that the mass part doesn't change, but the inertia
# matrix is updated.
b.Rp = rotmat_z(pi / 2)
b.calc_mass()
assert_array_equal(b.mass_vv[:3, :3], 4.3 * eye(3))
assert_array_equal(b.mass_vv[:3, 3:], 0)
assert_array_equal(b.mass_vv[3:, :3], 0)
assert_array_almost_equal(b.mass_vv[3:, 3:], diag([6.7, 4.2, 11.7]))
assert_array_equal(b.quad_forces, 0)
def test_transform(self):
"""
Test transformation matrix from global to local coordinates
"""
# values not important for this test
modes = ModesFromScratch(arange(10), 1, 1, 1, 1)
def getY(Rp, rp, rd):
rp, rd = asarray(rp), asarray(rd)
return modes.transformation_to_global_coords(Rp, rp, rd)
###### Simple case with Rp = I ######
Y = getY(eye(3), [0, 0, 0], [1, 0, 0])
# Velocity of distal node directly becomes displacement if prox fixed
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1])
# Check now what happens if proximal node has a velocity
assert_array_equal(dot(Y, [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0])
assert_array_equal(dot(Y, [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, -1])
###### Now with Rp = rotmat_z(45 deg) ######
Y = getY(rotmat_z(pi/4), [0, 0, 0], [1, 0, 0])
c, s = cos(pi/4), sin(pi/4)
# Input velocities aligned with global axes are transformed
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, c, -s, 0, 0, 0, 0])
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, c, s, 0, 0, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0])
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, c, s, 0]),
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
assert_array_equal(dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1])
# Check now what happens if proximal node has a velocity
assert_aae( # noqa
dot(Y, [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, -c, s, 0, 0, 0, 0])
assert_array_equal(dot(Y, [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
def test_transform(self):
"""
Test transformation matrix from global to local coordinates
"""
# values not important for this test
modes = ModesFromScratch(arange(10), 1, 1, 1, 1)
def getY(Rp, rp, rd):
rp, rd = asarray(rp), asarray(rd)
return modes.transformation_to_global_coords(Rp, rp, rd)
###### Simple case with Rp = I ######
Y = getY(eye(3), [0, 0, 0], [1, 0, 0])
# Velocity of distal node directly becomes displacement if prox fixed
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1])
# Check now what happens if proximal node has a velocity
assert_array_equal(
dot(Y, [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, -1])
###### Now with Rp = rotmat_z(45 deg) ######
Y = getY(rotmat_z(pi / 4), [0, 0, 0], [1, 0, 0])
c, s = cos(pi / 4), sin(pi / 4)
# Input velocities aligned with global axes are transformed
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, c, -s, 0, 0, 0, 0])
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, c, s, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0])
assert_aae( # noqa
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, c, s, 0]),
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
assert_array_equal(
dot(Y, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1])
# Check now what happens if proximal node has a velocity
assert_aae( # noqa
dot(Y, [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
[0, 0, 0, 0, 0, 0, -c, s, 0, 0, 0, 0])
assert_array_equal(
dot(Y, [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]), # noqa
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
