def Ry(cls, theta, unit='rad', t=None): """ Create an SE(3) pure rotation about the Y-axis :param θ: rotation angle about X-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.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis - ``SE3.Ry(θ, "deg")`` as above but θ is in degrees - ``SE3.Ry(θ, 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.Ry(0.3) >>> SE3.Ry([0.3, 0.4]) :seealso: :func:`~spatialmath.base.transforms3d.troty` :SymPy: supported """ return cls( [base.troty(x, t=t, unit=unit) for x in base.getvector(theta)], check=False)
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 Ry(cls, theta, unit='rad'): """ Create SE(3) pure rotation about the Y-axis :param theta: rotation angle about X-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 4x4 homogeneous transformation matrix :rtype: SE3 instance - ``SE3.Ry(THETA)`` is an SO(3) rotation of THETA radians about the y-axis - ``SE3.Ry(THETA, "deg")`` as above but THETA is in degrees """ return cls([tr.troty(x, unit) for x in argcheck.getvector(theta)])
def Ry(cls, theta, unit='rad'): """ Create SE(3) pure rotation about the Y-axis :param theta: rotation angle about X-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 4x4 homogeneous transformation matrix :rtype: SE3 instance - ``SE3.Ry(THETA)`` is an SO(3) rotation of THETA radians about the y-axis - ``SE3.Ry(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.troty(x, unit) for x in argcheck.getvector(theta)], check=False)
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 test_TRy(self): fl = 1.543 nt.assert_array_almost_equal(rp.ET.TRy(fl).T(), sm.troty(fl)) nt.assert_array_almost_equal(rp.ET.TRy(-fl).T(), sm.troty(-fl)) nt.assert_array_almost_equal(rp.ET.TRy(0).T(), sm.troty(0))