def f(self, x, uu): u = np.clip(uu, -6, 6) g = x[0:16].reshape((4, 4)) R, p = TransToRp(g) omega = x[16:19] v = x[19:] twist = x[16:] # u[0] *= 0.8 F = self.kt * (u[0] + u[1] + u[2] + u[3]) M = np.array([ self.kt * self.arm_length * (u[1] - u[3]), self.kt * self.arm_length * (u[2] - u[0]), self.km * (u[0] - u[1] + u[2] - u[3]) ]) inertia_dot_omega = np.dot(self.inertia_matrix, omega) inner_rot = M + cross(inertia_dot_omega, omega) omegadot = np.dot(self.inv_inertia_matrix, inner_rot) - self.ang_damping * omega vdot = self.inv_mass * F * np.array([0., 0., 1.]) - cross( omega, v) - 9.81 * np.dot(R.T, np.array([0., 0., 1. ])) - self.vel_damping * v dg = np.dot(g, VecTose3(twist)).ravel() return np.concatenate((dg, omegadot, vdot))
def look_at(self, world_position): #%% world_position = self.get_world_position()[:3] eye = world_position target = np.array([0, 0, 0]) up = np.array([0, 1, 0]) zaxis = (target - eye) / np.linalg.norm(target - eye) xaxis = (np.cross(up, zaxis)) / np.linalg.norm(np.cross(up, zaxis)) yaxis = np.cross(zaxis, xaxis) R = np.eye(4) R = np.array([[xaxis[0], yaxis[0], zaxis[0], 0], [xaxis[1], yaxis[1], zaxis[1], 0], [xaxis[2], yaxis[2], zaxis[1], 0], [0, 0, 0, 1]]) R = np.array([[xaxis[0], xaxis[1], xaxis[2], 0], [yaxis[0], yaxis[1], yaxis[2], 0], [zaxis[0], zaxis[1], zaxis[2], 0], [0, 0, 0, 1]]) t = np.eye(4, dtype=np.float32) # translation t[:3, 3] = -eye self.R = R self.Rt = np.dot(R, t) self.t = np.eye(4, dtype=np.float32) self.t[:3, 3] = self.Rt[:3, 3]
def calc_dihedral(coords3d, dihedral_ind, cos_tol=1e-9): m, o, p, n = dihedral_ind u_dash = coords3d[m] - coords3d[o] v_dash = coords3d[n] - coords3d[p] w_dash = coords3d[p] - coords3d[o] u_norm = np.linalg.norm(u_dash) v_norm = np.linalg.norm(v_dash) w_norm = np.linalg.norm(w_dash) u = u_dash / u_norm v = v_dash / v_norm w = w_dash / w_norm phi_u = np.arccos(np.dot(u, w)) phi_v = np.arccos(-np.dot(w, v)) uxw = np.cross(u, w) vxw = np.cross(v, w) cos_dihed = np.dot(uxw, vxw) / (np.sin(phi_u) * np.sin(phi_v)) # Restrict cos_dihed to [-1, 1] if cos_dihed >= 1 - cos_tol: dihedral_rad = 0 elif cos_dihed <= -1 + cos_tol: dihedral_rad = np.arccos(-1) else: dihedral_rad = np.arccos(cos_dihed) if dihedral_rad != np.pi: # wxv = np.cross(w, v) # if wxv.dot(u) < 0: if vxw.dot(u) < 0: dihedral_rad *= -1 return dihedral_rad
def dihedrals(self): d_angles = [] for quad in self.psf.dihedrals: a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4 v1 = np.array(self.positions[a_2]) - np.array(self.positions[a_1]) v2 = np.array(self.positions[a_3]) - np.array(self.positions[a_1]) n1 = np.cross(v1, v2) v3 = np.array(self.positions[a_4]) - np.array(self.positions[a_3]) v4 = -v2 n2 = np.cross(v3, v4) prod = np.dot(n1, n2) if prod == 0: angle = 0 else: prod /= (np.linalg.norm(n1) * np.linalg.norm(n2)) prod = np.clip(prod, -1, 1) angle = np.arccos(prod) d_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type), np.degrees(angle)]) return d_angles
def look_at(self, world_position): """ :param world_position: :return: """ cam_world_position = self.get_world_position() # used to set new cam.t #self.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0)) # first we need to set new R world_position = self.get_world_position()[:3] eye = world_position target = np.array([0, 0, 0]) up = np.array([0, 1, 0]) if np.linalg.norm(target - eye) == 0: # avoid cam position in [0,0,0] zaxis = np.array([0., 0., 1.]) else: zaxis = (target - eye) / np.linalg.norm(target - eye) if zaxis[1] == -1.: # handle cam position on Y-axis xaxis = np.array([-1., 0., 0.]) else: xaxis = (np.cross(up, zaxis)) / np.linalg.norm(np.cross(up, zaxis)) yaxis = np.cross(zaxis, xaxis) R = np.eye(4) R = np.array([[xaxis[0], yaxis[0], zaxis[0], 0], [xaxis[1], yaxis[1], zaxis[1], 0], [xaxis[2], yaxis[2], zaxis[2], 0], [0, 0, 0, 1]]) # R = np.array([[xaxis[0], xaxis[1], xaxis[2], 0], # [yaxis[0], yaxis[1], yaxis[2], 0], # [zaxis[0], zaxis[1], zaxis[2], 0], # [ 0, 0, 0, 1]]) R = R.T # R is rotation matrix from camera to world coordinate not from world to camera coordinate!!! self.R = R self.set_t(cam_world_position[0], cam_world_position[1], cam_world_position[2], 'world') # !!! set new t
def getHOD_basic(self, a_norm, z_body, j_des, s_des): a_norm_dot = j_des.dot(z_body) z_body_dot = (j_des - a_norm_dot * z_body) / a_norm theta_vel = np.cross(z_body, z_body_dot) a_norm_ddot = s_des.dot(z_body) + j_des.dot(z_body_dot) z_body_ddot = (s_des - a_norm_ddot * z_body - 2 * a_norm_dot * z_body_dot) / a_norm theta_acc = np.cross(z_body, z_body_ddot) return theta_vel, theta_acc
def dVelocityVectordh(self, x, mu): """ derivatives of Velocity vector wrt angular momentum vector typed """ TA = x[5] sTA = np.sin(TA) cTA = np.cos(TA) h = self.hVector(x) e = self.eVector(x, mu) eCross = self.mathUtil.crossmat(e) hMag = np.linalg.norm(h) eMag = np.linalg.norm(e) eUnit = e / eMag crossProduct = np.cross(h, e) crossProductMag = np.linalg.norm(crossProduct) factor1 = 1.0 / hMag**3 term1 = factor1 * (-np.outer( sTA * eUnit - ((eMag + cTA) / crossProductMag) * crossProduct, h)) factor2 = (-(eMag + cTA)) / (hMag * crossProductMag) term2 = factor2 * (-eCross + (1.0 / crossProductMag**2) * np.dot( np.outer(crossProduct, crossProduct), eCross)) dvdh = -mu * (term1 + term2) return dvdh
def f_scp(self, s, a): # input: # s, nd array, (n,) # a, nd array, (m,1) # output # dsdt, nd array, (n,1) dsdt = agnp.zeros(self.n) q = s[6:10] omega = s[10:] # get input a = agnp.reshape(a, (self.m, )) eta = agnp.dot(self.B0, a) f_u = agnp.array([0, 0, eta[0]]) tau_u = agnp.array([eta[1], eta[2], eta[3]]) # dynamics # dot{p} = v dpdt = s[3:6] # mv = mg + R f_u dvdt = self.g + qrotate(q, f_u) / self.mass # dot{R} = R S(w) # to integrate the dynamics, see # https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and # https://arxiv.org/pdf/1604.08139.pdf qnew = qintegrate(q, omega, self.ave_dt) qnew = qnormalize(qnew) # transform qnew to a "delta q" that works with the usual euler integration dqdt = (qnew - q) / self.ave_dt # mJ = Jw x w + tau_u dwdt = self.inv_J * (agnp.cross(self.J * omega, omega) + tau_u) return agnp.concatenate((dpdt, dvdt, dqdt, dwdt))
def solve(self, angles0, target): joint_indexes = list(reversed(self._fk_solver.joint_indexes)) angles = angles0[:] pmap = { 'x': [1., 0., 0., 0.], 'y': [0., 1., 0., 0.], 'z': [0., 0., 1., 0.] } prev_dist = sys.float_info.max for _ in range(self.maxiter): for i, idx in enumerate(joint_indexes): axis = self._fk_solver.solve( angles, p=pmap[self._fk_solver.components[idx].axis], index=idx) pj = self._fk_solver.solve(angles, index=idx) ee = self._fk_solver.solve(angles) eorp = self.p_on_rot_plane(ee, pj, axis) torp = self.p_on_rot_plane(target, pj, axis) ve = (eorp - pj) / np.linalg.norm(eorp - pj) vt = (torp - pj) / np.linalg.norm(torp - pj) a = np.arccos(np.dot(vt, ve)) sign = 1 if np.dot(axis, np.cross(ve, vt)) > 0 else -1 angles[-(i + 1)] += (a * sign) ee = self._fk_solver.solve(angles) dist = np.linalg.norm(target - ee) delta = prev_dist - dist if delta < self.tol: break prev_dist = dist return angles
def tVector_derivs(self, x): """ derivatives of the T unit vector typed """ # choose the reference vector here k = self.mathUtil.k_unit() referenceVector = k dreferenceVectordx = np.zeros( (3, 6)) # for k reference vector (or any constant reference vector) s = self.sVector(x) crossProduct = np.cross(s, referenceVector) crossProductMag = np.linalg.norm(crossProduct) dcrossProductds, dcrossProductdreferenceVector = self.mathUtil.d_crossproduct( s, referenceVector) dsdx = self.sVector_derivs(x) dtdxNotUnit = np.dot(dcrossProductds, dsdx) + np.dot( dcrossProductdreferenceVector, dreferenceVectordx) dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot( crossProduct, dtdxNotUnit) dtdx = (1.0 / crossProductMag) * dtdxNotUnit + np.outer( crossProduct, dOneByCrossProductMagdx) return dtdx
def hVector(self, r, v): """ angular momentum vector """ h = np.cross(r, v) return h
def rVector_derivs(self, x): """ bplane R unit vector derivatives typed """ s = self.sVector(x) t = self.tVector(x) crossProduct = np.cross(s, t) crossProductMag = np.linalg.norm(crossProduct) dcrossProductds, dcrossProductdt = self.mathUtil.d_crossproduct(s, t) dsdx = self.sVector_derivs(x) dtdx = self.tVector_derivs(x) dRdxNotUnit = np.dot(dcrossProductds, dsdx) + np.dot( dcrossProductdt, dtdx) dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot( crossProduct, dRdxNotUnit) dRdx = (1.0 / crossProductMag) * dRdxNotUnit + np.outer( crossProduct, dOneByCrossProductMagdx) #term1 = (1.0 / crossProductMag) * dRdxNotUnit #term2 = np.outer(crossProduct, dOneByCrossProductMagdx) #print('***') #print('dRdx') #print('crossProduct: ', crossProduct) #print('dRdxNotUnit column1: ', dRdxNotUnit[0:3,1]) #print(crossProduct[0]*dRdxNotUnit[0,1], crossProduct[1]*dRdxNotUnit[1,1]) #print(crossProduct[2], dOneByCrossProductMagdx[1]) #print(dRdxNotUnit[2,1], term2[2,1]) #print('***') return dRdx
def dVelocityVectorde(self, x, mu): """ derivatives of Velocity vector wrt eccentricity vector typed """ TA = x[5] sTA = np.sin(TA) cTA = np.cos(TA) h = self.hVector(x) e = self.eVector(x, mu) hCross = self.mathUtil.crossmat(h) hMag = np.linalg.norm(h) eMag = np.linalg.norm(e) crossProduct = np.cross(h, e) crossProductMag = np.linalg.norm(crossProduct) term1 = sTA * (1.0 / eMag) * (np.identity(3) - (1.0 / eMag**2) * np.outer(e, e)) term21 = np.outer((1.0 / crossProductMag) * crossProduct, (1.0 / eMag) * e) term22 = (eMag + cTA) * (1.0 / crossProductMag) * ( hCross - (1.0 / crossProductMag**2) * np.dot(np.outer(crossProduct, crossProduct), hCross)) term2 = -(term21 + term22) factor = -mu / hMag dvde = factor * (term1 + term2) return dvde
def dPositionVectordh(self, x, mu): """ derivatives of position vector wrt angular momentum vector typed """ TA = x[5] cTA = np.cos(TA) sTA = np.sin(TA) h = self.hVector(x) e = self.eVector(x, mu) hMag = self.hMag(x) eMag = self.eMag(x, mu) eUnit = e / eMag crossProduct = np.cross(h, e) crossProductMag = np.linalg.norm(crossProduct) factor = 1.0 / (mu * (1.0 + eMag * cTA)) term1 = cTA * np.outer(eUnit, 2.0 * h) #term2 = sTA * np.dot((1.0 / crossProductMag) * (np.identity(3) - (1.0 / crossProductMag**2) * np.outer(crossProduct, crossProduct)), -self.mathUtil.crossmat(e)) term2 = (sTA / crossProductMag) * ( 2. * np.outer(crossProduct, h) + hMag**2 * np.dot( (-np.identity(3) + (1. / crossProductMag**2) * np.outer( crossProduct, crossProduct)), self.mathUtil.crossmat(e))) drdh = factor * (term1 + term2) return drdh
def rVectorNotUnit(self, r, v, mu): """ bplane R vector """ s = self.sVector(r, v, mu) t = self.tVector(r, v, mu) R = np.cross(s, t) return R
def nVector(self, r, v, mu): """ not the node vector: h cross e; i think battin calls it p """ h = self.hVector(r, v) e = self.eVector(r, v, mu) n = np.cross(h, e) return n
def rVectorNotUnit(self, x): """ bplane R vector """ s = self.sVector(x) t = self.tVector(x) R = np.cross(s, t) return R
def hUnit(self, x): """ angular momentum unit vector """ b = self.bVector(x) s = self.sVector(x) hUnit = np.cross(b, s) hUnit = hUnit / np.linalg.norm(hUnit) return hUnit
def get_neighs(vertex, r): verts = list() # level_0 verts.append(vertex) v = vertex # find closest point in level 1 if sparse.issparse(adj_mtx): nz = adj_mtx.tolil().rows ix_list = nz[v] else: row = adj_mtx[v] ix_list = np.nonzero(row) ix_list = ix_list[0] dists = [] for j in ix_list: d = get_dist(coords, v, j) dists.append(d) ix_min = ix_list[dists.index(min(dists))] closest_ix = ix_min # levels_>=1 for i in range(1, r + 1): # this is the closest vertex of the new level # find the ordering of the level arr = get_order(adj_mtx, coords, ix_list, closest_ix, verts) verts = verts + arr # get next level: for each in ix_list, get neighbors that are not in <verts>, then add them to the new list next_list = [] for j in ix_list: if sparse.issparse(adj_mtx): new_row = nz[j] else: new_row = adj_mtx[j] new_row = np.nonzero(new_row) new_row = new_row[0] for k in new_row: if k not in verts: next_list.append(k) next_list = list(set(next_list)) # find starting point of next level using line eq c1 = coords[vertex] c2 = coords[closest_ix] line_dists = [] for j in next_list: c3 = coords[j] line_dist = LA.norm(np.cross(c2 - c1, c1 - c3)) / LA.norm( c2 - c1) # calculate distance to line line_dists.append(line_dist) ix_list = next_list closest_ix = next_list[line_dists.index(min(line_dists))] return verts
def tVectorNotUnit(self, x): """ T vector, not necessarily as a unit vector """ s = self.sVector(x) k = self.mathUtil.k_unit() #h = self.hVector(x) referenceVector = k t = np.cross(s, referenceVector) return t
def calculate_normal(self, tri_ind_info, centroid, q): # normal_xyz = np.zeros((h, w, 3)) # normal_sph = np.zeros((h, w, 2)) normal_xyz = {} normal_sph = {} for i in range(self.h): for j in range(self.w): normal_xyz[(i,j)] = 0 normal_sph[(i,j)] = 0 tri_ver = q[self.tri_mesh_data[tri_ind_info[i, j]-1, :],:] a = tri_ver[0,:] b = tri_ver[1,:] c = tri_ver[2,:] normal_xyz[(i,j)] = np.cross(a-b, b-c)/np.linalg.norm(np.cross(a-b, b-c)) if np.dot(np.mean(tri_ver, 0)-centroid, normal_xyz[(i,j)])<0: normal_xyz[(i,j)] *= -1 normal_sph[(i,j)] = self.cart2sph(normal_xyz[(i,j)]) return normal_sph
def tVectorNotUnit(self, r, v, mu): """ t vector """ # reference vector is chosen here k = self.mathUtil.k_unit() referenceVector = k h = self.hVector(r, v) s = self.sVector(r, v, mu) t = np.cross(s, referenceVector) return t
def syncRot(T): ''' returns the rotation matrix of the approximation of the projector created by proj_deformable_approx :param T : the motion matrix calculated in PoseFromKpts_WP :return : [R,C] the rotation matrix and the values of a sorte of norm that is do not really understand ''' [_, L, Q] = proj_deformable_approx(np.transpose(T)) s = np.sign(L[0]) C = s * L[0] R = np.zeros((3, 3)) R[0:2, 0:3] = s * np.transpose(Q) R[2, 0:3] = np.cross(Q[0:3, 0], Q[0:3, 1]) return R, C
def dPositionVectorde(self, x, mu): """ derivatives of position vector wrt eccentricity vector typed """ TA = x[5] cTA = np.cos(TA) sTA = np.sin(TA) h = self.hVector(x) e = self.eVector(x, mu) hMag = self.hMag(x) eMag = self.eMag(x, mu) eUnit = e / eMag crossProduct = np.cross(h, e) crossProductMag = np.linalg.norm(crossProduct) crossProductUnit = crossProduct / crossProductMag factor = hMag**2 / mu #term1_old = np.outer((-cTA / ((1.0 + eMag * cTA)**2)) * eUnit, eUnit * cTA + crossProduct / crossProductMag * sTA) term1 = (-cTA / (1. + eMag * cTA)**2) * np.outer( cTA * eUnit + sTA * crossProductUnit, eUnit) factor2 = 1.0 / (1.0 + eMag * cTA) #term2_old = factor2 * ((cTA / eMag) * (np.identity(3) - (1.0 / eMag**2) * np.outer(e, e)) + (sTA / crossProductMag) * np.dot((np.identity(3) - (1.0 / crossProductMag**2) * np.outer(crossProduct, crossProduct)), self.mathUtil.crossmat(h))) term2 = factor2 * ( (cTA / eMag) * (np.identity(3) - (1. / eMag**2) * np.outer(e, e)) + (sTA / crossProductMag) * np.dot( (np.identity(3) - (1. / crossProductMag**2) * np.outer( crossProduct, crossProduct)), self.mathUtil.crossmat(h))) #term2 = term2_old # print(term1_old) # print("\n") # print(term1) # print("\n") # print("\n") # print("\n") # print("\n") # print(term2_old) # print("\n") # print(term2) # print("\n") # print(term3) # print("\n") drde = factor * (term1 + term2) return drde
def compute_rotation_velocity_geometry_axes(self, points): # Computes the effective velocity due to rotation at a set of points. # Input: a Nx3 array of points # Output: a Nx3 array of effective velocities angular_velocity_vector_geometry_axes = np.array( [-self.p, self.q, -self.r]) # signs convert from body axes to geometry axes angular_velocity_vector_geometry_axes = np.expand_dims( angular_velocity_vector_geometry_axes, axis=0) rotation_velocity_geometry_axes = np.cross( angular_velocity_vector_geometry_axes, points, axis=1) rotation_velocity_geometry_axes = -rotation_velocity_geometry_axes # negative sign, since we care about the velocity the WING SEES, not the velocity of the wing. return rotation_velocity_geometry_axes
def initial_state(Xa, Xg): """ estimate initial orientation and biases given a series of measurements during which the IMU was stationary """ g = 9.81 bg = np.mean(Xg, axis=0) a = np.mean(Xa, axis=0) na = np.linalg.norm(a) sa = g / na a /= na rot = np.cross(np.array([0, 0, -1.0]), a) # print 'accel scale', sa # print 'gyro bias', bg # print 'initial orientation', rot # print np.dot(so3.exp(rot), np.array([0, 0, -1.0])), a return sa, bg, rot
def initial_state(Xa, Xg): """ estimate initial orientation and biases given a series of measurements during which the IMU was stationary """ g = 9.81 bg = np.mean(Xg, axis=0) a = np.mean(Xa, axis=0) na = np.linalg.norm(a) sa = g / na a /= na rot = np.cross(np.array([0, 0, -1.0]), a) # print 'accel scale', sa # print 'gyro bias', bg # print 'initial orientation', rot # print np.dot(so3.exp(rot), np.array([0, 0, -1.0])), a return sa, bg, rot
def hUnit_derivs(self, x): """ derivatives of angular momentum unit vector typed """ b = self.bVector(x) s = self.sVector(x) crossProduct = np.cross(b, s) crossProductMag = np.linalg.norm(crossProduct) dcrossProductdb, dcrossProductds = self.mathUtil.d_crossproduct(b, s) dbdx = self.bVector_derivs(x) dsdx = self.sVector_derivs(x) dhdxNotUnit = np.dot(dcrossProductdb, dbdx) + np.dot( dcrossProductds, dsdx) dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot( crossProduct, dhdxNotUnit) dhUnitdx = (1.0 / crossProductMag) * dhdxNotUnit + np.outer( crossProduct, dOneByCrossProductMagdx) return dhUnitdx
def get_ceq(x): n = (x.shape[0] - 1) // 4 tk, xk, yk, thetak = split_state(x) xk = np.concatenate(([start_x], xk, [goal_x])) yk = np.concatenate(([start_y], yk, [goal_y])) thetak = np.concatenate(([start_theta], thetak, [goal_theta])) dx = xk[1:] - xk[0:-1] dy = yk[1:] - yk[0:-1] dk = np.array([dx, dy, np.zeros(n + 1)]).T # constant curvature (trapezoidal collocation, see teb paper) ceq = np.array([ np.cos(thetak[0:-1]) + np.cos(thetak[1:]), np.sin(thetak[0:-1]) + np.sin(thetak[1:]), np.zeros(n + 1) ]).T ceq = np.fabs(np.cross(ceq, dk, axisa=1, axisb=1)[:, 2]) return ceq
def impropers(self): i_angles = [] for quad in self.psf.impropers: # a_1 bonded to a_2, a_1 is central atom a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4 v1 = np.array(self.positions[a_3]) - np.array(self.positions[a_1]) v2 = np.array(self.positions[a_4]) - np.array(self.positions[a_1]) n = np.cross(v1, v2) v3 = np.array(self.positions[a_2]) - np.array(self.positions[a_1]) prod = np.dot(n, v3) if prod == 0: angle = np.pi / 2 else: prod /= (np.linalg.norm(n) * np.linalg.norm(v3)) prod = np.clip(prod, -1, 1) angle = np.pi / 2 - np.arccos(prod) i_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type), np.degrees(angle)]) return i_angles
def f(self, s, a): # input: # s, nd array, (n,) # a, nd array, (m,1) # output # dsdt, nd array, (n,1) dsdt = np.zeros(self.n) q = s[6:10] omega = s[10:] # get input a = np.reshape(a, (self.m, )) eta = np.dot(self.B0, a) f_u = np.array([0, 0, eta[0]]) tau_u = np.array([eta[1], eta[2], eta[3]]) # dynamics # dot{p} = v dsdt[0:3] = s[3:6] # mv = mg + R f_u dsdt[3:6] = self.g + rowan.rotate(q, f_u) / self.mass # dot{R} = R S(w) # to integrate the dynamics, see # https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and # https://arxiv.org/pdf/1604.08139.pdf qnew = rowan.calculus.integrate(q, omega, self.ave_dt) qnew = rowan.normalize(qnew) if qnew[0] < 0: qnew = -qnew # transform qnew to a "delta q" that works with the usual euler integration dsdt[6:10] = (qnew - q) / self.ave_dt # mJ = Jw x w + tau_u dsdt[10:] = self.inv_J * (np.cross(self.J * omega, omega) + tau_u) return dsdt.reshape((len(dsdt), 1))
def __init__(self, NSIDE, npix, clv=True): """ Args: NSIDE (int) : the healpix NSIDE parameter, must be a power of 2, less than 2**30 npix (int) : number of pixel in the X and Y axis of the final projected map rot_velocity (float) : rotation velocity of the star in the equator in km/s Returns: None """ self.NSIDE = int(NSIDE) self.npix = int(npix) self.hp_npix = hp.nside2npix(NSIDE) # self.rot_velocity = rot_velocity self.clv = clv # Generate the indices of all healpix pixels self.indices = np.arange(hp.nside2npix(NSIDE), dtype='int') self.n_healpix_pxl = len(self.indices) # Define the orthographic projector that generates the maps of the star on the plane of the sky self.projector = hp.projector.OrthographicProj(xsize=int(self.npix)) # This function returns the pixel associated with a vector (x,y,z). This is needed by the projector self.f_vec2pix = lambda x, y, z: hp.pixelfunc.vec2pix(int(self.NSIDE), x, y, z) # Generate a mesh grid of X and Y points in the plane of the sky that covers only the observed hemisphere of the star x = np.linspace(-2.0,0.0,int(self.npix/2)) y = np.linspace(-1.0,1.0,int(self.npix/2)) X, Y = np.meshgrid(x,y) # Rotational velocity vector (pointing in the z direction and unit vector) omega = np.array([0,0,1]) # Compute the radial vector at each position in the map and the projected velocity on the plane of the sky radial_vector = np.array(self.projector.xy2vec(X.flatten(), Y.flatten())).reshape((3,int(self.npix/2),int(self.npix/2))) self.vel_projection = np.cross(omega[:,None,None], radial_vector, axisa=0, axisb=0)[:,:,0] # Compute the mu angle (astrocentric angle) self.mu_angle = radial_vector[0,:,:] # Read all Kurucz models from the database. Hardwired temperature and mu angles print("Reading Kurucz spectra...") self.T = 3500 + 250 * np.arange(27) self.mus = np.array([1.0,0.9,0.8,0.7,0.6,0.5,0.4,0.3,0.2,0.1,0.05,0.02])[::-1] for i in tqdm(range(27)): f = 'kurucz_models/RESULTS/T_{0:d}_logg4.0_feh0.0.spec'.format(self.T[i]) vel, _, spec = _read_kurucz_spec(f) if (i == 0): self.nlambda, self.nmus = spec.shape self.velocity = np.zeros((self.nlambda)) self.spectrum = np.zeros((27,self.nmus,self.nlambda)) self.velocity = vel self.spectrum[i,:,:] = spec[:,::-1].T # Generate a fake temperature map in the star using spherical harmonics # self.temperature_map = 5000 * np.ones(self.npix) # self.temperature_map = 5000 + 250 * hp.sphtfunc.alm2map(np.ones(10,dtype='complex'),self.NSIDE) #np.random.rand(self.n_healpix_pxl) * 2000 + 5000 # self.temperature_map = 5000 * np.ones(self.hp_npix) self.coeffs = hp.sphtfunc.map2alm(self.temperature_map) self.velocity_per_pxl = self.velocity[1] - self.velocity[0] self.freq_grid = np.fft.fftfreq(self.nlambda) self.gradient = value_and_grad(self.loss)