8000 fast hessian and 3+ partial dierivative progress · lars-x/robotics-toolbox-python@e21ccb1 · GitHub
[go: up one dir, main page]
Skip to content

Commit e21ccb1

Browse files
jhavljhavl
committed
fast hessian and 3+ partial dierivative progress
1 parent bcd2b04 commit e21ccb1

File tree

2 files changed

+220
-8
lines changed

2 files changed

+220
-8
lines changed

‎examples/swifty.py

Lines changed: 68 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
import roboticstoolbox as rtb
77
import spatialmath as sm
8+
import numpy as np
89

910
# Launch the simulator Swift
1011
# env = rtb.backends.Swift()
@@ -67,9 +68,70 @@
6768
# print(panda.fkine(panda.q))
6869
# print(T)
6970

70-
r = rtb.models.Puma560()
71-
# r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
72-
env = rtb.backends.Swift()
73-
env.launch()
74-
env.add(r)
75-
env.hold()
71+
# r = rtb.models.Puma560()
72+
# # r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
73+
# env = rtb.backends.Swift()
74+
# env.launch()
75+
# env.add(r)
76+
# env.hold()
77+
78+
r = rtb.models.ETS.Panda()
79+
r.q = r.qr
80+
81+
q1 = r.qr
82+
q2 = q1 + 0.1
83+
84+
85+
def derivative(f, a, method='central', h=0.01):
86+
'''Compute the difference formula for f'(a) with step size h.
87+
88+
Parameters
89+
----------
90+
f : function
91+
Vectorized function of one variable
92+
a : number
93+
Compute derivative at x = a
94+
method : string
95+
Difference formula: 'forward', 'backward' or 'central'
96+
h : number
97+
Step size in difference formula
98+
99+
Returns
100+
-------
101+
float
102+
Difference formula:
103+
central: f(a+h) - f(a-h))/2h
104+
forward: f(a+h) - f(a))/h
105+
backward: f(a) - f(a-h))/h
106+
'''
107+
if method == 'central':
108+
return (f(a + h) - f(a - h))/(2*h)
109+
elif method == 'forward':
110+
return (f(a + h) - f(a))/h
111+
elif method == 'backward':
112+
return (f(a) - f(a - h))/h
113+
else:
114+
raise ValueError("Method must be 'central', 'forward' or 'backward'.")
115+
116+
117+
# Numerical hessian wrt q[0]
118+
# d = derivative(lambda x: r.jacob0(np.r_[x, q1[1:]]), q1[0], h=0.1)
119+
# print(np.round(h1[:, :, 0], 3))
120+
# print(np.round(d, 3))
121+
122+
# Numerical third wrt q[0]
123+
d = derivative(lambda x: r.hessian0(np.r_[x, q1[1:]]), q1[0], h=0.01)
124+
print(np.round(d[3:, :, 0], 3))
125+
print(np.round(r.third(q1)[3:, :, 0, 0], 3))
126+
127+
l = r.deriv(q1, 3)
128+
print(np.round(l[3:, :, 0, 0], 3))
129+
130+
# def runner():
131+
# for i in range(10000):
132+
# r.jacob0(r.q)
133+
# # r.hessian0(r.q)
134+
135+
136+
# import cProfile
137+
# cProfile.run('runner()')
Original file line numberDiff line numberDiff line change
@@ -1364,6 +1364,150 @@ def jacobe(self, q, endlink=None, startlink=None, offset=None, T=None):
13641364
Je = self.jacobev(q, endlink, startlink, offset, T) @ J0
13651365
return Je
13661366

