def angvec2r(theta, v, unit='rad'): """ Create an SO(3) rotation matrix from rotation angle and axis :param theta: rotation :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param v: rotation axis, 3-vector :type v: array_like :return: 3x3 rotation matrix :rtype: numpdy.ndarray, shape=(3,3) ``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix equivalent to a rotation of ``THETA`` about the vector ``V``. Notes: - If ``THETA == 0`` then return identity matrix. - If ``THETA ~= 0`` then ``V`` must have a finite length. :seealso: :func:`~angvec2tr`, :func:`~tr2angvec` """ assert np.isscalar(theta) and argcheck.isvector( v, 3), "Arguments must be theta and vector" if np.linalg.norm(v) < 10 * _eps: return np.eye(3) theta = argcheck.getunit(theta, unit) # Rodrigue's equation sk = trn.skew(vec.unitvec(v)) R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk return R
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 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")
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")