diff --git a/src/roboticstoolbox/ets/ET.py b/src/roboticstoolbox/ets/ET.py index 0b00225a2..43824e96f 100644 --- a/src/roboticstoolbox/ets/ET.py +++ b/src/roboticstoolbox/ets/ET.py @@ -182,7 +182,7 @@ def Rx( """ Pure rotation about the x-axis - :param param: rotation about the x-axis + :param param: rotation about the x-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param unit: angular unit, "rad" [default] or "deg" :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction @@ -217,7 +217,7 @@ def Ry( """ Pure rotation about the y-axis - :param param: rotation about the y-axis + :param param: rotation about the y-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param unit: angular unit, "rad" [default] or "deg" :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction @@ -252,7 +252,7 @@ def Rz( """ Pure rotation about the z-axis - :param param: rotation about the z-axis + :param param: rotation about the z-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param unit: angular unit, "rad" [default] or "deg" :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction @@ -286,7 +286,7 @@ def tx( """ Pure translation along the x-axis - :param param: translation distance along the x-axis + :param param: translation distance along the x-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction :returns: An elementary transform @@ -331,7 +331,7 @@ def ty( """ Pure translation along the y-axis - :param param: translation distance along the y-axis + :param param: translation distance along the y-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction :returns: An elementary transform diff --git a/src/roboticstoolbox/ets/ET2.py b/src/roboticstoolbox/ets/ET2.py index 983426310..4910d85e9 100644 --- a/src/roboticstoolbox/ets/ET2.py +++ b/src/roboticstoolbox/ets/ET2.py @@ -56,7 +56,7 @@ def R( """ Pure rotation - :param param: rotation angle + :param param: rotation angle, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param unit: angular unit, "rad" [default] or "deg" :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction @@ -95,7 +95,7 @@ def tx( """ Pure translation along the x-axis - :param param: translation distance along the x-axis + :param param: translation distance along the x-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction :returns: An elementary transform @@ -128,7 +128,7 @@ def ty( """ Pure translation along the y-axis - :param param: translation distance along the y-axis + :param param: translation distance along the y-axis, or a joint name such as ``"q2"``, see :class:`~roboticstoolbox.ets._ET.BaseET` :param j: Explicit joint number within the robot :param flip: Joint moves in opposite direction :returns: An elementary transform diff --git a/src/roboticstoolbox/ets/_ET.py b/src/roboticstoolbox/ets/_ET.py index 420adf48e..8d0c55df2 100644 --- a/src/roboticstoolbox/ets/_ET.py +++ b/src/roboticstoolbox/ets/_ET.py @@ -52,28 +52,91 @@ def _resolve_param( return param +#: characters that negate the joint variable when they start a joint descriptor +_MINUS_SIGNS = ("-", "\u2212") # hyphen-minus and the Unicode minus sign + + def _parse_joint_descriptor(s: str) -> tuple[int | None, bool]: """ Parse a joint descriptor string into (jindex, flip). - A leading '-' sets flip (and is stripped); a leading '+' is stripped and - ignored. The first run of digits found anywhere in what remains becomes - the joint index - this treats 'theta2', 'q2', 'q(3)' and 'θ_3' the same - regardless of how the index is set off from the rest of the name. If no - digit is found, jindex is None, so the joint falls through to ETS's - existing auto-numbering for unassigned joints. + A leading '-' (or the Unicode minus sign) sets flip, and a leading '+' is + ignored; the sign is then stripped. The first run of digits found anywhere + in what remains becomes the joint index - this treats 'theta2', 'q2', 'q(3)' + and 'θ_3' the same regardless of how the index is set off from the rest of + the name. If no digit is found, jindex is None, so the joint falls through + to ETS's existing auto-numbering for unassigned joints. + + Joint indices count from zero. A name that ends with an underscore, such as + 'A1_' or 'θ2_', counts from one: the index is one less than the digits. + + :raises ValueError: the name counts from one and the number is zero """ - flip = s.startswith("-") - if s[:1] in "+-": + flip = s[:1] in _MINUS_SIGNS + if s[:1] in _MINUS_SIGNS or s[:1] == "+": s = s[1:] match = re.search(r"\d+", s) jindex = int(match.group()) if match else None + if jindex is not None and s.endswith("_"): + if jindex == 0: + raise ValueError( + f"joint name {s!r} ends with an underscore, so the number must " + "count from one, but it is zero" + ) + jindex -= 1 + return jindex, flip +def _joint_display_name(s: str) -> str: + """ + The name of a joint, as displayed, from its descriptor + + The underscore that says the number counts from one is an instruction and is + not part of the name, so 'A1_' is displayed as 'A1'. A name without a number + is shown as given. + """ + body = s[1:] if s[:1] in _MINUS_SIGNS or s[:1] == "+" else s + if s.endswith("_") and re.search(r"\d", body): + return s[:-1] + return s + + class BaseET: + r""" + Elementary transform, the base class of :class:`ET` and :class:`ET2` + + **Joint descriptors** + + The parameter ``param`` of a transform may be a string naming a variable + joint, such as ``"q2"`` or ``"theta2"``, instead of a number. The rules are: + + - the first number in the string is the joint index, so ``"q0"`` is joint 0 + and ``"theta2"`` is joint 2; ``"q(3)"`` and ``"θ_3"`` are joint 3 + - a leading ``-`` (or the Unicode minus sign ``−``) negates the joint + variable, ``flip``, so ``"-q1"`` is joint 1, flipped. A leading ``+`` is + ignored + - joint indices count from zero. If the names count from one, as ``"A1"`` to + ``"A6"`` of a Kuka arm do, end the name with an underscore: ``"A1_"`` is + joint 0, and ``"-A1_"`` is joint 0, flipped. The underscore is not part of + the name that is displayed + - a string with no number, such as ``"joint"``, is a variable joint that is + numbered automatically + - a string that is a number, such as ``"1.5"``, is a constant + - ``jindex`` and ``flip`` must not also be given + + The name is shown when the transform is displayed, ``Rz(A1)``. + + .. note:: A robot built from names ``"A1"`` to ``"A6"``, without the + underscores, has joint indices 1 to 6, and is rejected as the index 6 is + out of range for 6 joints. + + .. versionchanged:: 1.5.0 + A trailing underscore, and the Unicode minus sign, are accepted. + """ + def __init__( self, axis: str, @@ -113,7 +176,7 @@ def __init__( "joint descriptor for `param`" ) jindex, flip = _parse_joint_descriptor(descriptor) - self._joint_name = descriptor + self._joint_name = _joint_display_name(descriptor) param = None flip = bool(flip) # None (not given, and not set above) -> False diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 27c0aee67..cf4f8cddb 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -385,10 +385,20 @@ def visit_link(link: Any, jindex: Any) -> None: # Jindex set on all, check they are unique and contiguous if check_jindex: jset = set(range(n)) + jindices = {link.jindex for link in links if link.isjoint} for link in links: if link.isjoint and link.jindex not in jset: + hint = "" + if jindices == set(range(1, n + 1)): + hint = ( + f"; joint indices count from zero, but these are" + f" 1 to {n}. Joint names that count from one," + " such as 'A1', need a trailing underscore," + " 'A1_', to be numbered from zero" + ) raise ValueError( - f"joint index {link.jindex} was repeated or out of range" + f"joint index {link.jindex} was repeated or out of" + f" range{hint}" ) jset -= set([link.jindex]) if len(jset) > 0: # pragma nocover # is impossible diff --git a/tests/test_et_joint_descriptor.py b/tests/test_et_joint_descriptor.py new file mode 100644 index 000000000..e5eef47ee --- /dev/null +++ b/tests/test_et_joint_descriptor.py @@ -0,0 +1,142 @@ +"""String joint descriptors for ET and ET2, such as "q2", "-theta3" and "A1_" + +A string for ``param`` that is not a number names a variable joint: + +- a leading ``-`` (or the Unicode minus) sets ``flip``, a leading ``+`` is ignored +- the first run of digits is the joint index, counted from zero +- a trailing underscore says the name counts from one, so the index is one less +- the name is shown when the ET is displayed, without that underscore +""" + +import numpy as np +import numpy.testing as nt +import pytest + +from roboticstoolbox import ET, ET2, ERobot, ETS + +MINUS = "−" # Unicode minus sign + +# descriptor, jindex, flip, str(ET.Rz(descriptor)) +CASES = [ + ("q0", 0, False, "Rz(q0)"), + ("q12", 12, False, "Rz(q12)"), + ("-q1", 1, True, "Rz(-q1)"), + ("+q1", 1, False, "Rz(+q1)"), + ("theta2", 2, False, "Rz(theta2)"), + ("θ_3", 3, False, "Rz(θ_3)"), + ("q(3)", 3, False, "Rz(q(3))"), + ("-q(3)", 3, True, "Rz(-q(3))"), + (f"{MINUS}q1", 1, True, f"Rz({MINUS}q1)"), + # one-based names, trailing underscore + ("A1_", 0, False, "Rz(A1)"), + ("A6_", 5, False, "Rz(A6)"), + ("A10_", 9, False, "Rz(A10)"), + ("A01_", 0, False, "Rz(A01)"), + ("-A1_", 0, True, "Rz(-A1)"), + (f"{MINUS}A2_", 1, True, f"Rz({MINUS}A2)"), + ("+A2_", 1, False, "Rz(+A2)"), + ("θ2_", 1, False, "Rz(θ2)"), + ("θ_3_", 2, False, "Rz(θ_3)"), + ("q(3)_", 2, False, "Rz(q(3))"), + # without the underscore the digits are the index, as written + ("A1", 1, False, "Rz(A1)"), + ("A6", 6, False, "Rz(A6)"), + # no digits: auto-numbered, and the underscore means nothing + ("joint", None, False, "Rz(joint)"), + ("joint_", None, False, "Rz(joint_)"), + ("-joint", None, True, "Rz(-joint)"), +] + + +@pytest.mark.parametrize("descriptor, jindex, flip, text", CASES) +def test_descriptor_sets_index_flip_and_name(descriptor, jindex, flip, text) -> None: + et = ET.Rz(descriptor) + assert et.isjoint + assert et.jindex == jindex + assert et.isflip is flip + assert str(et) == text + + +@pytest.mark.parametrize("make", [ET.Rx, ET.Ry, ET.Rz, ET.tx, ET.ty, ET.tz]) +def test_every_elementary_transform_takes_a_descriptor(make) -> None: + et = make("-A3_") + assert et.isjoint and et.jindex == 2 and et.isflip + + +def test_planar_et2_takes_a_descriptor() -> None: + et = ET2.R("-A2_") + assert et.isjoint and et.jindex == 1 and et.isflip + + +@pytest.mark.parametrize("descriptor", ["A0_", "-A0_", "q0_", "θ0_"]) +def test_one_based_name_cannot_be_zero(descriptor: str) -> None: + with pytest.raises(ValueError, match="count from one"): + ET.Rz(descriptor) + + +@pytest.mark.parametrize("keyword", [{"jindex": 0}, {"flip": True}]) +def test_descriptor_and_keyword_are_exclusive(keyword) -> None: + with pytest.raises(ValueError, match="cannot specify"): + ET.Rz("A1_", **keyword) + + +@pytest.mark.parametrize("value, expected", [("1.5", 1.5), ("-2", -2.0), ("1_0", 10.0)]) +def test_a_numeric_string_is_a_constant(value: str, expected: float) -> None: + et = ET.tz(value) + assert not et.isjoint + nt.assert_allclose(et.A()[2, 3], expected) + + +def test_flipped_descriptor_negates_the_joint_variable() -> None: + q = 0.4 + nt.assert_allclose(ET.Rz("-A1_").A(q), ET.Rz("A1_").A(-q), atol=1e-12) + nt.assert_allclose(ET.Rz("A1_").A(q), ET.Rz(jindex=0).A(q), atol=1e-12) + + +def _kuka(names: list[str]) -> ETS: + # six revolute joints, alternating z and x axes, with link offsets + ets = ETS() + for k, name in enumerate(names): + axis = ET.Rz if k % 2 == 0 else ET.Rx + ets = ets * axis(name) * ET.tz(0.2 + 0.05 * k) * ET.tx(0.1) + return ets + + +def test_one_based_names_build_the_same_robot_as_zero_based() -> None: + one = ETS(_kuka([f"A{i}_" for i in range(1, 7)])) + zero = ETS(_kuka([f"q{i}" for i in range(6)])) + + assert [j.jindex for j in one.joints()] == [0, 1, 2, 3, 4, 5] + assert [str(j) for j in one.joints()][:2] == ["Rz(A1)", "Rx(A2)"] + assert "A1" in str(one) and "A6" in str(one) and "_" not in str(one) + + r1, r0 = ERobot(one), ERobot(zero) + assert r1.n == r0.n == 6 + q = np.array([0.1, -0.4, 0.7, 0.2, -0.9, 0.5]) + nt.assert_allclose(r1.fkine(q).A, r0.fkine(q).A, atol=1e-12) + nt.assert_allclose(r1.jacob0(q), r0.jacob0(q), atol=1e-12) + + +def test_flipped_one_based_joint_in_a_robot() -> None: + names = [f"A{i}_" for i in range(1, 7)] + plain = ERobot(ETS(_kuka(names))) + flipped = ERobot(ETS(_kuka(["-A1_"] + names[1:]))) + q = np.array([0.3, 0.1, -0.2, 0.4, 0.5, -0.6]) + q_flipped = q.copy() + q_flipped[0] = -q[0] + nt.assert_allclose(flipped.fkine(q).A, plain.fkine(q_flipped).A, atol=1e-12) + + +def test_one_based_names_without_underscore_are_rejected_with_a_hint() -> None: + ets = ETS(_kuka([f"A{i}" for i in range(1, 7)])) + assert [j.jindex for j in ets.joints()] == [1, 2, 3, 4, 5, 6] + with pytest.raises(ValueError, match="trailing underscore") as info: + ERobot(ets) + assert "out of range" in str(info.value) + + +def test_other_out_of_range_indices_get_no_hint() -> None: + ets = ETS(_kuka(["q0", "q1", "q2", "q3", "q4", "q9"])) + with pytest.raises(ValueError, match="out of range") as info: + ERobot(ets) + assert "underscore" not in str(info.value)