-
Notifications
You must be signed in to change notification settings - Fork 0
/
null_space.py
94 lines (78 loc) · 2.36 KB
/
null_space.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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
import numpy as np
import matplotlib.pyplot as plt
import math
import scipy
H = np.array([[2, -1], [-4, 2]])
H = np.array([[2, 2, 1], [2, -3, -4], [4, -1, -3]])
H = np.array([[3, 3, 15, 11], [1, -3, 1, 1], [2, 3, 11, 8]])
a = scipy.linalg.null_space(H, rcond=0.01)
print(a)
def nullspace(A, atol=1e-13, rtol=0):
A = np.atleast_2d(A)
u, s, vh = np.linalg.svd(A)
tol = max(atol, rtol * s[0])
nnz = (s >= tol).sum()
ns = vh[nnz:].conj().T
return ns
a = nullspace(H)
print(a)
R_flex = np.array(
[
[-2.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[-1.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[
0.0,
0.0,
1.0,
-1.0,
-1.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
],
[0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 2.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -1.0, 0.0, 0.0, 1.0, 1.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, -1.0, 1.0],
[0.0, 0.0, 0.0, 0.0, 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0],
]
)
R_rigid = np.array(
[
[-2.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[-1.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[
0.0,
0.0,
1.0,
-1.0,
-1.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
],
[0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 2.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1, -1.0, 0.0, 0.0, 1.1, 1.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9, -1.0, -0.9, 1.0],
[0.0, 0.0, 0.0, 0.0, -0.1, -4.0, 0.0, 0.0, 0.0, 0.0, 0.1, 4.0],
]
)
print(R_flex)
print(R_rigid)
K_R_flex = scipy.linalg.null_space(R_flex)
K_R_rigid = scipy.linalg.null_space(R_rigid)
print(K_R_flex)
print(K_R_rigid)
angle = scipy.linalg.subspace_angles(K_R_flex, K_R_flex[:, 0].reshape(12, 1))
print(angle)