|
2 | 2 |
|
3 | 3 | from geometry import * |
4 | 4 | import numpy as np |
5 | | -from numpy import cos,sin |
| 5 | +from numpy import cos, sin |
6 | 6 |
|
7 | 7 | verbose = True |
8 | 8 |
|
9 | | -def isapprox(a,b,epsilon=1e-6): |
10 | | - if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray): |
11 | | - return np.allclose(a,b,epsilon) |
| 9 | + |
| 10 | +def isapprox(a, b, epsilon=1e-6): |
| 11 | + if issubclass(a.__class__, np.ndarray) and issubclass(b.__class__, np.ndarray): |
| 12 | + return np.allclose(a, b, epsilon) |
12 | 13 | else: |
13 | | - return abs(a-b)<epsilon |
| 14 | + return abs(a - b) < epsilon |
| 15 | + |
14 | 16 |
|
15 | 17 | # --- Quaternion --------------------------------------------------------------- |
16 | 18 | # Coefficient initialisation |
17 | 19 | verbose and print("[Quaternion] Coefficient initialisation") |
18 | | -q = Quaternion(1,2,3,4) |
| 20 | +q = Quaternion(1, 2, 3, 4) |
19 | 21 | q.normalize() |
20 | | -assert(isapprox(np.linalg.norm(q.coeffs()),q.norm())) |
21 | | -assert(isapprox(np.linalg.norm(q.coeffs()),1)) |
| 22 | +assert isapprox(np.linalg.norm(q.coeffs()), q.norm()) |
| 23 | +assert isapprox(np.linalg.norm(q.coeffs()), 1) |
22 | 24 |
|
23 | 25 | # Coefficient-vector initialisation |
24 | 26 | verbose and print("[Quaternion] Coefficient-vector initialisation") |
25 | | -v = np.array([0.5,-0.5,0.5,0.5]) |
| 27 | +v = np.array([0.5, -0.5, 0.5, 0.5]) |
26 | 28 | qv = Quaternion(v) |
27 | | -assert(isapprox(qv.coeffs(), v)) |
| 29 | +assert isapprox(qv.coeffs(), v) |
28 | 30 |
|
29 | 31 | # Angle axis initialisation |
30 | 32 | verbose and print("[Quaternion] AngleAxis initialisation") |
31 | 33 | r = AngleAxis(q) |
32 | 34 | q2 = Quaternion(r) |
33 | | -assert(q==q) |
34 | | -assert(isapprox(q.coeffs(),q2.coeffs())) |
35 | | -assert(q2.isApprox(q2)) |
36 | | -assert(q2.isApprox(q2,1e-2)) |
| 35 | +assert q == q |
| 36 | +assert isapprox(q.coeffs(), q2.coeffs()) |
| 37 | +assert q2.isApprox(q2) |
| 38 | +assert q2.isApprox(q2, 1e-2) |
37 | 39 |
|
38 | 40 | Rq = q.matrix() |
39 | 41 | Rr = r.matrix() |
40 | | -assert(isapprox(Rq.dot(Rq.T),np.eye(3))) |
41 | | -assert(isapprox(Rr,Rq)) |
| 42 | +assert isapprox(Rq.dot(Rq.T), np.eye(3)) |
| 43 | +assert isapprox(Rr, Rq) |
42 | 44 |
|
43 | 45 | # Rotation matrix initialisation |
44 | 46 | verbose and print("[Quaternion] Rotation Matrix initialisation") |
45 | 47 | qR = Quaternion(Rr) |
46 | | -assert(q.isApprox(qR)) |
47 | | -assert(isapprox(q.coeffs(),qR.coeffs())) |
| 48 | +assert q.isApprox(qR) |
| 49 | +assert isapprox(q.coeffs(), qR.coeffs()) |
48 | 50 |
|
49 | | -assert(isapprox(qR[3],1./np.sqrt(30))) |
| 51 | +assert isapprox(qR[3], 1.0 / np.sqrt(30)) |
50 | 52 | try: |
51 | | - qR[5] |
52 | | - print("Error, this message should not appear.") |
| 53 | + qR[5] |
| 54 | + print("Error, this message should not appear.") |
53 | 55 | except RuntimeError as e: |
54 | | - if verbose: print("As expected, caught exception: ", e) |
| 56 | + if verbose: |
| 57 | + print("As expected, caught exception: ", e) |
55 | 58 |
|
56 | 59 | # --- Angle Vector ------------------------------------------------ |
57 | | -r = AngleAxis(.1,np.array([1,0,0],np.double)) |
58 | | -if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n") |
59 | | -assert( isapprox(r.matrix()[2,2],cos(r.angle))) |
60 | | -assert( isapprox(r.axis,np.array([1.,0,0])) ) |
61 | | -assert( isapprox(r.angle,0.1) ) |
62 | | -assert(r.isApprox(r)) |
63 | | -assert(r.isApprox(r,1e-2)) |
| 60 | +r = AngleAxis(0.1, np.array([1, 0, 0], np.double)) |
| 61 | +if verbose: |
| 62 | + print("Rx(.1) = \n\n", r.matrix(), "\n") |
| 63 | +assert isapprox(r.matrix()[2, 2], cos(r.angle)) |
| 64 | +assert isapprox(r.axis, np.array([1.0, 0, 0])) |
| 65 | +assert isapprox(r.angle, 0.1) |
| 66 | +assert r.isApprox(r) |
| 67 | +assert r.isApprox(r, 1e-2) |
64 | 68 |
|
65 | | -r.axis = np.array([0,1,0],np.double).T |
66 | | -assert( isapprox(r.matrix()[0,0],cos(r.angle))) |
| 69 | +r.axis = np.array([0, 1, 0], np.double).T |
| 70 | +assert isapprox(r.matrix()[0, 0], cos(r.angle)) |
67 | 71 |
|
68 | 72 | ri = r.inverse() |
69 | | -assert( isapprox(ri.angle,-.1) ) |
| 73 | +assert isapprox(ri.angle, -0.1) |
70 | 74 |
|
71 | 75 | R = r.matrix() |
72 | | -r2 = AngleAxis(np.dot(R,R)) |
73 | | -assert( isapprox(r2.angle,r.angle*2) ) |
| 76 | +r2 = AngleAxis(np.dot(R, R)) |
| 77 | +assert isapprox(r2.angle, r.angle * 2) |
74 | 78 |
|
75 | 79 | # --- USER FUNCTIONS ----------------------------------------------------------- |
76 | 80 | ro = testOutAngleAxis() |
77 | | -assert(ro.__class__ == AngleAxis) |
| 81 | +assert ro.__class__ == AngleAxis |
78 | 82 | res = testInAngleAxis(r) |
79 | | -assert( res==r.angle ) |
| 83 | +assert res == r.angle |
80 | 84 |
|
81 | 85 | qo = testOutQuaternion() |
82 | | -assert(qo.__class__ == Quaternion) |
| 86 | +assert qo.__class__ == Quaternion |
83 | 87 | res = testInQuaternion(q) |
84 | | -assert(q.norm() == res) |
| 88 | +assert q.norm() == res |
0 commit comments