def test_distal_node_is_transformed_by_joint_freedoms(self):
j = FreeJoint('joint')
j.rp = array([3.5, 9.21, 8.6])
j.Rp = eye(3)
# Test distal transform -- translation
j.xstrain[:3] = [2, 3, 4]
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
assert_array_equal(j.Rd, j.Rp)
# Now add a rotation of 60 deg about x axis
j.xstrain[3] = pi / 3
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
# new unit vectors after rotation
assert_array_almost_equal(j.Rd, c_[[1, 0, 0],
[0, cos(pi/3), sin(pi/3)],
[0, -sin(pi/3), cos(pi/3)]])
# Check combination of Euler angles: 90deg yaw and pitch
j.xstrain[3:] = [0, pi/2, pi/2]
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
# new unit vectors after rotation.
# 1) 90deg yaw -> y, -x, z
# 2) 90deg pitch -> -z, -x, y
assert_array_almost_equal(j.Rd, c_[[0, 0, -1], [-1, 0, 0], [0, 1, 0]])
def test_distal_node_is_transformed_by_joint_freedoms(self):
j = FreeJoint('joint')
j.rp = array([3.5, 9.21, 8.6])
j.Rp = eye(3)
# Test distal transform -- translation
j.xstrain[:3] = [2, 3, 4]
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
assert_array_equal(j.Rd, j.Rp)
# Now add a rotation of 60 deg about x axis
j.xstrain[3] = pi / 3
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
# new unit vectors after rotation
assert_array_almost_equal(j.Rd, c_[[1, 0, 0],
[0, cos(pi/3), sin(pi/3)],
[0, -sin(pi/3), cos(pi/3)]])
# Check combination of Euler angles: 90deg yaw and pitch
j.xstrain[3:] = [0, pi/2, pi/2]
j.calc_distal_pos()
assert_array_equal(j.rd, j.rp + [2, 3, 4])
# new unit vectors after rotation.
# 1) 90deg yaw -> y, -x, z
# 2) 90deg pitch -> -z, -x, y
assert_array_almost_equal(j.Rd, c_[[0, 0, -1], [-1, 0, 0], [0, 1, 0]])
def test_velocity_transforms_depend_on_joint_orientation(self):
j = FreeJoint('joint')
j.rp = array([0, 0, 8.6])
# x-velocity: shouldn't depend on speed
j.vstrain[:] = [2.3, 0, 0, 0, 0, 0]
j.calc_kinematics()
assert_array_equal(j.F_vp, eye(6)) # movement of p node moves d node
assert_array_equal(j.F_ve, eye(6)) # with zero rotation, direct
assert_array_equal(j.F_v2, 0)
# now apply a 90 deg yaw angle (about z axis): now 'pitch'
# causes a -ve global x-rotation, and 'roll' causes a +ve
# global y-rotation
j.xstrain[5] = pi / 2
j.calc_kinematics()
assert_array_equal(j.F_vp, eye(6)) # movement of p node moves d node
assert_array_equal(j.F_v2, 0)
F_ve = eye(6)
F_ve[3:, 3] = [0, 1, 0]
F_ve[3:, 4] = [-1, 0, 0]
assert_array_almost_equal(j.F_ve, F_ve)
def test_velocity_transforms_depend_on_joint_orientation(self):
j = FreeJoint('joint')
j.rp = array([0, 0, 8.6])
# x-velocity: shouldn't depend on speed
j.vstrain[:] = [2.3, 0, 0, 0, 0, 0]
j.calc_kinematics()
assert_array_equal(j.F_vp, eye(6)) # movement of p node moves d node
assert_array_equal(j.F_ve, eye(6)) # with zero rotation, direct
assert_array_equal(j.F_v2, 0)
# now apply a 90 deg yaw angle (about z axis): now 'pitch'
# causes a -ve global x-rotation, and 'roll' causes a +ve
# global y-rotation
j.xstrain[5] = pi / 2
j.calc_kinematics()
assert_array_equal(j.F_vp, eye(6)) # movement of p node moves d node
assert_array_equal(j.F_v2, 0)
F_ve = eye(6)
F_ve[3:, 3] = [0, 1, 0]
F_ve[3:, 4] = [-1, 0, 0]
assert_array_almost_equal(j.F_ve, F_ve)
