コード例 #1
0
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)
コード例 #2
0
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)
コード例 #3
0
ファイル: transform.py プロジェクト: sot/chandra_aca
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
コード例 #4
0
ファイル: aca_lts_eval.py プロジェクト: sot/aca_required_temp
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
コード例 #5
0
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)
コード例 #6
0
ファイル: test_all.py プロジェクト: sot/chandra_aca
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)
コード例 #7
0
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
コード例 #8
0
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
コード例 #9
0
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
コード例 #10
0
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
コード例 #11
0
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
コード例 #12
0
ファイル: asp_l1.py プロジェクト: sot/mica
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([]), []
コード例 #13
0
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
コード例 #14
0
ファイル: test_catalog_fetches.py プロジェクト: sot/mica
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
コード例 #15
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
コード例 #16
0
ファイル: test_all.py プロジェクト: sot/chandra_aca
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.
コード例 #17
0
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
コード例 #18
0
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
コード例 #19
0
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
コード例 #20
0
ファイル: test_catalog_fetches.py プロジェクト: sot/mica
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
コード例 #21
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
コード例 #22
0
ファイル: update_acq_stats.py プロジェクト: sot/mica
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
コード例 #23
0
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
コード例 #24
0
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
コード例 #25
0
ファイル: dynamic_point.py プロジェクト: KKalem/toolbox
    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
コード例 #26
0
ファイル: eye_3D.py プロジェクト: youssefzaky/phd
    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
コード例 #27
0
ファイル: test_catalog_fetches.py プロジェクト: sot/mica
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
コード例 #28
0
ファイル: calc_si_align.py プロジェクト: sot/aimpoint_mon
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
コード例 #29
0
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
コード例 #30
0
 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
コード例 #31
0
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)
コード例 #32
0
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
コード例 #33
0
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
コード例 #34
0
ファイル: transform.py プロジェクト: sot/chandra_aca
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
コード例 #35
0
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])
コード例 #36
0
ファイル: test_catalog_fetches.py プロジェクト: sot/mica
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
        ]))
コード例 #37
0
ファイル: calc_si_align.py プロジェクト: sot/aimpoint_mon
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)
コード例 #38
0
ファイル: Connection.py プロジェクト: jjmantilla/FilesM
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())