1367+
# def deriv(self, q, n, J0=None, endlink=None, startlink=None):
1368+
# endlink, startlink = self._get_limit_links(endlink, startlink)
1369+
1370+
# def cross(a, b):
1371+
# x = a[1] * b[2] - a[2] * b[1]
1372+
# y = a[2] * b[0] - a[0] * b[2]
1373+
# z = a[0] * b[1] - a[1] * b[0]
1374+
# return np.array([x, y, z])
1375+
1376+
# _, nl = self.get_path(endlink, startlink)
1377+
1378+
# J = self.jacob0(q, endlink=endlink, startlink=startlink)
1379+
# H = self.hessian0(q, J, endlink, startlink)
1380+
1381+
# d = [J, H]
1382+
# size = [6, nl, nl]
1383+
# count = np.array([0, 0])
1384+
# c = 2
1385+
1386+
# def add_indices(indices, c):
1387+
# total = len(indices * 2)
1388+
# new_indices = []
1389+
1390+
# for i in range(total):
1391+
# j = i // 2
1392+
# new_indices.append([])
1393+
# new_indices[i].append(indices[j][0].copy())
1394+
# new_indices[i].append(indices[j][1].copy())
1395+
1396+
# # if even number
1397+
# if i % 2 == 0:
1398+
# new_indices[i][0].append(c)
1399+
# # if odd number
1400+
# else:
1401+
# new_indices[i][1].append(c)
1402+
1403+
# return new_indices
1404+
1405+
# def add_pdi(pdi):
1406+
# total = len(pdi * 2)
1407+
# 10000 new_pdi = []
1408+
1409+
# for i in range(total):
1410+
# j = i // 2
1411+
# new_pdi.append([])
1412+
# new_pdi[i].append(pdi[j][0])
1413+
# new_pdi[i].append(pdi[j][1])
1414+
1415+
# # if even number
1416+
# if i % 2 == 0:
1417+
# new_pdi[i][0] += 1
1418+
# # if odd number
1419+
# else:
1420+
# new_pdi[i][1] += 1
1421+
1422+
# return new_pdi
1423+
1424+
# # these are the indices used for the hessian
1425+
# # indices = [[[2, 3], [1]], [[2], [1, 3]]]
1426+
# indices = [[[1], [0]]]
1427+
1428+
# # the are the pd indices used in the corss prods
1429+
# # pdi = [[2, 1], [1, 2]]
1430+
# pdi = [[0, 0]]
1431+
1432+
# while len(d) != n:
1433+
# size.append(nl)
1434+
# count = np.r_[count, 0]
1435+
# c += 1
1436+
# indices = add_indices(indices, 2)
1437+
# pdi = add_pdi(pdi)
1438+
# # print(indices)
1439+
# # print(pdi)
1440+
1441+
# # print("Statring:")
1442+
# # print(count)
1443+
# # print(size)
1444+
# # print(len(d))
1445+
1446+
# pd = np.zeros(size)
1447+
1448+
# for i in range(nl ** c):
1449+
# # print('Count: {0}'.format(count))
1450+
1451+
# res = np.zeros(3)
1452+
1453+
# for j in range(len(indices)):
1454+
# # print(pdi[j][0])
1455+
# pdr0 = d[pdi[j][0]]
1456+
# pdr1 = d[pdi[j][1]]
1457+
1458+
# idx0 = count[indices[j][0]]
1459+
# idx1 = count[indices[j][1]]
1460+
1461+
# res += cross(
1462+
# pdr0[(slice(3, 6), *idx0)],
1463+
# pdr1[(slice(3, 6), *idx1)])
1464+
# # print(pdr1[:3, idx1].shape)
1465+
# # res += pdr[:3, :, indices[j][0]]
1466+
1467+
# pd[(slice(3, 6), *count)] = res
1468+
1469+
# count[0] += 1
1470+
# for j in range(len(count)):
1471+
# if count[j] == nl:
1472+
# count[j] = 0
1473+
# if (j != len(count) - 1):
1474+
# count[j + 1] += 1
1475+
1476+
# d.append(pd)
1477+
# return d[-1]
1478+
1479+
# def third(self, q=None, J0=None, endlink=None, startlink=None):
1480+
# endlink, startlink = self._get_limit_links(endlink, startlink)
1481+
# path, n = self.get_path(endlink, startlink)
1482+
1483+
# def cross(a, b):
1484+
# x = a[1] * b[2] - a[2] * b[1]
1485+
# y = a[2] * b[0] - a[0] * b[2]
1486+
# z = a[0] * b[1] - a[1] * b[0]
1487+
# return np.array([x, y, z])
1488+
1489+
# if J0 is None:
1490+
# q = getvector(q, n)
1491+
# J0 = self.jacob0(q, endlink=endlink)
1492+
# else:
1493+
# verifymatrix(J0, (6, n))
1494+
1495+
# H0 = self.hessian0(q, J0, endlink, startlink)
1496+
1497+
# L = np.zeros((6, n, n, n))
1498+
1499+
# for l in range(n):
1500+
# for k in range(n):
1501+
# for j in range(n):
1502+
1503+
# # H[:3, i, j] = cross(J0[3:, j], J0[:3, i])
1504+
# L[3:, j, k, l] = cross(H0[3:, k, l], J0[3:, j]) + \
1505+
# cross(J0[3:, k], H0[3:, j, l])
1506+
# # if i != j:
1507+
# # H[:3, j, i] = H[:3, i, j]
1508+
1509+
# return L
1510+
13671511
def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
13681512
"""
13691513
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):
14061550
endlink, startlink = self._get_limit_links(endlink, startlink)
14071551
path, n = self.get_path(endlink, startlink)
14081552

1553+
def cross(a, b):
1554+
x = a[1] * b[2] - a[2] * b[1]
1555+
y = a[2] * b[0] - a[0] * b[2]
1556+
z = a[0] * b[1] - a[1] * b[0]
1557+
return np.array([x, y, z])
1558+
14091559
if J0 is None:
14101560
q = getvector(q, n)
14111561
J0 = self.jacob0(q, endlink=endlink)
@@ -1417,8 +1567,8 @@ def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
14171567
for j in range(n):
14181568
for i in range(j, n):
14191569

1420-
H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
1421-
H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
1570+
H[:3, i, j] = cross(J0[3:, j], J0[:3, i])
1571+
H[3:, i, j] = cross(J0[3:, j], J0[3:, i])
14221572

14231573
if i != j:
14241574
H[:3, j, i] = H[:3, i, j]