def compare_obc_and_asol(atts, times, recs, ptol=2, ytol=2, rtol=65):
"""
Check that obc solution and ground solution have relatively close quats.
Note that because the ground aspect solution is not continuous (only run over science obsids
in aspect intervals) that this method only checks those intervals.
:param atts: attitudes from get_atts
:param times: times from get_atts
:param recs: list of dicts of header records from get_atts
:param ptol: dq quaternion pitch tolerance in arcsecs
:param ytol: dq quaternion yaw tolerance in arcsecs
:param rtol: dq quaternion roll tolerance in arcsecs
"""
for ai in recs:
telem = fetch.Msidset(['aoattqt*'], ai['TSTART'], ai['TSTOP'])
obc_atts = np.vstack([telem['aoattqt{}'.format(idx)].vals
for idx in [1, 2, 3, 4]]).transpose()
obc_times = telem['aoattqt1'].times
# Test that, at the obc times, the onboard solution and the ground solution
# are reasonably close
idxs = np.searchsorted(times[:-1], obc_times)
dps = []
dys = []
drs = []
for att, obc_att in zip(atts[idxs], obc_atts):
dq = Quat(normalize(att)).dq(Quat(normalize(obc_att)))
dps.append(dq.pitch * 3600)
dys.append(dq.yaw * 3600)
drs.append(dq.roll0 * 3600)
dps = np.array(dps)
dys = np.array(dys)
drs = np.array(drs)
assert np.all(np.abs(dys) < ytol)
assert np.all(np.abs(dps) < ptol)
assert np.all(np.abs(drs) < rtol)
def test_calc_roll_pitch_yaw(stars, pitch, yaw, roll):
roll /= 3600
pitch /= 3600
yaw /= 3600
q0 = Quat([0, 0, 45])
dq = Quat([yaw, -pitch, roll])
assert np.isclose(dq.roll, roll)
assert np.isclose(dq.pitch, pitch)
assert np.isclose(dq.yaw, yaw)
q0_offset = q0 * dq
assert np.isclose(q0_offset.roll, q0.roll + roll)
yags = []
zags = []
yags_obs = []
zags_obs = []
for ra, dec in stars:
yag, zag = radec2yagzag(ra, dec, q0)
yags.append(yag * 3600)
zags.append(zag * 3600)
yag, zag = radec2yagzag(ra, dec, q0_offset)
yags_obs.append(yag * 3600)
zags_obs.append(zag * 3600)
sigma = np.arange(len(yags)) + 1
out_roll, out_pitch, out_yaw = calc_roll_pitch_yaw(yags, zags, yags_obs,
zags_obs, sigma)
# Computed pitch, yaw, roll within 0.5 arcsec in roll, 0.02 arcsec pitch/yaw
assert np.isclose(roll, out_roll, atol=0.5 / 3600, rtol=0.0)
assert np.isclose(pitch, out_pitch, atol=0.02 / 3600, rtol=0.0)
assert np.isclose(yaw, out_yaw, atol=0.02 / 3600, rtol=0.0)
def calc_target_offsets(aca, ra_targ, dec_targ, si_align=None):
"""
Calculates required Y and Z offsets (deg) required from a target RA, Dec to
arrive at the desired PCAD pointing ``aca``.
:param aca: PCAD attitude (any Quat-compliant initializer)
:param ra_targ: RA of science target from OR/Obscat
:param dec_targ: Dec of science target from OR/Obscat
:param si_align: SI ALIGN matrix (default=ODB)
:rtype: tuple (y_off, z_off) degrees
"""
if si_align is None:
si_align = ODB_SI_ALIGN
# Convert si_align transform matrix into a Quaternion
q_si_align = Quat(si_align)
# Pointing quaternion
q_aca = Quat(aca)
# Pointing quaternion of nominal HRMA frame after adjusting for the alignment offset.
# The sense of si_align is that q_hrma = q_pcad * si_align, where si_align is
# effectively a delta quaternion.
q_hrma = q_aca * q_si_align
# the y and z offsets of the target in that frame, in degrees
y_off, z_off = radec_to_yagzag(ra_targ, dec_targ, q_hrma)
return y_off / 3600, z_off / 3600
def pcad_point(ra, dec, roll, y_offset, z_offset):
# offsets are in arcmin, convert to degrees
offset = Quat([y_offset / 60., z_offset / 60., 0])
q_targ = Quat([ra, dec, roll])
q_hrma = q_targ * offset.inv()
q_pnt = Quat(np.dot(q_hrma.transform, ODB_SI_ALIGN))
return q_pnt.ra, q_pnt.dec
def test_calc_roll(roll):
stars = [
(-1, 1), # ra, dec in deg
(1, 1),
(0, 0),
(1, -1)
]
q0 = Quat([0, 0, 45])
dq = Quat([0, 0, roll])
assert np.isclose(dq.roll, roll)
q0_roll = q0 * dq
assert np.isclose(q0_roll.roll, q0.roll + roll)
yags = []
zags = []
yags_obs = []
zags_obs = []
for ra, dec in stars:
yag, zag = radec2yagzag(ra, dec, q0)
yags.append(yag)
zags.append(zag)
yag, zag = radec2yagzag(ra, dec, q0_roll)
yags_obs.append(yag)
zags_obs.append(zag)
out_roll = calc_roll(yags, zags, yags_obs, zags_obs)
# Computed roll is within 0.1% of actual roll
assert np.isclose(roll, out_roll, atol=0.0, rtol=0.001)
def test_radec_yagzag_multidim(shape):
"""Test radec_to_yagzag and yagzag_to_radec for multidimensional inputs"""
dec = 0.5
ras = np.linspace(0., 1., 24)
eqs = np.empty(shape=(24, 3))
eqs[..., 0] = 0.1
eqs[..., 1] = np.linspace(-0.1, 0.1, 24)
eqs[..., 2] = np.linspace(-1, 1, 24)
ras_nd = ras.reshape(shape)
qs_nd = Quat(equatorial=eqs.reshape(shape + (3, )))
# First do everything as scalars
yagzags_list = [
radec_to_yagzag(ra, dec, Quat(equatorial=eq))
for ra, eq in zip(ras.tolist(), eqs.tolist())
]
yagzags_nd_from_list = np.array(yagzags_list).reshape(shape + (2, ))
# Test broadcasting by providing a scalar for `dec`
yags, zags = radec_to_yagzag(ras_nd, dec, qs_nd)
assert yags.shape == shape
assert zags.shape == shape
assert np.allclose(yags, yagzags_nd_from_list[..., 0])
assert np.allclose(zags, yagzags_nd_from_list[..., 1])
ras_rt, decs_rt = yagzag_to_radec(yags, zags, qs_nd)
assert ras_rt.shape == shape
assert decs_rt.shape == shape
assert np.allclose(ras_rt, ras_nd)
assert np.allclose(decs_rt, dec)
def get_data_2d():
stars0 = stars[0]
q0 = Quat([0, 0, 45])
yags = []
zags = []
for ra, dec in stars0:
yag, zag = radec2yagzag(ra, dec, q0)
yags.append(yag * 3600)
zags.append(zag * 3600)
yags_obs_list = []
zags_obs_list = []
times = np.linspace(0, 1000, 10)
rolls = np.sin(2 * np.pi * times / 666) * 100 / 3600
pitches = np.sin(2 * np.pi * times / 1000) * 50 / 3600
yaws = np.sin(2 * np.pi * times / 707) * 30 / 3600
qs = []
for roll, pitch, yaw in zip(rolls, pitches, yaws):
dq = Quat([yaw, -pitch, roll])
q0_offset = q0 * dq
qs.append(q0_offset)
yags_obs = []
zags_obs = []
for ra, dec in stars0:
yag, zag = radec2yagzag(ra, dec, q0_offset)
yags_obs.append(yag * 3600)
zags_obs.append(zag * 3600)
yags_obs_list.append(yags_obs)
zags_obs_list.append(zags_obs)
return q0, rolls, pitches, yaws, qs, yags, zags, yags_obs_list, zags_obs_list
def calc_si_align(dy_align=0.0, dz_align=0.0, check_consistency=True):
"""
Compute the SI alignment matrix that includes an alignment offset
(dy_align, dz_align) from the baseline ODB_SI_ALIGN.
:param dy_align: Aimpoint alignment offset in Dy (deg)
:param dz_align: Aimpoint alignment offset in Dz (deg)
:param check_consistency: double check the output by essentially inverting it
:rtype: 3x3 alignment matrix
"""
# Define new SI_ALIGN as a delta quaternion update to existing.
# The minus signs here were empirically determined to pass tests.
# This is the only place where signs are chosen without a clear justification.
q_align = Quat([dy_align, dz_align, 0.0])
q_si_align = Quat(ODB_SI_ALIGN) * q_align
si_align = q_si_align.transform
# Double check that the si_align corresponds to the y and z offsets of the target in
# that frame, in degrees.
if check_consistency:
q_pcad = Quat([1, 2, 3]) # Arbitrary attitude
q_targ = q_pcad * q_si_align
y_off, z_off = calc_offsets(q_targ.ra, q_targ.dec, q_pcad.ra,
q_pcad.dec, q_pcad.roll)
# Offset as expected?
assert np.allclose(dy_align, y_off, atol=1e-6)
assert np.allclose(dz_align, z_off, atol=1e-6)
# Orthonormal?
assert np.allclose(si_align.T.dot(si_align), np.eye(3))
return si_align
def calc_offsets(ra_targ, dec_targ, ra_pcad, dec_pcad, roll_pcad):
"""
Calculates required Y and Z offsets (deg) required from a target to
arrive at the desired PCAD pointing.
:param ra_targ: RA of science target from OR/Obscat
:param dec_targ: Dec of science target from OR/Obscat
:param ra_pcad: RA of desired PCAD Pointing
:param dec_pcad: Dec of desired PCAD Pointing
:param roll_pcad: Roll of desired PCAD Pointing
:rtype: tuple (y_off, z_off) arcmins
"""
# Convert si_align transform matrix into a Quaternion
q_si_align = Quat(ODB_SI_ALIGN)
# Pointing quaternion
q_pcad = Quat([ra_pcad, dec_pcad, roll_pcad])
# Pointing quaternion of nominal HRMA frame after adjusting for the alignment offset.
# The sense of si_align is that q_hrma = q_pcad * si_align, where si_align is
# effectively a delta quaternion.
q_hrma = q_pcad * q_si_align
# the y and z offsets of the target in that frame, in degrees
y_off, z_off = radec2yz(ra_targ, dec_targ, q_hrma)
return y_off, z_off
def ground_truth(model_ply, scene_ply, gt_pos):
pos_i = gt_pos[os.path.basename(model_ply)]
pos_j = gt_pos[os.path.basename(scene_ply)]
q_i = Quat(normalize(numpy.array(pos_i[3::])))
q_j = Quat(normalize(numpy.array(pos_j[3::])))
q_ji = q_i / q_j
q_ij = q_j / q_i
return q_ij, q_ji
def lookup_ground_truth(i, j):
model_ply = PLY_FILES[i]
scene_ply = PLY_FILES[j]
pos_i = GT_POS[os.path.basename(model_ply)]
pos_j = GT_POS[os.path.basename(scene_ply)]
q_i = Quat(normalize(np.array(pos_i[3::])))
q_j = Quat(normalize(np.array(pos_j[3::])))
q_ji = q_i / q_j
q_ij = q_j / q_i
return q_ij, q_ji
def get_atts_from_files(asol_files, acal_files, aqual_files, filter=True):
"""
From ASP1 source files (asol, acal, aqual) get the ground aspect solution quaternions and times covering
the range of asol_files in the ACA frame. The asol, acl, and aqual files are assumed to have one-to-one correspondence
(though the asol to acal times are checked).
:asol_files: list of aspect asol1 files
:acal_files: list of acal1 files associated with asol_files
:aqual_files: list of aqual files associated with asol_files
:filter: boolean, true means returned values will not include quaternions during times when asp_sol_status is non-zero
:returns: Nx4 np.array of quaternions, np.array of N times, list of dict with header from each asol file.
"""
# There should be one asol and one acal file for each aspect interval in the range
att_chunks = []
time_chunks = []
records = []
for asol_f, acal_f, aqual_f in zip(asol_files, acal_files, aqual_files):
asol = Table.read(asol_f, hdu=1)
acal = Table.read(acal_f, hdu=1)
aqual = Table.read(aqual_f, hdu=1)
# Check that the time ranges match from the fits headers (meta in the table)
if not np.allclose(np.array([asol.meta['TSTART'], asol.meta['TSTOP']]),
np.array([acal.meta['TSTART'], acal.meta['TSTOP']]),
atol=10):
raise ValueError("ACAL and ASOL have mismatched time ranges")
if filter and np.any(aqual['asp_sol_status'] != 0):
# For each sample with bad status find the overlapping time range in asol
# and remove from set
for idx in np.flatnonzero(aqual['asp_sol_status']):
nok = ((asol['time'] >= (aqual['time'][idx] - 1.025))
& (asol['time'] <= (aqual['time'][idx] + 1.025)))
asol = asol[~nok]
# Transpose of transform/rotation matrix is the inverse
aca_mis_inv = acal['aca_misalign'][0].transpose()
# Repeat the transform to match asol
aca_mis_inv = np.repeat(aca_mis_inv[np.newaxis, ...],
repeats=len(asol),
axis=0)
q_mis_inv = Quat(transform=aca_mis_inv)
# Quaternion multiply the asol quats with that inv misalign and save
q_att_name = 'q_att_raw' if 'q_att_raw' in asol.colnames else 'q_att'
att_chunks.append((Quat(q=asol[q_att_name]) * q_mis_inv).q)
time_chunks.append(np.array(asol['time']))
records.append(asol.meta)
if len(att_chunks) > 0 and len(time_chunks) > 0:
return np.vstack(att_chunks), np.hstack(time_chunks), records
else:
return np.array([]), np.array([]), []
def calc_att(att, yag, zag, yag_obs, zag_obs, sigma=None):
"""Calc S/C attitude for observed star positions relative to reference.
This function computes a S/C attitude that transforms the
reference star positions yag/zag into the observed positions
yag_obs/zag_obs. The units for these values must be in arcsec.
The attitude ``att`` is the reference attitude for the reference star
catalog. It can be any value that initializes a Quat object.
The ``yag`` and ``zag`` values correspond to the reference star catalog
positions. These must be a 1-d list or array of length M (number of
stars).
The ``yag_obs`` and ``zag_obs`` values must be either a 1-d or 2-d array
with shape M (single readout of M stars) or shape N x M (N rows of M
stars).
The ``sigma`` parameter can be None or a 1-d array of length M.
The algorithm is a simple but fast linear least-squared solution which uses
a small angle assumption to linearize the rotation matrix from
[[cos(th) -sin(th)], [sin(th), cos(th)]] to [[1, -th], [th, 1]].
In practice anything below 1.0 degree is fine.
:param att: reference attitude (Quat-compatible)
:param yag: reference yag (list or array, arcsec)
:param zag: reference zag (list or array, arcsec)
:param yag_obs: observed yag (list or array, arcsec)
:param zag_obs: observed zag (list or array, arcsec)
:param sigma: centroid uncertainties (None or list or array, arcsec)
:returns: Quat or list of Quat
"""
from Quaternion import Quat
q_att = Quat(att)
rolls, pitches, yaws = calc_roll_pitch_yaw(yag, zag, yag_obs, zag_obs, sigma)
if isinstance(rolls, np.ndarray) and rolls.ndim >= 1:
out = []
for roll, pitch, yaw in zip(rolls, pitches, yaws):
dq = Quat([yaw, -pitch, roll])
out.append(q_att * dq)
else:
dq = Quat([yaws, -pitches, rolls])
out = q_att * dq
return out
def test_validate_catalogs_over_range():
start = '2017:001:12:00:00'
stop = '2017:002:12:00:00'
dwells = events.dwells.filter(start, stop)
for dwell in dwells:
print(dwell)
try:
telem_quat = get_cmd_quat(dwell.start)
# try to get the tracked telemetry for 1ks at the beginning of the dwell,
# or if the dwell is shorter than that, just get the dwell
cat, telem = get_trak_cat_from_telem(
dwell.start, np.min([dwell.tstart + 100, dwell.tstop]),
telem_quat)
except ValueError as err:
if 'MSID' in str(err):
pytest.skip('Eng archive MSID missing {}'.format(err))
sc = starcheck.get_starcheck_catalog_at_date(dwell.start)
sc_quat = Quat(
[sc['manvr'][-1]["target_Q{}".format(i)] for i in [1, 2, 3, 4]])
dq = sc_quat.dq(telem_quat)
if ((np.abs(dq.pitch) * 3600 > 1) or (np.abs(dq.yaw) * 3600 > 1)):
if dwell.manvr.template != 'unknown':
raise ValueError("Unexpected offset in pointing")
else:
print(dwell.start,
"pointing offset but has unknown manvr template")
continue
trak_sc = sc['cat'][sc['cat']['type'] != 'ACQ']
for slot in range(0, 8):
if slot in cat:
slot_match = trak_sc[trak_sc['slot'] == slot]
# the if statement may see false positives if fewer than a total of 8 BOT/GUI/MON
# slots were commanded
if not len(slot_match):
raise ValueError("missing slot in catalog")
trak_sc_slot = slot_match[0]
if trak_sc_slot['type'] == 'FID':
assert cat[slot]['type'] == 'FID'
assert np.abs(cat[slot]['yag'] -
trak_sc_slot['yang']) < 20.0
assert np.abs(cat[slot]['zag'] -
trak_sc_slot['zang']) < 20.0
else:
assert np.abs(cat[slot]['yag'] -
trak_sc_slot['yang']) < 5.0
assert np.abs(cat[slot]['zag'] -
trak_sc_slot['zang']) < 5.0
def computeIMUMultiple(imuData):
utmLoc = np.zeros((len(imuData['x134_Position']), 2))
for i in range(len(imuData['x134_Position'])):
u = utm.from_latlon(imuData['x134_Position'][i, 0],
imuData['x134_Position'][i, 1])
utmLoc[i, 0] = u[0]
utmLoc[i, 1] = u[1]
qNorm = normalize(imuData['x131_Quaternion'][0, :])
eulerAngles = np.zeros((len(imuData['x131_Quaternion']), 3))
for i in range(len(imuData['x131_Quaternion'])):
qNorm = normalize(imuData['x131_Quaternion'][i, :])
Q = Quat(qNorm)
eulerAngles[i, :] = [Q.ra, Q.dec, Q.roll]
relPath = utmLoc - utmLoc[0, :]
rotAngle = (180 - eulerAngles[0, 2]) * np.pi / 180.
R2 = np.array([[cos(rotAngle), -1 * sin(rotAngle)],
[sin(rotAngle), cos(rotAngle)]])
pathR = np.dot(R2, relPath.T)
# else:
# utmLoc = np.full((5,2),np.NaN)
# eulerAngles = np.full((5,3),np.NaN)
# pathR = np.full((2,5),np.NaN)
# velocity = np.full((5,3),np.NaN)
# time = np.full((5,8),np.NaN)
return utmLoc, eulerAngles, pathR
def test_aca_targ_transforms():
"""
Observation request:
ID=13928,TARGET=(191.321250,27.125556,{Haro 9}),DURATION=(17000.000000),
PRIORITY=9,SI=ACIS-S,GRATING=NONE,SI_MODE=TE_0045A,ACA_MODE=DEFAULT,
TARGET_OFFSET=(0.002500,-0.004167),
DITHER=(ON,0.002222,0.360000,0.000000,0.002222,0.509100,0.000000),
SEGMENT=(1,15300.000000),PRECEDING=(13632),MIN_ACQ=1,MIN_GUIDE=1
ACA (PCAD): As-planned pointing from starcheck
Q1,Q2,Q3,Q4: -0.18142595 -0.37811633 -0.89077416 0.17502588
"""
# Attitude quaternion for the as-run PCAD attitude
q_aca = Quat([-0.18142595, -0.37811633, -0.89077416, 0.17502588])
# Target coordinates and quaternion, using the PCAD roll
ra_targ, dec_targ = 191.321250, 27.125556
# Offsets from OR (Target DY, DZ) in degrees
y_off, z_off = 0.002500, -0.004167
q_targ = chandra_aca.calc_targ_from_aca(q_aca, y_off, z_off)
assert np.allclose(ra_targ, q_targ.ra, atol=1e-5, rtol=0)
assert np.allclose(dec_targ, q_targ.dec, atol=1e-5, rtol=0)
q_aca_rt = chandra_aca.calc_aca_from_targ(q_targ, y_off, z_off)
dq = q_aca_rt.inv() * q_aca
assert np.degrees(np.abs(dq.q[0] * 2)) < 30 / 3600.
assert np.degrees(np.abs(dq.q[1] * 2)) < 1 / 3600.
assert np.degrees(np.abs(dq.q[2] * 2)) < 1 / 3600.
def getPFull(W, armKin):
# return (normal, rPos) where normal is the normal of the plane and
# rPos is a point on the plane ( position of the end effector )
# Get camera extrinsic parameters from robot arm
T = armKin.forward_position_kinematics()[0:3]
rOrientation = armKin.forward_position_kinematics()[3:7]
#print "lPos", T
#print "lOrientation", rOrientation
myQuat = Quat(rOrientation)
R = myQuat._quat2transform()
M = np.append(R,T.reshape(3,1),1)
P = np.dot(W,M)
return P
def normalise_quaternions(quat_dict):
norm_quat_dict = OrderedDict()
for k, v in quat_dict.items():
if v.shape[1] == 4:
norm_quats = []
for r in v:
q = np.array([r[1], r[2], r[3], r[0]]) # [x, y, z, w]
nq = Quat(normalize(q))
nq_v = nq._get_q()
norm_quats.append([nq_v[3], nq_v[0], nq_v[1],
nq_v[2]]) # [w, x, y, z]
norm_quat_dict[k] = np.array(norm_quats)
else:
norm_quat_dict[k] = v
return norm_quat_dict
def convert_to_axis_angle(quaternion_dict):
axis_angle_dict = OrderedDict()
for k, v in quaternion_dict.items():
if v.shape[1] == 4:
axis_angle = []
for r in v:
q = np.array([r[1], r[2], r[3], r[0]]) # [x, y, z, w]
quat = Quat(q)
a = quat._get_angle_axis()
axis_angle.append(np.array([a[0], a[1][0], a[1][1],
a[1][2]])) # [Ө, x, y, z]
axis_angle_dict[k] = np.array(axis_angle)
else:
axis_angle_dict[k] = v
return axis_angle_dict
def get_trak_cat_from_telem(start, stop, cmd_quat):
start = DateTime(start)
stop = DateTime(stop)
msids = [
"{}{}".format(m, i)
for m in ['AOACYAN', 'AOACZAN', 'AOACFID', 'AOIMAGE', 'AOACFCT']
for i in range(0, 8)
]
telem = fetch.MSIDset(
['AOACASEQ', 'CORADMEN', 'AOPCADMD', 'AONSTARS', 'AOKALSTR'] + msids,
start, stop)
att = fetch.MSIDset(['AOATTQT{}'.format(i) for i in [1, 2, 3, 4]], start,
stop)
cat = {}
for slot in range(0, 8):
track = telem['AOACFCT{}'.format(slot)].vals == 'TRAK'
fid = telem['AOACFID{}'.format(slot)].vals == 'FID '
star = telem['AOIMAGE{}'.format(slot)].vals == 'STAR'
n = 30
if np.count_nonzero(track) < n:
continue
if np.any(fid & track):
cat[slot] = {
'type': 'FID',
'yag': telem['AOACYAN{}'.format(slot)].vals[fid & track][0],
'zag': telem['AOACZAN{}'.format(slot)].vals[fid & track][0]
}
else:
n_samples = np.count_nonzero(track & star)
if n_samples < (n + 4):
continue
# If there is tracked data with a star, let's try to get our n samples from about
# the middle of the range
mid_point = int(n_samples / 2.)
yags = []
zags = []
for sample in range(mid_point - int(n / 2.),
mid_point + int(n / 2.)):
qref = Quat(
normalize([
att['AOATTQT{}'.format(i)].vals[track & star][sample]
for i in [1, 2, 3, 4]
]))
ra, dec = yagzag2radec(
telem['AOACYAN{}'.format(slot)].vals[track & star][sample]
/ 3600.,
telem['AOACZAN{}'.format(slot)].vals[track & star][sample]
/ 3600., qref)
yag, zag = radec2yagzag(ra, dec, cmd_quat)
yags.append(yag)
zags.append(zag)
# This doesn't detect MON just yet
cat[slot] = {
'type': 'STAR',
'yag': np.median(yags) * 3600.,
'zag': np.median(zags) * 3600.
}
return cat, telem
def calc_pcad_from_targ(ra_targ,
dec_targ,
roll_targ,
y_off,
z_off,
si_align=ODB_SI_ALIGN):
"""
Calculates required Y and Z offsets (deg) required from a target to
arrive at the desired PCAD pointing.
:param ra_targ: RA of science target from OR/Obscat
:param dec_targ: Dec of science target from OR/Obscat
:param roll_targ: Roll of science target attitude
:param y_off: Y offset (deg, sign per OR-list convention)
:param z_off: Z offset (deg,sign per OR-list convention)
:param si_align: SI ALIGN matrix (default=ODB_SI_ALIGN)
:rtype: q_pcad (Quat)
"""
q_si_align = Quat(si_align)
q_targ = Quat([ra_targ, dec_targ, roll_targ])
q_off = Quat([y_off, z_off, 0])
q_pcad = q_targ * q_off.inv() * q_si_align.inv()
return q_pcad
def get_delta_quat(eng_data, times, manvr):
guide = eng_data[times >= DateTime(manvr.guide_start).secs - 1]
first_guide = guide[0]
# skip the duplicate and go to index 2
second_guide = guide[2]
q1 = Quat([
first_guide['AOATTQT1'], first_guide['AOATTQT2'],
first_guide['AOATTQT3'], first_guide['AOATTQT4']
])
q2 = Quat([
second_guide['AOATTQT1'], second_guide['AOATTQT2'],
second_guide['AOATTQT3'], second_guide['AOATTQT4']
])
dq = q2 / q1
dot_q = np.sum(q1.q * q2.q)
if dot_q > 1:
dot_q = 1.0
return dq, dot_q
def create_pixel_vecmap(_geo_h, _geo_w):
vec_map = np.zeros((_geo_h, _geo_w)).tolist()
for i in range(_geo_h):
for j in range(_geo_w):
theta, phi = pixel_to_ang(i, j, _geo_h, _geo_w)
t = Quat([0.0, theta, phi]).q #nolonger use Quat
q = Quaternion([t[3], t[2], -t[1], t[0]])
vec_map[i][j] = extract_direction(q)
return vec_map
def compare_rotation(ply_files, reg_result, gt_pos):
res = {}
for i, j in reg_result:
model_ply = ply_files[i]
scene_ply = ply_files[j]
q_ij, q_ji = ground_truth(model_ply, scene_ply, gt_pos)
param = reg_result[(i, j)][0]
q = Quat(normalize(param[0:4]))
res[(i, j)] = abs(numpy.dot(q.q, q_ji.q))
return res
def get_orientation_quat(self):
# use the velocity as orientation
# ra=heading/yaw, dec=pitch, roll=roll
yaw, pitch = geom.vec3_to_yaw_pitch(self.vel)
roll = 0
yaw *= RADTODEG
pitch *= RADTODEG
return Quat([yaw, pitch, roll]).q
def foveate_quat(self, target):
eye_r = self.head.objs[1]
ret = eye_r.intersection(target)
d = eye_r.front - ret
q_d = Quat(solve_matrix(eye.bfx, d))
q_d = Quat(q_d)
q = eye_r.quat
E = q_d * q.inv()
fovea_traj = []
while np.linalg.norm(E.q - [0, 0, 0, 1]) > 0.01:
w = E.q[:3]
eye_r.apply_vel(w / 2, self.head.trans_s, self.head.quat_s)
q = integ_quat(q, w)
E = q_d * q.inv()
fovea_traj.append(eye_r.back)
traj = np.array(fovea_traj)
eye_r.quat = q
def test_validate_catalogs_over_range():
start = '2017:001'
stop = '2017:002'
dwells = events.dwells.filter(start, stop)
for dwell in dwells:
print(dwell)
try:
telem_quat = get_cmd_quat(dwell.start)
# try to get the tracked telemetry for 1ks at the beginning of the dwell,
# or if the dwell is shorter than that, just get the dwell
cat, telem = get_trak_cat_from_telem(dwell.start,
np.min([dwell.tstart + 100, dwell.tstop]),
telem_quat)
except ValueError as err:
if 'MSID' in str(err):
pytest.skip('Eng archive MSID missing {}'.format(err))
sc = starcheck.get_starcheck_catalog_at_date(dwell.start)
sc_quat = Quat([sc['manvr'][-1]["target_Q{}".format(i)] for i in [1, 2, 3, 4]])
dq = sc_quat.dq(telem_quat)
if ((np.abs(dq.pitch) * 3600 > 1) or (np.abs(dq.yaw) * 3600 > 1)):
if dwell.manvr.template != 'unknown':
raise ValueError("Unexpected offset in pointing")
else:
print(dwell.start, "pointing offset but has unknown manvr template")
continue
trak_sc = sc['cat'][sc['cat']['type'] != 'ACQ']
for slot in range(0, 8):
if slot in cat:
slot_match = trak_sc[trak_sc['slot'] == slot]
# the if statement may see false positives if fewer than a total of 8 BOT/GUI/MON
# slots were commanded
if not len(slot_match):
raise ValueError("missing slot in catalog")
trak_sc_slot = slot_match[0]
if trak_sc_slot['type'] == 'FID':
assert cat[slot]['type'] == 'FID'
assert np.abs(cat[slot]['yag'] - trak_sc_slot['yang']) < 20.0
assert np.abs(cat[slot]['zag'] - trak_sc_slot['zang']) < 20.0
else:
assert np.abs(cat[slot]['yag'] - trak_sc_slot['yang']) < 5.0
assert np.abs(cat[slot]['zag'] - trak_sc_slot['zang']) < 5.0
def calc_pcad_from_targ(ra_targ, dec_targ, roll_targ, y_off, z_off, si_align=ODB_SI_ALIGN):
"""
Calculates required Y and Z offsets (deg) required from a target to
arrive at the desired PCAD pointing.
:param ra_targ: RA of science target from OR/Obscat
:param dec_targ: Dec of science target from OR/Obscat
:param roll_targ: Roll of science target attitude
:param y_off: Y offset (deg, sign per OR-list convention)
:param z_off: Z offset (deg,sign per OR-list convention)
:param si_align: SI ALIGN matrix (default=ODB_SI_ALIGN)
:rtype: q_pcad (Quat)
"""
q_si_align = Quat(si_align)
q_targ = Quat([ra_targ, dec_targ, roll_targ])
q_off = Quat([y_off, z_off, 0])
q_pcad = q_targ * q_off.inv() * q_si_align.inv()
return q_pcad
def recv_data():
# def recv_date(vehicle_name)
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
# "Create a data socket."
PORT = 1511
MULTICAST_ADDRESS = '239.255.42.99'
SOCKET_BUFSIZE = 1024
datasock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
datasock.bind((MULTICAST_ADDRESS, PORT))
# join a multicast group
mreq = struct.pack("4sl", socket.inet_aton(MULTICAST_ADDRESS),
socket.INADDR_ANY)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 32)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, SOCKET_BUFSIZE)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
datasock.settimeout(3)
print(datasock.gettimeout())
# dsock = rx.mkdatasock(ip_address='', multicast_address='239.255.42.99', port=1511)
while 1:
print("looping")
data = datasock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
# print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
"""
@todo: ought to be able to specify which rigid body we're looking for, i.e. all_bodies['name_of_helicopter']
Take a second look at Optitrack data (get rigid body by name), then return data for just the helicopter
"""
for body in all_bodies:
contortion = body._asdict()['orientation']
euler = Quat([elem for elem in contortion]).equatorial
print(euler)
# print(dumps(packet._asdict(), indent=4))
# count += 1
return contortion
def create_pixel_vecmap(self):
vec_map = np.zeros(
(head_orientation_lib.H, head_orientation_lib.W)).tolist()
for i in range(head_orientation_lib.H):
for j in range(head_orientation_lib.W):
theta, phi = head_orientation_lib.pixel_to_ang(
i, j, head_orientation_lib.H, head_orientation_lib.W)
t = Quat([0.0, theta, phi]).q #nolonger use Quat
q = Quaternion([t[3], t[2], -t[1], t[0]])
vec_map[i][j] = self.f_extract_direction(q)
return vec_map
def getPlaneParams(armKin):
# return (normal, rPos) where normal is the normal of the plane and
# rPos is a point on the plane ( position of the end effector )
# Get laser plane parameters from a connected and running Baxter
rPos = armKin.forward_position_kinematics()[0:3]
rOrientation = armKin.forward_position_kinematics()[3:7]
#print "rPos", rPos
#print "rOrientation", rOrientation
myQuat = Quat(rOrientation)
R_0R = myQuat._quat2transform()
# we know that the x direction in the manipulator frame is the direction
# perpendicular to the laser plane through inspection of the baxter model
# and the transform tree
ex = np.array([[1],[0],[0]])
normal = np.dot(R_0R.T, ex)
print normal
return (normal, rPos)
def stars_test(dwells, dur_thres=7000):
"""
Check that the appropriate stars are identified in agasc.
For a given obsid, star id's and mags can be compared with those in starcheck.
Example:
=======
>> from monwin import stars_test, get_dwells
>> from parce_cm import read_dot_as_list
>> dot = read_dot_as_list("/data/mpcrit1/mplogs/2017/JAN0917/scheduled_b/md007_2305.dot")
>> dwells = get_dwells(dot)
>> stars_test(dwells)
Obsid = 0, duration = 0 sec, skipped
Obsid = 50411, duration = 3258 sec, skipped
Obsid = 50410, duration = 3270 sec, skipped
Obsid = 50409, duration = 3248 sec, skipped
Obsid = 50408, duration = 1242 sec, skipped
Obsid = 50407, duration = 3243 sec, skipped
Obsid = 50406, duration = 11594 sec
agasc_id color mag
--------- -------- -------
389949248 0.95285 7.53174
389953880 0.12495 7.79542
389954848 0.113901 8.71616
389954920 0.54995 9.34569
390865160 1.0302 8.71366
390867952 1.5 8.68183
390339464 0.48875 9.34993
391254944 0.2176 7.91407
Obsid = 18048, duration = 71020 sec, skipped
...
...
...
"""
for dwell in dwells:
obsid = dwell['obsid']
duration = dwell['duration']
if obsid < 60000 and obsid > 50000 and duration > dur_thres:
strcat = dwell['strcat']
q1 = dwell['q1']
q2 = dwell['q2']
q3 = dwell['q3']
q4 = np.sqrt(1. - q1**2 - q2**2 - q3**2)
quat = Quat(normalize([q1, q2, q3, q4]))
stars = identify_stars(obsid, strcat, quat)
print('Obsid = {}, duration = {:.0f} sec'.format(obsid, duration))
print(Table(stars))
else:
print('Obsid = {}, duration = {:.0f} sec, skipped'.format(
obsid, duration))
return
def computeIMU(imuData):
utmLoc = np.zeros(2)
u = utm.from_latlon(imuData['x134_Position'][0],
imuData['x134_Position'][1])
utmLoc[0] = u[0]
utmLoc[1] = u[1]
eulerAngles = np.zeros(3)
qNorm = normalize(imuData['x131_Quaternion'][:])
Q = Quat(qNorm)
eulerAngles[:] = [Q.ra, Q.dec, Q.roll]
return utmLoc, eulerAngles
def calc_aca_from_targ(targ, y_off, z_off, si_align=None):
"""
Calculate the PCAD (ACA) pointing attitude from target attitude and
Y,Z offset (per OR-list).
:param targ: science target from OR/Obscat (Quat-compliant object)
:param y_off: Y offset (deg, sign per OR-list convention)
:param z_off: Z offset (deg, sign per OR-list convention)
:param si_align: SI ALIGN matrix (default=ODB_SI_ALIGN)
:rtype: q_aca (Quat)
"""
if si_align is None:
si_align = ODB_SI_ALIGN
q_si_align = Quat(si_align)
q_targ = Quat(targ)
q_off = Quat([y_off, z_off, 0])
q_aca = q_targ * q_off.inv() * q_si_align.inv()
return q_aca
def quat_x_v2(v2):
"""Generate quaternion that rotates X-axis into v2 by the shortest path"""
x = np.array([1., 0, 0])
v2 = norm(np.array(v2))
dot = np.dot(x, v2)
if abs(dot) > 1 - 1e-8:
x = norm(np.array([1., 0., 1e-5]))
dot = np.dot(v2, x)
angle = np.arccos(dot)
axis = norm(np.cross(x, v2))
sin_a = np.sin(angle / 2)
cos_a = np.cos(angle / 2)
return Quat([axis[0] * sin_a, axis[1] * sin_a, axis[2] * sin_a, cos_a])
def get_cmd_quat(date):
date = DateTime(date)
cmd_quats = fetch.MSIDset(['AOCMDQT{}'.format(i) for i in [1, 2, 3]],
date.secs, date.secs + 120)
cmd_q4 = np.sqrt(
np.abs(1 - cmd_quats['AOCMDQT1'].vals[0]**2 -
cmd_quats['AOCMDQT2'].vals[0]**2 -
cmd_quats['AOCMDQT3'].vals[0]**2))
return Quat(
normalize([
cmd_quats['AOCMDQT1'].vals[0], cmd_quats['AOCMDQT2'].vals[0],
cmd_quats['AOCMDQT3'].vals[0], cmd_q4
]))
def test_si_align_with_data():
"""
Test an actual observation planned with a target offset in Y and Z:
Strategy: use a real observation with non-trivial and non-identical
OR-specified target offsets. This provides:
- Target RA, Dec (science target, not OBC target)
- Target offset DY, DZ
- PCAD pointing attitude that gets uplinked via command loads.
The PCAD attitude is at the target attitude with the ODB_SI_ALIGN
transform applied and the specified target offset applied.
The aimpoint alignment offset from http://cxc.cfa.harvard.edu/mta/ASPECT/aimpoint_mon/
is defined as an ADDITIVE offset to the Target offset. Specifically, define:
DY_total = DY_target + dy_align
DZ_total = DZ_target + dz_align
These DY_total, DZ_total offsets *could* be used in place of the OR-specified values
in the planning process to correct for aimpoint drift. But in fact the aimpoint drift
is being corrected via the ODB_SI_ALIGN matrix.
A function calc_si_align(dy_align, dz_align) computes the new SI_ALIGN matrix for
given dy_align, dz_align values.
To test that this is giving correct values (and the signs in particular), *pretend*
that the required aimpoint alignment offset is exactly the Target offset from obsid
13928. In this case we compute a new SI_ALIGN with that offset and then zero out
the target offset. By the additive property above the final PCAD attitude must be the
same as the originally scheduled (and uplinked) attitude.
----------------------
Observation request:
ID=13928,TARGET=(191.321250,27.125556,{Haro 9}),DURATION=(17000.000000),
PRIORITY=9,SI=ACIS-S,GRATING=NONE,SI_MODE=TE_0045A,ACA_MODE=DEFAULT,
TARGET_OFFSET=(0.002500,-0.004167),
DITHER=(ON,0.002222,0.360000,0.000000,0.002222,0.509100,0.000000),
SEGMENT=(1,15300.000000),PRECEDING=(13632),MIN_ACQ=1,MIN_GUIDE=1
PCAD: As-planned pointing from starcheck
Q1,Q2,Q3,Q4: -0.18142595 -0.37811633 -0.89077416 0.17502588
"""
# Attitude quaternion for the as-run PCAD attitude
q_pcad = Quat([-0.18142595, -0.37811633, -0.89077416, 0.17502588])
# Target coordinates and quaternion, using the PCAD roll
ra_targ, dec_targ = 191.321250, 27.125556
q_targ = Quat([ra_targ, dec_targ, q_pcad.roll])
# Offsets from OR (Target DY, DZ) in degrees
dy_offset, dz_offset = 0.002500, -0.004167
# Sanity check that offset between as-planned PCAD and target are as expected. This
# uses the baseline ODB_SI_ALIGN. The calc_offsets() function has been independently
# validated for planning purposes.
dy_nom_align, dz_nom_align = calc_offsets(ra_targ, dec_targ,
q_pcad.ra, q_pcad.dec, q_pcad.roll)
assert np.allclose(dy_nom_align, dy_offset, atol=3e-6) # 0.01 arcsec
assert np.allclose(dz_nom_align, dz_offset, atol=3e-6)
# Test calc_pcad_from_targ() for baseline SI_ALIGN
q_pcad_calc = calc_pcad_from_targ(ra_targ, dec_targ, q_pcad.roll, dy_offset, dz_offset)
dq = q_pcad.inv() * q_pcad_calc
assert np.allclose(dq.q[0] * 2, 0, atol=36 * ARCSEC2RAD)
assert np.allclose(dq.q[1] * 2, 0, atol=0.01 * ARCSEC2RAD)
assert np.allclose(dq.q[2] * 2, 0, atol=0.01 * ARCSEC2RAD)
# Compute PCAD attitude using a new SI ALIGN values based on OR-list TARGET_OFFSET
# values and with no target offset applied. This must match the as-planned
# attitude using the baseline ODB_SI_ALIGN and WITH a target offset applied.
si_align = calc_si_align(dy_offset, dz_offset, check_consistency=True)
q_pcad_align = q_targ * Quat(si_align).inv()
dy_align, dz_align = calc_offsets(ra_targ, dec_targ, *q_pcad_align.equatorial)
dq = q_pcad.inv() * q_pcad_align
# Check delta quaternion (error) is less than 0.01 arcsec
# in pitch and yaw, 36 arcsec in roll.
assert np.allclose(dq.q[0] * 2, 0, atol=36 * ARCSEC2RAD)
assert np.allclose(dq.q[1] * 2, 0, atol=0.01 * ARCSEC2RAD)
assert np.allclose(dq.q[2] * 2, 0, atol=0.01 * ARCSEC2RAD)
# Check offsets match to better than 0.01 arcsec
assert np.allclose(dy_align, dy_offset, atol=3e-6)
assert np.allclose(dz_align, dz_offset, atol=3e-6)
from Quaternion import Quat
u=np.array([0.465391, -0.711934, 0.438905, -0.289695])
nu = np.sqrt((u*u).sum())
u /= nu
w, x, y, z = u[0], u[1], u[2], u[3]
Rmat = [[1 - 2*(y*y + z*z), 2*(x*y - z*w), 2*(x*z + y*w)],
[2*(x*y + z*w), 1 - 2*(x*x + z*z), 2*(y*z - x*w)],
[2*(x*z - y*w), 2*(y*z + x*w), 1 - 2*(x*x + y*y)]]
assert np.isclose(np.trace(Rmat)+1, 4*w*w, 1e-3)
Rmat = -np.array(Rmat)
print(Rmat)
u=np.array([-0.711934, 0.438905,-0.289695,0.465391,])
# x y z w
q = Quat(u)
matrix=q._get_transform()
print(matrix)
db = database.Database(debug=1)
ex = db.get_data(fname="CHA-Nic-YO-11-TCon-PARTX.csv")[0]
print("tot ",len(ex.data_earth))
d=4
Fs=100
print(len(ex.data_sensor[3,0:d*Fs]))
import db_marche.process.Segmentation as seg
s = seg.Segmentation(ex, level=0.2)
s.fit()
print(s.seg_from_labels())