Finish p_servo and tests

Author: jhavl

roboticstoolbox/tools/__init__.py

@@ -1,5 +1,5 @@
 from roboticstoolbox.tools.null import null
-from roboticstoolbox.tools.p_servo import p_servo, p_servo_old
+from roboticstoolbox.tools.p_servo import p_servo
 from roboticstoolbox.tools.Ticker import Ticker
 from roboticstoolbox.tools.urdf import *  # noqa
 from roboticstoolbox.tools.trajectory import (
@@ -21,7 +21,6 @@
 __all__ = [
     "null",
     "p_servo",
-    "p_servo_old",
     "Ticker",
     "tpoly",
     "tpoly_func",

roboticstoolbox/tools/p_servo.py

@@ -5,22 +5,50 @@
 import math
 
 
-def p_servo_old(wTe, wTep, gain=2, threshold=0.1):
+def _angle_axis(T, Td):
+    e = np.empty(6)
+    e[:3] = Td[:3, -1] - T[:3, -1]
+    R = Td[:3, :3] @ T[:3, :3].T
+    li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
+
+    if base.iszerovec(li):
+        # diagonal matrix case
+        if np.trace(R) > 0:
+            # (1,1,1) case
+            a = np.zeros((3,))
+        else:
+            a = np.pi / 2 * (np.diag(R) + 1)
+    else:
+        # non-diagonal matrix case
+        ln = base.norm(li)
+        a = math.atan2(ln, np.trace(R) - 1) * li / ln
+
+    e[3:] = a
+
+    return e
+
+
+def p_servo(wTe, wTep, gain=1.0, threshold=0.1, method="rpy"):
     """
     Position-based servoing.
 
     Returns the end-effector velocity which will cause the robot to approach
     the desired pose.
 
     :param wTe: The current pose of the end-effecor in the base frame.
-    :type wTe: SE3
+    :type wTe: SE3 or ndarray
     :param wTep: The desired pose of the end-effecor in the base frame.
-    :type wTep: SE3
-    :param gain: The gain for the controller
-    :type gain: float
+    :type wTep: SE3 or ndarray
+    :param gain: The gain for the controller. Can be vector corresponding to each
+        axis, or scalar corresponding to all axes.
+    :type gain: float, or array-like
     :param threshold: The threshold or tolerance of the final error between
         the robot's pose and desired pose
     :type threshold: float
+    :param method: The method used to calculate the error. Default is 'rpy' -
+        error in the end-effector frame. 'angle-axis' - error in the base frame
+        using angle-axis method.
+    :type method: string: 'rpy' or 'angle-axis'
 
     :returns v: The velocity of the end-effecotr which will casue the robot
         to approach wTep
@@ -31,71 +59,31 @@ def p_servo_old(wTe, wTep, gain=2, threshold=0.1):
 
     """
 
-    if not isinstance(wTe, SE3):
-        wTe = SE3(wTe)
-
-    if not isinstance(wTep, SE3):
-        wTep = SE3(wTep)
-
-    # Pose difference
-    eTep = wTe.inv() * wTep
+    if isinstance(wTe, SE3):
+        wTe = wTe.A
 
-    # Translational error
-    ev = eTep.t
+    if isinstance(wTep, SE3):
+        wTep = wTep.A
 
-    # Angular error
-    ew = eTep.rpy("rad")
+    if method == "rpy":
+        # Pose difference
+        eTep = np.linalg.inv(wTe) @ wTep
+        e = np.empty(6)
 
-    # Form error vector
-    e = np.r_[ev, ew]
+        # Translational error
+        e[:3] = eTep[:3, -1]
 
-    # Desired end-effector velocity
-    v = gain * e
-
-    if np.sum(np.abs(e)) < threshold:
-        arrived = True
+        # Angular error
+        e[3:] = base.tr2rpy(eTep, unit="rad", order="zyx", check=False)
     else:
-        arrived = False
-
-    return v, arrived
+        e = _angle_axis(wTe, wTep)
 
-
-def _angle_axis(T, Td):
-    e = np.zeros(6)
-    e[:3] = Td.t - T.t
-    R = Td.R @ T.R.T
-    li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
-
-    if base.iszerovec(li):
-        # diagonal matrix case
-        if np.trace(R) > 0:
-            # (1,1,1) case
-            a = np.zeros((3,))
-        else:
-            a = np.pi / 2 * (np.diag(R) + 1)
+    if base.isscalar(gain):
+        k = gain * np.eye(6)
     else:
-        # non-diagonal matrix case
-        ln = base.norm(li)
-        a = math.atan2(ln, np.trace(R) - 1) * li / ln
-
-    e[3:] = a
-
-    return e
-
-
-def p_servo(wTe, wTep, gain=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0], threshold=0.1):
-    if not isinstance(wTe, SE3):
-        wTe = SE3(wTe)
-
-    if not isinstance(wTep, SE3):
-        wTep = SE3(wTep)
-
-    k = np.diag(gain)
-
-    e = _angle_axis(wTe, wTep)
+        k = np.diag(gain)
 
     v = k @ e
-
     arrived = True if np.sum(np.abs(e)) < threshold else False
 
     return v, arrived

tests/test_tools.py

@@ -11,39 +11,19 @@
 
 
 class Testtools(unittest.TestCase):
-
     def test_null(self):
 
         a0 = np.array([1, 2, 3])
 
-        a1 = np.array([
-            [1, 2, 3],
-            [4, 5, 6]
-        ])
-
-        a2 = np.array([
-            [1, 2, 3],
-            [4, 5, 6],
-            [7, 8, 9]
-        ])
-
-        ans0 = np.array([
-            [-0.5345, -0.8018],
-            [0.7745, -0.3382],
-            [-0.3382,  0.4927]
-        ])
-
-        ans1 = np.array([
-            [0.4082],
-            [-0.8165],
-            [0.4082]
-        ])
-
-        ans2 = np.array([
-            [-0.4082],
-            [0.8165],
-            [-0.4082]
-        ])
+        a1 = np.array([[1, 2, 3], [4, 5, 6]])
+
+        a2 = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
+
+        ans0 = np.array([[-0.5345, -0.8018], [0.7745, -0.3382], [-0.3382, 0.4927]])
+
+        ans1 = np.array([[0.4082], [-0.8165], [0.4082]])
+
+        ans2 = np.array([[-0.4082], [0.8165], [-0.4082]])
 
         r0 = rp.null(a0)
         r1 = rp.null(a1)
@@ -53,16 +33,33 @@ def test_null(self):
         nt.assert_array_almost_equal(np.abs(r1), np.abs(ans1), decimal=4)
         nt.assert_array_almost_equal(np.abs(r2), np.abs(ans2), decimal=4)
 
-    def test_p_servo(self):
+    def test_p_servo_rpy(self):
+        a = sm.SE3()
+        b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1)
+        c = sm.SE3.Tz(0.59)
+
+        v0, arrived0 = rp.p_servo(a, b, method="rpy")
+        v1, _ = rp.p_servo(a.A, b.A, method="rpy")
+        _, arrived1 = rp.p_servo(a, c, threshold=0.6, method="rpy")
+
+        ans = np.array([1, 0, 0, 0.7, -0, 0])
+
+        nt.assert_array_almost_equal(v0, ans, decimal=4)
+        nt.assert_array_almost_equal(v1, ans, decimal=4)
+
+        self.assertFalse(arrived0)
+        self.assertTrue(arrived1)
+
+    def test_p_servo_angle_axis(self):
         a = sm.SE3()
         b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1)
         c = sm.SE3.Tz(0.59)
 
-        v0, arrived0 = rp.p_servo(a, b)
+        v0, arrived0 = rp.p_servo(a, b, method="angle-axis")
         v1, _ = rp.p_servo(a.A, b.A)
         _, arrived1 = rp.p_servo(a, c, threshold=0.6)
 
-        ans = np.array([2, 0, 0, 1.4, -0,  0])
+        ans = np.array([1, 0, 0, 0.7, -0, 0])
 
         nt.assert_array_almost_equal(v0, ans, decimal=4)
         nt.assert_array_almost_equal(v1, ans, decimal=4)
@@ -76,6 +73,7 @@ def test_jsingu(self):
 
         rp.jsingu(J)
 
-if __name__ == '__main__':  # pragma nocover
+
+if __name__ == "__main__":  # pragma nocover
     unittest.main()
-    # pytest.main(['tests/test_SerialLink.py'])
+    # pytest.main(['tests/test_SerialLink.py'])