def __init__(self, m=None, c=None, I=None): """ Create a new spatial inertia :param m: mass :type m: float :param c: centre of mass relative to link frame :type c: 3-element array_like :param I: inertia about the centre of mass, axes aligned with link frame :type I: numpy.array, shape=(6,6) - ``SpatialInertia(M, C, I)`` is a spatial inertia object for a rigid-body with mass ``M``, centre of mass at ``C`` relative to the link frame, and an inertia matrix ``I`` (3x3) about the centre of mass. - ``SpatialInertia(I)`` is a spatial inertia object with a value equal to ``I`` (6x6). """ if m is not None and c is not None: assert arg.isvector(c, 3), 'c must be 3-vector' if I is None: I = np.zeros((3,3)) else: assert arg.ismatrix(I, (3,3)), 'I must be 3x3 matrix' C = tr.skew(c) self.I = np.array([ [m * np.eye(3), m @ C.T], [m @ C, I + m * C @ C.T] ]) elif m is None and c is None and I is not None: assert arg.ismatrix(I, (6, 6)), 'I must be 6x6 matrix'
def __init__(self, value): """ Create a new spatial vector (abstract superclass) :param value: Value of the - ``SpatialVector(vec)`` is a spatial vector constructed from the 6-element array-like ``vec`` - ``SpatialVector([V1, V2, ... VN])`` is a spatial vector array with N elements, constructed from the 6-element array-like values ``Vi`` - ``SpatialVector(A)`` is a spatial vector array with N elements, constructed from the columns of the 6xN array ``A``. :seealso: :func:`SpatialVelocity`, :func:`SpatialAcceleration`, :func:`SpatialForce`, :func:`SpatialMomentum`. """ print('spatialVec6 init') super().__init__() if value is None: self.data = [np.zeros((6,))] elif arg.isvector(value, 6): self.data = [np.array(value)] elif isinstance(value, list): assert all(map(lambda x: arg.isvector(x, 6), value)), 'all elements of list must have valid shape and value for the class' self.data = [np.array(x) for x in value] elif arg.ismatrix(value, (6, None)): self.data = [x for x in value.T] else: raise ValueError('bad arguments to constructor')
def Exp(cls, S, so3=False): """ Create an SO(3) rotation matrix from so(3) :param S: Lie algebra so(3) :type S: numpy ndarray :param so3: accept input as an so(3) matrix [default False] :type so3: bool :return: 3x3 rotation matrix :rtype: SO3 instance - ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra which is a 3x3 so(3) matrix (skew symmetric) - ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist vector (the unique elements of the so(3) skew-symmetric matrix) - ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix of twist vectors, one per row. Note: - an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the parameter `so3` is the decider. :seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew` """ if argcheck.ismatrix(S, (-1, 3)) and not so3: return cls([tr.trexp(s) for s in S]) else: return cls(tr.trexp(S), check=False)
def I(self, I_new): # noqa if ismatrix(I_new, (3, 3)): # 3x3 matrix passed if np.any(np.abs(I_new - I_new.T) > 1e-8): raise ValueError('3x3 matrix is not symmetric') elif isvector(I_new, 9): # 3x3 matrix passed as a 1d vector I_new = I_new.reshape(3, 3) if np.any(np.abs(I_new - I_new.T) > 1e-8): raise ValueError('3x3 matrix is not symmetric') elif isvector(I_new, 6): # 6-vector passed, moments and products of inertia, # [Ixx Iyy Izz Ixy Iyz Ixz] I_new = np.array([ [I_new[0], I_new[3], I_new[5]], [I_new[3], I_new[1], I_new[4]], [I_new[5], I_new[4], I_new[2]] ]) elif isvector(I_new, 3): # 3-vector passed, moments of inertia [Ixx Iyy Izz] I_new = np.diag(I_new) else: raise ValueError('invalid shape passed: must be (3,3), (6,), (3,)') self._I = I_new
def Exp(cls, S, check=True, so3=True): r""" Create an SO(3) rotation matrix from so(3) :param S: Lie algebra so(3) :type S: numpy ndarray :param check: check that passed matrix is valid so(3), default True :type check: bool :param so3: input is an so(3) matrix (default True) :type so3: bool :return: SO(3) rotation :rtype: SO3 instance - ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra which is a 3x3 so(3) matrix (skew symmetric) - ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist vector (the unique elements of the so(3) skew-symmetric matrix) - ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix of twist vectors, one per row. Note: - if :math:`\theta \eq 0` the result in an identity matrix - an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the parameter `so3` is the decider. :seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew` """ if argcheck.ismatrix(S, (-1, 3)) and not so3: return cls([tr.trexp(s, check=check) for s in S], check=False) else: return cls(tr.trexp(S, check=check), check=False)
def contains(self, x, tol=50 * _eps): """ Test if points are on the line :param x: 3D point :type x: 3-element array_like, or numpy.ndarray, shape=(3,N) :param tol: Tolerance, defaults to 50*_eps :type tol: float, optional :raises ValueError: Bad argument :return: Whether point is on the line :rtype: bool or numpy.ndarray(N) of bool ``line.contains(X)`` is true if the point ``X`` lies on the line defined by the Plucker object self. If ``X`` is an array with 3 rows, the test is performed on every column and an array of booleans is returned. """ if arg.isvector(x, 3): x = arg.getvector(x) return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol elif arg.ismatrix(x, (3, None)): return [ np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol for _ in x.T ] else: raise ValueError('bad argument')
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 isvalid(v, check=True): if argcheck.isvector(v, 3): return True elif argcheck.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 tr.isskew(v[:2, :2]): # top left 2x2is skew symmetric return False return True return False
def contains(self, x, tol=50 * _eps): """ Plucker.contains Test if point is on the line ``line.contains(X)`` is true if the point X (3x1) lies on the line defined by the Plucker object self. """ if arg.isvector(x, 3): return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol elif arg.ismatrix(x, (3, None)): return [ np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol for _ in x.T ] else: raise ValueError('bad argument')
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 P3(cls, p): """ Create a plane object from three points :param p: Three points in the plane :type p: numpy.ndarray, shape=(3,3) :return: a Plane object :rtype: Plane """ p = arg.ismatrix((3, 3)) v1 = p[:, 0] v2 = p[:, 1] v3 = p[:, 2] # compute a normal n = np.cross(v2 - v1, v3 - v1) return cls(n, v1)
def __rmul__(right, left): """ Plucker.mtimes Plucker multiplication M * PL is the product of M (Nx4) and the Plucker skew matrix (4x4). Notes:: - The * operator is overloaded for convenience. - Multiplication or composition of Plucker lines is not defined. - Premultiplying by an SE3 will transform the line with respect to the world coordinate frame. See also Plucker.skew, SE3.mtimes. """ if arg.ismatrix(left, (None, 4)): return left @ right.skew # premultiply by Nx4 elif isinstance(left, SE3): return left.A @ right.skew # premultiply by 4x4
def Exp(cls, S, check=True): """ Construct new SO(2) rotation matrix from so(2) Lie algebra :param S: element of Lie algebra so(2) :type S: numpy ndarray :param check: check that passed matrix is valid so(2), default True :type check: bool :return: SO(2) rotation matrix :rtype: SO2 instance - ``SO2.Exp(S)`` is an SO(2) rotation defined by its Lie algebra which is a 2x2 so(2) matrix (skew symmetric) :seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew` """ if argcheck.ismatrix(S, (-1, 2)) and not so2: return cls([tr.trexp2(s, check=check) for s in S]) else: return cls(tr.trexp2(S, check=check), check=False)
def transl(x, y=None, z=None): """ Create SE(3) pure translation, or extract translation from SE(3) matrix :param x: translation along X-axis :type x: float :param y: translation along Y-axis :type y: float :param z: translation along Z-axis :type z: float :return: 4x4 homogeneous transformation matrix :rtype: numpy.ndarray, shape=(4,4) Create a translational SE(3) matrix: - ``T = transl( X, Y, Z )`` is an SE(3) homogeneous transform (4x4) representing a pure translation of X, Y and Z. - ``T = transl( V )`` as above but the translation is given by a 3-element list, dict, or a numpy array, row or column vector. Extract the translational part of an SE(3) matrix: - ``P = TRANSL(T)`` is the translational part of a homogeneous transform T as a 3-element numpy array. :seealso: :func:`~spatialmath.base.transforms2d.transl2` """ if np.isscalar(x): T = np.identity(4) T[:3, 3] = [x, y, z] return T elif argcheck.isvector(x, 3): T = np.identity(4) T[:3, 3] = argcheck.getvector(x, 3, out='array') return T elif argcheck.ismatrix(x, (4, 4)): return x[:3, 3] else: ValueError('bad argument')
def __mul__(left, right): """ Plucker.mtimes Plucker multiplication PL1 * PL2 is the scalar reciprocal product. PL * M is the product of the Plucker skew matrix and M (4xN). Notes:: - The * operator is overloaded for convenience. - Multiplication or composition of Plucker lines is not defined. - Premultiplying by an SE3 will transform the line with respect to the world coordinate frame. See also Plucker.skew, SE3.mtimes. """ if isinstance(left, Plucker) and isinstance(right, Plucker): # reciprocal product return np.dot(left.uw, right.v) + np.dot(right.uw, left.v) elif isinstance(left, Plucker) and arg.ismatrix(right, (4, None)): return left.skew @ right
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 argcheck.isvector(v, 6): return True elif argcheck.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 tr.isskew(v[:3, :3]): # top left 3x3 is skew symmetric return False return True return False
def transl2(x, y=None): """ Create SE(2) pure translation, or extract translation from SE(2) matrix :param x: translation along X-axis :type x: float :param y: translation along Y-axis :type y: float :return: homogeneous transform matrix or the translation elements of a homogeneous transform :rtype: numpy.ndarray, shape=(3,3) Create a translational SE(2) matrix: - ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a pure translation. - ``T = transl2( V )`` as above but the translation is given by a 2-element list, dict, or a numpy array, row or column vector. Extract the translational part of an SE(2) matrix: P = TRANSL2(T) is the translational part of a homogeneous transform as a 2-element numpy array. """ if np.isscalar(x): T = np.identity(3) T[:2, 2] = [x, y] return T elif argcheck.isvector(x, 2): T = np.identity(3) T[:2, 2] = argcheck.getvector(x, 2) return T elif argcheck.ismatrix(x, (3, 3)): return x[:2, 2] else: ValueError('bad argument')
def trinterp2(start, end, s=None): """ Interpolate SE(2) matrices :param start: initial SO(2) or SE(2) matrix value when s=0, if None then identity is used :type T1: np.ndarray, shape=(2,2), (3,3) or None :param end: final SO(2) or SE(2) matrix, value when s=1 :type T0: np.ndarray, shape=(2,2), (3,3) :param s: interpolation coefficient, range 0 to 1 :type s: float :return: SO(2) or SE(2) matrix :rtype: np.ndarray, shape=(2,2), (3,3) - ``trinterp2(None, T, S)`` is a homogeneous transform (3x3) interpolated between identity when S=0 and T (3x3) when S=1. - ``trinterp2(T0, T1, S)`` as above but interpolated between T0 (3x3) when S=0 and T1 (3x3) when S=1. - ``trinterp2(None, R, S)`` is a rotation matrix (2x2) interpolated between identity when S=0 and R (2x2) when S=1. - ``trinterp2(R0, R1, S)`` as above but interpolated between R0 (2x2) when S=0 and R1 (2x2) when S=1. Notes: - Rotation angle is linearly interpolated. :seealso: :func:`~spatialmath.base.transforms3d.trinterp` %## 2d homogeneous trajectory """ if argcheck.ismatrix(end, (2, 2)): # SO(2) case if start is None: # TRINTERP2(T, s) th0 = math.atan2(end[1, 0], end[0, 0]) th = s * th0 else: # TRINTERP2(T1, start= s) assert start.shape == end.shape, 'start and end matrices must be same shape' th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) th = th0 * (1 - s) + s * th1 return rot2(th) elif argcheck.ismatrix(end, (3, 3)): if start is None: # TRINTERP2(T, s) th0 = math.atan2(end[1, 0], end[0, 0]) p0 = transl2(end) th = s * th0 pr = s * p0 else: # TRINTERP2(T0, T1, s) assert start.shape == end.shape, 'both matrices must be same shape' th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) p0 = transl2(start) p1 = transl2(end) pr = p0 * (1 - s) + s * p1 th = th0 * (1 - s) + s * th1 return trn.rt2tr(rot2(th), pr) else: return ValueError('Argument must be SO(2) or SE(2)')
def trinterp2(end, start=None, s=None): """ Interpolate SE(2) matrices :param end: final SE(3) matrix, value when s=1 :type T0: np.ndarray, shape=(3,3) :param start: initial SE(3) matrix, value when s=0, optional, defaults to identity :type T1: np.ndarray, shape=(3,3) :param s: interpolation coefficient, range 0 to 1 :type s: float :return: SE(2) matrix :rtype: np.ndarray, shape=(3,3) - ``trinterp2(T1, s=S)`` is a homogeneous transform (3x3) interpolated between identity when S=0 and T1 when S=1. - ``trinterp2(T1, start=T0, s=S)`` as above but interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (3x3). Notes: - Rotation angle is linearly interpolated. :seealso: :func:`~spatialmath.base.transforms3d.trinterp` %## 2d homogeneous trajectory """ if argcheck.ismatrix(start, (2,2)): # SO(2) case if T1 is None: # TRINTERP2(T, s) th0 = math.atan2(start[1,0], start[0,0]) th = s * th0 else: # TRINTERP2(T0, T1, s) assert start.shape == end.shape, 'both matrices must be same shape' th0 = math.atan2(start[1,0], start[0,0]) th1 = math.atan2(end[1,0], end[0,0]) th = th0 * (1 - s) + s * th1 return rot2(th) elif argcheck.ismatrix(start, (3,3)): if end is None: # TRINTERP2(T, s) th0 = math.atan2(start[1,0], start[0,0]) p0 = transl2(start) th = s * th0 pr = s * p0 else: # TRINTERP2(T0, T1, s) assert start.shape == end.shape, 'both matrices must be same shape' th0 = math.atan2(start[1,0], start[0,0]) th1 = math.atan2(end[1,0], end[0,0]) p0 = transl2(start) p1 = transl2(end) pr = p0 * (1 - s) + s * p1; th = th0 * (1 - s) + s * th1 return trn.rt2tr(rot2(th), pr) else: return ValueError('Argument must be SO(2) or SE(2)')
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 trinterp2(T0, T1=None, s=None): """ Interpolate SE(2) matrices :param T0: first SE(2) matrix :type T0: np.ndarray, shape=(3,3) :param T1: second SE(2) matrix :type T1: np.ndarray, shape=(3,3) :param s: interpolation coefficient, range 0 to 1 :type s: float :return: SE(2) matrix :rtype: np.ndarray, shape=(3,3) - ``trinterp2(T0, T1, S)`` is a homogeneous transform (3x3) interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (3x3). - ``trinterp2(T1, S)`` as above but interpolated between the identity matrix when S=0 to T1 when S=1. Notes: - Rotation angle is linearly interpolated. :seealso: :func:`~spatialmath.base.transforms3d.trinterp` %## 2d homogeneous trajectory """ if argcheck.ismatrix(T0, (2, 2)): # SO(2) case if T1 is None: # TRINTERP2(T, s) th0 = math.atan2(T0[1, 0], T0[0, 0]) th = s * th0 else: # TRINTERP2(T0, T1, s) assert T0.shape == T1.shape, 'both matrices must be same shape' th0 = math.atan2(T0[1, 0], T0[0, 0]) th1 = math.atan2(T1[1, 0], T1[0, 0]) th = th0 * (1 - s) + s * th1 return rot2(th) elif argcheck.ismatrix(T0, (3, 3)): if T1 is None: # TRINTERP2(T, s) th0 = math.atan2(T0[1, 0], T0[0, 0]) p0 = transl2(T0) th = s * th0 pr = s * p0 else: # TRINTERP2(T0, T1, s) assert T0.shape == T1.shape, 'both matrices must be same shape' th0 = math.atan2(T0[1, 0], T0[0, 0]) th1 = math.atan2(T1[1, 0], T1[0, 0]) p0 = transl2(T0) p1 = transl2(T1) pr = p0 * (1 - s) + s * p1 th = th0 * (1 - s) + s * th1 return trn.rt2tr(rot2(th), pr) else: return ValueError('Argument must be SO(2) or SE(2)')
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