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.
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_rotation_matrix(self):
# - Constructors.
R = mut.RotationMatrix()
self.assertTrue(
np.allclose(mut.RotationMatrix(other=R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3)))
self.assertTrue(
np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(R=np.eye(3))
self.assertTrue(np.allclose(R.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)))
if six.PY3:
self.assertTrue(
np.allclose(eval("R.inverse() @ R").matrix(), np.eye(3)))
def test_rotation_matrix(self):
# - Constructors.
R = mut.RotationMatrix()
self.assertTrue(np.allclose(
mut.RotationMatrix(other=R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(
mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(R=np.eye(3))
self.assertTrue(np.allclose(R.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)))
if six.PY3:
self.assertTrue(np.allclose(
eval("R.inverse() @ R").matrix(), np.eye(3)))
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()))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(
np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(matrix=np.eye(3))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
# - Converting changes in orientation
self.assertTrue(
np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]),
np.zeros((3, 3))))
self.assertTrue(
np.allclose(
rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent(
rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDDtFromAngularAccelInChild(rpyDt=[0, 0, 0],
alpha_AD_D=[0, 0, 0]),
[0, 0, 0]))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(
np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(
mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(
mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(matrix=np.eye(3))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
# - Converting changes in orientation
self.assertTrue(np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]),
np.zeros((3, 3))))
self.assertTrue(np.allclose(
rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent(
rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.CalcRpyDDtFromAngularAccelInChild(
rpyDt=[0, 0, 0], alpha_AD_D=[0, 0, 0]), [0, 0, 0]))
