From beef079f85c3698f5ff77e1f3d0e3a83b5b25c81 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 27 Jul 2026 13:13:26 +1000 Subject: [PATCH 1/2] fix(twist): add missing denominator in Twist3.pitch calculation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The pitch calculation was missing the denominator, returning an incorrect value. Pitch should be normalized by |w|²: pitch = (w · v) / (w · w) This represents the translation distance per radian along the screw axis. All 25 twist tests pass. --- spatialmath/twist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index dcefa840..9b9cee92 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -945,7 +945,7 @@ def pitch(self): >>> S.pitch """ - return np.dot(self.w, self.v) + return np.dot(self.w, self.v) / np.dot(self.w, self.w) def line(self): """ From 3b97759099e2795d979aea557dc60a8c699ff8bb Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 27 Jul 2026 13:45:49 +1000 Subject: [PATCH 2/2] test(twist): add regression test for pitch denominator fix Twist3.pitch previously omitted the |w|^2 denominator; this only happened to work when w was a unit vector, which UnitRevolute always produces. Add a case with non-unit w to catch that regression. --- tests/test_twist.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/tests/test_twist.py b/tests/test_twist.py index 70f237a8..dfbd9f24 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -179,6 +179,17 @@ def test_exp(self): tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0]) array_compare(tw.exp(pi / 2), SE3.Rz(pi / 2)) + def test_pitch(self): + # pitch = (w . v) / (w . w), regression test for missing denominator + + # non-unit w: exercises the normalization, would fail without it + tw = Twist3([0, 0, 4], [0, 0, 2]) + self.assertAlmostEqual(tw.pitch, 2.0) + + # unit w: denominator is 1, sanity check against UnitRevolute's pitch arg + tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0], pitch=3) + self.assertAlmostEqual(tw.pitch, 3.0) + def test_arith(self): # check overloaded *