def isvalid(v, check=True): """ Test if matrix is valid twist :param x: array to test :type x: ndarray :return: Whether the value is a 6-vector or a valid 4x4 se(3) element :rtype: bool A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix, for example: .. runblock:: pycon >>> from spatialmath import Twist3, base >>> import numpy as np >>> Twist3.isvalid([1, 2, 3, 4, 5, 6]) >>> a = base.skewa([1, 2, 3, 4, 5, 6]) >>> a >>> Twist3.isvalid(a) >>> Twist3.isvalid(np.random.rand(4,4)) """ if base.isvector(v, 6): return True elif base.ismatrix(v, (4, 4)): # maybe be an se(3) if not base.iszerovec(v.diagonal()): # check diagonal is zero return False if not base.iszerovec(v[3, :]): # check bottom row is zero return False if check and not base.isskew(v[:3, :3]): # top left 3x3 is skew symmetric return False return True return False
def isvalid(v, check=True): if base.isvector(v, 3): return True elif base.ismatrix(v, (3, 3)): # maybe be an se(2) if not all(v.diagonal() == 0): # check diagonal is zero return False if not all(v[2, :] == 0): # check bottom row is zero return False if not base.isskew(v[:2, :2]): # top left 2x2is skew symmetric return False return True return False
def isvalid(v, check=True): """ Test if matrix is valid twist :param x: array to test :type x: numpy.ndarray :return: true of the matrix is a 6-vector or a 4x4 se(3) element :rtype: bool A twist can be reprented by a 6-vector or a 4x4 skew symmetric matrix, for example:: Twist3.isvalid([1, 2, 3, 4, 5, 6]) >>> a = base.skewa([1, 2, 3, 4, 5, 6]) >>> a array([[ 0., -6., 5., 1.], [ 6., 0., -4., 2.], [-5., 4., 0., 3.], [ 0., 0., 0., 0.]]) >>> Twist3.isvalid(a) True >>> b=np.random.rand(4,4) >>> Twist3.isvalid(b) False """ if base.isvector(v, 6): return True elif base.ismatrix(v, (4, 4)): # maybe be an se(3) if not all(v.diagonal() == 0): # check diagonal is zero return False if not all(v[3, :] == 0): # check bottom row is zero return False if not base.isskew(v[:3, :3]): # top left 3x3 is skew symmetric return False return True return False
def isvalid(v, check=True): """ Test if matrix is valid twist :param x: array to test :type x: numpy.ndarray :return: true of the matrix is a 6-vector or a 4x4 se(3) element :rtype: bool """ if base.isvector(v, 6): return True elif base.ismatrix(v, (4, 4)): # maybe be an se(3) if not all(v.diagonal() == 0): # check diagonal is zero return False if not all(v[3, :] == 0): # check bottom row is zero return False if not base.isskew(v[:3, :3]): # top left 3x3 is skew symmetric return False return True return False
def trexp2(S, theta=None, check=True): """ Exponential of so(2) or se(2) matrix :param S: se(2), so(2) matrix or equivalent velctor :type T: ndarray(3,3) or ndarray(2,2) :param theta: motion :type theta: float :return: matrix exponential in SE(2) or SO(2) :rtype: ndarray(3,3) or ndarray(2,2) :raises ValueError: bad argument An efficient closed-form solution of the matrix exponential for arguments that are se(2) or so(2). For se(2) the results is an SE(2) homogeneous transformation matrix: - ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is a 3x3 augmented skew-symmetric matrix. - ``trexp2(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. - ``trexp2(S)`` is the matrix exponential of the se(3) element ``S`` represented as a 3-vector which can be considered a screw motion. - ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. .. runblock:: pycon >>> from spatialmath.base import * >>> trexp2(skew(1)) >>> trexp2(skew(1), 2) # revolute unit twist >>> trexp2(1) >>> trexp2(1, 2) # revolute unit twist For so(2) the results is an SO(2) rotation matrix: - ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2 skew-symmetric matrix. - ``trexp2(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given by ``θ``. - ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as a 1-vector. - ``trexp2(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a unit-norm vector representing a rotation axis and a rotation magnitude given by ``θ``. ``ω`` is expressed as a 1-vector. .. runblock:: pycon >>> from spatialmath.base import * >>> trexp2(skewa([1, 2, 3])) >>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist >>> trexp2([1, 2, 3]) >>> trexp2([1, 0, 0], 2) :seealso: trlog, trexp2 """ if base.ismatrix(S, (3, 3)) or base.isvector(S, 3): # se(2) case if base.ismatrix(S, (3, 3)): # augmentented skew matrix if check and not base.isskewa(S): raise ValueError("argument must be a valid se(2) element") tw = base.vexa(S) else: # 3 vector tw = base.getvector(S) if base.iszerovec(tw): return np.eye(3) if theta is None: (tw, theta) = base.unittwist2_norm(tw) elif not base.isunittwist2(tw): raise ValueError("If theta is specified S must be a unit twist") t = tw[0:2] w = tw[2] R = base.rodrigues(w, theta) skw = base.skew(w) V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + ( theta - math.sin(theta)) * skw @ skw return base.rt2tr(R, V @ t) elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1): # so(2) case if base.ismatrix(S, (2, 2)): # skew symmetric matrix if check and not base.isskew(S): raise ValueError("argument must be a valid so(2) element") w = base.vex(S) else: # 1 vector w = base.getvector(S) if theta is not None and not base.isunitvec(w): raise ValueError("If theta is specified S must be a unit twist") # do Rodrigues' formula for rotation return base.rodrigues(w, theta) else: raise ValueError( " First argument must be SO(2), 1-vector, SE(2) or 3-vector")