def test_isometry_implicit(self, T):
Isometry3 = Isometry3_[T]
# Explicitly disabled, to mirror C++ API.
with self.assertRaises(TypeError):
self.assertTrue(mtest.TakeRigidTransform(Isometry3()))
with catch_drake_warnings(expected_count=1):
self.assertTrue(mtest.TakeIsometry3(mut.RigidTransform()))
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)
R_I = mut.RotationMatrix()
p_I = np.zeros(3)
rpy_I = mut.RollPitchYaw(0, 0, 0)
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(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)
def test_rigid_transform_deprecated_isometry3_workalikes(self, T):
X_AB = mut.RigidTransform()
with catch_drake_warnings(expected_count=1):
mat = X_AB.matrix()
with catch_drake_warnings(expected_count=1):
lin = X_AB.linear()
self.assertIsInstance(mat, np.ndarray)
self.assertIsInstance(lin, np.ndarray)
self.assertEqual(mat.shape, (4, 4))
self.assertEqual(lin.shape, (3, 3))
def test_isometry_implicit(self, T):
Isometry3 = Isometry3_[T]
# Explicitly disabled, to mirror C++ API.
with self.assertRaises(TypeError):
self.assertTrue(mtest.TakeRigidTransform(Isometry3()))
self.assertTrue(mtest.TakeIsometry3(mut.RigidTransform()))
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)
check_equality(copy.copy(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)
if six.PY3:
self.assertIsInstance(eval("X @ mut.RigidTransform()"),
mut.RigidTransform)
self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
