def make_calibration_vector(self): head_ori = qa.norm(self.store_head_ori.mean(axis=0)) Ni = self.store_head_pos.mean(axis=0) self.Qi = qa.norm(self.store_glasses_ori.mean(axis=0)) Ti = self.store_glasses_pos.mean(axis=0) self.tp = TP(self.Qi, Ni, Ti) if ((np.any(np.isnan(head_ori))) == True or (np.any(np.isnan(Ni))) == True or (np.any(np.isnan(self.Qi))) == True or (np.any(np.isnan(Ti))) == True): sys.stdout.write("!!!!!!!! Calibration complete! !!!!!!!!!!!\n") self.user_start_calibrate() return msg_str_pos = "%.5e, " * 3 msg_str_ori = "%.5e, " * 4 sys.stdout.write('Cz orientation: ') sys.stdout.write(msg_str_ori % (head_ori[0], head_ori[1], head_ori[2],\ head_ori[3]) + "\n") sys.stdout.write('Cz position: ') sys.stdout.write(msg_str_pos % (Ni[0], Ni[1], Ni[2]) + "\n") sys.stdout.write('Glasses orientation: ') sys.stdout.write(msg_str_ori % (self.Qi[0], self.Qi[1], self.Qi[2], self.Qi[3]) + "\n") sys.stdout.write('Glasses position: ') sys.stdout.write(msg_str_pos % (Ti[0], Ti[1], Ti[2]) + "\n") sys.stdout.write("********** Calibration complete! ***********\n") sys.stdout.flush() self.calibrated = True self.got_coil = False self.got_head = False
def test_slerp(self): q = qarray.norm(np.array([[2., 3, 4, 5], [6, 7, 8, 9]])) time = [0, 9] targettime = [0, 3, 4.5, 9] q_interp = qarray.slerp( targettime, time, q) self.assertEquals(len(q_interp), 4) np.testing.assert_array_almost_equal(q_interp[0], q[0]) np.testing.assert_array_almost_equal(q_interp[-1], q[-1]) np.testing.assert_array_almost_equal(q_interp[1], qarray.norm(q[0] * 2/3 + q[1]/3),decimal=4) np.testing.assert_array_almost_equal(q_interp[2], qarray.norm((q[0] + q[1])/2),decimal=4)
def load_config(self, config_file): cfg = PyFileConfigLoader(config_file) cfg.load_config() self.config = cfg.config # special casing for SAMPLE_GENERATED if (self.config.trigger == 'SAMPLE_GENERATED'): self.config.trigger_msg = rc.MT_SAMPLE_GENERATED self.config.trigger_mdf = rc.MDF_SAMPLE_GENERATED else: self.config.trigger_msg = \ eval('rc.MT_' + self.config.trigger) self.config.trigger_mdf = \ eval('rc.MDF_' + self.config.trigger) print "Triggering with", self.config.trigger print "CoilPlotter: loading config" #Getting tool message number self.plate = self.config.tools.index('CB609') self.marker = self.config.tools.index('CT315') self.glasses = self.config.tools.index('ST568') self.pointer = self.config.tools.index('P717') #Get tool P717 calibration values for pointer calculation self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti) self.pointer_Qi = qa.norm(np.array(self.config.tool_list[self.pointer].Qi)) self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni) self.pointer_tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def quaternion_mean(q): average = np.array([0, 0, 0, 0]) for i in q: if check_same(q, i) == False: i = change_sign(i) average = average + i return qa.norm(average / q.shape[0])
def process_message(self, msg): # read a Dragonfly message msg_type = msg.GetHeader().msg_type dest_mod_id = msg.GetHeader().dest_mod_id if msg_type == MT_EXIT: if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()): print 'Received MT_EXIT, disconnecting...' self.mod.SendSignal(rc.MT_EXIT_ACK) self.mod.DisconnectFromMMM() return elif msg_type == rc.MT_PING: respond_to_ping(self.mod, msg, 'PlotHead') elif msg_type == rc.MT_POLARIS_POSITION: in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, msg) positions = np.asarray(in_mdf.xyz[:]) orientations = self.shuffle_q(np.asarray(in_mdf.ori[:])) if in_mdf.tool_id == (self.pointer + 1): Qf = qa.norm(orientations) Qr = qa.mult(Qf, qa.inv(self.pointer_Qi)).flatten() #find_nans(self.store_head, Qr, 'Qr') Tk = positions #find_nans(self.store_head, Tk, 'Tk') tip_pos = (qa.rotate(Qr, self.pointer_Xi) + Tk).flatten() self.pointer_position = np.append(self.pointer_position, (tip_pos[np.newaxis, :]), axis=0) #self.pl.reset(x=self.pointer_position[:,0], y=self.pointer_position[:,1], z=self.pointer_position[:,2]) print("old=", tip_pos) print("new=", self.tp.get_pos(orientations, positions)[0])
def get_4piconv(self, ch, theta, phi, psi, horn_pointing=False): l.info('Computing dipole temperature with 4pi convolver') vel = qarray.amplitude(self.satellite_v).flatten() beta = vel / physcon.c gamma = 1./np.sqrt(1-beta**2) unit_vel = self.satellite_v/vel[:,None] if horn_pointing: # psi comes from the S channel, so there is no need # to remove psi_pol psi_nopol = psi else: # remove psi_pol psi_nopol = psi - np.radians(ch.get_instrument_db_field("psi_pol")) # rotate vel to ecliptic # phi around z #ecl_rotation = qarray.rotation([0,0,1], -phi) # theta around y ecl_rotation = qarray.norm( qarray.mult( qarray.rotation([0,0,1], -psi_nopol) , qarray.mult( qarray.rotation([0,1,0], -theta) , qarray.rotation([0,0,1], -phi) ) )) # psi around z #ecl_rotation = qarray.mult(qarray.rotation([0,0,1], -psi) , ecl_rotation) # vel in beam ref frame vel_rad = qarray.rotate(ecl_rotation, unit_vel) cosdir = qarray.arraylist_dot(vel_rad, self.beam_sum[ch.tag]).flatten() #return beta * cosdir * T_CMB return (1. / ( gamma * (1 - beta * cosdir ) ) - 1) * T_CMB
def compute_psi_dx8(self, theta, phi, rad): z = np.dot(self.siam.get(rad),[0, 0, 1]) vecz = qarray.norm(qarray.rotate(self.qsatgal_interp, z)) e_phi = np.hstack([-np.sin(phi)[:,np.newaxis], np.cos(phi)[:,np.newaxis], np.zeros([len(phi),1])]) e_theta = np.hstack([(np.cos(theta)*np.cos(phi))[:,np.newaxis], (np.cos(theta)*np.sin(phi))[:,np.newaxis], -np.sin(theta)[:,np.newaxis]]) psi = np.arctan2(-qarray.arraylist_dot(vecz, e_phi), -qarray.arraylist_dot(vecz, e_theta)) return psi.flatten()
def process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.array(in_mdf.xyz[:]) orientations = self.shuffle_q(np.array(in_mdf.ori[:])) # np.testing.assert_array_equal(positions[:,0], orientations[:,0], err_msg='Samples are not aligned') if self.calibrated: if in_mdf.tool_id == (self.marker + 1): # calculating output self.Qk = qa.norm( orientations ) # need to find a way to discriminate the tools files in the messages??? Qr = qa.mult(self.Qk, qa.inv(self.Qi)).flatten() Tk = positions hotspot_position = (qa.rotate(Qr, self.Xi) + Tk).flatten() hotspot_vector_head = qa.rotate(Qr, plate_vector) if np.any(np.isnan(hotspot_position)) == True: print "x", # print ' *****nan present, check coil is within frame!*****' # creating output message out_mdf = rc.MDF_HOTSPOT_POSITION() out_mdf.xyz[:] = hotspot_position out_mdf.ori[:3] = hotspot_vector_head # Qk - coil active orientation out_mdf.sample_header = in_mdf.sample_header msg = CMessage(rc.MT_HOTSPOT_POSITION) copy_to_msg(out_mdf, msg) self.mod.SendMessage(msg) sys.stdout.write("o") else: if np.any(np.isnan(positions)) == True: raise Exception, "nan present" if np.any(np.isnan(orientations)) == True: raise Exception, "nan present" if ( (self.store_plate >= self.store_plate_pos.shape[0]) & (self.store_plate >= self.store_plate_ori.shape[0]) & (self.store_coil >= self.store_coil_pos.shape[0]) & (self.store_coil >= self.store_coil_ori.shape[0]) ): self.calibrating = False self.make_calibration_vector() elif in_mdf.tool_id == (self.marker + 1): self.store_coil_pos[self.store_coil, :] = positions self.store_coil_ori[self.store_coil, :] = orientations self.store_coil += 1 elif in_mdf.tool_id == (self.plate + 1): self.store_plate_pos[self.store_plate, :] = positions self.store_plate_ori[self.store_plate, :] = orientations self.store_plate += 1
def process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type #print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing))) if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) if in_mdf.tool_id == (self.glasses + 1): positions = np.array(in_mdf.xyz[:]) orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) self.find_pos_to_glasses(positions, orientations)
def make_calibration_vector(self): plate_ori = qa.norm(self.store_plate_ori.mean(axis=0)) Ni = self.store_plate_pos.mean(axis=0) self.Qi = qa.norm(self.store_coil_ori.mean(axis=0)) Ti = self.store_coil_pos.mean(axis=0) msg_str_pos = "%.5e, " * 3 msg_str_ori = "%.5e, " * 4 sys.stdout.write("Plate orientation: ") sys.stdout.write(msg_str_ori % (plate_ori[0], plate_ori[1], plate_ori[2], plate_ori[3]) + "\n") sys.stdout.write("Plate position: ") sys.stdout.write(msg_str_pos % (Ni[0], Ni[1], Ni[2]) + "\n") sys.stdout.write("Coil orientation: ") sys.stdout.write(msg_str_ori % (self.Qi[0], self.Qi[1], self.Qi[2], self.Qi[3]) + "\n") sys.stdout.write("Coil position: ") sys.stdout.write(msg_str_pos % (Ti[0], Ti[1], Ti[2]) + "\n") self.Xi = Ni - Ti sys.stdout.write("Vector: ") sys.stdout.write(msg_str_pos % (self.Xi[0], self.Xi[1], self.Xi[2]) + "\n") sys.stdout.write("********** Calibration complete! ***********\n") sys.stdout.flush() self.calibrated = True
def process_message(self, in_msg): # read a Dragonfly message msg_type = in_msg.GetHeader().msg_type if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.array(in_mdf.xyz[:]) orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) if in_mdf.tool_id == (self.pointer + 1): pointer_pos, Qr = self.pointer_tp.get_pos( orientations, positions) print pointer_pos
def load_config(self, config_file): cfg = PyFileConfigLoader(config_file) cfg.load_config() self.config = cfg.config print "HotspotLocator: loading config" #self.ntools = len(self.config.tool_list) self.plate = self.config.tools.index('CB609') self.marker = self.config.tools.index('CT315') self.glasses = self.config.tools.index('ST568') self.pointer = self.config.tools.index('P717') self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti) self.pointer_Qi = qa.norm( np.array(self.config.tool_list[self.pointer].Qi)) self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni) self.pointer_tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def load_config(self, config_file): cfg = PyFileConfigLoader(config_file) cfg.load_config() self.config = cfg.config self.filename = self.config.head_model self.plate = self.config.tools.index('CB609') self.marker = self.config.tools.index('CT315') self.glasses = self.config.tools.index('ST568') self.pointer = self.config.tools.index('P717') self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti) self.pointer_Qi = qa.norm( np.array(self.config.tool_list[self.pointer].Qi)) self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni) self.pointer_Xi = self.pointer_Ni - self.pointer_Ti self.tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def get(self, ch, vec, maximum=False): l.info('Computing dipole temperature') #T_dipole_CMB = doppler_factor(qarray.arraylist_dot(self.satellite_v,vec).flatten()) * T_CMB vel = qarray.amplitude(self.satellite_v).flatten() beta = vel / physcon.c gamma=1/np.sqrt(1-beta**2) if maximum: cosdir = 1 else: cosdir = qarray.arraylist_dot(qarray.norm(self.satellite_v), vec).flatten() T_dipole_CMB = T_CMB / (gamma * ( 1 - beta * cosdir )) #T_dipole_CMB = T_CMB * (1 - beta * cosdir ) if self.K_CMB: return T_dipole_CMB - T_CMB else: T_dipole_RJ = ch.Planck_to_RJ( T_dipole_CMB ) - ch.Planck_to_RJ(T_CMB) return T_dipole_RJ
def calibrate_head(self, in_msg): msg_type = in_msg.GetHeader().msg_type if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.asarray(in_mdf.xyz[:]) orientations = self.shuffle_q(np.asarray(in_mdf.ori[:])) #When arrays have been filled the calibration vector is generated if ( (self.store_glasses >= self.store_glasses_pos.shape[0]) & (self.store_glasses >= self.store_glasses_ori.shape[0]) & (self.store_head >= self.store_head_pos.shape[0]) & (self.store_head >= self.store_head_ori.shape[0]) ): self.calibrating = False self.make_calibration_vector() #Pointer is measured from ball 1, pointer end must be calculated elif in_mdf.tool_id == (self.pointer + 1): if self.store_head < self.store_head_pos.shape[0]: if np.any(np.isnan(positions)) == True: raise Exception, 'nan present' elif np.any(np.isnan(orientations)) == True: raise Exception, 'nan present' Qf = qa.norm(orientations) #Sometimes gets nan, source unknown if np.any(np.isnan(Qf)): print(self.store_head, 'Qf', orientations) #find_nans(self.store_head, Tk, 'Tk') Cz_pos, Qr = self.pointer_tp.get_pos(orientations, positions) #find_nans(self.store_head, Cz_pos, 'Cz') self.store_head_pos[self.store_head, :] = Cz_pos self.store_head_ori[self.store_head, :] = orientations self.store_head += 1 elif in_mdf.tool_id == (self.glasses + 1): if self.store_glasses < self.store_glasses_pos.shape[0]: if np.any(np.isnan(positions)) == True: raise Exception, 'nan present' if np.any(np.isnan(orientations)) == True: raise Exception, 'nan present' self.store_glasses_pos[self.store_glasses, :] = positions self.store_glasses_ori[self.store_glasses, :] = orientations self.store_glasses += 1
def get_4piconv_dx10(self, ch, theta, phi, psi): l.info('Computing dipole temperature with 4pi convolver') rel_vel = self.satellite_v/physcon.c # remove psi_pol psi_nopol = psi - np.radians(ch.get_instrument_db_field("psi_pol")) # rotate vel to horn reference frame tohorn_rotation = qarray.norm( qarray.mult( qarray.rotation([0,0,1], -psi_nopol) , qarray.mult( qarray.rotation([0,1,0], -theta) , qarray.rotation([0,0,1], -phi) ) )) # vel in beam ref frame vel_rad = qarray.rotate(tohorn_rotation, rel_vel) dipole_amplitude = self.get_fourpi_prod(vel_rad, ["S100", "S010", "S001"], ch) # relative corrections dipole_amplitude += vel_rad[:,0] * self.get_fourpi_prod(vel_rad, ["S200", "S110", "S101"], ch)/2 dipole_amplitude += vel_rad[:,1] * self.get_fourpi_prod(vel_rad, ["S110", "S020", "S011"], ch)/2 dipole_amplitude += vel_rad[:,2] * self.get_fourpi_prod(vel_rad, ["S101", "S011", "S002"], ch)/2 return dipole_amplitude * T_CMB
def process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type #print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing))) if self.status == False: pass elif self.testing: if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.array(in_mdf.xyz[:]) orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) if self.Ptest_count == 250 & self.Gtest_count == 250: self.testing = False self.status = False print 'done' else: if in_mdf.tool_id == (self.pointer + 1): self.pointer_test[self.Ptest_count, :] = positions if np.any(np.isnan(positions)) == True: sys.stdout.write('\n Pointer: NaN present') self.Gtest_count -= 1 elif np.any(np.isnan(orientations)) == True: sys.stdout.write('\n Pointer: NaN present') self.Gtest_count -= 1 elif np.any(np.isinf(positions)) == True: sys.stdout.write('\n Pointer: inf present') self.Gtest_count -= 1 elif np.any(np.isinf(orientations)) == True: sys.stdout.write('\n Pointer: inf present') self.Gtest_count -= 1 elif np.all(positions) == positions[0]: sys.stdout.write('\n Pointer: Zeros present') self.Gtest_count -= 1 elif np.all(orientations) == orientations[0]: sys.stdout.write('\n Pointer: Zeros present') self.Gtest_count -= 1 else: self.Ptest_count += 1 if in_mdf.tool_id == (self.glasses + 1): self.headP_test[self.Gtest_count, :] = positions self.headQ_test[self.Gtest_count, :] = orientations if np.any(np.isnan(positions)) == True: sys.stdout.write('\n Glasses: NaN present') self.Ptest_count -= 1 elif np.any(np.isnan(orientations)) == True: sys.stdout.write('\n Glasses: NaN present') self.Ptest_count -= 1 elif np.any(np.isinf(positions)) == True: sys.stdout.write('\n Glasses: inf present') self.Ptest_count -= 1 elif np.any(np.isinf(orientations)) == True: sys.stdout.write('\n Glasses: inf present') self.Ptest_count -= 1 elif np.all(positions) == positions[0]: sys.stdout.write('\n Glasses: Zeros present') self.Ptest_count -= 1 elif np.all(orientations) == orientations[0]: sys.stdout.write('\n Glasses: Zeros present') self.Ptest_count -= 1 else: self.Gtest_count += 1 print orientations if (self.Ptest_count < 0) or (self.Gtest_count < 0): self.Gtest_count = 0 else: if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) if in_mdf.tool_id == (self.glasses + 1): positions = np.array(in_mdf.xyz[:]) orientations = qa.norm( self.shuffle_q(np.array(in_mdf.ori[:]))) self.glasses_pos_buffer = positions self.glasses_orientation_buffer = orientations else: pass elif msg_type == rc.MT_HOTSPOT_POSITION: in_mdf = rc.MDF_HOTSPOT_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.array(in_mdf.xyz[:]) vector = np.array(in_mdf.ori[:3]) self.coil_pos_buffer = positions self.coil_vector_buffer = vector elif msg_type == rc.MT_TMS_TRIGGER: self.find_hotspot_to_cz(self.coil_pos_buffer, self.glasses_orientation_buffer, self.glasses_pos_buffer, self.coil_vector_buffer)
# In[13]: q = np.empty([len(vec), 4], dtype=np.float) # In[14]: n = len(vec) for i in range(n): rotmat = np.hstack([ vec_north[i,:][:,np.newaxis], np.cross(vec[i,:], vec_north[i,:])[:,np.newaxis], vec[i,:][:,np.newaxis] ]) q[i] = qa.norm(qa.from_rotmat(rotmat)) if (i > 0): if np.dot(q[i], q[i-1]) < 0: q[i] *= -1 # `rotmat` / `q` are the rotation matrix / quaternion between the local system and Equatorial coordinates # ### Interpolation # In[18]: jd = pointing.index.to_julian_date() # In[19]:
prec_ang_speed = 2 * np.pi / prec_period_seconds rot_prec_opening = qa.rotation(z_axis, -np.radians(40)) prec_angles = (timestamps * prec_ang_speed) % (2 * np.pi) rot_prec = qa.mult(qa.rotation(x_axis, prec_angles), rot_prec_opening) # Spin spin_period_seconds = 60 spin_ang_speed = 2 * np.pi / spin_period_seconds spin_angles = (timestamps * spin_ang_speed) % (2 * np.pi) rot_opening = qa.rotation(z_axis, -np.radians(10)) rot_spin = qa.mult(qa.rotation(x_axis, spin_angles), rot_opening) # Total quaternions to boresight bore_quat = qa.norm(qa.mult(rot_earth_orbit, qa.mult(rot_prec, rot_spin))) bore_v = qa.rotate(bore_quat, x_axis) pix_1det = hp.vec2pix(nside, bore_v[:, 0], bore_v[:, 1], bore_v[:, 2], nest=True) pixels = np.tile(pix_1det, ndet) # polarization weights bore_v_proj_ortog = np.hstack([ -bore_v[:, [1]], bore_v[:, [0]], np.zeros(len(bore_v))[:, np.newaxis]
def get_pos(self, Qk, Tk): Qk = qa.norm(Qk) Qr = (qa.mult(Qk, qa.inv(self.Qi))).flatten() pos = (qa.rotate(Qr, self.Xi)).flatten() + Tk return pos, Qr
def test_norm(self): np.testing.assert_array_almost_equal(qarray.norm(self.q1), self.q1/np.linalg.norm(self.q1)) np.testing.assert_array_almost_equal(qarray.norm(self.qtonormalize) , self.qnormalized)