def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_rotation(), Quaternion))
self.assertTrue(isinstance(value.get_translation(), np.ndarray))
# - Value.
self.assertTrue(
np.allclose(value.get_isometry().matrix(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = Isometry3(q, p)
value.set_translation(p)
value.set_rotation(q)
self.assertTrue(
np.allclose(value.get_isometry().matrix(), X_expected.matrix()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
self.assertTrue(
np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
with catch_drake_warnings(expected_count=1):
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_transform(), RigidTransform))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - Value.
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = RigidTransform(quaternion=q, p=p)
value.set_translation(p)
value.set_rotation(q)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value1.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# Test mutation via RigidTransform
p2 = [10, 20, 30]
q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8]))
X2_expected = RigidTransform(quaternion=q2, p=p2)
value.set_transform(X2_expected)
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))