def test_Rt(self):
nt.assert_array_almost_equal(rot2(0.3), t2r(trot2(0.3)))
nt.assert_array_almost_equal(trot2(0.3), r2t(rot2(0.3)))
R = rot2(0.2)
t = [1, 2]
T = rt2tr(R, t)
nt.assert_array_almost_equal(t2r(T), R)
nt.assert_array_almost_equal(transl2(T), np.array(t))
def tr2eul(T, unit='rad', flip=False, check=False):
r"""
Convert SO(3) or SE(3) to ZYX Euler angles
: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 flip: choose first Euler angle to be in quadrant 2 or 3
:type flip: bool
:param check: check that rotation matrix is valid
:type check: bool
:return: ZYZ Euler angles
:rtype: numpy.ndarray, shape=(3,)
``tr2eul(R)`` are the Euler angles corresponding to
the rotation part of ``R``.
The 3 angles :math:`[\phi, \theta, \psi` correspond to sequential rotations about the
Z, Y and Z axes respectively.
By default the angles are in radians but can be changed setting `unit='deg'`.
Notes:
- There is a singularity for the case where :math:`\theta=0` in which case :math:`\phi` is arbitrarily set to zero and :math:`\phi` is set to :math:`\phi+\psi`.
- If the input is SE(3) the translation component is ignored.
:seealso: :func:`~eul2r`, :func:`~eul2tr`, :func:`~tr2rpy`, :func:`~tr2angvec`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
eul = np.zeros((3, ))
if abs(R[0, 2]) < 10 * _eps and abs(R[1, 2]) < 10 * _eps:
eul[0] = 0
sp = 0
cp = 1
eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0],
-sp * R[0, 1] + cp * R[1, 1])
else:
if flip:
eul[0] = math.atan2(-R[1, 2], -R[0, 2])
else:
eul[0] = math.atan2(R[1, 2], R[0, 2])
sp = math.sin(eul[0])
cp = math.cos(eul[0])
eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0],
-sp * R[0, 1] + cp * R[1, 1])
if unit == 'deg':
eul *= 180 / math.pi
return eul
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 tr2rpy(T, unit='rad', order='zyx', check=False):
"""
Convert SO(3) or SE(3) to roll-pitch-yaw angles
: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 order: 'xyz', 'zyx' or 'yxz' [default 'zyx']
:type unit: str
:param check: check that rotation matrix is valid
:type check: bool
:return: Roll-pitch-yaw angles
:rtype: numpy.ndarray, shape=(3,)
``tr2rpy(R)`` are the roll-pitch-yaw angles corresponding to
the rotation part of ``R``.
The 3 angles RPY=[R,P,Y] correspond to sequential rotations about the
Z, Y and X axes respectively. The axis order sequence can be changed by
setting:
- `order='xyz'` for sequential rotations about X, Y, Z axes
- `order='yxz'` for sequential rotations about Y, X, Z axes
By default the angles are in radians but can be changed setting `unit='deg'`.
Notes:
- There is a singularity for the case where P=:math:`\pi/2` in which case R is arbitrarily set to zero and Y is the sum (R+Y).
- If the input is SE(3) the translation component is ignored.
:seealso: :func:`~rpy2r`, :func:`~rpy2tr`, :func:`~tr2eul`, :func:`~tr2angvec`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
rpy = np.zeros((3, ))
if order == 'xyz' or order == 'arm':
# XYZ order
if abs(abs(R[0, 2]) - 1) < 10 * _eps: # when |R13| == 1
# singularity
rpy[0] = 0 # roll is zero
if R[0, 2] > 0:
rpy[2] = math.atan2(R[2, 1], R[1, 1]) # R+Y
else:
rpy[2] = -math.atan2(R[1, 0], R[2, 0]) # R-Y
rpy[1] = math.asin(R[0, 2])
else:
rpy[0] = -math.atan2(R[0, 1], R[0, 0])
rpy[2] = -math.atan2(R[1, 2], R[2, 2])
k = np.argmax(np.abs([R[0, 0], R[0, 1], R[1, 2], R[2, 2]]))
if k == 0:
rpy[1] = math.atan(R[0, 2] * math.cos(rpy[0]) / R[0, 0])
elif k == 1:
rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[0]) / R[0, 1])
elif k == 2:
rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[2]) / R[1, 2])
elif k == 3:
rpy[1] = math.atan(R[0, 2] * math.cos(rpy[2]) / R[2, 2])
elif order == 'zyx' or order == 'vehicle':
# old ZYX order (as per Paul book)
if abs(abs(R[2, 0]) - 1) < 10 * _eps: # when |R31| == 1
# singularity
rpy[0] = 0 # roll is zero
if R[2, 0] < 0:
rpy[2] = -math.atan2(R[0, 1], R[0, 2]) # R-Y
else:
rpy[2] = math.atan2(-R[0, 1], -R[0, 2]) # R+Y
rpy[1] = -math.asin(R[2, 0])
else:
rpy[0] = math.atan2(R[2, 1], R[2, 2]) # R
rpy[2] = math.atan2(R[1, 0], R[0, 0]) # Y
k = np.argmax(np.abs([R[0, 0], R[1, 0], R[2, 1], R[2, 2]]))
if k == 0:
rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[2]) / R[0, 0])
elif k == 1:
rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[2]) / R[1, 0])
elif k == 2:
rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[0]) / R[2, 1])
elif k == 3:
rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[0]) / R[2, 2])
elif order == 'yxz' or order == 'camera':
if abs(abs(R[1, 2]) - 1) < 10 * _eps: # when |R23| == 1
# singularity
rpy[0] = 0
if R[1, 2] < 0:
rpy[2] = -math.atan2(R[2, 0], R[0, 0]) # R-Y
else:
rpy[2] = math.atan2(-R[2, 0], -R[2, 1]) # R+Y
rpy[1] = -math.asin(R[1, 2]) # P
else:
rpy[0] = math.atan2(R[1, 0], R[1, 1])
rpy[2] = math.atan2(R[0, 2], R[2, 2])
k = np.argmax(np.abs([R[1, 0], R[1, 1], R[0, 2], R[2, 2]]))
if k == 0:
rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[0]) / R[1, 0])
elif k == 1:
rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[0]) / R[1, 1])
elif k == 2:
rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[2]) / R[0, 2])
elif k == 3:
rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[2]) / R[2, 2])
else:
raise ValueError('Invalid order')
if unit == 'deg':
rpy *= 180 / math.pi
return rpy