def tr2angvec(T, unit='rad', check=False):
r"""
Convert SO(3) or SE(3) to angle and rotation vector
:param R: SO(3) or SE(3) matrix
:type R: numpy.ndarray, shape=(3,3) or (4,4)
:param unit: 'rad' or 'deg'
:type unit: str
:param check: check that rotation matrix is valid
:type check: bool
:return: :math:`(\theta, {\bf v})`
:rtype: float, numpy.ndarray, shape=(3,)
``tr2angvec(R)`` is a rotation angle and a vector about which the rotation
acts that corresponds to the rotation part of ``R``.
By default the angle is in radians but can be changed setting `unit='deg'`.
Notes:
- If the input is SE(3) the translation component is ignored.
:seealso: :func:`~angvec2r`, :func:`~angvec2tr`, :func:`~tr2rpy`, :func:`~tr2eul`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
v = trn.vex(trlog(R))
if vec.iszerovec(v):
theta = 0
v = np.r_[0, 0, 0]
else:
theta = vec.norm(v)
v = vec.unitvec(v)
if unit == 'deg':
theta *= 180 / math.pi
return (theta, v)
def iseye(S, tol=10):
"""
Test if matrix is identity
:param S: matrix to test
:type S: numpy.ndarray
:param tol: tolerance in units of eps
:type tol: float
:return: whether matrix is a proper skew-symmetric matrix
:rtype: bool
Check if matrix is an identity matrix. We test that the trace tom row is zero
We check that the norm of the residual is less than ``tol * eps``.
:seealso: isskew, isskewa
"""
s = S.shape
if len(s) != 2 or s[0] != s[1]:
return False # not a square matrix
return vec.norm(S - np.eye(s[0])) < tol * _eps
def rodrigues(w, theta):
"""
Rodrigues' formula for rotation
:param w: rotation vector
:type w: array_like
:param theta: rotation angle
:type theta: float or None
"""
w = argcheck.getvector(w)
if vec.iszerovec(w):
# for a zero so(n) return unit matrix, theta not relevant
if len(w) == 1:
return np.eye(2)
else:
return np.eye(3)
if theta is None:
theta = vec.norm(w)
w = vec.unitvec(w)
skw = skew(w)
return np.eye(skw.shape[0]) + math.sin(theta) * skw + (
1.0 - math.cos(theta)) * skw @ skw
def trlog(T, check=True):
"""
Logarithm of SO(3) or SE(3) matrix
:param T: SO(3) or SE(3) matrix
:type T: numpy.ndarray, shape=(3,3) or (4,4)
:return: logarithm
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
:raises: ValueError
An efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3).
- ``trlog(R)`` is the logarithm of the passed rotation matrix ``R`` which will be
3x3 skew-symmetric matrix. The equivalent vector from ``vex()`` is parallel to rotation axis
and its norm is the amount of rotation about that axis.
- ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be
4x4 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist
vector (6x1) comprising [v w].
:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`
"""
if ishom(T, check=check):
# SE(3) matrix
if trn.iseye(T):
# is identity matrix
return np.zeros((4, 4))
else:
[R, t] = trn.tr2rt(T)
if trn.iseye(R):
# rotation matrix is identity
skw = np.zeros((3, 3))
v = t
theta = 1
else:
S = trlog(R, check=False) # recurse
w = trn.vex(S)
theta = vec.norm(w)
skw = trn.skew(w / theta)
Ginv = np.eye(3) / theta - skw / 2 + (
1 / theta - 1 / np.tan(theta / 2) / 2) * skw @ skw
v = Ginv @ t
return trn.rt2m(skw, v) * theta
elif isrot(T, check=check):
# deal with rotation matrix
R = T
if trn.iseye(R):
# matrix is identity
return np.zeros((3, 3))
elif abs(np.trace(R) + 1) < 100 * _eps:
# rotation by +/- pi, +/- 3pi etc.
diagonal = R.diagonal()
k = diagonal.argmax()
mx = diagonal[k]
I = np.eye(3)
col = R[:, k] + I[:, k]
w = col / np.sqrt(2 * (1 + mx))
theta = math.pi
return trn.skew(w * theta)
else:
# general case
theta = np.arccos((np.trace(R) - 1) / 2)
skw = (R - R.T) / 2 / np.sin(theta)
return skw * theta
else:
raise ValueError("Expect SO(3) or SE(3) matrix")