| 1 | from __future__ import print_function |
|---|
| 2 | |
|---|
| 3 | import sympy as sp |
|---|
| 4 | sp.init_printing() |
|---|
| 5 | dphi, dpsi, dtheta = sp.var("phi_d psi_d theta_d") |
|---|
| 6 | phi, psi, theta = sp.var("phi psi theta") |
|---|
| 7 | x, y, z = sp.var("x y z") |
|---|
| 8 | qx, qy, qz = sp.var("qx qy qz") |
|---|
| 9 | qa, qb, qc = sp.var("qa qb qc") |
|---|
| 10 | |
|---|
| 11 | def Rx(a): |
|---|
| 12 | return sp.Matrix([[1, 0, 0], [0, sp.cos(a), sp.sin(a)], [0, -sp.sin(a), sp.cos(a)]]) |
|---|
| 13 | def Ry(a): |
|---|
| 14 | return sp.Matrix([[sp.cos(a), 0, sp.sin(a)], [0, 1, 0], [-sp.sin(a), 0, sp.cos(a)]]) |
|---|
| 15 | def Rz(a): |
|---|
| 16 | return sp.Matrix([[sp.cos(a), sp.sin(a), 0], [-sp.sin(a), sp.cos(a), 0], [0, 0, 1]]) |
|---|
| 17 | |
|---|
| 18 | print("==== asymmetric ====") |
|---|
| 19 | q_xy = sp.Matrix([[qx], [qy], [0]]) |
|---|
| 20 | q_abc = sp.Matrix([[qa], [qb], [qc]]) |
|---|
| 21 | p_xyz = sp.Matrix([[x], [y], [z]]) |
|---|
| 22 | jitter = Rx(dphi)*Ry(dtheta)*Rz(dpsi) |
|---|
| 23 | view = Rz(phi)*Ry(theta)*Rz(psi) |
|---|
| 24 | view_inv = Rz(-psi)*Ry(-theta)*Rz(-phi) |
|---|
| 25 | print(">> jitter rotation") |
|---|
| 26 | for row in jitter*p_xyz: sp.pprint(row) |
|---|
| 27 | #print("\n>> jitter plus view") |
|---|
| 28 | #for row in view*jitter*p_xyz: sp.pprint(row) |
|---|
| 29 | print("\n>> view reverse") |
|---|
| 30 | for row in view_inv*q_xy: sp.pprint(row) |
|---|
| 31 | print("\n>> jitter reverse") |
|---|
| 32 | jitter_inv = Rz(-psi)*Ry(-theta)*Rx(-phi) |
|---|
| 33 | for row in jitter_inv*q_abc: sp.pprint(row) |
|---|
| 34 | print("\n>> jitter view reverse") |
|---|
| 35 | jitter_inv = Rz(-dpsi)*Ry(-dtheta)*Rx(-dphi) |
|---|
| 36 | for row in jitter_inv*view_inv*q_xy: sp.pprint(row) |
|---|
| 37 | |
|---|
| 38 | print("\n\n==== symmetric ====") |
|---|
| 39 | q_x = sp.Matrix([[qx], [0], [0]]) |
|---|
| 40 | q_ac = sp.Matrix([[qa], [0], [qc]]) |
|---|
| 41 | p_xyz = sp.Matrix([[x], [y], [z]]) |
|---|
| 42 | jitter = Rx(dphi)*Ry(dtheta) |
|---|
| 43 | view = Rz(phi)*Ry(theta) |
|---|
| 44 | jitter_inv = Ry(-dtheta)*Rx(-dphi) |
|---|
| 45 | view_inv = Ry(-theta)*Rz(-phi) |
|---|
| 46 | print(">> jitter rotation") |
|---|
| 47 | for row in jitter*p_xyz: sp.pprint(row) |
|---|
| 48 | #print("\n>> jitter plus view") |
|---|
| 49 | #for row in view*jitter*p_xyz: sp.pprint(row) |
|---|
| 50 | print("\n>> view reverse") |
|---|
| 51 | for row in view_inv*q_x: sp.pprint(row) |
|---|
| 52 | print("\n>> jitter reverse") |
|---|
| 53 | for row in jitter_inv*q_ac: sp.pprint(row) |
|---|
