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 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 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)