Implement Plücker-Screw-Twist conversions

spatialmath/geom3d.py

@@ -1179,14 +1179,50 @@ def side(self, other):
 
     # Static factory methods for constructors from exotic representations
 
+
 class Plucker(Line3):
 
     def __init__(self, v=None, w=None):
-        import warnings
-
-        warnings.warn('use Line class instead', DeprecationWarning)
+        if np.linalg.norm(w) < 1e-4:  # edge-case -> line at infinity.
+            pass
+        elif abs(np.linalg.norm(w) - 1) > 1e-4:
+            raise ValueError(
+                'Action line vector of Plucker coordinates is not unit!')
+        assert abs(np.dot(
+            v, w)) < _eps, 'vectors are not orthogonal, they do not constitute a Plücker object'
         super().__init__(v, w)
-
+
+
+class Screw(Line3):
+    """
+    Class for Screw coordinates
+    .. note:: This class needs to strictly NOT derive from Plucker, because a Screw coordinate is generally
+    not a valid Plucker coordinate.
+    """
+
+    def __init__(self, v, w, pitch):
+        assert abs(np.linalg.norm(w) - 1) < 1e-4, 'Action line vector of Screw coordinates is not unit!'
+        if pitch == np.inf:
+            s = np.zeros(3)
+            sm = w
+        else:
+            s = w
+            sm = v + pitch * w
+        super().__init__(sm, s)
+
+    @property
+    def pitch(self):
+        return np.dot(self.w, self.v) / np.dot(self.w, self.w)
+
+    @classmethod
+    def FromPlucker(cls, plucker, pitch):
+        return cls(plucker.v, plucker.w, pitch)
+    """
+    Retrieves the Plucker line of action from Screw coordinates
+    """
+    def ToPlucker(self):
+        return Plucker(self.v - self.pitch*self.w, self.w)
+
 if __name__ == '__main__':   # pragma: no cover
 
     import pathlib

spatialmath/twist.py

@@ -6,7 +6,7 @@
 
 from spatialmath.pose3d import SO3, SE3
 from spatialmath.pose2d import SE2
-from spatialmath.geom3d import Line3
+from spatialmath.geom3d import Line3, Plucker, Screw
 import spatialmath.base as base
 from spatialmath.baseposelist import BasePoseList
 
@@ -735,6 +735,30 @@ def _twist(x, y, z, r):
 
         return cls([_twist(x, y, z, r) for (x, y, z, r) in zip(X, Y, Z, R)], check=False)
 
+    @classmethod
+    def FromScrew(cls, screw: Screw, theta=1.0):
+        """
+            Create a new 3D twist from a unit Screw coordinate and magnitude of that screw.
+        """
+        s_norm = np.linalg.norm(screw.w)
+        if s_norm > 1e-4:
+            w = theta * screw.w / s_norm
+            v = theta * screw.v / s_norm
+        return cls(v, w)
+
+    @classmethod
+    def FromPlucker(cls, plucker: Plucker, d=1.0, theta=1.0):
+        """
+            Create a new 3D twist from:
+                - Plucker coordinates of a line,
+                - the distance desired along that line,
+                - the rotation desired about that line.
+        """
+        if abs(theta) > 1e-4:
+            pitch = d / theta
+        else:
+            pitch = np.inf
+        return Twist3.FromScrew(Screw.FromPlucker(plucker, pitch), theta)
 
     # -------------------------  methods -------------------------------#
 
@@ -861,13 +885,13 @@ def pitch(self):
 
         ``X.pitch()`` is the pitch of the twist as a scalar in units of distance
         per radian. 
-        
+
         If we consider the twist as a screw, this is the distance of
-        translation along the screw axis for a one radian rotation about the
+        translation along the screw axis for ``X.theta()`` radian rotation about the
         screw axis.
 
         Example:
-        
+
         .. runblock:: pycon
 
             >>> from spatialmath import SE3, Twist3
@@ -876,7 +900,7 @@ def pitch(self):
             >>> S.pitch
 
         """
-        return np.dot(self.w, self.v)
+        return np.dot(self.w, self.v) / pow(self.theta,2)
 
     def line(self):
         """
@@ -1005,7 +1029,16 @@ def exp(self, theta=1, unit='rad'):
         else:
             raise ValueError('length mismatch')
 
