Added argument checking and testing

jhavl · jhavl · commit d8e8cc650b70 · 2020-05-03T11:30:22.000+10:00
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -7,6 +7,7 @@
 
 import numpy as np
 import spatialmath.base as sp
+from spatialmath.base.argcheck import getvector, ismatrix
 from roboticstoolbox.robot.ET import ET
 
 
@@ -175,10 +176,7 @@ def fkine(self, q):
             Sequence, J. Haviland and P. Corke
         '''
 
-        # if not isinstance(q, np.ndarray):
-        #     raise TypeError('q array must be a numpy ndarray.')
-        # if q.shape != (self._n,):
-        #     raise ValueError('q must be a 1 dim (n,) array')
+        q = getvector(q, self.n)
 
         j = 0
         trans = np.eye(4)
@@ -209,6 +207,9 @@ def jacob0(self, q):
         References: Kinematic Derivatives using the Elementary Transform
             Sequence, J. Haviland and P. Corke
         """
+
+        q = getvector(q, self.n)
+
         T = self.fkine(q)
         U = np.eye(4)
         j = 0
@@ -236,47 +237,59 @@ def jacob0(self, q):
 
         return J
 
-    def hessian0(self, q, J=None):
+    def hessian0(self, q=None, J0=None):
         """
         The manipulator Hessian tensor maps joint acceleration to end-effector
         spatial acceleration, expressed in the world-coordinate frame. This
-        function calulcates this based on the ETS of the robot.
+        function calulcates this based on the ETS of the robot. One of J0 or q
+        is required. Supply J0 if already calculated to save computation time
 
         :param q: The joint coordinates of the robot
         :type q: float np.ndarray(n,)
+        :param J0: The manipulator Jacobian in the 0 frame
+        :type J0: float np.ndarray(6,n)
         :return: The manipulator Hessian in 0 frame
         :rtype: float np.ndarray(6,n,n)
 
         References: Kinematic Derivatives using the Elementary Transform
             Sequence, J. Haviland and P. Corke
         """
-        if J is None:
-            J = self.jacob0(q)
+
+        if J0 is None:
+            if q is not None:
+                q = getvector(q, self.n)
+                J0 = self.jacob0(q)
+            else:
+                raise ValueError('One of q or J0 must be supplied')
+        # TODO: Check J
 
         H = np.zeros((6, self.n, self.n))
 
         for j in range(self.n):
             for i in range(j, self.n):
 
-                H[:3, i, j] = np.cross(J[3:, j], J[:3, i])
-                H[3:, i, j] = np.cross(J[3:, j], J[3:, i])
+                H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
+                H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
 
                 if i != j:
                     H[:3, j, i] = H[:3, i, j]
 
         return H
 
-    def manipulability(self, q, J=None):
+    def manipulability(self, q=None, J=None):
         """
         Calculates the manipulability index (scalar) robot at the joint
         configuration q. It indicates dexterity, that is, how isotropic the
         robot's motion is with respect to the 6 degrees of Cartesian motion.
         The measure is high when the manipulator is capable of equal motion
         in all directions and low when the manipulator is close to a
-        singularity.
+        singularity. One of J or q is required. Supply J if already
+        calculated to save computation time
 
         :param q: The joint coordinates of the robot
         :type q: float np.ndarray(n,)
+        :param J: The manipulator Jacobian in any frame
+        :type J: float np.ndarray(6,n)
         :return: The manipulability index
         :rtype: float
 
@@ -287,33 +300,48 @@ def manipulability(self, q, J=None):
         """
 
         if J is None:
-            J = self.jacob0(q)
+            if q is not None:
+                q = getvector(q, self.n)
+                J = self.jacob0(q)
+            else:
+                raise ValueError('One of q or J must be supplied')
+        # TODO: Check J
 
         return np.sqrt(np.linalg.det(J @ np.transpose(J)))
 
-    def jacobm(self, q, J=None, H=None, manipulability=None):
+    def jacobm(self, q=None, J=None, H=None):
         """
         Calculates the manipulability Jacobian. This measure relates the rate
         of change of the manipulability to the joint velocities of the robot.
+        One of J or q is required. Supply J and H if already calculated to
+        save computation time
 
         :param q: The joint coordinates of the robot
         :type q: float np.ndarray(n,)
+        :param J: The manipulator Jacobian in any frame
+        :type J: float np.ndarray(6,n)
+        :param H: The manipulator Hessian in any frame
+        :type H: float np.ndarray(6,n,n)
         :return: The manipulability Jacobian
         :rtype: float np.ndarray(n,1)
 
         References: Maximising Manipulability in Resolved-Rate Motion Control,
             J. Haviland and P. Corke
         """
 
-        if manipulability is None:
-            manipulability = self.manipulability(q)
-
         if J is None:
-            J = self.jacob0(q)
+            if q is not None:
+                q = getvector(q, self.n)
+                J = self.jacob0(q)
+            else:
+                raise ValueError('One of q or J must be supplied')
+        # TODO: Check J
 
         if H is None:
-            H = self.hessian0(q)
+            H = self.hessian0(J0=J)
+        # TODO: Check H
 
+        manipulability = self.manipulability(J=J)
         b = np.linalg.inv(J @ np.transpose(J))
         Jm = np.zeros((self.n, 1))
 
diff --git a/test/test_ets.py b/test/test_ets.py
@@ -38,7 +38,10 @@ def test_str_ets(self):
 
     def test_fkine(self):
         panda = rp.Panda()
-        q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
+        q3 = np.expand_dims(q1, 0)
+        q4 = np.expand_dims(q1, 1)
 
         ans = np.array([
             [-0.50827907, -0.57904589,  0.63746234,  0.44682295],
@@ -47,11 +50,17 @@ def test_fkine(self):
             [0.,           0.,          0.,          1.]
         ])
 
-        nt.assert_array_almost_equal(panda.fkine(q), ans)
+        nt.assert_array_almost_equal(panda.fkine(q1), ans)
+        nt.assert_array_almost_equal(panda.fkine(q2), ans)
+        nt.assert_array_almost_equal(panda.fkine(q3), ans)
+        nt.assert_array_almost_equal(panda.fkine(q3), ans)
 
     def test_jacob0(self):
         panda = rp.Panda()
-        q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
+        q3 = np.expand_dims(q1, 0)
+        q4 = np.expand_dims(q1, 1)
 
         ans = np.array([
             [-1.61683957e-01,  1.07925929e-01, -3.41453006e-02,
@@ -74,11 +83,17 @@ def test_jacob0(self):
                 7.48247732e-01]
         ])
 
-        nt.assert_array_almost_equal(panda.jacob0(q), ans)
+        nt.assert_array_almost_equal(panda.jacob0(q1), ans)
+        nt.assert_array_almost_equal(panda.jacob0(q2), ans)
+        nt.assert_array_almost_equal(panda.jacob0(q3), ans)
+        nt.assert_array_almost_equal(panda.jacob0(q4), ans)
 
     def test_hessian0(self):
         panda = rp.Panda()
-        q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
+        q3 = np.expand_dims(q1, 0)
+        q4 = np.expand_dims(q1, 1)
 
         ans = np.array([
             [
@@ -221,19 +236,39 @@ def test_hessian0(self):
             ]
         ])
 
-        nt.assert_array_almost_equal(panda.hessian0(q), ans)
+        nt.assert_array_almost_equal(panda.hessian0(q1), ans)
+        nt.assert_array_almost_equal(panda.hessian0(q2), ans)
+        nt.assert_array_almost_equal(panda.hessian0(q3), ans)
+        nt.assert_array_almost_equal(panda.hessian0(q4), ans)
+        nt.assert_array_almost_equal(panda.hessian0(J0=panda.jacob0(q1)), ans)
+        nt.assert_array_almost_equal(panda.hessian0(
+            q1, J0=panda.jacob0(q1)), ans)
+        self.assertRaises(ValueError, panda.hessian0)
+        self.assertRaises(ValueError, panda.hessian0, [1, 3])
+        self.assertRaises(TypeError, panda.hessian0, 'Wfgsrth')
 
     def test_manipulability(self):
         panda = rp.Panda()
-        q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
+        q3 = np.expand_dims(q1, 0)
+        q4 = np.expand_dims(q1, 1)
 
         ans = 0.006559178039088341
 
-        nt.assert_array_almost_equal(panda.manipulability(q), ans)
+        nt.assert_array_almost_equal(panda.manipulability(q1), ans)
+        nt.assert_array_almost_equal(panda.manipulability(q2), ans)
+        nt.assert_array_almost_equal(panda.manipulability(q3), ans)
+        nt.assert_array_almost_equal(panda.manipulability(q4), ans)
+        self.assertRaises(ValueError, panda.manipulability)
+        self.assertRaises(TypeError, panda.manipulability, 'Wfgsrth')
 
     def test_jacobm(self):
         panda = rp.Panda()
-        q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+        q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
+        q3 = np.expand_dims(q1, 0)
+        q4 = np.expand_dims(q1, 1)
 
         ans = np.array([
             [1.27080875e-17],
@@ -245,7 +280,12 @@ def test_jacobm(self):
             [0.00000000e+00]
         ])
 
-        nt.assert_array_almost_equal(panda.jacobm(q), ans)
+        nt.assert_array_almost_equal(panda.jacobm(q1), ans)
+        nt.assert_array_almost_equal(panda.jacobm(q2), ans)
+        nt.assert_array_almost_equal(panda.jacobm(q3), ans)
+        nt.assert_array_almost_equal(panda.jacobm(q4), ans)
+        self.assertRaises(ValueError, panda.jacobm)
+        self.assertRaises(TypeError, panda.jacobm, 'Wfgsrth')
 
 
 if __name__ == '__main__':
