def exp(self, theta=None, units='rad'): """ Exponentiate a twist :param theta: DESCRIPTION, defaults to None :type theta: TYPE, optional :param units: DESCRIPTION, defaults to 'rad' :type units: TYPE, optional :return: DESCRIPTION :rtype: TYPE TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3). TW.exp(THETA) as above but with a rotation of THETA about the Twist3. Notes:: - For the second form the twist must, if rotational, have a unit rotational component. See also Twist3.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): return SE3(base.trexp(self.S * theta)) else: return SE3([base.trexp(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'): """ Twist3.exp Convert twist to homogeneous transformation TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3). TW.exp(THETA) as above but with a rotation of THETA about the Twist3. Notes:: - For the second form the twist must, if rotational, have a unit rotational component. See also Twist3.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): return SE2(base.trexp2(self.S * theta)) else: return SE2([base.trexp2(self.S * t) for t in theta])
def rot2(theta, unit='rad'): """ Create SO(2) rotation :param theta: rotation angle :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: SO(2) rotation matrix :rtype: ndarray(2,2) - ``rot2(θ)`` is an SO(2) rotation matrix (2x2) representing a rotation of θ radians. - ``rot2(θ, 'deg')`` as above but θ is in degrees. .. runblock:: pycon >>> from spatialmath.base import * >>> rot2(0.3) >>> rot2(45, 'deg') """ theta = base.getunit(theta, unit) ct = base.sym.cos(theta) st = base.sym.sin(theta) R = np.array([[ct, -st], [st, ct]]) return R
def Rz(cls, theta, unit='rad', t=None): """ Create a new 3D twist for pure rotation about the Z-axis :param θ: rotation angle about Z-axis :type θ: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 3D twist vector :rtype: Twist3 instance - ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis - ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined by consecutive elements. Example: .. runblock:: pycon >>> from spatialmath import Twist3 >>> Twist3.Rz(0.3) >>> Twist3.Rz([0.3, 0.4]) :seealso: :func:`~spatialmath.base.transforms3d.trotz` :SymPy: supported """ return cls( [np.r_[0, 0, 0, 0, 0, x] for x in base.getunit(theta, unit=unit)])
def exp(self, theta=None, units='rad'): """ Exponentiate a 3D twist :param theta: rotation magnitude, defaults to None :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional :return: SE(3) matrix :rtype: SE3 instance - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: .. runblock:: pycon >>> from spatialmath import SE3, Twist3 >>> T = SE3(1, 2, 3) * SE3.Rx(0.3) >>> S = Twist3(T) >>> S.exp(0) >>> S.exp(1) .. notes:: - For the second form, the twist must, if rotational, have a unit rotational component. :seealso: :func:`spatialmath.base.trexp` """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): # theta is a scalar return SE3(base.trexp(self.S * theta)) else: # theta is a vector if len(self) == 1: return SE3([base.trexp(self.S * t) for t in theta]) elif len(self) == len(theta): return SE3( [base.trexp(S * t) for S, t in zip(self.data, theta)]) else: raise ValueError('length of twist and theta not consistent')
def exp(self, theta=None, units='rad'): r""" Exponentiate a 2D twist :param theta: rotation magnitude, defaults to None :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional :return: SE(2) matrix :rtype: SE2 instance - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: .. runblock:: pycon >>> from spatialmath import SE2, Twist2 >>> T = SE2(1, 2, 0.3) >>> S = Twist2(T) >>> S.exp(0) >>> S.exp(1) .. notes:: - For the second form, the twist must, if rotational, have a unit rotational component. :seealso: :func:`spatialmath.base.trexp2` """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): return SE2(base.trexp2(self.S * theta)) else: return SE2([base.trexp2(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'): """ Exponentiate a 3D twist :param theta: DESCRIPTION, defaults to None :type theta: TYPE, optional :param units: DESCRIPTION, defaults to 'rad' :type units: TYPE, optional :return: homogeneous transformation :rtype: SE3 -``X.exp()`` is the homogeneous transformation equivalent to the twist. -``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis. .. notes:: - For the second form, the twist must, if rotational, have a unit rotational component. See also Twist3.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): # theta is a scalar return SE3(base.trexp(self.S * theta)) else: # theta is a vector if len(self) == 1: return SE3([base.trexp(self.S * t) for t in theta]) elif len(self) == len(theta): return SE3( [base.trexp(S * t) for S, t in zip(self.data, theta)]) else: raise ValueError('length of twist and theta not consistent')
def __init__(self, axis_func=None, axis=None, eta=None, unit='rad', j=None, flip=False): super().__init__() # init UserList superclass if axis_func is None and axis is None and eta is None: # ET() # create instance with no values self.data = [] return elif isinstance(axis_func, ETS): # copy constructor e = axis_func axis_func = e.axis_func axis = e.axis # et = e.eta j = e.jindex flip = e.isflip joint = e.isjoint T = e.T elif callable(axis_func): if eta is None: # no value, it's a variable joint if unit != 'rad': raise ValueError( 'can only use radians for a variable transform') joint = True T = None else: # constant value specified joint = False eta = getunit(eta, unit) T = axis_func(eta) if j is not None: raise ValueError( 'cannot specify joint index for a constant ET') if flip: raise ValueError('cannot specify flip for a constant ET') elif axis == 'C': # it's a constant element Ci if isinstance(self, ETS): # ETS if not isinstance(eta, np.ndarray): T = eta.A else: T = eta if T.shape != (4, 4): raise ValueError('argument must be ndarray(4,4) or SE3') else: # ETS2 if not isinstance(eta, np.ndarray): T = eta.A else: T = eta if T.shape != (3, 3): raise ValueError('argument must be ndarray(3,3) or SE2') axis = "C" joint = False axis_func = None else: raise ValueError('axis_func must be callable or ndarray') # Save all the params in a named tuple e = SimpleNamespace(eta=eta, axis_func=axis_func, axis=axis, joint=joint, T=T, jindex=j, flip=flip) # And make it the only value of this instance self.data = [e]
def __init__(self, axis=None, eta=None, axis_func=None, unit='rad', j=None, flip=False, qlim=None): """ Elementary transform sequence (superclass) :param axis: the axis. For 2D case: 'r', 'tx', 'ty'. For 3D case: 'rx', 'ry', 'rz', 'tx', 'ty', 'tz'. :type axis: str :param eta: the constant associated with this transform, not given for a joint transform :type eta: float or symbol, optional :param axis_func: [description], defaults to None :type axis_func: [type], optional :param unit: unit for ``eta``, 'rad' [default] or 'deg' :type unit: str :param j: joint number, for joint transforms only :type j: int, optional :param flip: flip the sign of joint variable, defaults to False :type flip: bool, optional Examples: .. runblock:: pycon >>> from roboticstoolbox import ETS, ETS2 ETS2.r() # variable 2D rotation ETS2.r(90, unit='deg') # 2D constant rotation ETS.rx() # variable 3D rotation about x-axis ETS.tx(1) # constant 3D translation along x-axis Composition ----------- These transforms can be composed, for example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e = ETS.rx() * ETS.tx(1) * ETS.rx() * ETS.tx(1) >>> print(e) >>> len(e) >>> e[0] >>> e[1] Under the hood -------------- The value of an ETS is obtained using its ``T`` method. For a joint ETS the joint variable must be passed ``e.T(q)``, otherwise the value based on ``eta`` is computed and cached at constructor time. """ super().__init__() # init UserList superclass if axis is None and eta is None and axis_func is None: # ET() # create instance with no values self.data = [] return elif isinstance(axis, ETS): # copy constructor # e = axis_func # axis_func = e.axis_func # axis = e.axis # # et = e.eta # j = e.jindex # flip = e.isflip # joint = e.isjoint # T = e.T self.data = copy.copy(axis.data) return if axis in ('R', 'Rx', 'Ry', 'Rz', 'tx', 'ty', 'tz'): # it's a regular axis if eta is None: # no value, it's a variable joint if unit != 'rad': raise ValueError( 'can only use radians for a variable transform') joint = True T = None else: # constant value specified if not callable(axis_func): raise ValueError('axis func must be callable') joint = False eta = getunit(eta, unit) T = axis_func(eta) if j is not None: raise ValueError( 'cannot specify joint index for a constant ET') if flip: raise ValueError('cannot specify flip for a constant ET') elif axis == 'C': # it's a constant element Ci if isinstance(self, ETS): # ETS if not isinstance(eta, np.ndarray): T = eta.A else: T = eta if T.shape != (4, 4): raise ValueError('argument must be ndarray(4,4) or SE3') else: # ETS2 if not isinstance(eta, np.ndarray): T = eta.A else: T = eta if T.shape != (3, 3): raise ValueError('argument must be ndarray(3,3) or SE2') axis = "C" joint = False axis_func = None else: raise ValueError('bad axis specified') # Save all the params in a named tuple e = SimpleNamespace(eta=eta, axis_func=axis_func, axis=axis, joint=joint, T=T, jindex=j, flip=flip, qlim=qlim) # And make it the only value of this instance self.data = [e]