def test_rotation_matrix(self): R = mut.RotationMatrix() self.assertTrue(np.allclose(R.matrix(), np.eye(3))) self.assertTrue( np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3))) R = mut.RotationMatrix(quaternion=Quaternion.Identity()) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0])) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) # - Nontrivial quaternion. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) R = mut.RotationMatrix(quaternion=q) q_R = R.ToQuaternion() self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz())) # - Inverse. R_I = R.inverse().multiply(R) self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
def test_rigid_transform(self): def check_equality(X_actual, X_expected_matrix): # TODO(eric.cousineau): Use `IsNearlyEqualTo`. self.assertIsInstance(X_actual, mut.RigidTransform) self.assertTrue( np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix)) # - Constructors. X_I = np.eye(4) check_equality(mut.RigidTransform(), X_I) check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I) R_I = mut.RotationMatrix() p_I = np.zeros(3) rpy_I = mut.RollPitchYaw(0, 0, 0) quaternion_I = Quaternion.Identity() angle = np.pi * 0 axis = [0, 0, 1] angle_axis = AngleAxis(angle=angle, axis=axis) check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I) check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I) check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I) check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I) check_equality(mut.RigidTransform(R=R_I), X_I) check_equality(mut.RigidTransform(p=p_I), X_I) # - Accessors, mutators, and general methods. X = mut.RigidTransform() X.set(R=R_I, p=p_I) X.SetFromIsometry3(pose=Isometry3.Identity()) check_equality(mut.RigidTransform.Identity(), X_I) self.assertIsInstance(X.rotation(), mut.RotationMatrix) X.set_rotation(R=R_I) self.assertIsInstance(X.translation(), np.ndarray) X.set_translation(p=np.zeros(3)) self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I)) self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3])) self.assertIsInstance(X.GetAsIsometry3(), Isometry3) check_equality(X.inverse(), X_I) self.assertIsInstance(X.multiply(other=mut.RigidTransform()), mut.RigidTransform) self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray)