-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathget_G.py
45 lines (43 loc) · 8.14 KB
/
get_G.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
import numpy as np
def G_mtx(q):
q1 = q[0]
q2 = q[1]
q3 = q[2]
q4 = q[3]
q5 = q[4]
q6 = q[5]
q7 = q[6]
g0 = 9.80665
a4 = 0.0825
a5 = -0.0825
a7 = 0.088
d1 = 0.333
d3 = 0.316
d5 = 0.384
t2 = np.cos(q2)
t3 = np.sin(q2)
t4 = np.cos(q3)
t5 = np.cos(q4)
t6 = np.sin(q4)
t7 = np.sin(q3)
t8 = np.cos(q5)
t9 = np.sin(q5)
t10 = np.cos(q6)
t11 = np.sin(q6)
t12 = np.cos(q7)
t13 = np.sin(q7)
GravityMatrix = [[0],
[
g0 * t2 * 5.368784261474552e-3 - g0 * t3 * 3.10257567176716 - g0 * t2 * t4 * 6.873533102775262e-1 - g0 * t3 * t5 * 1.718488169611198 + g0 * t2 * t7 * 2.337649457371796e-2 + g0 * t3 * t6 * 4.888360246332429e-1 + g0 * t2 * t4 * t5 * 4.888360246332429e-1 + g0 * t2 * t4 * t6 * 1.718488169611198 + g0 * t2 * t7 * t8 * 7.705324195893029e-2 + g0 * t3 * t6 * t8 * 1.00039522535591e-2 - g0 * t2 * t7 * t9 * 1.00039522535591e-2 + g0 * t3 * t5 * t10 * 6.771174144247946e-2 + g0 * t3 * t6 * t9 * 7.705324195893029e-2 - g0 * t3 * t5 * t11 * 1.656809687593008e-1 + g0 * t2 * t4 * t5 * t8 * 1.00039522535591e-2 + g0 * t2 * t4 * t5 * t9 * 7.705324195893029e-2 - g0 * t2 * t4 * t6 * t10 * 6.771174144247946e-2 + g0 * t2 * t4 * t6 * t11 * 1.656809687593008e-1 - g0 * t3 * t6 * t8 * t10 * 1.656809687593008e-1 + g0 * t2 * t7 * t9 * t10 * 1.656809687593008e-1 - g0 * t3 * t6 * t8 * t11 * 6.771174144247946e-2 + g0 * t2 * t7 * t8 * t12 * 4.883521085127657e-4 + g0 * t2 * t7 * t9 * t11 * 6.771174144247946e-2 - g0 * t2 * t7 * t8 * t13 * 6.230758965393622e-3 + g0 * t3 * t6 * t9 * t12 * 4.883521085127657e-4 - g0 * t3 * t5 * t11 * t12 * 6.230758965393622e-3 - g0 * t3 * t6 * t9 * t13 * 6.230758965393622e-3 - g0 * t3 * t5 * t11 * t13 * 4.883521085127657e-4 - g0 * t2 * t4 * t5 * t8 * t10 * 1.656809687593008e-1 - g0 * t2 * t4 * t5 * t8 * t11 * 6.771174144247946e-2 + g0 * t2 * t4 * t5 * t9 * t12 * 4.883521085127657e-4 - g0 * t2 * t4 * t5 * t9 * t13 * 6.230758965393622e-3 + g0 * t2 * t4 * t6 * t11 * t12 * 6.230758965393622e-3 + g0 * t2 * t4 * t6 * t11 * t13 * 4.883521085127657e-4 - g0 * t3 * t6 * t8 * t10 * t12 * 6.230758965393622e-3 + g0 * t2 * t7 * t9 * t10 * t12 * 6.230758965393622e-3 - g0 * t3 * t6 * t8 * t10 * t13 * 4.883521085127657e-4 + g0 * t2 * t7 * t9 * t10 * t13 * 4.883521085127657e-4 - g0 * t2 * t4 * t5 * t8 * t10 * t12 * 6.230758965393622e-3 - g0 * t2 * t4 * t5 * t8 * t10 * t13 * 4.883521085127657e-4],
[
g0 * t3 * t4 * 2.337649457371796e-2 + g0 * t3 * t7 * 6.873533102775262e-1 + g0 * t3 * t4 * t8 * 7.705324195893029e-2 - g0 * t3 * t5 * t7 * 4.888360246332429e-1 - g0 * t3 * t4 * t9 * 1.00039522535591e-2 - g0 * t3 * t6 * t7 * 1.718488169611198 - g0 * t3 * t5 * t7 * t8 * 1.00039522535591e-2 - g0 * t3 * t5 * t7 * t9 * 7.705324195893029e-2 + g0 * t3 * t4 * t9 * t10 * 1.656809687593008e-1 + g0 * t3 * t6 * t7 * t10 * 6.771174144247946e-2 + g0 * t3 * t4 * t8 * t12 * 4.883521085127657e-4 + g0 * t3 * t4 * t9 * t11 * 6.771174144247946e-2 - g0 * t3 * t6 * t7 * t11 * 1.656809687593008e-1 - g0 * t3 * t4 * t8 * t13 * 6.230758965393622e-3 + g0 * t3 * t5 * t7 * t8 * t10 * 1.656809687593008e-1 + g0 * t3 * t5 * t7 * t8 * t11 * 6.771174144247946e-2 - g0 * t3 * t5 * t7 * t9 * t12 * 4.883521085127657e-4 + g0 * t3 * t5 * t7 * t9 * t13 * 6.230758965393622e-3 + g0 * t3 * t4 * t9 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t9 * t10 * t13 * 4.883521085127657e-4 - g0 * t3 * t6 * t7 * t11 * t12 * 6.230758965393622e-3 - g0 * t3 * t6 * t7 * t11 * t13 * 4.883521085127657e-4 + g0 * t3 * t5 * t7 * t8 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t5 * t7 * t8 * t10 * t13 * 4.883521085127657e-4],
[g0 * t2 * t5 * (
-4.888360246332429e-1) - g0 * t2 * t6 * 1.718488169611198 + g0 * t3 * t4 * t5 * 1.718488169611198 - g0 * t3 * t4 * t6 * 4.888360246332429e-1 - g0 * t2 * t5 * t8 * 1.00039522535591e-2 - g0 * t2 * t5 * t9 * 7.705324195893029e-2 + g0 * t2 * t6 * t10 * 6.771174144247946e-2 - g0 * t2 * t6 * t11 * 1.656809687593008e-1 - g0 * t3 * t4 * t6 * t8 * 1.00039522535591e-2 - g0 * t3 * t4 * t5 * t10 * 6.771174144247946e-2 - g0 * t3 * t4 * t6 * t9 * 7.705324195893029e-2 + g0 * t3 * t4 * t5 * t11 * 1.656809687593008e-1 + g0 * t2 * t5 * t8 * t10 * 1.656809687593008e-1 + g0 * t2 * t5 * t8 * t11 * 6.771174144247946e-2 - g0 * t2 * t5 * t9 * t12 * 4.883521085127657e-4 + g0 * t2 * t5 * t9 * t13 * 6.230758965393622e-3 - g0 * t2 * t6 * t11 * t12 * 6.230758965393622e-3 - g0 * t2 * t6 * t11 * t13 * 4.883521085127657e-4 + g0 * t3 * t4 * t6 * t8 * t10 * 1.656809687593008e-1 + g0 * t3 * t4 * t6 * t8 * t11 * 6.771174144247946e-2 - g0 * t3 * t4 * t6 * t9 * t12 * 4.883521085127657e-4 + g0 * t3 * t4 * t5 * t11 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t6 * t9 * t13 * 6.230758965393622e-3 + g0 * t3 * t4 * t5 * t11 * t13 * 4.883521085127657e-4 + g0 * t2 * t5 * t8 * t10 * t12 * 6.230758965393622e-3 + g0 * t2 * t5 * t8 * t10 * t13 * 4.883521085127657e-4 + g0 * t3 * t4 * t6 * t8 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t6 * t8 * t10 * t13 * 4.883521085127657e-4],
[g0 * t2 * t6 * t8 * (
-7.705324195893029e-2) + g0 * t2 * t6 * t9 * 1.00039522535591e-2 - g0 * t3 * t7 * t8 * 1.00039522535591e-2 - g0 * t3 * t7 * t9 * 7.705324195893029e-2 + g0 * t3 * t4 * t5 * t8 * 7.705324195893029e-2 - g0 * t3 * t4 * t5 * t9 * 1.00039522535591e-2 - g0 * t2 * t6 * t9 * t10 * 1.656809687593008e-1 - g0 * t2 * t6 * t8 * t12 * 4.883521085127657e-4 - g0 * t2 * t6 * t9 * t11 * 6.771174144247946e-2 + g0 * t3 * t7 * t8 * t10 * 1.656809687593008e-1 + g0 * t2 * t6 * t8 * t13 * 6.230758965393622e-3 + g0 * t3 * t7 * t8 * t11 * 6.771174144247946e-2 - g0 * t3 * t7 * t9 * t12 * 4.883521085127657e-4 + g0 * t3 * t7 * t9 * t13 * 6.230758965393622e-3 + g0 * t3 * t4 * t5 * t9 * t10 * 1.656809687593008e-1 + g0 * t3 * t4 * t5 * t8 * t12 * 4.883521085127657e-4 + g0 * t3 * t4 * t5 * t9 * t11 * 6.771174144247946e-2 - g0 * t3 * t4 * t5 * t8 * t13 * 6.230758965393622e-3 - g0 * t2 * t6 * t9 * t10 * t12 * 6.230758965393622e-3 - g0 * t2 * t6 * t9 * t10 * t13 * 4.883521085127657e-4 + g0 * t3 * t7 * t8 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t7 * t8 * t10 * t13 * 4.883521085127657e-4 + g0 * t3 * t4 * t5 * t9 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t5 * t9 * t10 * t13 * 4.883521085127657e-4],
[
g0 * t2 * t5 * t10 * 1.656809687593008e-1 + g0 * t2 * t5 * t11 * 6.771174144247946e-2 + g0 * t3 * t4 * t6 * t10 * 1.656809687593008e-1 + g0 * t3 * t4 * t6 * t11 * 6.771174144247946e-2 + g0 * t2 * t6 * t8 * t10 * 6.771174144247946e-2 - g0 * t2 * t6 * t8 * t11 * 1.656809687593008e-1 + g0 * t2 * t5 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t7 * t9 * t10 * 6.771174144247946e-2 + g0 * t2 * t5 * t10 * t13 * 4.883521085127657e-4 - g0 * t3 * t7 * t9 * t11 * 1.656809687593008e-1 - g0 * t3 * t4 * t5 * t8 * t10 * 6.771174144247946e-2 + g0 * t3 * t4 * t5 * t8 * t11 * 1.656809687593008e-1 + g0 * t3 * t4 * t6 * t10 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t6 * t10 * t13 * 4.883521085127657e-4 - g0 * t2 * t6 * t8 * t11 * t12 * 6.230758965393622e-3 - g0 * t2 * t6 * t8 * t11 * t13 * 4.883521085127657e-4 - g0 * t3 * t7 * t9 * t11 * t12 * 6.230758965393622e-3 - g0 * t3 * t7 * t9 * t11 * t13 * 4.883521085127657e-4 + g0 * t3 * t4 * t5 * t8 * t11 * t12 * 6.230758965393622e-3 + g0 * t3 * t4 * t5 * t8 * t11 * t13 * 4.883521085127657e-4],
[
g0 * t2 * t6 * t9 * t12 * 6.230758965393622e-3 + g0 * t2 * t5 * t11 * t12 * 4.883521085127657e-4 + g0 * t2 * t6 * t9 * t13 * 4.883521085127657e-4 - g0 * t3 * t7 * t8 * t12 * 6.230758965393622e-3 - g0 * t2 * t5 * t11 * t13 * 6.230758965393622e-3 - g0 * t3 * t7 * t8 * t13 * 4.883521085127657e-4 - g0 * t3 * t4 * t5 * t9 * t12 * 6.230758965393622e-3 - g0 * t3 * t4 * t5 * t9 * t13 * 4.883521085127657e-4 + g0 * t3 * t4 * t6 * t11 * t12 * 4.883521085127657e-4 - g0 * t3 * t4 * t6 * t11 * t13 * 6.230758965393622e-3 + g0 * t2 * t6 * t8 * t10 * t12 * 4.883521085127657e-4 - g0 * t2 * t6 * t8 * t10 * t13 * 6.230758965393622e-3 + g0 * t3 * t7 * t9 * t10 * t12 * 4.883521085127657e-4 - g0 * t3 * t7 * t9 * t10 * t13 * 6.230758965393622e-3 - g0 * t3 * t4 * t5 * t8 * t10 * t12 * 4.883521085127657e-4 + g0 * t3 * t4 * t5 * t8 * t10 * t13 * 6.230758965393622e-3]]
GravityMatrix = np.array(GravityMatrix)
return GravityMatrix