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.

Author: petercorke

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 *