def ad(self): """ Logarithm of adjoint of 3D twist :return: logarithm of adjoint matrix :rtype: ndarray(6,6) ``S.ad()`` is the 6x6 logarithm of the adjoint matrix of the corresponding homogeneous transformation. For a twist representing motion from frame {B} to {A}, the adjoint will transform a twist relative to frame {A} to one relative to frame {B}. Example: .. runblock:: pycon >>> from spatialmath import Twist3 >>> S = Twist3.Rx(0.3) >>> S.ad() .. note:: An alternative approach to computing the adjoint is to exponentiate this 6x6 matrix. :seealso: :func:`Twist3.Ad` """ return np.block([[base.skew(self.w), base.skew(self.v)], [np.zeros((3, 3)), base.skew(self.w)]])
def ad(self): """ Twist3.ad Logarithm of adjoint TW.ad is the logarithm of the adjoint matrix of the corresponding homogeneous transformation. See also SE3.Ad. """ return np.array([ base.skew(self.w), base.skew(self.v), [np.zeros((3, 3)), base.skew(self.w)] ])
def ad(self): """ Logarithm of adjoint :return: logarithm of adjoint matrix :rtype: numpy.ndarray, shape=(6,6) - ``X.ad()`` is the 6x6 logarithm of the adjoint matrix of the corresponding homogeneous transformation. """ return np.array([ base.skew(self.w), base.skew(self.v), [np.zeros((3, 3)), base.skew(self.w)] ])
def dotb(q, w): """ Rate of change of unit-quaternion :arg q0: unit-quaternion :type q0: array_like(4) :arg w: 3D angular velocity in body frame :type w: array_like(3) :return: rate of change of unit quaternion :rtype: ndarray(4) ``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q`` which represents the orientation of a body frame with angular velocity ``w`` in the body frame. .. runblock:: pycon >>> from spatialmath.base import dotb, qprint >>> from math import sqrt >>> q = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> dotb(q, [1, 2, 3]) .. warning:: There is no check that the passed values are unit-quaternions. """ q = base.getvector(q, 4) w = base.getvector(w, 3) E = q[0] * (np.eye(3, 3)) + base.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
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 Ad(self): """ Adjoint of SO(3) :return: adjoint matrix :rtype: numpy.ndarray, shape=(3,3) - ``SEO.Ad`` is the 6x6 adjoint matrix :seealso: Twist.ad. """ return np.array([[self.R, tr.skew(self.t) @ self.R], [np.zeros((3, 3)), self.R]])
def ad(self): """ Logarithm of adjoint of 3D twist :return: logarithm of adjoint matrix :rtype: numpy.ndarray, shape=(6,6) ``t.ad()`` is the 6x6 logarithm of the adjoint matrix of the corresponding homogeneous transformation. For a twist representing motion from frame {B} to {A}, the adjoint will transform a twist relative to frame {A} to one relative to frame {B}. .. notes:: - An alternative path to the adjoint is to exponentiate this 6x6 matrix. :seealso: :func:`Twist3.Ad` """ return np.array([ base.skew(self.w), base.skew(self.v), [np.zeros((3, 3)), base.skew(self.w)] ])
def __init__(self, m=None, r=None, I=None): """ Create a new spatial inertia :param m: mass :type m: float :param r: centre of mass relative to link frame :type r: 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, r I)`` is a spatial inertia object for a rigid-body with mass ``m``, centre of mass at ``r`` 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). :SymPy: supported """ super().__init__() if m is None and r is None and I is None: # no arguments I = SpatialInertia._identity() elif m is not None and r is None and I is None and base.ismatrix( m, (6, 6)): I = base.getmatrix(m, (6, 6)) elif m is not None and r is not None: r = base.getvector(r, 3) if I is None: I = np.zeros((3, 3)) else: I = base.getmatrix(I, (3, 3)) C = base.skew(r) M = np.diag((m, ) * 3) # sym friendly I = np.block([[M, m * C.T], [m * C, I + m * C @ C.T]]) else: raise ValueError('bad values') self.data = [I]
def __rmul__(right, left): """ Line transformation :param left: Rigid-body transform :type left: SE3 :param right: Right operand :type right: Plucker :return: transformed line :rtype: Plucker ``T * line`` is the line transformed by the rigid body transformation ``T``. :seealso: Plucker.__mul__ """ if isinstance(left, SE3): A = np.r_[np.c_[left.R, sm.skew(-left.t) @ left.R], np.c_[np.zeros( (3, 3)), left.R]] return Plucker(A @ right.vec) # premultiply by SE3 else: raise ValueError('bad arguments')
def dotb(q, w): """ Rate of change of unit-quaternion :arg q0: unit-quaternion as a 4-vector :type q0: array_like :arg w: angular velocity in body frame as a 3-vector :type w: array_like :return: rate of change of unit quaternion :rtype: numpy.ndarray, shape=(4,) ``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q`` which represents the orientation of a body frame with angular velocity ``w`` in the body frame. .. warning:: There is no check that the passed values are unit-quaternions. """ q = argcheck.getvector(q, 4) w = argcheck.getvector(w, 3) E = q[0] * (np.eye(3, 3)) + tr.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
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")