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 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 R_mat(model):
"""
Rotation matrix according to tilt and asymut.
Parameters
----------
model: Model
Model structure
"""
# Rotation matrix
Rt = rot_axis2(model.theta)
if model.dim == 3:
Rt *= rot_axis3(model.phi)
else:
Rt = Rt[[0, 2], [0, 2]]
return TensorFunction(name="R", grid=model.grid, components=Rt, symmetric=False)
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 rotation_transform(input_field,
output_field,
rotation_angle,
rotation_axis=None,
interpolation_mode='linear'):
if isinstance(rotation_angle, pystencils.Field):
rotation_angle = rotation_angle.center
if input_field.spatial_dimensions == 3:
assert rotation_axis is not None, "You must specify a rotation_axis for 3d rotations!"
transform_matrix = getattr(sympy, 'rot_axis%i' %
(rotation_axis + 1))(rotation_angle)
elif input_field.spatial_dimensions == 2:
# 2d rotation is 3d rotation around 3rd axis
transform_matrix = sympy.rot_axis3(rotation_angle)[:2, :2]
else:
raise NotImplementedError('Rotations only implemented for 2d and 3d')
return generic_spatial_matrix_transform(
input_field,
output_field,
transform_matrix,
inverse_matrix=transform_matrix.T,
interpolation_mode=interpolation_mode)
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)
def R_z(self, angle):
H = sym.eye(4)
H[:3, :3] = sym.rot_axis3(-angle)
return HomogenousTransform(self.E * H)
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,
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))
import ipdb
logging.basicConfig(level=logging.CRITICAL,
format='%(levelname)s: %(message)s')
import sympy as sp
import numpy as np
l_x, l_y, a, a_i, θ_O, θ_Oi, θ_Of, t, t_i, t_f = sp.symbols(
'l_x, l_y, a, a_i, omega_O, θ_Oi, θ_Of, t, t_i, t_f')
"""Constraint Equation of Circle offset to Pt_0 AKA t_i """
CIRC = sp.Matrix(
[a * (sp.cos(t) - sp.cos(t_i)), a * (sp.sin(t) - sp.sin(t_i)), 1])
CIRC
"""Rotation around P_0 AKA t_i"""
# Convention is for z-, not z+ for CCW--> so '-θ_O'
CIRC_ROT1 = sp.rot_axis3(-θ_O) * (CIRC)
CIRC_ROT1
"""
Translate back to circle origin
based off original circle radius
"""
CIRC_ROT1_TRAN1 = sp.Matrix([[1, 0, a_i * sp.cos(t_i)],
[0, 1, a_i * sp.sin(t_i)], [0, 0, 1]]) * CIRC_ROT1
CIRC_ROT1_TRAN1
""" "a" as a function of θ_O """
# ri = idler radius
# a = radius of circle
# n = number of idlers
# θi = angle between idlers
a_f, ri, θi, n = sp.symbols('a_f, ri, θi, n')
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
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
IR1 = IRz * IRy * IRx
assert IR1.transpose() == R1
assert R1.transpose() == IR1
# row3 as translation is used for simpler matching with glm/OpenGL standard practice when serializing transforms
# (col3 as translation is the other possibility)
def get_trellipse(fname: str = TRELLIPSE_FNAME):
data = pickle.load(open(fname, 'rb'))
return tp.cast(
tp.Tuple[_trellipse_callable, _trellipse_callable,
_trellipse_callable],
tuple(
map(partial(sym.lambdify, data['args']),
itemgetter('ow', 'oh', 'oa')(data))))
if __name__ == '__main__' and True:
w, h, t = sym.symbols('w, h, t', positive=True)
theta = sym.Symbol('theta')
TR = sym.Matrix(((1, 0), (0, t))) @ sym.rot_axis3(theta)[:2, :2]
U, L = (TR @ sym.Matrix(
((w**2, 0), (0, h**2))) @ TR.T).diagonalize(normalize=True)
# use covariance matrix as input
# ------------------------------
a, b = sym.symbols('a, b', positive=True)
c = sym.Symbol('c')
T = sym.Matrix(((1, 0), (0, t)))
U, L = (T @ sym.Matrix(((a, c), (c, b))) @ T.T).diagonalize(normalize=True)
# args = (w, h, theta, t)
ow, oh = sym.sqrt(L[0, 0]), sym.sqrt(L[1, 1]) / t
oa = sym.atan2(sym.sign(theta) * U[1, 1], U[1, 0])
#
# pickle.dump({'args': args, 'ow': ow, 'oh': oh, 'oa': oa},
def rotz(q):
r = sm.rot_axis3(-q)
return r
