def GetVh(alpha,beta,Lambda,theta,mu):
## a/c to wind transform
Ac_V = sym.rot_axis3(beta)*sym.rot_axis2(alpha) # transform from a/c to the velocity frame
## a/c to Hinge transform
H_Ac = sym.trigsimp(sym.rot_axis3(-Lambda)*\
sym.rot_axis1(-theta)* \
sym.rot_axis3(Lambda)*sym.rot_axis1(-mu)) # transform from a/c to the hinge frame
H_V = sym.trigsimp(H_Ac*Ac_V) # transform from velocity to hinge reference frame
# Velocity vector in velocity frame is of the form [v 0 0]
Vv = sym.Matrix([1,0,0])
# Transform into the hinge reference frame
return (H_V * Vv)
def _rot(axis, angle):
"""DCM for simple axis 1, 2 or 3 rotations. """
if axis == 1:
return Matrix(rot_axis1(angle).T)
elif axis == 2:
return Matrix(rot_axis2(angle).T)
elif axis == 3:
return Matrix(rot_axis3(angle).T)
def _rot(axis, angle):
"""DCM for simple axis 1, 2 or 3 rotations. """
if axis == 1:
return Matrix(rot_axis1(angle).T)
elif axis == 2:
return Matrix(rot_axis2(angle).T)
elif axis == 3:
return Matrix(rot_axis3(angle).T)
def homogenous_transform(a, alpha, d, theta):
# sympy's rotation convention is different from the one normally used
rx = sp.rot_axis1(-alpha)
rx = rx.row_join(sp.Matrix([a, 0, 0]))
rx = rx.col_join(sp.Matrix([[0, 0, 0, 1]]))
rz = sp.rot_axis3(-theta)
rz = rz.row_join(sp.Matrix([0, 0, d]))
rz = rz.col_join(sp.Matrix([[0, 0, 0, 1]]))
return rx * rz
def my_plot(fig):
global figures_i
X = np.loadtxt(f"{FOLDER}/{figures_i}/X.txt", delimiter=",")
U = np.loadtxt(f"{FOLDER}/{figures_i}/U.txt", delimiter=",")
if U.shape[0] == X.shape[0] - 1:
U = np.vstack((U, U[-1, :]))
K = X.shape[0]
# 3D
ax = fig.add_subplot(1, 1, 1, projection='3d')
ax.set_xlabel('X, east')
ax.set_ylabel('Y, north')
ax.set_zlabel('Z, up')
T_max = np.max(np.linalg.norm(U, axis=1))
for k in range(K):
rx, ry, rz = X[k, 0:3]
# vx, vy, vz = X[k, 3:6]
phi, theta = X[k, 6:8]
Rx = rot_axis1(phi).T
Ry = rot_axis2(theta).T
R = Rx * Ry
Fx, Fy, Fz = np.dot(R, U[k, :] / T_max)
dx, dy, dz = np.dot(R, np.array([0., 0., 1.]))
# tx, ty, tz = np.dot(R, np.array([1., 0., 0.]))
# # speed vector
# ax.quiver(rx, ry, rz, vx, vy, vz, length=0.1, color='green')
# attitude vector
ax.quiver(rx, ry, rz, dx, dy, dz, length=0.5,
arrow_length_ratio=0.0, color='blue')
# # up vector
# ax.quiver(rx, ry, rz, tx, ty, tz, length=0.01,
# arrow_length_ratio=0.0, color='green')
# thrust vector
ax.quiver(rx, ry, rz, -Fx, -Fy, -Fz, length=0.5,
arrow_length_ratio=0.0, color='red')
scale = 1.2 * np.abs(np.max(X[:, 0:3]))
ax.set_xlim3d(-scale, scale)
ax.set_ylim3d(-scale, scale)
ax.set_zlim3d(0, scale)
ax.plot(X[:, 0], X[:, 1], X[:, 2], color='gray')
# fig.suptitle(f"Iteration {figures_i}", fontsize=14)
plt.savefig(f"output/graphs/{figures_i}.png")
def rpy_to_homogenous(rpy, origin, to_numeric=True):
x_rot = sp.rot_axis1(-rpy[0])
y_rot = sp.rot_axis2(-rpy[1])
z_rot = sp.rot_axis3(-rpy[2])
origin = sp.Matrix(origin)
rot = z_rot * y_rot * x_rot
rot = rot.row_join(origin)
rot = rot.col_join(sp.Matrix([[0, 0, 0, 1]]))
rot = sp.simplify(rot)
if to_numeric:
rot = np.array(rot.tolist(), dtype=np.float64)
return rot
def _derive_stereographic():
"""Compute the formulae to cut-and-paste into the routine below."""
from sympy import symbols, atan2, acos, rot_axis1, rot_axis3, Matrix
x_c, y_c, z_c, x, y, z = symbols('x_c y_c z_c x y z')
# The angles we'll need to rotate through.
around_z = atan2(x_c, y_c)
around_x = acos(-z_c)
# Apply rotations to produce an "o" = output vector.
v = Matrix([x, y, z])
xo, yo, zo = rot_axis1(around_x) * rot_axis3(-around_z) * v
# Which we then use the stereographic projection to produce the
# final "p" = plotting coordinates.
xp = xo / (1 - zo)
yp = yo / (1 - zo)
return xp, yp
def _derive_stereographic():
"""Compute the formulae to cut-and-paste into the routine below."""
from sympy import symbols, atan2, acos, rot_axis1, rot_axis3, Matrix
x_c, y_c, z_c, x, y, z = symbols('x_c y_c z_c x y z')
# The angles we'll need to rotate through.
around_z = atan2(x_c, y_c)
around_x = acos(-z_c)
# Apply rotations to produce an "o" = output vector.
v = Matrix([x, y, z])
xo, yo, zo = rot_axis1(around_x) * rot_axis3(-around_z) * v
# Which we then use the stereographic projection to produce the
# final "p" = plotting coordinates.
xp = xo / (1 - zo)
yp = yo / (1 - zo)
return xp, yp
def generate_projections(vol):
import pycuda.autoinit # noqa
from conebeam_projector import CudaProjector
import pyconrad.config
from pycuda.gpuarray import to_gpu, zeros
pyconrad.config.set_reco_shape(vol.shape)
pyconrad.config.center_volume()
rotation = np.random.rand(3) * 2 * np.pi
rotation_matrix = np.array(
sympy.rot_axis1(rotation[0]) * sympy.rot_axis2(rotation[1]) *
sympy.rot_axis3(rotation[2]))
extended_rotation = sympy.Identity(4).as_explicit().as_mutable()
extended_rotation[:3, :3] = rotation_matrix
extended_rotation = np.array(extended_rotation)
matrices = pyconrad.config.get_projection_matrices()
idx = random.randint(0, len(matrices) - 1)
my_matrix = matrices[idx] @ extended_rotation
from edu.stanford.rsl.conrad.geometry import Projection
from edu.stanford.rsl.conrad.numerics import SimpleMatrix
pyconrad.config.get_geometry().setProjectionMatrix(
idx, Projection(SimpleMatrix.from_numpy(my_matrix)))
projector = CudaProjector()
vol_gpu = to_gpu(vol)
proj_gpu = zeros(pyconrad.config.get_sino_shape()[1:], np.float32)
projector.forward_project_cuda_idx(vol_gpu, proj_gpu, idx)
proj = proj_gpu.get()
proj /= np.max(proj)
detector_spacing = pyconrad.config.get_geometry().getPixelDimensionX()
return proj, projector._projectionMatrices[idx], detector_spacing
def R_x(self, angle):
H = sym.eye(4)
H[:3, :3] = sym.rot_axis1(-angle)
return HomogenousTransform(self.E * H)
def linearize(self, linearize_state: np.ndarray,
linearize_input: np.ndarray) -> (np.ndarray, np.ndarray):
"""
Linearizes the quadrotor dynamics about arbitrary states / inputs.
The first time this function is called, the relevant Jacobians of the
dynamics are computed symbolically using SymPy, therefore it is slower.
The results are stored internally using SymPy's Lambdify functionality,
so future calls will be sped up significantly.
:param linearize_state: The state to linearize about as a 12-dimensional
vector.
:param linearize_input: The input to linearize about as a 4-dimensional
vector.
:return: A pair of matrices (A, B), where A is the state matrix and B
is the input matrix.
"""
if not self._computed_dynamics_jacobian:
x, y, z = dynamicsymbols('x y z')
phi, theta, psi = dynamicsymbols('phi theta psi')
p, q, r = dynamicsymbols('p q r')
u1, u2, u3, u4 = syp.symbols('u1 u2 u3 u4')
t = syp.symbols('t')
x_dot = syp.diff(x, t)
y_dot = syp.diff(y, t)
z_dot = syp.diff(z, t)
state_list = [
x, y, z, phi, theta, psi, x_dot, y_dot, z_dot, p, q, r
]
input_list = [u1, u2, u3, u4]
state = syp.Matrix(state_list)
input = syp.Matrix(input_list)
d = syp.Matrix([x, y, z])
d_dot = syp.diff(d, t)
w_BW = syp.Matrix([p, q, r])
eTb = syp.Matrix([[
1,
syp.sin(phi) * syp.tan(theta),
syp.cos(phi) * syp.tan(theta)
], [0, syp.cos(phi), -syp.sin(phi)],
[
0,
syp.sin(phi) / syp.cos(theta),
syp.cos(phi) / syp.cos(theta)
]])
R = syp.rot_axis3(psi) * syp.rot_axis2(theta) * syp.rot_axis1(phi)
I = syp.Matrix(self.I)
d_ddot = syp.Matrix([0, 0, -self.gravity
]) + R * syp.Matrix([0, 0, u1 / self.mass])
euler_dot = eTb * syp.Matrix([p, q, r])
w_BW_dot = syp.Matrix(
self.invI) * (-w_BW.cross(I * w_BW) + syp.Matrix([u2, u3, u4]))
dynamics = syp.Matrix([d_dot, euler_dot, d_ddot, w_BW_dot])
A_helper = syp.lambdify(state_list + input_list,
syp.simplify(dynamics.jacobian(state)))
B_helper = syp.lambdify(state_list + input_list,
syp.simplify(dynamics.jacobian(input)))
self._A = lambda state_vec, input_vec: \
A_helper(*([x for x in state_vec] + [x for x in input_vec]))
self._B = lambda state_vec, input_vec: \
B_helper(*([x for x in state_vec] + [x for x in input_vec]))
self._computed_dynamics_jacobian = True
return (self._A(linearize_state, linearize_input),
self._B(linearize_state, linearize_input))
def rotate(rxn, lmk):
Rx = sm.rot_axis1(rxn[0]).T
Ry = sm.rot_axis2(rxn[1]).T
Rz = sm.rot_axis3(rxn[2]).T
return Rz * Ry * Rx * lmk
pmtid = 11336 # "BP=junoSD_PMT_v2::ProcessHits tds" debug session, see jnu/opticks-junoenv-hit.rst
it = Instance(3) # ridx 3 is Hamamatsu PMTs
tt = it.find_instance_transform(pmtid)
tr = it.find_instance_transform_inverse(pmtid)
lhit = it.find_local_pos(pmtid, hit)
assert np.allclose(lhit0, lhit), lhit0 - lhit
zhit = lhit[2]
rhit = np.sqrt(lhit[0] * lhit[0] + lhit[1] * lhit[1])
# 249 185 axes of cathode ellipsoid for Hamamatsu (see ana/gpmt.py)
one = ((zhit * zhit) / (185. * 185.)) + (
(rhit * rhit) /
(249. * 249.)) ## hmm generic way to get major axes of cathode ellipsoid ?
Rx = three_to_four(sp.rot_axis1(alpha))
IRx = three_to_four(sp.rot_axis1(-alpha))
Ry = three_to_four(sp.rot_axis2(beta))
IRy = three_to_four(sp.rot_axis2(-beta))
Rz = three_to_four(sp.rot_axis3(gamma))
IRz = three_to_four(sp.rot_axis3(-gamma))
R0 = Rz * Ry * Rx
IR0 = IRx * IRy * IRz # NB : reversed order
assert IR0.transpose(
) == R0 # the inverse of a rotation matrix is its transpose
assert R0.transpose() == IR0
R1 = Rx * Ry * Rz
def rotx(q):
r = sm.rot_axis1(-q)
return r
import sympy as sp
import sympy_helpers as sph
from argparse import ArgumentParser
import os
import codegen
R_W_IMU = sp.rot_axis1(sp.rad(-90)) * sp.rot_axis3(sp.rad(-90))
R_W_ACC = sp.rot_axis1(sp.rad(90)) * sp.rot_axis3(sp.rad(-90))
v_IMU = sp.MatrixSymbol("v_IMU", 3, 1)
v_ACC = sp.MatrixSymbol("v_ACC", 3, 1)
v_W = sp.MatrixSymbol("v_W", 3, 1)
def main(args):
exprs = {"IMU_to_W": [sp.Eq(v_W, R_W_IMU * v_IMU)],
"ACC_to_W": [sp.Eq(v_W, R_W_ACC * v_ACC)],
"W_to_IMU": [sp.Eq(v_IMU, R_W_IMU.T * v_W)],
"W_to_ACC": [sp.Eq(v_ACC, R_W_ACC.T * v_W)]}
out = codegen.gen_code(exprs, args.prefix, "trans")
os.makedirs(args.out_dir, exist_ok=True)
for fid in out:
path = os.path.join(args.out_dir, fid[0])
with open(path, "w") as f:
f.write(fid[1])
if __name__ == "__main__":
parser = ArgumentParser()
parser.add_argument("--outdir", dest="out_dir", type=str,
from sympy import symbols
from sympy import eye
import sympy as sym
from math import pi
th, chi, phi, gamma, tth, wavevector = symbols(
'th chi phi gamma tth wavevector')
a, b, c = symbols('a b c')
U = sym.Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
Rxth = sym.rot_axis1(-th)
Rychi = sym.rot_axis2(-chi)
Rzphi = sym.rot_axis3(-phi)
Rzgamma = sym.rot_axis3(-gamma)
Rxtth = sym.rot_axis1(-tth)
Wavevector = sym.Matrix([0, wavevector, 0])
Qi = sym.eye(3) / U / Rzphi / Rychi / Rxth * (Rxtth * Rzgamma -
eye(3)) * Wavevector
QH = a / 2 / pi * Qi[0]
QK = b / 2 / pi * Qi[1]
QL = c / 2 / pi * Qi[2]
print('QH =', QH)
print('QK =', QK)
print('QL =', QL)
