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)
check_equality(mut.RigidTransform(R=R_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_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_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_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[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()))
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)
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]))
