def eul2r(phi, theta=None, psi=None, unit='rad'): """ Create an SO(3) rotation matrix from Euler angles :param phi: Z-axis rotation :type phi: float :param theta: Y-axis rotation :type theta: float :param psi: Z-axis rotation :type psi: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 3x3 rotation matrix :rtype: numpdy.ndarray, shape=(3,3) - ``R = eul2r(PHI, THETA, PSI)`` is an SO(3) orthonornal rotation matrix equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively. - ``R = eul2r(EUL)`` as above but the Euler angles are taken from ``EUL`` which is a 3-vector (array_like) with values (PHI THETA PSI). :seealso: :func:`~rpy2r`, :func:`~eul2tr`, :func:`~tr2eul` """ if np.isscalar(phi): angles = [phi, theta, psi] else: angles = argcheck.getvector(phi, 3) angles = argcheck.getunit(angles, unit) return rotz(angles[0]) @ roty(angles[1]) @ rotz(angles[2])
def Rand(cls, N=1, xrange=(-1, 1), yrange=(-1, 1), arange=(0, 2 * math.pi), unit='rad'): # pylint: disable=arguments-differ r""" Construct a new random SE(2) :param xrange: x-axis range [min,max], defaults to [-1, 1] :type xrange: 2-element sequence, optional :param yrange: y-axis range [min,max], defaults to [-1, 1] :type yrange: 2-element sequence, optional :param arange: angle range [min,max], defaults to :math:`[0, 2\pi)` :type arange: 2-element sequence, optional :param N: number of random rotations, defaults to 1 :type N: int :param unit: angular units 'deg' or 'rad' [default] if applicable :type unit: str, optional :return: homogeneous rigid-body transformation matrix :rtype: SE2 instance Return an SE2 instance with random rotation and translation. - ``SE2.Rand()`` is a random SE(2) rotation. - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random poses. Example, create random ten vehicles in the xy-plane:: >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10 """ x = np.random.uniform(low=xrange[0], high=xrange[1], size=N) # random values in the range y = np.random.uniform(low=yrange[0], high=yrange[1], size=N) # random values in the range theta = np.random.uniform(low=arange[0], high=arange[1], size=N) # random values in the range return cls([base.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit))])
def angvec(cls, theta, v, unit='rad'): v = argcheck.getvector(v, 3) argcheck.isscalar(theta) theta = argcheck.getunit(theta, unit) return UnitQuaternion(s=math.cos(theta / 2), v=math.sin(theta / 2) * tr.unit(v), norm=False)
def exp(self, theta=None, units='rad'): """ Twist.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 twist. Notes:: - For the second form the twist must, if rotational, have a unit rotational component. See also Twist.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = argcheck.getunit(theta, units) if isinstance(theta, (int, np.int64, float, np.float64)): return SE2(tr.trexp2(self.S * theta)) else: return SE2([tr.trexp2(self.S * t) for t in theta])
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 twist. Notes:: - For the second form the twist must, if rotational, have a unit rotational component. See also Twist.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = argcheck.getunit(theta, units) if isinstance(theta, (int, np.int64, float, np.float64)): return SE3(tr.trexp(self.S * theta)) else: return SE3([tr.trexp(self.S * t) for t in theta])
def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'): """ Create an SO(3) rotation matrix from roll-pitch-yaw angles :param roll: roll angle :type roll: float :param pitch: pitch angle :type pitch: float :param yaw: yaw angle :type yaw: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz' :type unit: str :return: 3x3 rotation matrix :rtype: numpdy.ndarray, shape=(3,3) - ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles. These correspond to successive rotations about the axes specified by ``order``: - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, then by roll about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways. - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis, then by roll about the new z-axis. Covention for a robot gripper with z-axis forward and y-axis between the gripper fingers. - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis, then by roll about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows. - ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken from ``RPY`` which is a 3-vector (array_like) with values (ROLL, PITCH, YAW). :seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy` """ if np.isscalar(roll): angles = [roll, pitch, yaw] else: angles = argcheck.getvector(roll, 3) angles = argcheck.getunit(angles, unit) if order == 'xyz' or order == 'arm': R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0]) elif order == 'zyx' or order == 'vehicle': R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0]) elif order == 'yxz' or order == 'camera': R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0]) else: raise ValueError('Invalid angle order') return R
def rand(cls, *, xrange=[-1, 1], yrange=[-1, 1], trange=[0, 2 * math.pi], unit='rad', N=1): x = np.random.uniform(low=xrange[0], high=xrange[1], size=N) # random values in the range y = np.random.uniform(low=yrange[0], high=yrange[1], size=N) # random values in the range theta = np.random.uniform(low=trange[0], high=trange[1], size=N) # random values in the range return cls([ tr.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit)) ])
def Rand(cls, *, xrange=[-1, 1], yrange=[-1, 1], trange=[0, 2 * math.pi], unit='rad', N=1): r""" Construct a new random SE(2) :param xrange: x-axis range [min,max], defaults to [-1, 1] :type xrange: 2-element sequence, optional :param yrange: y-axis range [min,max], defaults to [-1, 1] :type yrange: 2-element sequence, optional :param trange: theta range [min,max], defaults to :math:`[0, 2\pi)` :type yrange: 2-element sequence, optional :param N: number of random rotations, defaults to 1 :type N: int :return: homogeneous rigid-body transformation matrix :rtype: SE2 instance Return an SE2 instance with random rotation and translation. - ``SE2.Rand()`` is a random SE(2) rotation. - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random poses. Example, create random ten vehicles in the xy-plane:: >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10 """ x = np.random.uniform(low=xrange[0], high=xrange[1], size=N) # random values in the range y = np.random.uniform(low=yrange[0], high=yrange[1], size=N) # random values in the range theta = np.random.uniform(low=trange[0], high=trange[1], size=N) # random values in the range return cls([ tr.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit)) ])
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: 2x2 rotation matrix :rtype: numpy.ndarray, shape=(2,2) - ``ROT2(THETA)`` is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians. - ``ROT2(THETA, 'deg')`` as above but THETA is in degrees. """ theta = argcheck.getunit(theta, unit) ct = sym.cos(theta) st = sym.sin(theta) R = np.array([[ct, -st], [st, ct]]) return R
def rotz(theta, unit="rad"): """ Create SO(3) rotation about Z-axis :param theta: rotation angle about Z-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 3x3 rotation matrix :rtype: numpy.ndarray, shape=(3,3) - ``rotz(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the z-axis - ``rotz(THETA, "deg")`` as above but THETA is in degrees :seealso: :func:`~yrotz` """ theta = argcheck.getunit(theta, unit) ct = _cos(theta) st = _sin(theta) R = np.array([[ct, -st, 0], [st, ct, 0], [0, 0, 1]]) return R
def Rand(cls, N=1, arange=(0, 2 * math.pi), unit='rad'): r""" Construct new SO(2) with random rotation :param arange: rotation range, defaults to :math:`[0, 2\pi)`. :type arange: 2-element array-like, optional :param unit: angular units as 'deg or 'rad' [default] :type unit: str, optional :param N: number of random rotations, defaults to 1 :type N: int :return: SO(2) rotation matrix :rtype: SO2 instance - ``SO2.Rand()`` is a random SO(2) rotation. - ``SO2.Rand([-90, 90], unit='deg')`` is a random SO(2) rotation between -90 and +90 degrees. - ``SO2.Rand(N)`` is a sequence of N random rotations. Rotations are uniform over the specified interval. """ rand = np.random.uniform(low=arange[0], high=arange[1], size=N) # random values in the range return cls([base.rot2(x) for x in argcheck.getunit(rand, unit)])
def addconfiguration(self, name, q, unit='rad'): """ Add a named joint configuration (Robot superclass) :param name: Name of the joint configuration :type name: str :param q: Joint configuration :type q: ndarray(n) or list Example: .. runblock:: pycon >>> import roboticstoolbox as rtb >>> robot = rtb.models.DH.Puma560() >>> robot.qz >>> robot.addconfiguration("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) >>> robot.mypos """ v = getvector(q, self.n) v = getunit(v, unit) self._configdict[name] = v setattr(self, name, v)
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 __init__(self, x = None, y = None, theta = None, *, unit='rad'): super().__init__() # activate the UserList semantics if x is None: # empty constructor self.data = [np.eye(3)] elif all(map(lambda x: isinstance(x, (int,float)), [x, y, theta])): # SE2(x, y, theta) self.data = [transforms.trot2(theta, t=[x,y], unit=unit)] elif argcheck.isvector(x) and argcheck.isvector(y) and argcheck.isvector(theta): # SE2(xvec, yvec, tvec) xvec = argcheck.getvector(x) yvec = argcheck.getvector(y, dim=len(xvec)) tvec = argcheck.getvector(theta, dim=len(xvec)) self.data = [transforms.trot2(_t, t=[_x, _y]) for (_x, _y, _t) in zip(xvec, yvec, argcheck.getunit(tvec, unit))] elif isinstance(x, np.ndarray) and y is None and theta is None: assert x.shape[1] == 3, 'array argument must be Nx3' self.data = [transforms.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x] else: super().arghandler(x)
def rand(cls, *, range=[0, 2*math.pi], unit='rad', N=1): rand = np.random.uniform(low=range[0], high=range[1], size=N) # random values in the range return cls([transforms.rot2(x) for x in argcheck.getunit(rand, unit)])