def test_trans_of(self, x, y, z, q):
r1 = np.array(spw.translation_of(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3]))).astype(float)
r2 = np.identity(4)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def test_frame3_quaternion(self, x, y, z, q):
r1 = np.array(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3])).astype(float)
r2 = quaternion_matrix(q)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def test_inverse_frame(self, x, y, z, q):
r1 = np.array(spw.inverse_frame(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3]))).astype(float)
r2 = PyKDL.Frame()
r2.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3])
r2.p[0] = x
r2.p[1] = y
r2.p[2] = z
r2 = r2.Inverse()
r2 = pykdl_frame_to_numpy(r2)
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def test_rot_of(self, x, y, z, q):
r1 = np.array(spw.rotation_of(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3]))).astype(float)
r2 = quaternion_matrix(q)
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def test_pos_of(self, x, y, z, q):
r1 = np.array(spw.position_of(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3]))).astype(float).T[0]
r2 = np.array([x, y, z, 1])
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))