def test_checks(self):
# 2D case, with rotation matrix
R = np.eye(2)
nt.assert_equal(isR(R), True)
nt.assert_equal(isrot2(R), True)
nt.assert_equal(ishom2(R), False)
nt.assert_equal(isrot2(R, True), True)
nt.assert_equal(ishom2(R, True), False)
# 2D case, invalid rotation matrix
R = np.array([[1, 1], [0, 1]])
nt.assert_equal(isR(R), False)
nt.assert_equal(isrot2(R), True)
nt.assert_equal(ishom2(R), False)
nt.assert_equal(isrot2(R, True), False)
nt.assert_equal(ishom2(R, True), False)
# 2D case, with homogeneous transformation matrix
T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
nt.assert_equal(isR(T), False)
nt.assert_equal(isrot2(T), False)
nt.assert_equal(ishom2(T), True)
nt.assert_equal(isrot2(T, True), False)
nt.assert_equal(ishom2(T, True), True)
# 2D case, invalid rotation matrix
T = np.array([[1, 1, 3], [0, 1, 4], [0, 0, 1]])
nt.assert_equal(isR(T), False)
nt.assert_equal(isrot2(T), False)
nt.assert_equal(ishom2(T), True)
nt.assert_equal(isrot2(T, True), False)
nt.assert_equal(ishom2(T, True), False)
# 2D case, invalid bottom row
T = np.array([[1, 1, 3], [0, 1, 4], [9, 0, 1]])
nt.assert_equal(isR(T), False)
nt.assert_equal(isrot2(T), False)
nt.assert_equal(ishom2(T), True)
nt.assert_equal(isrot2(T, True), False)
nt.assert_equal(ishom2(T, True), False)
def isrot(R, check=False, tol=10):
"""
Test if matrix belongs to SO(3)
:param R: matrix to test
:type R: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SO(3) rotation matrix
:rtype: bool
- ``ISROT(R)`` is True if the argument ``R`` is of dimension 3x3
- ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.
:seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~spatialmath.base.transforms2d.isrot2`, :func:`~ishom`
"""
return R.shape == (3, 3) and (not check or trn.isR(R, tol=tol))
def isrot2(R, check=False):
"""
Test if matrix belongs to SO(2)
:param R: matrix to test
:type R: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SO(2) rotation matrix
:rtype: bool
- ``ISROT(R)`` is True if the argument ``R`` is of dimension 2x2
- ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.
:seealso: isR, ishom2, isrot
"""
return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or trn.isR(R))
def ishom2(T, check=False):
"""
Test if matrix belongs to SE(2)
:param T: matrix to test
:type T: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SE(2) homogeneous transformation matrix
:rtype: bool
- ``ISHOM2(T)`` is True if the argument ``T`` is of dimension 3x3
- ``ISHOM2(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and
validitity of the bottom row.
:seealso: isR, isrot2, ishom, isvec
"""
return isinstance(T, np.ndarray) and T.shape == (3, 3) and (not check or (trn.isR(T[:2, :2]) and np.all(T[2, :] == np.array([0, 0, 1]))))
def ishom(T, check=False, tol=10):
"""
Test if matrix belongs to SE(3)
:param T: matrix to test
:type T: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SE(3) homogeneous transformation matrix
:rtype: bool
- ``ISHOM(T)`` is True if the argument ``T`` is of dimension 4x4
- ``ISHOM(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and
validitity of the bottom row.
:seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~isrot`, :func:`~spatialmath.base.transforms2d.ishom2`
"""
return T.shape == (4, 4) and (not check or (trn.isR(
T[:3, :3], tol=tol) and np.all(T[3, :] == np.array([0, 0, 0, 1]))))
