def Rz(cls, theta, unit='rad', t=None): """ Create an SE(3) 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 :param t: translation, optional :type t: 3-element array-like :return: SE(3) matrix :rtype: SE3 instance - ``SE3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis - ``SE3.Rz(θ, "deg")`` as above but θ is in degrees - ``SE3.Rz(θ, t=T)`` as above but also sets the translational component If ``θ`` is an array then the result is a sequence of rotations defined by consecutive elements. .. note:: The translation option only works for the scalar θ case. Example: .. runblock:: pycon >>> SE3.Rz(0.3) >>> SE3.Rz([0.3, 0.4]) :seealso: :func:`~spatialmath.base.transforms3d.trotz` :SymPy: supported """ return cls( [base.trotz(x, t=t, unit=unit) for x in base.getvector(theta)], check=False)
def __init__(self): # deg = np.pi/180 mm = 1e-3 tool_offset = (103) * mm flange = (107) * mm # d7 = (58.4)*mm L1 = Revolute(a=0.0, d=0.333, alpha=0.0, qlim=np.array([-2.8973, 2.8973]), mdh=1) L2 = Revolute(a=0.0, d=0.0, alpha=-np.pi / 2, qlim=np.array([-1.7628, 1.7628]), mdh=1) L3 = Revolute(a=0.0, d=0.316, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973]), mdh=1) L4 = Revolute(a=0.0825, d=0.0, alpha=np.pi / 2, qlim=np.array([-3.0718, -0.0698]), mdh=1) L5 = Revolute(a=-0.0825, d=0.384, alpha=-np.pi / 2, qlim=np.array([-2.8973, 2.8973]), mdh=1) L6 = Revolute(a=0.0, d=0.0, alpha=np.pi / 2, qlim=np.array([-0.0175, 3.7525]), mdh=1) L7 = Revolute(a=0.088, d=flange, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973]), mdh=1) L = [L1, L2, L3, L4, L5, L6, L7] # super(Panda, self).__init__(L, name = 'Panda', manufacturer = 'Franka Emika', tool = transl(0, 0, d7)) tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4) super(PandaMDH, self).__init__(L, name='Panda', manufacturer='Franka Emika', tool=tool) # tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4) self.qz = np.array([0, 0, 0, 0, 0, 0, 0]) # self.qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
def __init__(self): # deg = np.pi/180 mm = 1e-3 tool_offset = (103) * mm flange = (107) * mm # d7 = (58.4)*mm # This Panda model is defined using modified # Denavit-Hartenberg parameters L = [ RevoluteMDH(a=0.0, d=0.333, alpha=0.0, qlim=np.array([-2.8973, 2.8973])), RevoluteMDH(a=0.0, d=0.0, alpha=-np.pi / 2, qlim=np.array([-1.7628, 1.7628])), RevoluteMDH(a=0.0, d=0.316, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973])), RevoluteMDH(a=0.0825, d=0.0, alpha=np.pi / 2, qlim=np.array([-3.0718, -0.0698])), RevoluteMDH(a=-0.0825, d=0.384, alpha=-np.pi / 2, qlim=np.array([-2.8973, 2.8973])), RevoluteMDH(a=0.0, d=0.0, alpha=np.pi / 2, qlim=np.array([-0.0175, 3.7525])), RevoluteMDH(a=0.088, d=flange, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973])) ] tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4) super().__init__(L, name='Panda', manufacturer='Franka Emika', tool=tool) # tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4) self._qz = np.array([0, 0, 0, 0, 0, 0, 0]) # self.qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg self._qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
def __init__(self): # deg = np.pi/180 mm = 1e-3 tool_offset = (103) * mm flange = 0 * mm # d7 = (58.4)*mm # This Kuka model is defined using modified # Denavit-Hartenberg parameters L = [ RevoluteDH(a=0.0, d=0, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973])), RevoluteDH(a=0.0, d=0.0, alpha=-np.pi / 2, qlim=np.array([-1.7628, 1.7628])), RevoluteDH(a=0.0, d=0.40, alpha=-np.pi / 2, qlim=np.array([-2.8973, 2.8973])), RevoluteDH(a=0.0, d=0.0, alpha=np.pi / 2, qlim=np.array([-3.0718, -0.0698])), RevoluteDH(a=0.0, d=0.39, alpha=np.pi / 2, qlim=np.array([-2.8973, 2.8973])), RevoluteDH(a=0.0, d=0.0, alpha=-np.pi / 2, qlim=np.array([-0.0175, 3.7525])), RevoluteDH(a=0.0, d=flange, alpha=0.0, qlim=np.array([-2.8973, 2.8973])) ] tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4) super().__init__(L, name='LWR-IV', manufacturer='Kuka', tool=tool) # tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4) self.addconfiguration("qz", [0, 0, 0, 0, 0, 0, 0])
def test_T_real(self): fl = 1.543 rx = rp.ET.TRx(joint=86) ry = rp.ET.TRy(joint=86) rz = rp.ET.TRz(joint=86) tx = rp.ET.Ttx(joint=86) ty = rp.ET.Tty(joint=86) tz = rp.ET.Ttz(joint=86) nt.assert_array_almost_equal(rx.T(fl), sm.trotx(fl)) nt.assert_array_almost_equal(ry.T(fl), sm.troty(fl)) nt.assert_array_almost_equal(rz.T(fl), sm.trotz(fl)) nt.assert_array_almost_equal(tx.T(fl), sm.transl(fl, 0, 0)) nt.assert_array_almost_equal(ty.T(fl), sm.transl(0, fl, 0)) nt.assert_array_almost_equal(tz.T(fl), sm.transl(0, 0, fl))
def test_T_real_2(self): fl = 1.543 rx = rp.ET.rx() ry = rp.ET.ry() rz = rp.ET.rz() tx = rp.ET.tx() ty = rp.ET.ty() tz = rp.ET.tz() nt.assert_array_almost_equal(rx.T(fl).A, sm.trotx(fl)) nt.assert_array_almost_equal(ry.T(fl).A, sm.troty(fl)) nt.assert_array_almost_equal(rz.T(fl).A, sm.trotz(fl)) nt.assert_array_almost_equal(tx.T(fl).A, sm.transl(fl, 0, 0)) nt.assert_array_almost_equal(ty.T(fl).A, sm.transl(0, fl, 0)) nt.assert_array_almost_equal(tz.T(fl).A, sm.transl(0, 0, fl))
def Rz(cls, theta, unit='rad'): """ Create SE(3) pure rotation about the Z-axis :param theta: rotation angle about Z-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 4x4 homogeneous transformation matrix :rtype: SE3 instance - ``SE3.Rz(THETA)`` is an SO(3) rotation of THETA radians about the z-axis - ``SE3.Rz(THETA, "deg")`` as above but THETA is in degrees """ return cls([tr.trotz(x, unit) for x in argcheck.getvector(theta)])
def Rz(cls, theta, unit='rad'): """ Create SE(3) pure rotation about the Z-axis :param theta: rotation angle about Z-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 4x4 homogeneous transformation matrix :rtype: SE3 instance - ``SE3.Rz(THETA)`` is an SO(3) rotation of THETA radians about the z-axis - ``SE3.Rz(THETA, "deg")`` as above but THETA is in degrees If ``theta`` is an array then the result is a sequence of rotations defined by consecutive elements. """ return cls([tr.trotz(x, unit) for x in argcheck.getvector(theta)], check=False)
def __init__(self): deg = np.pi / 180 mm = 1e-3 tool_offset = (103) * mm et_list = [ ET.Ttz(0.333), ET.TRz(joint=1), ET.TRx(-90 * deg), ET.TRz(joint=2), ET.TRx(90 * deg), ET.Ttz(0.316), ET.TRz(joint=3), ET.Ttx(0.0825), ET.TRx(90 * deg), ET.TRz(joint=4), ET.Ttx(-0.0825), ET.TRx(-90 * deg), ET.Ttz(0.384), ET.TRz(joint=5), ET.TRx(90 * deg), ET.TRz(joint=6), ET.Ttx(0.088), ET.TRx(90 * deg), ET.Ttz(0.107), ET.TRz(joint=7), ] q_idx = [1, 3, 6, 9, 13, 15, 19] tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4) super(Panda, self).__init__(et_list, q_idx, name='Panda', manufacturer='Franka Emika', tool=tool) self._qz = np.array([0, 0, 0, 0, 0, 0, 0]) self._qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
def test_T_real(self): fl = 1.543 rx = rp.ET.TRx() ry = rp.ET.TRy() rz = rp.ET.TRz() tx = rp.ET.Ttx() ty = rp.ET.Tty() tz = rp.ET.Ttz() rx.j = 86 ry.j = 86 rz.j = 86 tx.j = 86 ty.j = 86 tz.j = 86 nt.assert_array_almost_equal(rx.T(fl).A, sm.trotx(fl)) nt.assert_array_almost_equal(ry.T(fl).A, sm.troty(fl)) nt.assert_array_almost_equal(rz.T(fl).A, sm.trotz(fl)) nt.assert_array_almost_equal(tx.T(fl).A, sm.transl(fl, 0, 0)) nt.assert_array_almost_equal(ty.T(fl).A, sm.transl(0, fl, 0)) nt.assert_array_almost_equal(tz.T(fl).A, sm.transl(0, 0, fl))
def __init__(self): # deg = np.pi/180 mm = 1e-3 tool_offset = (103)*mm flange = (107)*mm # d7 = (58.4)*mm # This Panda model is defined using modified # Denavit-Hartenberg parameters L = [ RevoluteMDH( a=0.0, d=0.333, alpha=0.0, qlim=np.array([-2.8973, 2.8973]) ), RevoluteMDH( a=0.0, d=0.0, alpha=-np.pi/2, qlim=np.array([-1.7628, 1.7628]) ), RevoluteMDH( a=0.0, d=0.316, alpha=np.pi/2, qlim=np.array([-2.8973, 2.8973]) ), RevoluteMDH( a=0.0825, d=0.0, alpha=np.pi/2, qlim=np.array([-3.0718, -0.0698]) ), RevoluteMDH( a=-0.0825, d=0.384, alpha=-np.pi/2, qlim=np.array([-2.8973, 2.8973]) ), RevoluteMDH( a=0.0, d=0.0, alpha=np.pi/2, qlim=np.array([-0.0175, 3.7525]) ), RevoluteMDH( a=0.088, d=flange, alpha=np.pi/2, qlim=np.array([-2.8973, 2.8973]) ) ] tool = transl(0, 0, tool_offset) @ trotz(-np.pi/4) super().__init__( L, name='Panda', manufacturer='Franka Emika', meshdir='meshes/FRANKA-EMIKA/Panda', tool=tool) self.addconfiguration("qz", [0, 0, 0, 0, 0, 0, 0]) self.addconfiguration("qr", np.r_[0, -0.3, 0, -2.2, 0, 2.0, np.pi/4])
def test_TRz(self): fl = 1.543 nt.assert_array_almost_equal(rp.ET.TRz(fl).T(), sm.trotz(fl)) nt.assert_array_almost_equal(rp.ET.TRz(-fl).T(), sm.trotz(-fl)) nt.assert_array_almost_equal(rp.ET.TRz(0).T(), sm.trotz(0))