diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..2c717a3a 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -986,7 +986,7 @@ def pole(self): >>> S = Twist3(T) >>> S.pole """ - return np.cross(self.w, self.v) / self.theta + return np.cross(self.w, self.v) / np.dot(self.w, self.w) def SE3(self, theta=1, unit="rad"): """ @@ -1500,7 +1500,7 @@ def pole(self): >>> S.pole() """ - p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta + p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.w**2 return p[:2] # ------------------------- methods -------------------------------# diff --git a/tests/test_twist_pole.py b/tests/test_twist_pole.py new file mode 100644 index 00000000..8acc2e34 --- /dev/null +++ b/tests/test_twist_pole.py @@ -0,0 +1,35 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import Twist2, Twist3 + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +def test_twist2_pole_is_rotation_center(speed): + """A change in angular speed must not move the center of rotation.""" + center = np.array([3.0, -2.0]) + twist = Twist2(speed * np.r_[center[1], -center[0], 1]) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + np.testing.assert_allclose( + transform @ np.r_[twist.pole, 1], np.r_[center, 1], atol=1e-14 + ) + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +@pytest.mark.parametrize("pitch", [0, 0.4]) +def test_twist3_pole_is_on_screw_axis(speed, pitch): + """The closest axis point is fixed except for translation along the axis.""" + axis = np.array([1.0, 2.0, -2.0]) / 3 + center = np.array([3.0, -2.0, 1.0]) + center -= (center @ axis) * axis + angular = speed * axis + linear = -np.cross(angular, center) + pitch * angular + twist = Twist3(linear, angular) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + expected = center + pitch * angular * 0.7 + np.testing.assert_allclose( + (transform @ np.r_[twist.pole, 1])[:3], expected, atol=1e-14 + )