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): """ 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 *