diff --git a/src/roboticstoolbox/robot/DHLink.py b/src/roboticstoolbox/robot/DHLink.py index 4b570901e..fbe2921c6 100644 --- a/src/roboticstoolbox/robot/DHLink.py +++ b/src/roboticstoolbox/robot/DHLink.py @@ -616,7 +616,7 @@ def A(self, q: float) -> SE3: sa = _sin(self.alpha) ca = _cos(self.alpha) - if self.ets[-1].isflip: + if self.isflip: q = -q + self.offset else: q = q + self.offset diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index f76706561..8c9cf42d6 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -1564,6 +1564,17 @@ def test_alpha(self): nt.assert_array_almost_equal(r0.alpha, np.r_[1, 0, -1, 1, -1, 0] * math.pi / 2) + def test_flip_with_nonzero_a(self): + # Issue #563: DHLink.A() honoured flip only when the joint ET happened + # to be the last ET. For a link with a nonzero a/d/alpha the joint ET is + # not last, so flip=True was silently ignored and q was not negated. + q = 0.5 + L = rp.RevoluteDH(a=1.0, flip=True) + # A(q) must agree with the link's own ETS evaluation, which honours flip. + nt.assert_array_almost_equal(L.A(q).A, L.ets.eval([q])) + # The rotation uses the negated angle, so the [1, 0] entry is sin(-q). + nt.assert_almost_equal(L.A(q).A[1, 0], math.sin(-q)) + def test_ets(self): panda = rp.models.DH.Panda() panda.ets()