def test_rot_from_roll_pitch_yaw(self):
roll = 1.8526989
pitch = -0.75699
yaw = -2.89421
rot1 = np.dot(rot_z(yaw), np.dot(rot_y(pitch), rot_x(roll)))
rot2 = rot_from_roll_pitch_yaw(roll, pitch, yaw)
self.check_rot_equal(rot1, rot2)
def test_rot_to_roll_pitch_yaw_singular(self):
roll = 1.8526989
pitch = np.pi / 2
yaw = 2.89421
rot = np.dot(rot_z(yaw), np.dot(rot_y(pitch), rot_x(roll)))
rpy = rot_to_roll_pitch_yaw(rot)
self.assertAlmostEqual(roll - yaw, rpy[0])
self.assertAlmostEqual(pitch, rpy[1])
self.assertAlmostEqual(0, rpy[2])
def test_rot_to_roll_pitch_yaw(self):
roll = 1.8526989
pitch = -0.75699
yaw = -2.89421
rot = np.dot(rot_z(yaw), np.dot(rot_y(pitch), rot_x(roll)))
rpy = rot_to_roll_pitch_yaw(rot)
self.assertAlmostEqual(roll, rpy[0])
self.assertAlmostEqual(pitch, rpy[1])
self.assertAlmostEqual(yaw, rpy[2])
def test_rot_to_roll_pitch_yaw_singular_2(self):
roll = 0.846
pitch = -np.pi / 2
yaw = 1.34321
rot = np.dot(rot_z(yaw), np.dot(rot_y(pitch), rot_x(roll)))
rpy = rot_to_roll_pitch_yaw(rot)
self.assertAlmostEqual(roll + yaw, rpy[0])
self.assertAlmostEqual(pitch, rpy[1])
self.assertAlmostEqual(0, rpy[2])
def test_rot_x(self):
angle = 2.1921
rot1 = rot_x(angle)
rot2 = rot_from_angle_axis(angle, np.array([1, 0, 0]))
self.check_rot_equal(rot1, rot2)