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 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 trexp2(S, theta=None): """ Exponential of so(2) or se(2) matrix :param S: so(2), se(2) matrix or equivalent velctor :type T: numpy.ndarray, shape=(2,2) or (3,3); array_like :param theta: motion :type theta: float :return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2) :rtype: numpy.ndarray, shape=(2,2) or (3,3) An efficient closed-form solution of the matrix exponential for arguments that are so(2) or se(2). For so(2) the results is an SO(2) rotation matrix: - ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2 skew-symmetric matrix. - ``trexp2(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given by ``THETA``. - ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as a 1-vector (array_like). - ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a unit-norm vector representing a rotation axis and a rotation magnitude given by ``THETA``. ``W`` is expressed as a 1-vector (array_like). For se(2) the results is an SE(2) homogeneous transformation matrix: - ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is a 3x3 augmented skew-symmetric matrix. - ``trexp2(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. - ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as a 3-vector which can be considered a screw motion. - ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. :seealso: trlog, trexp2 """ if argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3): # se(2) case if argcheck.ismatrix(S, (3, 3)): # augmentented skew matrix tw = trn.vexa(S) else: # 3 vector tw = argcheck.getvector(S) if vec.iszerovec(tw): return np.eye(3) if theta is None: (tw, theta) = vec.unittwist2(tw) else: assert vec.isunittwist2( tw), 'If theta is specified S must be a unit twist' t = tw[0:2] w = tw[2] R = trn._rodrigues(w, theta) skw = trn.skew(w) V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + ( theta - math.sin(theta)) * skw @ skw return trn.rt2tr(R, V @ t) elif argcheck.ismatrix(S, (2, 2)) or argcheck.isvector(S, 1): # so(2) case if argcheck.ismatrix(S, (2, 2)): # skew symmetric matrix w = trn.vex(S) else: # 1 vector w = argcheck.getvector(S) if theta is not None: assert vec.isunitvec( w), 'If theta is specified S must be a unit twist' # do Rodrigues' formula for rotation return trn._rodrigues(w, theta) else: raise ValueError( " First argument must be SO(2), 1-vector, SE(2) or 3-vector")