diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index b8d8d5de..03641b92 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -769,7 +769,24 @@ def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self: v = smb.cross(v1, v2) s = smb.norm(v) if abs(s) < tol * np.finfo(float).eps: - return cls(np.eye(3), check=False) + c = np.dot(v1, v2) + if c > 0: + # v1 and v2 already (anti)parallel in the same direction + return cls(np.eye(3), check=False) + # v1 and v2 point in opposite directions -- the formula below + # is singular here too (it divides by s**2), but unlike the + # c > 0 case the answer isn't identity: any 180 degree + # rotation about an axis perpendicular to v1 takes v1 to v2. + # Closed form for a 180 degree rotation about unit axis u: + # R = 2*u*u^T - I (Rodrigues at theta=pi, sin=0, cos=-1). + # Pick u by crossing v1 with whichever world axis it's least + # aligned with, so the cross product is never itself + # degenerate. + axis = np.zeros(3) + axis[np.argmin(np.abs(v1))] = 1.0 + u = smb.unitvec(smb.cross(v1, axis)) + R = 2 * np.outer(u, u) - np.eye(3) + return cls(R, check=False) else: c = np.dot(v1, v2) V = smb.skew(v) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index cdb80fbd..7b0d2414 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -726,6 +726,19 @@ def test_rotatedvector(self): Re = SO3.RotatedVector(v1, v1) np.testing.assert_almost_equal(Re, np.eye(3)) + # Antipodal case: v1 and v2 point in exactly opposite directions. + # The cross product used to find the rotation axis is zero here + # too, same as the parallel case above, but the correct answer is + # a 180 degree flip, not identity -- regression test for a bug + # where this silently returned identity instead. + for v1 in ([1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 2, 3], [-3, 1, 2]): + v1 = unitvec(v1) + v2 = [-x for x in v1] + Re = SO3.RotatedVector(v1, v2) + np.testing.assert_almost_equal(np.asarray(Re * v1).flatten(), v2) + # must actually be a 180 degree rotation, not identity + assert not np.allclose(Re.A, np.eye(3)) + R = SO3() # identity matrix case # Check log and exponential map