fast hessian and 3+ partial dierivative progress

jhavl · jhavl · commit e21ccb186721 · 2021-01-07T17:05:45.000+10:00
diff --git a/examples/swifty.py b/examples/swifty.py
@@ -5,6 +5,7 @@
 
 import roboticstoolbox as rtb
 import spatialmath as sm
+import numpy as np
 
 # Launch the simulator Swift
 # env = rtb.backends.Swift()
@@ -67,9 +68,70 @@
 # print(panda.fkine(panda.q))
 # print(T)
 
-r = rtb.models.Puma560()
-# r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
-env = rtb.backends.Swift()
-env.launch()
-env.add(r)
-env.hold()
+# r = rtb.models.Puma560()
+# # r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
+# env = rtb.backends.Swift()
+# env.launch()
+# env.add(r)
+# env.hold()
+
+r = rtb.models.ETS.Panda()
+r.q = r.qr
+
+q1 = r.qr
+q2 = q1 + 0.1
+
+
+def derivative(f, a, method='central', h=0.01):
+    '''Compute the difference formula for f'(a) with step size h.
+
+    Parameters
+    ----------
+    f : function
+        Vectorized function of one variable
+    a : number
+        Compute derivative at x = a
+    method : string
+        Difference formula: 'forward', 'backward' or 'central'
+    h : number
+        Step size in difference formula
+
+    Returns
+    -------
+    float
+        Difference formula:
+            central: f(a+h) - f(a-h))/2h
+            forward: f(a+h) - f(a))/h
+            backward: f(a) - f(a-h))/h
+    '''
+    if method == 'central':
+        return (f(a + h) - f(a - h))/(2*h)
+    elif method == 'forward':
+        return (f(a + h) - f(a))/h
+    elif method == 'backward':
+        return (f(a) - f(a - h))/h
+    else:
+        raise ValueError("Method must be 'central', 'forward' or 'backward'.")
+
+
+# Numerical hessian wrt q[0]
+# d = derivative(lambda x: r.jacob0(np.r_[x, q1[1:]]), q1[0], h=0.1)
+# print(np.round(h1[:, :, 0], 3))
+# print(np.round(d, 3))
+
+# Numerical third wrt q[0]
+d = derivative(lambda x: r.hessian0(np.r_[x, q1[1:]]), q1[0], h=0.01)
+print(np.round(d[3:, :, 0], 3))
+print(np.round(r.third(q1)[3:, :, 0, 0], 3))
+
+l = r.deriv(q1, 3)
+print(np.round(l[3:, :, 0, 0], 3))
+
+# def runner():
+#     for i in range(10000):
+#         r.jacob0(r.q)
+#         # r.hessian0(r.q)
+
+
+# import cProfile
+# cProfile.run('runner()')
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1364,6 +1364,150 @@ def jacobe(self, q, endlink=None, startlink=None, offset=None, T=None):
         Je = self.jacobev(q, endlink, startlink, offset, T) @ J0
         return Je
 