+    def ToPlucker(self):
+        if abs(self.theta) > 1e-4:
+            l = self.w / self.theta
+            return Plucker((self.v / self.theta) - self.pitch * l, l)
+        else:
+            return Plucker(self.v, np.zeros(3))
 
+    def ToScrew(self):
+        plucker = self.ToPlucker()
+        return Screw.FromPlucker(plucker, self.pitch)
 
     # ------------------------- arithmetic -------------------------------#
 

tests/test_geom3d.py

@@ -313,6 +313,62 @@ def test_methods(self):
     #     px.intersect_plane(plane)
     #     py.intersect_plane(plane)
 
+class PluckerTest(unittest.TestCase):
+    def test_validity(self):
+        import pytest
+        # Action line vector (w) is not unit.
+        with pytest.raises(Exception):
+            v = np.array([2, 2, 3])
+            w = np.array([-3, 1.5, 1])
+            Plucker(v,w)
+        # Direction and moment vectors are not orthogonal.
+        with pytest.raises(Exception):
+            v = np.array([2, 2, 3])
+            w = np.array([-3, 2, 1])
+            uw = w / np.linalg.norm(w)
+            Plucker(v, uw)
+        # Everything is valid, object should be constructed.
+        try:
+            v = np.array([2, 2, 3])
+            w = np.array([-3, 1.5, 1])
+            uw = w / np.linalg.norm(w)
+            Plucker(v, uw)
+        except:
+            pytest.fail('Inputs should have resulted in a valid Plucker coordinate')
+
+class ScrewTest(unittest.TestCase):
+    def test_validity(self):
+        import pytest
+        v = np.array([2, 2, 3])
+        w = np.array([-3, 1.5, 1])
+        pitch = 0.5
+        with pytest.raises(Exception):
+            screw = Screw(v, w, pitch)
+        uw = w / np.linalg.norm(w)
+        try:
+            screw = Screw(v, uw, pitch)
+        except:
+            pytest.fail('Inputs should have resulted in a valid Screw coordinate')
+
+    def test_conversion_Plucker(self):
+        v = np.array([2, 2, 3])
+        w = np.array([-3, 1.5, 1])
+        uw = w / np.linalg.norm(w)
+        pitch = 0.5
+        plucker = Plucker(v, uw)
+        screw = Screw.FromPlucker(plucker, pitch)
+        self.assertEqual(plucker, screw.ToPlucker())
+
+    def test_pitch_recovery(self):
+        v = np.array([2, 2, 3])
+        w = np.array([-3, 1.5, 1])
+        uw = w / np.linalg.norm(w)
+        pitch = 0.5
+        plucker = Plucker(v, uw)
+        screw = Screw.FromPlucker(plucker, pitch)
+        self.assertAlmostEqual(screw.pitch, pitch)
+
+
 if __name__ == "__main__":
 
     unittest.main()

tests/test_twist.py

@@ -78,8 +78,29 @@ def test_conversion_se3(self):
                     [ 0.,  0.,  0.,  0.]]))
 
     def test_conversion_Plucker(self):
-        pass
-
+        v = np.array([2, 2, 3])
+        w = np.array([-3, 1.5, 1])
+        uw = w / np.linalg.norm(w)
+        plucker = Plucker(v, uw)
+        pitch = 0.5
+        theta = 1.5
+        d = pitch * theta
+        twist =  Twist3.FromPlucker(plucker, d, theta)
+        self.assertEqual(plucker, twist.ToPlucker())
+
+    def test_conversion_Screw(self):
+        v = np.array([2, 2, 3])
+        w = np.array([-3, 1.5, 1])
+        uw = w / np.linalg.norm(w)
+        pitch = 0.5
+        screw = Screw(v, uw, pitch)
+        theta = 0.75
+        twist = Twist3.FromScrew(screw, theta)
+        self.assertEqual(screw, twist.ToScrew())
+        self.assertAlmostEqual(twist.theta, theta)
+        self.assertAlmostEqual(twist.pitch, pitch)
+        self.assertAlmostEqual(screw.pitch, pitch)
+
     def test_list_constuctor(self):
         x = Twist3([1, 0, 0, 0, 0, 0])