def test_isometry3(self, T): Isometry3 = mut.Isometry3_[T] # - Default constructor transform = Isometry3() self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T) X = np.eye(4, 4) numpy_compare.assert_float_equal(transform.matrix(), X) numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X) if T == float: self.assertEqual(str(transform), str(X)) # - Constructor with (X) transform = Isometry3(matrix=X) numpy_compare.assert_float_equal(transform.matrix(), X) # - Copy constructor. cp = Isometry3(other=transform) numpy_compare.assert_equal(transform.matrix(), cp.matrix()) # - Identity transform = Isometry3.Identity() numpy_compare.assert_float_equal(transform.matrix(), X) # - Constructor with (R, p) R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]]) p = np.array([1., 2, 3]) X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1])) transform = Isometry3(rotation=R, translation=p) numpy_compare.assert_float_equal(transform.matrix(), X) numpy_compare.assert_float_equal(transform.translation(), p) transform.set_translation(-p) numpy_compare.assert_float_equal(transform.translation(), -p) numpy_compare.assert_float_equal(transform.rotation(), R) transform.set_rotation(R.T) numpy_compare.assert_float_equal(transform.rotation(), R.T) # - Check transactions for bad values. if T != Expression: transform = Isometry3(rotation=R, translation=p) R_bad = np.copy(R) R_bad[0, 0] = 10. with self.assertRaises(RuntimeError): transform.set_rotation(R_bad) numpy_compare.assert_float_equal(transform.rotation(), R) X_bad = np.copy(X) X_bad[:3, :3] = R_bad with self.assertRaises(RuntimeError): transform.set_matrix(X_bad) numpy_compare.assert_float_equal(transform.matrix(), X) # Test `type_caster`s. if T == float: value = test_util.create_isometry() self.assertTrue(isinstance(value, mut.Isometry3)) test_util.check_isometry(value) # Operations. transform = Isometry3(rotation=R, translation=p) transform_I = transform.inverse().multiply(transform) numpy_compare.assert_float_equal(transform_I.matrix(), np.eye(4)) numpy_compare.assert_float_equal( transform.multiply(position=[10, 20, 30]), [21., -8, 33]) if six.PY3: numpy_compare.assert_float_equal( eval("transform.inverse() @ transform").matrix(), np.eye(4)) numpy_compare.assert_float_equal(eval("transform @ [10, 20, 30]"), [21., -8, 33])
def test_angle_axis(self, T): AngleAxis = mut.AngleAxis_[T] value_identity = AngleAxis.Identity() self.assertEqual(numpy_compare.resolve_type(value_identity.angle()), T) numpy_compare.assert_float_equal(value_identity.angle(), 0.) numpy_compare.assert_float_equal(value_identity.axis(), [1., 0, 0]) # Construct with rotation matrix. R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]]) value = AngleAxis(rotation=R) numpy_compare.assert_float_allclose(value.rotation(), R) numpy_compare.assert_float_allclose(copy.copy(value).rotation(), R) numpy_compare.assert_float_allclose(value.inverse().rotation(), R.T) numpy_compare.assert_float_allclose( value.multiply(value.inverse()).rotation(), np.eye(3)) if six.PY3: numpy_compare.assert_float_allclose( eval("value @ value.inverse()").rotation(), np.eye(3)) value.set_rotation(np.eye(3)) numpy_compare.assert_float_equal(value.rotation(), np.eye(3)) # Construct with quaternion. Quaternion = mut.Quaternion_[T] q = Quaternion(R) value = AngleAxis(quaternion=q) numpy_compare.assert_float_allclose(value.quaternion().wxyz(), numpy_compare.to_float(q.wxyz())) value.set_quaternion(Quaternion.Identity()) numpy_compare.assert_float_equal(value.quaternion().wxyz(), [1., 0, 0, 0]) # Test setters. value = AngleAxis(value_identity) value.set_angle(np.pi / 4) v = normalize(np.array([0.1, 0.2, 0.3])) if T != Expression: with self.assertRaises(RuntimeError): value.set_axis([0.1, 0.2, 0.3]) value.set_axis(v) numpy_compare.assert_float_equal(value.angle(), np.pi / 4) numpy_compare.assert_float_equal(value.axis(), v) # Cast. self.check_cast(mut.AngleAxis_, T) # Test symmetry based on accessors. # N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal # angles and axes to a half-plane. angle = np.pi / 6 axis = normalize([0.1, 0.2, 0.3]) value = AngleAxis(angle=angle, axis=axis) value_sym = AngleAxis(angle=-angle, axis=-axis) numpy_compare.assert_equal(value.rotation(), value_sym.rotation()) numpy_compare.assert_equal(value.angle(), -value_sym.angle()) numpy_compare.assert_equal(value.axis(), -value_sym.axis()) def get_vector(value): return np.hstack((value.angle(), value.axis())) assert_pickle(self, value, get_vector, T=T)
def check_angle_axis(self, T): AngleAxis = mut.AngleAxis_[T] value_identity = AngleAxis.Identity() self.assertEqual(npc.resolve_type(value_identity.angle()), T) npc.assert_float_equal(value_identity.angle(), 0.) npc.assert_float_equal(value_identity.axis(), [1., 0, 0]) # Construct with rotation matrix. R = np.array([ [0., 1, 0], [-1, 0, 0], [0, 0, 1]]) value = AngleAxis(rotation=R) npc.assert_float_allclose(value.rotation(), R) npc.assert_float_allclose(copy.copy(value).rotation(), R) npc.assert_float_allclose(value.inverse().rotation(), R.T) npc.assert_float_allclose( value.multiply(value.inverse()).rotation(), np.eye(3)) if six.PY3: npc.assert_float_allclose( eval("value @ value.inverse()").rotation(), np.eye(3)) value.set_rotation(np.eye(3)) npc.assert_float_equal(value.rotation(), np.eye(3)) # Construct with quaternion. Quaternion = mut.Quaternion_[T] q = Quaternion(R) value = AngleAxis(quaternion=q) npc.assert_float_allclose( value.quaternion().wxyz(), npc.to_float(q.wxyz())) value.set_quaternion(Quaternion.Identity()) npc.assert_float_equal(value.quaternion().wxyz(), [1., 0, 0, 0]) # Test setters. value = AngleAxis(value_identity) value.set_angle(np.pi / 4) v = normalize(np.array([0.1, 0.2, 0.3])) if T != Expression: with self.assertRaises(RuntimeError): value.set_axis([0.1, 0.2, 0.3]) value.set_axis(v) npc.assert_float_equal(value.angle(), np.pi / 4) npc.assert_float_equal(value.axis(), v) # Test symmetry based on accessors. # N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal # angles and axes to a half-plane. angle = np.pi / 6 axis = normalize([0.1, 0.2, 0.3]) value = AngleAxis(angle=angle, axis=axis) value_sym = AngleAxis(angle=-angle, axis=-axis) npc.assert_equal(value.rotation(), value_sym.rotation()) npc.assert_equal(value.angle(), -value_sym.angle()) npc.assert_equal(value.axis(), -value_sym.axis())
def test_resolve_type(self): Af = np.array([1., 2.]) self.assertEqual(numpy_compare.resolve_type(Af), float) Ac = np.array([Custom("a"), Custom("b")]) self.assertEqual(numpy_compare.resolve_type(Ac), Custom) with self.assertRaises(AssertionError): numpy_compare.resolve_type([]) with self.assertRaises(AssertionError): numpy_compare.resolve_type(["a", 1., None])
def test_resolve_type(self): Af = np.array([1., 2.]) self.assertEqual(npc.resolve_type(Af), float) Ac = np.array([Custom("a"), Custom("b")]) self.assertEqual(npc.resolve_type(Ac), Custom) with self.assertRaises(AssertionError): npc.resolve_type([]) with self.assertRaises(AssertionError): npc.resolve_type(["a", 1., None])
def test_quaternion(self, T): # Simple API. Quaternion = mut.Quaternion_[T] cast = np.vectorize(T) q_identity = Quaternion() self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T) numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0]) numpy_compare.assert_float_equal( copy.copy(q_identity).wxyz(), [1., 0, 0, 0]) numpy_compare.assert_equal(q_identity.wxyz(), Quaternion.Identity().wxyz()) if T == float: self.assertEqual(str(q_identity), "Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)") self.check_cast(mut.Quaternion_, T) # Test ordering. q_wxyz = normalize([0.1, 0.3, 0.7, 0.9]) q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3]) # - Accessors. numpy_compare.assert_float_equal(q.w(), q_wxyz[0]) numpy_compare.assert_float_equal(q.x(), q_wxyz[1]) numpy_compare.assert_float_equal(q.y(), q_wxyz[2]) numpy_compare.assert_float_equal(q.z(), q_wxyz[3]) numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz) # - Mutators. q_wxyz_new = q_wxyz[::-1] numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new) q.set_wxyz(wxyz=q_wxyz_new) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) q.set_wxyz(w=q_wxyz_new[0], x=q_wxyz_new[1], y=q_wxyz_new[2], z=q_wxyz_new[3]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) # Alternative constructors. q_other = Quaternion(wxyz=q_wxyz) numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz) R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]]) q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5]) q_other = Quaternion(q_wxyz_expected) numpy_compare.assert_float_equal(q_other.rotation(), R) R_I = np.eye(3, 3) q_other.set_rotation(R_I) numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz()) # - Copy constructor. cp = Quaternion(other=q) numpy_compare.assert_equal(q.wxyz(), cp.wxyz()) # Bad values. if T != Expression: q = Quaternion.Identity() # - wxyz q_wxyz_bad = [1., 2, 3, 4] with self.assertRaises(RuntimeError): q.set_wxyz(q_wxyz_bad) numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0]) # - Rotation. R_bad = np.copy(R) R_bad[0, 0] = 10 with self.assertRaises(RuntimeError): q_other.set_rotation(R_bad) numpy_compare.assert_float_equal(q_other.rotation(), R_I) # Operations. q_AB = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) q_I = q_AB.inverse().multiply(q_AB) numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0]) if six.PY3: numpy_compare.assert_float_equal( eval("q_AB.inverse() @ q_AB").wxyz(), [1., 0, 0, 0]) v_B = np.array([1., 2, 3]) v_A = np.array([3., 1, 2]) numpy_compare.assert_float_allclose(q_AB.multiply(vector=v_B), v_A) vlist_B = np.array([v_B, v_B]).T vlist_A = np.array([v_A, v_A]).T numpy_compare.assert_float_equal(q_AB.multiply(vector=vlist_B), vlist_A) # Test deprecation. with catch_drake_warnings(expected_count=2): self.assertEqual(q_AB.multiply(position=v_B).shape, v_B.shape) self.assertEqual( q_AB.multiply(position=vlist_B).shape, vlist_B.shape) with catch_drake_warnings(expected_count=0): # No deprecation should happen with position arguments. self.assertEqual(q_AB.multiply(v_B).shape, v_B.shape) self.assertEqual(q_AB.multiply(vlist_B).shape, vlist_B.shape) q_AB_conj = q_AB.conjugate() numpy_compare.assert_float_equal(q_AB_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]) # Test `type_caster`s. if T == float: value = test_util.create_quaternion() self.assertTrue(isinstance(value, mut.Quaternion)) test_util.check_quaternion(value)
def test_isometry3(self, T): Isometry3 = mut.Isometry3_[T] # - Default constructor transform = Isometry3() self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T) X_I_np = np.eye(4, 4) numpy_compare.assert_float_equal(transform.matrix(), X_I_np) numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X_I_np) if T == float: self.assertEqual(str(transform), str(X_I_np)) # - Constructor with (X_I_np) transform = Isometry3(matrix=X_I_np) numpy_compare.assert_float_equal(transform.matrix(), X_I_np) # - Copy constructor. cp = Isometry3(other=transform) numpy_compare.assert_equal(transform.matrix(), cp.matrix()) # - Identity transform = Isometry3.Identity() numpy_compare.assert_float_equal(transform.matrix(), X_I_np) # - Constructor with (R, p) R_AB = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]]) p_AB = np.array([1., 2, 3]) X_AB_np = np.eye(4) X_AB_np[:3, :3] = R_AB X_AB_np[:3, 3] = p_AB X_AB = Isometry3(rotation=R_AB, translation=p_AB) numpy_compare.assert_float_equal(X_AB.matrix(), X_AB_np) numpy_compare.assert_float_equal(X_AB.translation(), p_AB) numpy_compare.assert_float_equal(X_AB.rotation(), R_AB) # - Setters. X_AB = Isometry3() X_AB.set_translation(p_AB) numpy_compare.assert_float_equal(X_AB.translation(), p_AB) X_AB.set_rotation(R_AB) numpy_compare.assert_float_equal(X_AB.rotation(), R_AB) # - Cast self.check_cast(mut.Isometry3_, T) # - Check transactions for bad values. if T != Expression: X_temp = Isometry3(rotation=R_AB, translation=p_AB) R_bad = np.copy(R_AB) R_bad[0, 0] = 10. with self.assertRaises(RuntimeError): X_temp.set_rotation(R_bad) numpy_compare.assert_float_equal(X_temp.rotation(), R_AB) X_bad_np = np.copy(X_I_np) X_bad_np[:3, :3] = R_bad with self.assertRaises(RuntimeError): X_temp.set_matrix(X_bad_np) numpy_compare.assert_float_equal(X_temp.matrix(), X_AB_np) # Test `type_caster`s. if T == float: value = test_util.create_isometry() self.assertTrue(isinstance(value, mut.Isometry3)) test_util.check_isometry(value) # Operations. X_AB = Isometry3(rotation=R_AB, translation=p_AB) X_I = X_AB.inverse().multiply(X_AB) numpy_compare.assert_float_equal(X_I.matrix(), X_I_np) p_BQ = [10, 20, 30] p_AQ = [21., -8, 33] numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQ), p_AQ) p_BQlist = np.array([p_BQ, p_BQ]).T p_AQlist = np.array([p_AQ, p_AQ]).T numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQlist), p_AQlist) if six.PY3: numpy_compare.assert_float_equal( eval("X_AB.inverse() @ X_AB").matrix(), X_I_np) numpy_compare.assert_float_equal(eval("X_AB @ p_BQ"), p_AQ)
def test_quaternion(self, T): # Simple API. Quaternion = mut.Quaternion_[T] cast = np.vectorize(T) q_identity = Quaternion() self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T) numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0]) numpy_compare.assert_float_equal( copy.copy(q_identity).wxyz(), [1., 0, 0, 0]) numpy_compare.assert_equal(q_identity.wxyz(), Quaternion.Identity().wxyz()) if T == float: self.assertEqual(str(q_identity), "Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)") # Test ordering. q_wxyz = normalize([0.1, 0.3, 0.7, 0.9]) q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3]) # - Accessors. numpy_compare.assert_float_equal(q.w(), q_wxyz[0]) numpy_compare.assert_float_equal(q.x(), q_wxyz[1]) numpy_compare.assert_float_equal(q.y(), q_wxyz[2]) numpy_compare.assert_float_equal(q.z(), q_wxyz[3]) numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz) # - Mutators. q_wxyz_new = q_wxyz[::-1] numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new) q.set_wxyz(wxyz=q_wxyz_new) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) q.set_wxyz(w=q_wxyz_new[0], x=q_wxyz_new[1], y=q_wxyz_new[2], z=q_wxyz_new[3]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) # Alternative constructors. q_other = Quaternion(wxyz=q_wxyz) numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz) R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]]) q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5]) q_other = Quaternion(q_wxyz_expected) numpy_compare.assert_float_equal(q_other.rotation(), R) R_I = np.eye(3, 3) q_other.set_rotation(R_I) numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz()) # - Copy constructor. cp = Quaternion(other=q) numpy_compare.assert_equal(q.wxyz(), cp.wxyz()) # Bad values. if T != Expression: q = Quaternion.Identity() # - wxyz q_wxyz_bad = [1., 2, 3, 4] with self.assertRaises(RuntimeError): q.set_wxyz(q_wxyz_bad) numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0]) # - Rotation. R_bad = np.copy(R) R_bad[0, 0] = 10 with self.assertRaises(RuntimeError): q_other.set_rotation(R_bad) numpy_compare.assert_float_equal(q_other.rotation(), R_I) # Operations. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) numpy_compare.assert_float_equal(q.multiply(position=[1, 2, 3]), [3., 1, 2]) q_I = q.inverse().multiply(q) numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0]) if six.PY3: numpy_compare.assert_float_equal( eval("q.inverse() @ q").wxyz(), [1., 0, 0, 0]) q_conj = q.conjugate() numpy_compare.assert_float_equal(q_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]) # Test `type_caster`s. if T == float: value = test_util.create_quaternion() self.assertTrue(isinstance(value, mut.Quaternion)) test_util.check_quaternion(value)
def check_quaternion(self, T): # Simple API. Quaternion = mut.Quaternion_[T] cast = np.vectorize(T) q_identity = Quaternion() self.assertEqual(npc.resolve_type(q_identity.wxyz()), T) npc.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0]) npc.assert_float_equal(copy.copy(q_identity).wxyz(), [1., 0, 0, 0]) npc.assert_equal(q_identity.wxyz(), Quaternion.Identity().wxyz()) if T == float: self.assertEqual( str(q_identity), "Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)") # Test ordering. q_wxyz = normalize([0.1, 0.3, 0.7, 0.9]) q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3]) # - Accessors. npc.assert_float_equal(q.w(), q_wxyz[0]) npc.assert_float_equal(q.x(), q_wxyz[1]) npc.assert_float_equal(q.y(), q_wxyz[2]) npc.assert_float_equal(q.z(), q_wxyz[3]) npc.assert_float_equal(q.xyz(), q_wxyz[1:]) npc.assert_float_equal(q.wxyz(), q_wxyz) # - Mutators. q_wxyz_new = q_wxyz[::-1] npc.assert_not_equal(q_wxyz, q_wxyz_new) q.set_wxyz(wxyz=q_wxyz_new) npc.assert_float_equal(q.wxyz(), q_wxyz_new) q.set_wxyz( w=q_wxyz_new[0], x=q_wxyz_new[1], y=q_wxyz_new[2], z=q_wxyz_new[3]) npc.assert_float_equal(q.wxyz(), q_wxyz_new) # Alternative constructors. q_other = Quaternion(wxyz=q_wxyz) npc.assert_float_equal(q_other.wxyz(), q_wxyz) R = np.array([ [0., 0, 1], [1, 0, 0], [0, 1, 0]]) q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5]) q_other = Quaternion(q_wxyz_expected) npc.assert_float_equal(q_other.rotation(), R) R_I = np.eye(3, 3) q_other.set_rotation(R_I) npc.assert_equal(q_other.wxyz(), q_identity.wxyz()) # - Copy constructor. cp = Quaternion(other=q) npc.assert_equal(q.wxyz(), cp.wxyz()) # Bad values. if T != Expression: q = Quaternion.Identity() # - wxyz q_wxyz_bad = [1., 2, 3, 4] with self.assertRaises(RuntimeError): q.set_wxyz(q_wxyz_bad) npc.assert_float_equal(q.wxyz(), [1., 0, 0, 0]) # - Rotation. R_bad = np.copy(R) R_bad[0, 0] = 10 with self.assertRaises(RuntimeError): q_other.set_rotation(R_bad) npc.assert_float_equal(q_other.rotation(), R_I) # Operations. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) npc.assert_float_equal(q.multiply(position=[1, 2, 3]), [3., 1, 2]) q_I = q.inverse().multiply(q) npc.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0]) if six.PY3: npc.assert_float_equal( eval("q.inverse() @ q").wxyz(), [1., 0, 0, 0]) q_conj = q.conjugate() npc.assert_float_equal(q_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]) # Test `type_caster`s. if T == float: value = test_util.create_quaternion() self.assertTrue(isinstance(value, mut.Quaternion)) test_util.check_quaternion(value)
def check_isometry3(self, T): Isometry3 = mut.Isometry3_[T] # - Default constructor transform = Isometry3() self.assertEqual(npc.resolve_type(transform.matrix()), T) X = np.eye(4, 4) npc.assert_float_equal(transform.matrix(), X) npc.assert_float_equal(copy.copy(transform).matrix(), X) if T == float: self.assertEqual(str(transform), str(X)) # - Constructor with (X) transform = Isometry3(matrix=X) npc.assert_float_equal(transform.matrix(), X) # - Copy constructor. cp = Isometry3(other=transform) npc.assert_equal(transform.matrix(), cp.matrix()) # - Identity transform = Isometry3.Identity() npc.assert_float_equal(transform.matrix(), X) # - Constructor with (R, p) R = np.array([ [0., 1, 0], [-1, 0, 0], [0, 0, 1]]) p = np.array([1., 2, 3]) X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1])) transform = Isometry3(rotation=R, translation=p) npc.assert_float_equal(transform.matrix(), X) npc.assert_float_equal(transform.translation(), p) transform.set_translation(-p) npc.assert_float_equal(transform.translation(), -p) npc.assert_float_equal(transform.rotation(), R) transform.set_rotation(R.T) npc.assert_float_equal(transform.rotation(), R.T) # - Check transactions for bad values. if T != Expression: transform = Isometry3(rotation=R, translation=p) R_bad = np.copy(R) R_bad[0, 0] = 10. with self.assertRaises(RuntimeError): transform.set_rotation(R_bad) npc.assert_float_equal(transform.rotation(), R) X_bad = np.copy(X) X_bad[:3, :3] = R_bad with self.assertRaises(RuntimeError): transform.set_matrix(X_bad) npc.assert_float_equal(transform.matrix(), X) # Test `type_caster`s. if T == float: value = test_util.create_isometry() self.assertTrue(isinstance(value, mut.Isometry3)) test_util.check_isometry(value) # Operations. transform = Isometry3(rotation=R, translation=p) transform_I = transform.inverse().multiply(transform) npc.assert_float_equal(transform_I.matrix(), np.eye(4)) npc.assert_float_equal( transform.multiply(position=[10, 20, 30]), [21., -8, 33]) if six.PY3: npc.assert_float_equal( eval("transform.inverse() @ transform").matrix(), np.eye(4)) npc.assert_float_equal( eval("transform @ [10, 20, 30]"), [21., -8, 33])
def test_quaternion(self, T): # Simple API. Quaternion = mut.Quaternion_[T] cast = np.vectorize(T) q_identity = Quaternion() self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T) numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0]) numpy_compare.assert_float_equal( copy.copy(q_identity).wxyz(), [1., 0, 0, 0]) numpy_compare.assert_equal(q_identity.wxyz(), Quaternion.Identity().wxyz()) if T == float: self.assertEqual(str(q_identity), "Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)") self.check_cast(mut.Quaternion_, T) # Test ordering. q_wxyz = normalize([0.1, 0.3, 0.7, 0.9]) q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3]) # - Accessors. numpy_compare.assert_float_equal(q.w(), q_wxyz[0]) numpy_compare.assert_float_equal(q.x(), q_wxyz[1]) numpy_compare.assert_float_equal(q.y(), q_wxyz[2]) numpy_compare.assert_float_equal(q.z(), q_wxyz[3]) numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz) # - Mutators. q_wxyz_new = q_wxyz[::-1] numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new) q.set_wxyz(wxyz=q_wxyz_new) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) q.set_wxyz(w=q_wxyz_new[0], x=q_wxyz_new[1], y=q_wxyz_new[2], z=q_wxyz_new[3]) numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new) # Alternative constructors. q_other = Quaternion(wxyz=q_wxyz) numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz) R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]]) q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5]) q_other = Quaternion(q_wxyz_expected) numpy_compare.assert_float_equal(q_other.rotation(), R) R_I = np.eye(3, 3) q_other.set_rotation(R_I) numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz()) # - Copy constructor. cp = Quaternion(other=q) numpy_compare.assert_equal(q.wxyz(), cp.wxyz()) # Bad values. if T != Expression: q = Quaternion.Identity() # - wxyz q_wxyz_bad = [1., 2, 3, 4] with self.assertRaises(RuntimeError): q.set_wxyz(q_wxyz_bad) numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0]) # - Rotation. R_bad = np.copy(R) R_bad[0, 0] = 10 with self.assertRaises(RuntimeError): q_other.set_rotation(R_bad) numpy_compare.assert_float_equal(q_other.rotation(), R_I) # Operations. q_AB = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) q_I = q_AB.inverse().multiply(q_AB) numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0]) numpy_compare.assert_float_equal((q_AB.inverse() @ q_AB).wxyz(), [1., 0, 0, 0]) v_B = np.array([1., 2, 3]) v_A = np.array([3., 1, 2]) numpy_compare.assert_float_allclose(q_AB.multiply(vector=v_B), v_A) vlist_B = np.array([v_B, v_B]).T vlist_A = np.array([v_A, v_A]).T numpy_compare.assert_float_equal(q_AB.multiply(vector=vlist_B), vlist_A) q_AB_conj = q_AB.conjugate() numpy_compare.assert_float_equal(q_AB_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]) numpy_compare.assert_float_equal( q_I.slerp(t=0, other=q_I).wxyz(), [1., 0, 0, 0]) # - Test shaping (#13885). v = np.array([0., 0., 0.]) vs = np.array([[1., 2., 3.], [4., 5., 6.]]).T self.assertEqual((q_AB @ v).shape, (3, )) self.assertEqual((q_AB @ v.reshape((3, 1))).shape, (3, 1)) self.assertEqual((q_AB @ vs).shape, (3, 2)) # Test `type_caster`s. if T == float: value = test_util.create_quaternion() self.assertTrue(isinstance(value, mut.Quaternion)) test_util.check_quaternion(value) assert_pickle(self, q_AB, Quaternion.wxyz, T=T)