+    # def deriv(self, q, n, J0=None, endlink=None, startlink=None):
+    #     endlink, startlink = self._get_limit_links(endlink, startlink)
+
+    #     def cross(a, b):
+    #         x = a[1] * b[2] - a[2] * b[1]
+    #         y = a[2] * b[0] - a[0] * b[2]
+    #         z = a[0] * b[1] - a[1] * b[0]
+    #         return np.array([x, y, z])
+
+    #     _, nl = self.get_path(endlink, startlink)
+
+    #     J = self.jacob0(q, endlink=endlink, startlink=startlink)
+    #     H = self.hessian0(q, J, endlink, startlink)
+
+    #     d = [J, H]
+    #     size = [6, nl, nl]
+    #     count = np.array([0, 0])
+    #     c = 2
+
+    #     def add_indices(indices, c):
+    #         total = len(indices * 2)
+    #         new_indices = []
+
+    #         for i in range(total):
+    #             j = i // 2
+    #             new_indices.append([])
+    #             new_indices[i].append(indices[j][0].copy())
+    #             new_indices[i].append(indices[j][1].copy())
+
+    #             # if even number
+    #             if i % 2 == 0:
+    #                 new_indices[i][0].append(c)
+    #             # if odd number
+    #             else:
+    #                 new_indices[i][1].append(c)
+
+    #         return new_indices
+
+    #     def add_pdi(pdi):
+    #         total = len(pdi * 2)
+    #         new_pdi = []
+
+    #         for i in range(total):
+    #             j = i // 2
+    #             new_pdi.append([])
+    #             new_pdi[i].append(pdi[j][0])
+    #             new_pdi[i].append(pdi[j][1])
+
+    #             # if even number
+    #             if i % 2 == 0:
+    #                 new_pdi[i][0] += 1
+    #             # if odd number
+    #             else:
+    #                 new_pdi[i][1] += 1
+
+    #         return new_pdi
+
+    #     # these are the indices used for the hessian
+    #     # indices = [[[2, 3], [1]], [[2], [1, 3]]]
+    #     indices = [[[1], [0]]]
+
+    #     # the are the pd indices used in the corss prods
+    #     # pdi = [[2, 1], [1, 2]]
+    #     pdi = [[0, 0]]
+
+    #     while len(d) != n:
+    #         size.append(nl)
+    #         count = np.r_[count, 0]
+    #         c += 1
+    #         indices = add_indices(indices, 2)
+    #         pdi = add_pdi(pdi)
+    #         # print(indices)
+    #         # print(pdi)
+
+    #         # print("Statring:")
+    #         # print(count)
+    #         # print(size)
+    #         # print(len(d))
+
+    #         pd = np.zeros(size)
+
+    #         for i in range(nl ** c):
+    #             # print('Count: {0}'.format(count))
+
+    #             res = np.zeros(3)
+
+    #             for j in range(len(indices)):
+    #                 # print(pdi[j][0])
+    #                 pdr0 = d[pdi[j][0]]
+    #                 pdr1 = d[pdi[j][1]]
+
+    #                 idx0 = count[indices[j][0]]
+    #                 idx1 = count[indices[j][1]]
+
+    #                 res += cross(
+    #                     pdr0[(slice(3, 6), *idx0)],
+    #                     pdr1[(slice(3, 6), *idx1)])
+    #                 # print(pdr1[:3, idx1].shape)
+    #                 # res += pdr[:3, :, indices[j][0]]
+
+    #             pd[(slice(3, 6), *count)] = res
+
+    #             count[0] += 1
+    #             for j in range(len(count)):
+    #                 if count[j] == nl:
+    #                     count[j] = 0
+    #                     if (j != len(count) - 1):
+    #                         count[j + 1] += 1
+
+    #         d.append(pd)
+    #     return d[-1]
+
+    # def third(self, q=None, J0=None, endlink=None, startlink=None):
+    #     endlink, startlink = self._get_limit_links(endlink, startlink)
+    #     path, n = self.get_path(endlink, startlink)
+
+    #     def cross(a, b):
+    #         x = a[1] * b[2] - a[2] * b[1]
+    #         y = a[2] * b[0] - a[0] * b[2]
+    #         z = a[0] * b[1] - a[1] * b[0]
+    #         return np.array([x, y, z])
+
+    #     if J0 is None:
+    #         q = getvector(q, n)
+    #         J0 = self.jacob0(q, endlink=endlink)
+    #     else:
+    #         verifymatrix(J0, (6, n))
+
+    #     H0 = self.hessian0(q, J0, endlink, startlink)
+
+    #     L = np.zeros((6, n, n, n))
+
+    #     for l in range(n):
+    #         for k in range(n):
+    #             for j in range(n):
+
+    #                 # H[:3, i, j] = cross(J0[3:, j], J0[:3, i])
+    #                 L[3:, j, k, l] = cross(H0[3:, k, l], J0[3:, j]) + \
+    #                     cross(J0[3:, k], H0[3:, j, l])
+    #                 # if i != j:
+    #                 #     H[:3, j, i] = H[:3, i, j]
+
+    #     return L
+
     def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
         """
         The manipulator Hessian tensor maps joint acceleration to end-effector
@@ -1406,6 +1550,12 @@ def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
         endlink, startlink = self._get_limit_links(endlink, startlink)
         path, n = self.get_path(endlink, startlink)
 
+        def cross(a, b):
+            x = a[1] * b[2] - a[2] * b[1]
+            y = a[2] * b[0] - a[0] * b[2]
+            z = a[0] * b[1] - a[1] * b[0]
+            return np.array([x, y, z])
+
         if J0 is None:
             q = getvector(q, n)
             J0 = self.jacob0(q, endlink=endlink)
@@ -1417,8 +1567,8 @@ def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
         for j in range(n):
             for i in range(j, n):
 
-                H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
-                H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
+                H[:3, i, j] = cross(J0[3:, j], J0[:3, i])
+                H[3:, i, j] = cross(J0[3:, j], J0[3:, i])
 
                 if i != j:
                     H[:3, j, i] = H[:3, i, j]
