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))
