def _calc_secondary_centers_unc(self, c1s, c1s_err, c2s,
                                    c2s_err, data_frame_object: object):
        """Calculate the coordinates of the cluster centers for the coordinate system that
        was not used for the fit.

        Standard errors are calculated with typical standard error propagation methods.
        Assign the attributes 'centers_array_' and 'noise_colors_'.

        Parameters
        ----------
        c1s : array-like, shape (n_components,)
            The x-coordinates of the cluster centers if clustering
            with Cartesian coordinates, otherwise the r-coordinates
            of the cluster centers.

        c1s_err : array-like, shape (n_components,)
            The standard error in the c1s.

        c2s : array-like, shape (n_components,)
            The y-coordinates of the cluster centers if clustering
            with Cartesian coordinates, otherwise the p-coordinates
            of the cluster centers.

        c2s_err : array-like, shape (n_components,)
            The standard error in the c2s.

        data_frame_object : DataFrame class object
            The object that contains the processed data and
            information that is used by the algorithms to do the
            fits.
        """
        xC, yC = data_frame_object.center
        xC_unc, yC_unc = data_frame_object.center_unc

        if self.coordinates == 'Cartesian':
            # Calculate the radii and phases of cluster centers, with standard errors
            rs = np.sqrt(np.square(np.subtract(c1s, xC)) +
                         np.square(np.subtract(c2s, yC)))
            rs_err = (1 / rs) * np.sqrt((c1s_err * (c1s - xC)) ** 2 +
                                        (xC_unc * (c1s - xC)) ** 2 +
                                        (c2s_err * (c2s - yC)) ** 2 +
                                        (yC_unc * (c2s - yC)) ** 2)
            ps = np.rad2deg(np.arctan(np.divide(np.subtract(c2s, yC), np.subtract(c1s, xC))))
            for i in range(len(c1s)):
                if c1s[i] - xC < 0:
                    ps[i] += 180
                if c1s[i] - xC > 0 > c2s[i] - yC:
                    ps[i] += 360
            ps_err = np.rad2deg((1 / rs ** 2) * np.sqrt((c2s_err * (c1s - xC)) ** 2 +
                                                        (yC_unc * (c1s - xC)) ** 2 +
                                                        (c1s_err * (c2s - yC)) ** 2 +
                                                        (xC_unc * (c2s - yC)) ** 2))

            cluster_err = np.sqrt(np.add(np.square(c1s_err),
                                         np.square(c2s_err)))

            self.centers_array_ = np.vstack((c1s, c1s_err, c2s,
                                             c2s_err, rs, rs_err,
                                             ps, ps_err,
                                             cluster_err)).T

        else:  # if self.coordinates == 'Polar':
            # Calculate the x- and y- coordinates of the cluster centers, with standard errors.
            # First, ensure data is not phase shifted.
            if data_frame_object.phase_shifted_:
                shift = data_frame_object.phase_shift_

                c2s += shift
                c2s = np.where(c2s > 360, c2s - 360, c2s)

                self.means_[:, 1] += shift
                self.means_[:, 1] = np.where(
                    self.means_[:, 1] > 360, self.means_[:, 1] - 360, self.means_[:, 1])

                p_raw = data_frame_object.data_array_[:, 3]
                p_raw += shift
                p_raw = np.where(p_raw > 360, p_raw - 360, p_raw)
                data_frame_object.data_array_[:, 3] = p_raw
                data_frame_object.phase_shifted_ = False

            # Convert to radians
            phases = np.deg2rad(c2s)
            phases_err = np.deg2rad(c2s_err)

            xs = (c1s * np.cos(phases)) + xC
            xs_err = np.sqrt((c1s_err * np.cos(phases)) ** 2 +
                             (phases_err * c1s * np.sin(phases)) ** 2 +
                             xC_unc ** 2)
            ys = (c1s * np.sin(phases)) + yC
            ys_err = np.sqrt((c1s_err * np.sin(phases)) ** 2 +
                             (phases_err * c1s * np.cos(phases)) ** 2 +
                             yC_unc ** 2)
            cluster_err = np.sqrt(np.add(np.square(xs_err),
                                         np.square(ys_err)))

            self.centers_array_ = np.vstack((xs, xs_err, ys, ys_err,
                                             c1s, c1s_err, c2s,
                                             c2s_err, cluster_err)).T

        self._identify_noise_colors()
Beispiel #2
0
def _get_vhdr_info(vhdr_fname, eog, misc, scale, montage):
    """Extract all the information from the header file.

    Parameters
    ----------
    vhdr_fname : str
        Raw EEG header to be read.
    eog : list of str
        Names of channels that should be designated EOG channels. Names should
        correspond to the vhdr file.
    misc : list or tuple of str | 'auto'
        Names of channels or list of indices that should be designated
        MISC channels. Values should correspond to the electrodes
        in the vhdr file. If 'auto', units in vhdr file are used for inferring
        misc channels. Default is ``'auto'``.
    scale : float
        The scaling factor for EEG data. Unless specified otherwise by
        header file, units are in microvolts. Default scale factor is 1.
    montage : str | None | instance of Montage
        Path or instance of montage containing electrode positions. If None,
        read sensor locations from header file if present, otherwise (0, 0, 0).
        See the documentation of :func:`mne.channels.read_montage` for more
        information.

    Returns
    -------
    info : Info
        The measurement info.
    fmt : str
        The data format in the file.
    edf_info : dict
        A dict containing Brain Vision specific parameters.
    events : array, shape (n_events, 3)
        Events from the corresponding vmrk file.
    """
    scale = float(scale)
    ext = op.splitext(vhdr_fname)[-1]
    if ext != '.vhdr':
        raise IOError("The header file must be given to read the data, "
                      "not a file with extension '%s'." % ext)
    with open(vhdr_fname, 'rb') as f:
        # extract the first section to resemble a cfg
        header = f.readline()
        codepage = 'utf-8'
        # we don't actually need to know the coding for the header line.
        # the characters in it all belong to ASCII and are thus the
        # same in Latin-1 and UTF-8
        header = header.decode('ascii', 'ignore').strip()
        _check_hdr_version(header)

        settings = f.read()
        try:
            # if there is an explicit codepage set, use it
            # we pretend like it's ascii when searching for the codepage
            cp_setting = re.search('Codepage=(.+)',
                                   settings.decode('ascii', 'ignore'),
                                   re.IGNORECASE & re.MULTILINE)
            if cp_setting:
                codepage = cp_setting.group(1).strip()
            # BrainAmp Recorder also uses ANSI codepage
            # an ANSI codepage raises a LookupError exception
            # python recognize ANSI decoding as cp1252
            if codepage == 'ANSI':
                codepage = 'cp1252'
            settings = settings.decode(codepage)
        except UnicodeDecodeError:
            # if UTF-8 (new standard) or explicit codepage setting fails,
            # fallback to Latin-1, which is Windows default and implicit
            # standard in older recordings
            settings = settings.decode('latin-1')

    if settings.find('[Comment]') != -1:
        params, settings = settings.split('[Comment]')
    else:
        params, settings = settings, ''
    cfg = configparser.ConfigParser()
    if hasattr(cfg, 'read_file'):  # newer API
        cfg.read_file(StringIO(params))
    else:
        cfg.readfp(StringIO(params))

    # get sampling info
    # Sampling interval is given in microsec
    cinfostr = 'Common Infos'
    if not cfg.has_section(cinfostr):
        cinfostr = 'Common infos'  # NeurOne BrainVision export workaround

    # get sampling info
    # Sampling interval is given in microsec
    sfreq = 1e6 / cfg.getfloat(cinfostr, 'SamplingInterval')
    info = _empty_info(sfreq)

    order = cfg.get(cinfostr, 'DataOrientation')
    if order not in _orientation_dict:
        raise NotImplementedError('Data Orientation %s is not supported'
                                  % order)
    order = _orientation_dict[order]

    data_format = cfg.get(cinfostr, 'DataFormat')
    if data_format == 'BINARY':
        fmt = cfg.get('Binary Infos', 'BinaryFormat')
        if fmt not in _fmt_dict:
            raise NotImplementedError('Datatype %s is not supported' % fmt)
        fmt = _fmt_dict[fmt]
    else:
        if order == 'C':  # channels in rows
            raise NotImplementedError('BrainVision files with ASCII data in '
                                      'vectorized order (i.e. channels in rows'
                                      ') are not supported yet.')
        fmt = dict((key, cfg.get('ASCII Infos', key))
                   for key in cfg.options('ASCII Infos'))

    # locate EEG binary file and marker file for the stim channel
    path = op.dirname(vhdr_fname)
    data_filename = op.join(path, cfg.get(cinfostr, 'DataFile'))
    mrk_fname = op.join(path, cfg.get(cinfostr, 'MarkerFile'))

    # Try to get measurement date from marker file
    # Usually saved with a marker "New Segment", see BrainVision documentation
    regexp = r'^Mk\d+=New Segment,.*,\d+,\d+,\d+,(\d{20})$'
    with open(mrk_fname, 'r') as tmp_mrk_f:
        lines = tmp_mrk_f.readlines()

    for line in lines:
        match = re.findall(regexp, line.strip())

        # Always take first measurement date we find
        if match and match[0] != '00000000000000000000':
            date_str = match[0]
            meas_date = datetime.strptime(date_str, '%Y%m%d%H%M%S%f')

            # We need list of unix time in milliseconds and as second entry
            # the additional amount of microseconds
            epoch = datetime.utcfromtimestamp(0)
            unix_time = (meas_date - epoch).total_seconds()
            unix_secs = int(modf(unix_time)[1])
            microsecs = int(modf(unix_time)[0] * 1e6)
            info['meas_date'] = [unix_secs, microsecs]
            break

    else:
        info['meas_date'] = DATE_NONE

    # load channel labels
    nchan = cfg.getint(cinfostr, 'NumberOfChannels') + 1
    n_samples = None
    if order == 'C':
        try:
            n_samples = cfg.getint(cinfostr, 'DataPoints')
        except configparser.NoOptionError:
            logger.warning('No info on DataPoints found. Inferring number of '
                           'samples from the data file size.')
            with open(data_filename, 'rb') as fid:
                fid.seek(0, 2)
                n_bytes = fid.tell()
                n_samples = n_bytes // _fmt_byte_dict[fmt] // (nchan - 1)

    ch_names = [''] * nchan
    cals = np.empty(nchan)
    ranges = np.empty(nchan)
    cals.fill(np.nan)
    ch_dict = dict()
    misc_chs = dict()
    for chan, props in cfg.items('Channel Infos'):
        n = int(re.findall(r'ch(\d+)', chan)[0]) - 1
        props = props.split(',')
        # default to microvolts because that's what the older brainvision
        # standard explicitly assumed; the unit is only allowed to be
        # something else if explicitly stated (cf. EEGLAB export below)
        if len(props) < 4:
            props += (u'µV',)
        name, _, resolution, unit = props[:4]
        ch_dict[chan] = name
        ch_names[n] = name
        if resolution == "":
            if not(unit):  # For truncated vhdrs (e.g. EEGLAB export)
                resolution = 0.000001
            else:
                resolution = 1.  # for files with units specified, but not res
        unit = unit.replace(u'\xc2', u'')  # Remove unwanted control characters
        cals[n] = float(resolution)
        ranges[n] = _unit_dict.get(unit, 1) * scale
        if unit not in ('V', 'nV', u'µV', 'uV'):
            misc_chs[name] = (FIFF.FIFF_UNIT_CEL if unit == 'C'
                              else FIFF.FIFF_UNIT_NONE)
    misc = list(misc_chs.keys()) if misc == 'auto' else misc

    # create montage
    if cfg.has_section('Coordinates') and montage is None:
        from ...transforms import _sph_to_cart
        from ...channels.montage import Montage
        montage_pos = list()
        montage_names = list()
        to_misc = list()
        for ch in cfg.items('Coordinates'):
            ch_name = ch_dict[ch[0]]
            montage_names.append(ch_name)
            radius, theta, phi = map(float, ch[1].split(','))
            # 1: radius, 2: theta, 3: phi
            pol = np.deg2rad(theta)
            az = np.deg2rad(phi)
            pos = _sph_to_cart(np.array([[radius * 85., az, pol]]))[0]
            if (pos == 0).all() and ch_name not in list(eog) + misc:
                to_misc.append(ch_name)
            montage_pos.append(pos)
        montage_sel = np.arange(len(montage_pos))
        montage = Montage(montage_pos, montage_names, 'Brainvision',
                          montage_sel)
        if len(to_misc) > 0:
            misc += to_misc
            warn('No coordinate information found for channels {}. '
                 'Setting channel types to misc. To avoid this warning, set '
                 'channel types explicitly.'.format(to_misc))

    ch_names[-1] = 'STI 014'
    cals[-1] = 1.
    ranges[-1] = 1.
    if np.isnan(cals).any():
        raise RuntimeError('Missing channel units')

    # Attempts to extract filtering info from header. If not found, both are
    # set to zero.
    settings = settings.splitlines()
    idx = None

    if 'Channels' in settings:
        idx = settings.index('Channels')
        settings = settings[idx + 1:]
        hp_col, lp_col = 4, 5
        for idx, setting in enumerate(settings):
            if re.match(r'#\s+Name', setting):
                break
            else:
                idx = None

    # If software filters are active, then they override the hardware setup
    # But we still want to be able to double check the channel names
    # for alignment purposes, we keep track of the hardware setting idx
    idx_amp = idx

    if 'S o f t w a r e  F i l t e r s' in settings:
        idx = settings.index('S o f t w a r e  F i l t e r s')
        for idx, setting in enumerate(settings[idx + 1:], idx + 1):
            if re.match(r'#\s+Low Cutoff', setting):
                hp_col, lp_col = 1, 2
                warn('Online software filter detected. Using software '
                     'filter settings and ignoring hardware values')
                break
            else:
                idx = idx_amp

    if idx:
        lowpass = []
        highpass = []

        # for newer BV files, the unit is specified for every channel
        # separated by a single space, while for older files, the unit is
        # specified in the column headers
        divider = r'\s+'
        if 'Resolution / Unit' in settings[idx]:
            shift = 1  # shift for unit
        else:
            shift = 0

        # Extract filter units and convert from seconds to Hz if necessary.
        # this cannot be done as post-processing as the inverse t-f
        # relationship means that the min/max comparisons don't make sense
        # unless we know the units.
        #
        # For reasoning about the s to Hz conversion, see this reference:
        # `Ebersole, J. S., & Pedley, T. A. (Eds.). (2003).
        # Current practice of clinical electroencephalography.
        # Lippincott Williams & Wilkins.`, page 40-41
        header = re.split(r'\s\s+', settings[idx])
        hp_s = '[s]' in header[hp_col]
        lp_s = '[s]' in header[lp_col]

        for i, ch in enumerate(ch_names[:-1], 1):
            line = re.split(divider, settings[idx + i])
            # double check alignment with channel by using the hw settings
            if idx == idx_amp:
                line_amp = line
            else:
                line_amp = re.split(divider, settings[idx_amp + i])
            assert ch in line_amp

            highpass.append(line[hp_col + shift])
            lowpass.append(line[lp_col + shift])
        if len(highpass) == 0:
            pass
        elif len(set(highpass)) == 1:
            if highpass[0] in ('NaN', 'Off'):
                pass  # Placeholder for future use. Highpass set in _empty_info
            elif highpass[0] == 'DC':
                info['highpass'] = 0.
            else:
                info['highpass'] = float(highpass[0])
                if hp_s:
                    # filter time constant t [secs] to Hz conversion: 1/2*pi*t
                    info['highpass'] = 1. / (2 * np.pi * info['highpass'])

        else:
            heterogeneous_hp_filter = True
            if hp_s:
                # We convert channels with disabled filters to having
                # highpass relaxed / no filters
                highpass = [float(filt) if filt not in ('NaN', 'Off', 'DC')
                            else np.Inf for filt in highpass]
                info['highpass'] = np.max(np.array(highpass, dtype=np.float))
                # Coveniently enough 1 / np.Inf = 0.0, so this works for
                # DC / no highpass filter
                # filter time constant t [secs] to Hz conversion: 1/2*pi*t
                info['highpass'] = 1. / (2 * np.pi * info['highpass'])

                # not exactly the cleanest use of FP, but this makes us
                # more conservative in *not* warning.
                if info['highpass'] == 0.0 and len(set(highpass)) == 1:
                    # not actually heterogeneous in effect
                    # ... just heterogeneously disabled
                    heterogeneous_hp_filter = False
            else:
                highpass = [float(filt) if filt not in ('NaN', 'Off', 'DC')
                            else 0.0 for filt in highpass]
                info['highpass'] = np.min(np.array(highpass, dtype=np.float))
                if info['highpass'] == 0.0 and len(set(highpass)) == 1:
                    # not actually heterogeneous in effect
                    # ... just heterogeneously disabled
                    heterogeneous_hp_filter = False

            if heterogeneous_hp_filter:
                warn('Channels contain different highpass filters. '
                     'Lowest (weakest) filter setting (%0.2f Hz) '
                     'will be stored.' % info['highpass'])

        if len(lowpass) == 0:
            pass
        elif len(set(lowpass)) == 1:
            if lowpass[0] in ('NaN', 'Off'):
                pass  # Placeholder for future use. Lowpass set in _empty_info
            else:
                info['lowpass'] = float(lowpass[0])
                if lp_s:
                    # filter time constant t [secs] to Hz conversion: 1/2*pi*t
                    info['lowpass'] = 1. / (2 * np.pi * info['lowpass'])

        else:
            heterogeneous_lp_filter = True
            if lp_s:
                # We convert channels with disabled filters to having
                # infinitely relaxed / no filters
                lowpass = [float(filt) if filt not in ('NaN', 'Off')
                           else 0.0 for filt in lowpass]
                info['lowpass'] = np.min(np.array(lowpass, dtype=np.float))
                try:
                    # filter time constant t [secs] to Hz conversion: 1/2*pi*t
                    info['lowpass'] = 1. / (2 * np.pi * info['lowpass'])

                except ZeroDivisionError:
                    if len(set(lowpass)) == 1:
                        # No lowpass actually set for the weakest setting
                        # so we set lowpass to the Nyquist frequency
                        info['lowpass'] = info['sfreq'] / 2.
                        # not actually heterogeneous in effect
                        # ... just heterogeneously disabled
                        heterogeneous_lp_filter = False
                    else:
                        # no lowpass filter is the weakest filter,
                        # but it wasn't the only filter
                        pass
            else:
                # We convert channels with disabled filters to having
                # infinitely relaxed / no filters
                lowpass = [float(filt) if filt not in ('NaN', 'Off')
                           else np.Inf for filt in lowpass]
                info['lowpass'] = np.max(np.array(lowpass, dtype=np.float))

                if np.isinf(info['lowpass']):
                    # No lowpass actually set for the weakest setting
                    # so we set lowpass to the Nyquist frequency
                    info['lowpass'] = info['sfreq'] / 2.
                    if len(set(lowpass)) == 1:
                        # not actually heterogeneous in effect
                        # ... just heterogeneously disabled
                        heterogeneous_lp_filter = False

            if heterogeneous_lp_filter:
                # this isn't clean FP, but then again, we only want to provide
                # the Nyquist hint when the lowpass filter was actually
                # calculated from dividing the sampling frequency by 2, so the
                # exact/direct comparison (instead of tolerance) makes sense
                if info['lowpass'] == info['sfreq'] / 2.0:
                    nyquist = ', Nyquist limit'
                else:
                    nyquist = ""
                warn('Channels contain different lowpass filters. '
                     'Highest (weakest) filter setting (%0.2f Hz%s) '
                     'will be stored.' % (info['lowpass'], nyquist))

    # Creates a list of dicts of eeg channels for raw.info
    logger.info('Setting channel info structure...')
    info['chs'] = []
    for idx, ch_name in enumerate(ch_names):
        if ch_name in eog or idx in eog or idx - nchan in eog:
            kind = FIFF.FIFFV_EOG_CH
            coil_type = FIFF.FIFFV_COIL_NONE
            unit = FIFF.FIFF_UNIT_V
        elif ch_name in misc or idx in misc or idx - nchan in misc:
            kind = FIFF.FIFFV_MISC_CH
            coil_type = FIFF.FIFFV_COIL_NONE
            if ch_name in misc_chs:
                unit = misc_chs[ch_name]
            else:
                unit = FIFF.FIFF_UNIT_NONE
        elif ch_name == 'STI 014':
            kind = FIFF.FIFFV_STIM_CH
            coil_type = FIFF.FIFFV_COIL_NONE
            unit = FIFF.FIFF_UNIT_NONE
        else:
            kind = FIFF.FIFFV_EEG_CH
            coil_type = FIFF.FIFFV_COIL_EEG
            unit = FIFF.FIFF_UNIT_V
        info['chs'].append(dict(
            ch_name=ch_name, coil_type=coil_type, kind=kind, logno=idx + 1,
            scanno=idx + 1, cal=cals[idx], range=ranges[idx],
            loc=np.full(12, np.nan),
            unit=unit, unit_mul=0.,  # always zero- mne manual pg. 273
            coord_frame=FIFF.FIFFV_COORD_HEAD))

    info._update_redundant()
    info._check_consistency()
    return info, data_filename, fmt, order, mrk_fname, montage, n_samples
Beispiel #3
0
def MakeParams():
    # TODO: Write helper function to do the landing zone(s)
    # coordinate assembly and rotation.

    # Coordinates of prepared area along East-West radial,
    # in GS reference frame when flying EW radial.
    primary_radial = np.deg2rad(91.0)
    primary_x_low = -500.0
    primary_x_high = 0.0
    primary_y_low = -100.0
    primary_y_high = 20.0

    primary_radial_coords = np.array(
        [[primary_x_low, primary_x_high, primary_x_high, primary_x_low],
         [primary_y_low, primary_y_low, primary_y_high, primary_y_high]])

    primary_vertices_list = np.transpose(primary_radial_coords).tolist()

    # Coordinates of prepared area along North-South radial,
    # in GS reference frame when flying EW radial.
    secondary_radial = np.deg2rad(10.0)
    secondary_x_low = -500.0
    secondary_x_high = 0.0
    secondary_y_low = -20.0
    secondary_y_high = 100.0

    radial_diff = secondary_radial - primary_radial
    dcm_secondary_radial = np.array(
        [[np.cos(radial_diff), -np.sin(radial_diff)],
         [np.sin(radial_diff), np.cos(radial_diff)]])
    secondary_radial_coords = np.array([[
        secondary_x_low, secondary_x_high, secondary_x_high, secondary_x_low
    ], [secondary_y_low, secondary_y_low, secondary_y_high, secondary_y_high]])

    secondary_vertices = np.dot(dcm_secondary_radial, secondary_radial_coords)
    secondary_vertices_list = np.transpose(secondary_vertices).tolist()

    # Parker Ranch pad coordinates (no-landing zone).
    pr_pad_coords = np.array([[
        -18.288, 30.48, 30.48, -30.48, -30.48, -57.912000000000006, -74.0664,
        -102.108, -103.632, -99.6696, -89.91600000000001, -51.816, -30.48,
        -18.288
    ],
                              [
                                  -30.48, -30.48, 30.48, 30.48, -6.096, -6.096,
                                  -18.288, -59.436, -68.58, -77.724, -79.248,
                                  -68.58, -50.292, -30.48
                              ]])
    pr_pad_vertices_list = np.transpose(pr_pad_coords).tolist()

    # Nominal flight path angle [rad] of a gliding descent at
    # WingSave-trimmed angle of attack, empirically adjusted to account
    # for motor drag.
    gs = -0.122

    return {
        'primary_vertices': primary_vertices_list,
        'secondary_vertices': secondary_vertices_list,
        'pr_pad_vertices': pr_pad_vertices_list,

        # Flight path angle [rad] to display on manual monitor.
        'glideslope': gs,

        # Downwind and upwind x-locations [m] of landing area, respectively.
        'threshold': primary_x_low,
        'overrun': primary_x_high
    }
def radial_hours(N):
    hours = np.deg2rad(np.linspace(0, 360, N-1, endpoint=False))
    hours = np.append(hours, hours[0])
    return hours
Beispiel #5
0
def evaluateCross(param,
                  im,
                  verbose=0,
                  fig=None,
                  istep=1,
                  istepmodel=1,
                  linewidth=2,
                  usemask=False,
                  use_abs=False,
                  w=2.5):
    """ Calculate cross matching score

    Args:
        param (array or list): used by createCross to create image template
        im (numpy array): 

    Returns:
        cost, patch, cdata, tuple

    See also:
        createCross

    """
    samplesize = [
        int(im.shape[1] * istep / istepmodel),
        int(im.shape[0] * istep / istepmodel)
    ]
    param = np.array(param)
    aa = param[3:]

    H = evaluateCross.scaling0.copy()
    H[0, 0] = istep / istepmodel
    H[1, 1] = istep / istepmodel

    dsize = (samplesize[0], samplesize[1])
    patch = cv2.warpPerspective(im.astype(np.float32), H, dsize, None,
                                (cv2.INTER_LINEAR), cv2.BORDER_CONSTANT, -1)

    if verbose:
        print('evaluateCross: patch shape %s' % (patch.shape, ))
    modelpatch, cdata = createCross(param,
                                    samplesize,
                                    centermodel=False,
                                    istep=istepmodel,
                                    verbose=verbose >= 2,
                                    w=w)
    (cc, lp, hp, ip, op, _, _, _) = cdata

    if use_abs:
        dd = np.abs(patch) - modelpatch
    else:
        dd = patch - modelpatch

    if usemask:
        # near model mask
        #mask = (modelpatch>1).astype(int)

        # distance centre mask
        imtmp = 10 + 0 * modelpatch.copy()
        imtmp[int(imtmp.shape[1] / 2), int(imtmp.shape[0] / 2)] = 0
        mask = scipy.ndimage.distance_transform_edt(imtmp)
        mask = 1 - .75 * mask / mask.max()

        dd = dd * mask

    cost = np.linalg.norm(dd)

    # area of intersection
    rr = np.array([[0., im.shape[1]], [0, im.shape[0]]])
    ppx = pgeometry.region2poly(
        np.array([[0, samplesize[0]], [0., samplesize[1]]]))
    ppimage = pgeometry.region2poly(rr)
    pppatch = pgeometry.projectiveTransformation(H, ppimage)
    ppi = pgeometry.polyintersect(ppx.T, pppatch.T).T
    A = pgeometry.polyarea(ppi.T)
    A0 = pgeometry.polyarea(ppx.T)

    # special rules
    if np.abs(A / A0) < .85:
        if verbose:
            print('  add cost for A/A0: A %f A0 %f' % (A, A0))
        cost += 4000
    if aa[0] < 0 or aa[0] > np.pi / 2 - np.deg2rad(5):
        cost += 10000
    if aa[1] < np.pi or aa[1] > 3 * np.pi / 2:
        if verbose:
            print('  add cost for alpha')
        cost += 10000
    if aa[2] < np.pi or aa[2] > 3 * np.pi / 2:
        if verbose:
            print('  add cost for alpha')
        cost += 10000
    if aa[3] < 0 or aa[3] > np.pi / 2:
        if verbose:
            print('  add cost for alpha')
        cost += 10000

    if 1:
        ccim = (np.array(im.shape) / 2 + .5) * istep
        tmp = np.linalg.norm(ccim - param[0:2])
        dcost = 2000 * pgeometry.logistic(tmp, np.mean(ccim), istep)
        if verbose:
            print('  add cost for image cc: %.1f' % dcost)
        cost += dcost
    if pgeometry.angleDiff(aa[0], aa[1]) < np.deg2rad(30):
        if verbose:
            print('  add cost for angle diff')
        cost += 1000
    if pgeometry.angleDiff(aa[2], aa[3]) < np.deg2rad(30):
        if verbose:
            print('  add cost for angle diff')
        cost += 1000

    if pgeometry.angleDiffOri(aa[0], aa[2]) > np.deg2rad(10):
        cost += 10000
    if pgeometry.angleDiffOri(aa[1], aa[3]) > np.deg2rad(10):
        cost += 10000
    if param[2] < 0:
        if verbose:
            print('  add cost for negative param')
        cost += 10000

    if np.abs(param[2]) > 7.:
        if verbose:
            print('  add cost large param[2]')
        cost += 10000

    if np.abs(param[2] - 10) > 8:
        cost += 10000

    if len(param) > 7:
        if np.abs(pgeometry.angleDiff(param[7], np.pi / 4)) > np.deg2rad(30):
            cost += 10000

    if not fig is None:
        _showIm(patch, fig=fig)
        plt.title('Image patch: cost %.1f: istep %.2f' % (cost, istepmodel))
        pgeometry.addfigurecopy(fig=fig)
        plt.plot([float(lp[0]), float(hp[0])],
                 [float(lp[1]), float(hp[1])],
                 '.--m',
                 linewidth=linewidth,
                 markersize=10,
                 label='transition line')
        plt.plot(cc[0], cc[1], '.m', markersize=12)
        for ii in range(4):
            if ii == 0:
                lbl = 'electron line'
            else:
                lbl = None
            plt.plot([op[ii, 0], ip[ii, 0]], [op[ii, 1], ip[ii, 1]],
                     '.-',
                     linewidth=linewidth,
                     color=[0, .7, 0],
                     label=lbl)
            pgeometry.plotLabels(
                np.array((op[ii, :] + ip[ii, :]) / 2).reshape((2, -1)),
                '%d' % ii)
        if verbose >= 1:
            _showIm(modelpatch, fig=fig + 1)
            plt.title('Model patch: cost %.1f' % cost)
            _showIm(np.abs(dd), fig=fig + 2)
            plt.title('diff patch: cost %.1f' % cost)
            plt.colorbar()
            plt.show()

    if verbose:
        print('evaluateCross: cost %.4f' % cost)
        #print('evaluateCross: param %s' % (str(param), ))
    return cost, patch, cdata, (H, )
    pass
Beispiel #6
0
def rotate(image, angle, resize=False, order=1, mode='constant', cval=0.):
    """Rotate image by a certain angle around its center.

    Parameters
    ----------
    image : ndarray
        Input image.
    angle : float
        Rotation angle in degrees in counter-clockwise direction.
    resize: bool, optional
        Determine whether the shape of the output image will be automatically
        calculated, so the complete rotated image exactly fits. Default is
        False.

    Returns
    -------
    rotated : ndarray
        Rotated version of the input.

    Other parameters
    ----------------
    order : int
        Order of splines used in interpolation.  See
        `scipy.ndimage.map_coordinates` for detail.
    mode : string
        How to handle values outside the image borders.  See
        `scipy.ndimage.map_coordinates` for detail.
    cval : string
        Used in conjunction with mode 'constant', the value outside
        the image boundaries.

    """

    rows, cols = image.shape[0], image.shape[1]

    # rotation around center
    translation = np.array((cols, rows)) / 2. - 0.5
    tform1 = SimilarityTransform(translation=-translation)
    tform2 = SimilarityTransform(rotation=np.deg2rad(angle))
    tform3 = SimilarityTransform(translation=translation)
    tform = tform1 + tform2 + tform3

    output_shape = None
    if resize:
        # determine shape of output image
        corners = np.array([[1, 1], [1, rows], [cols, rows], [cols, 1]])
        corners = tform(corners - 1)
        minc = corners[:, 0].min()
        minr = corners[:, 1].min()
        maxc = corners[:, 0].max()
        maxr = corners[:, 1].max()
        out_rows = maxr - minr + 1
        out_cols = maxc - minc + 1
        output_shape = np.ceil((out_rows, out_cols))

        # fit output image in new shape
        translation = ((cols - out_cols) / 2., (rows - out_rows) / 2.)
        tform4 = SimilarityTransform(translation=translation)
        tform = tform4 + tform

    return warp(image,
                tform,
                output_shape=output_shape,
                order=order,
                mode=mode,
                cval=cval)
    Returns error e given two states xgoal and x.
    Angle differences are taken properly on SO3.

    """
    e = xgoal - x
    c = np.cos(x[2])
    s = np.sin(x[2])
    cg = np.cos(xgoal[2])
    sg = np.sin(xgoal[2])
    e[2] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
    return e

################################################# OBJECTIVES

# Initial condition and goal
x0 = np.array([0, 0, np.deg2rad(0), 0, 0, 0])
goal = [40, 40, np.deg2rad(90), 0, 0, 0]
goal_buffer = [8, 8, np.inf, np.inf, np.inf, np.inf]
error_tol = np.copy(goal_buffer)/8

################################################# CONSTRAINTS

obs_choice = 'grid'

# No obstacles
if obs_choice == 'none':
    obs = []

# Some obstacles [x, y, radius]
elif obs_choice == 'some':
    obs = np.array([[20, 20, 5],
Beispiel #8
0
def sigmoid(phi, a, b, c, d):
    return a * np.cos((np.deg2rad(phi) * b + c)) + d
Beispiel #9
0
def get_transform(angle, displacement):
    angle = np.deg2rad(angle)
    rot = np.array([[m.cos(angle), -m.sin(angle), displacement[0]],
                    [m.sin(angle), m.cos(angle), displacement[1]], [0, 0, 1]])
    return rot
Beispiel #10
0
def FUV(t, tref, lind, lat, ngflgs):
    """
    UT_FUV()
    compute nodal/satellite correction factors and astronomical argument
    inputs
      t = times [datenum UTC] (nt x 1)
      tref = reference time [datenum UTC] (1 x 1)
      lind = list indices of constituents in ut_constants.mat (nc x 1)
      lat = latitude [deg N] (1 x 1)
      ngflgs = [NodsatLint NodsatNone GwchLint GwchNone] each 0/1
    output
      F = real nodsat correction to amplitude [unitless] (nt x nc)
      U = nodsat correction to phase [cycles] (nt x nc)
      V = astronomical argument [cycles] (nt x nc)
    UTide v1p0 9/2011 [email protected]
    (uses parts of t_vuf.m from t_tide, Pawlowicz et al 2002)
    """

    t = np.atleast_1d(t).flatten()
    nt = len(t)
    nc = len(lind)

    # nodsat

    if ngflgs[1]:
        F = np.ones((nt, nc))
        U = np.zeros((nt, nc))
    else:
        if ngflgs[0]:
            tt = np.array([tref])
        else:
            tt = t
        ntt = len(tt)

        astro, ader = ut_astron(tt)

        if abs(lat) < 5:
            lat = np.sign(lat) * 5

        slat = np.sin(np.deg2rad(lat))
        rr = sat.amprat.copy()

        j = sat.ilatfac == 1
        rr[j] *= 0.36309 * (1.0 - 5.0 * slat**2) / slat

        j = sat.ilatfac == 2
        rr[j] *= 2.59808 * slat

        # sat.deldood is (162, 3); all other sat vars are (162,)
        uu = np.dot(sat.deldood, astro[3:6, :]) + sat.phcorr[:, None]
        np.fmod(uu, 1, out=uu)  # fmod is matlab rem; differs from % op
        mat = rr[:, None] * np.exp(1j * 2 * np.pi * uu)

        nfreq = len(const.isat)  # 162
        F = np.ones((nfreq, ntt), dtype=complex)

        iconst = sat.iconst - 1
        ind = np.unique(iconst)
        for ii in ind:
            F[ii, :] = 1 + np.sum(mat[iconst == ii], axis=0)

        U = np.angle(F) / (2 * np.pi)  # cycles
        F = np.abs(F)

        for i0, nshal, k in zip(ishallow, nshallow, kshallow):
            ik = i0 + np.arange(nshal)
            j = shallow.iname[ik] - 1
            exp1 = shallow.coef[ik, None]
            exp2 = np.abs(exp1)
            F[k, :] = np.prod(F[j, :]**exp2, axis=0)
            U[k, :] = np.sum(U[j, :] * exp1, axis=0)

        F = F[lind, :].T
        U = U[lind, :].T

        # if ngflgs[0]:  # Nodal/satellite with linearized times.
        #    F = F[np.ones((nt, 1)), :]
        #    U = U[np.ones((nt, 1)), :]
        # Let's try letting broadcasting take care of it.

    # gwch (astron arg)
    if ngflgs[3]:  # None (raw phase lags not Greenwich phase lags).
        freq = linearized_freqs(tref)
        V = 24 * (t[:, np.newaxis] - tref) * freq[lind]

    else:
        if ngflgs[2]:  # Linearized times.
            tt = np.array([tref])
        else:
            tt = t  # Exact times.
        ntt = len(tt)

        astro, ader = ut_astron(tt)

        V = np.dot(const.doodson, astro) + const.semi[:, None]
        np.fmod(V, 1, out=V)

        for i0, nshal, k in zip(ishallow, nshallow, kshallow):
            ik = i0 + np.arange(nshal)
            j = shallow.iname[ik] - 1
            exp1 = shallow.coef[ik, None]
            V[k, :] = np.sum(V[j, :] * exp1, axis=0)

        V = V[lind, :].T

        if ngflgs[2]:  # linearized times
            freq = linearized_freqs(tref)
            V = V + 24 * (t[:, None] - tref) * freq[None, lind]

    return F, U, V
Beispiel #11
0
# phiM, UM = np.genfromtxt("data/phaseMIT.dat", unpack=True)
def sigmoid(phi, a, b, c, d):
    return a * np.cos((np.deg2rad(phi) * b + c)) + d


params, covariance_matrix = curve_fit(sigmoid,
                                      phi,
                                      U,
                                      p0=(94.67, 0.80, -16 + 3.1415, -4.3))

uncertainties = np.sqrt(np.diag(covariance_matrix))

for name, value, uncertainty in zip('abcd', params, uncertainties):
    print(f'{name} = {value:.4f} ± {uncertainty:.4f}')

plt.plot(phi, U, "kx", label="Messwerte OHNE", linewidth=1.5)

x = np.linspace(0, 350)
plt.plot(x - 20, (-94.671 * np.cos((np.deg2rad(x) * 0.807 - 16.500)) - 4.382),
         'b-',
         label='Lineare Ausgleichsgerade',
         linewidth=1.5)

plt.plot(x, (params[0] * np.cos(
    (np.deg2rad(x) * params[1] + params[2])) + params[3]),
         'b-',
         label='Lineare Ausgleichsgerade',
         linewidth=1.5)

plt.show()
Beispiel #12
0
# get size of the object (Y and X dimensions, to follow Numpy style)
print n.shape()

# get list with coordinates of the object corners
print n.get_corners()

# get lists with coordinates of the object borders
print n.get_border()

raw_counts = n[1]
inc_angle = n[2]

#~ sigma0 = n[3]

sigma0 = raw_counts**2.0 * sin(deg2rad(inc_angle))
sigma0 = 10*log10(sigma0)
n.add_band(bandID=4, array=sigma0)

# 1. Remove speckle noise (using Lee-Wiener filter)
speckle_filter('wiener', 7)

# Reprojected image into Lat/Lon WGS84 (Simple Cylindrical) projection
# 1. Cancel previous reprojection
# 2. Get corners of the image and the pixel resolution
# 3. Create Domain with stereographic projection, corner coordinates and resolution 1000m
# 4. Reproject
# 5. Write image
n.reproject() # 1.
lons, lats = n.get_corners() # 2.
pxlRes = distancelib.getPixelResolution(array(lats), array(lons), n.shape(), units="deg")
Beispiel #13
0
def rtclose(rt1, rt2, r_thresh=10.0, t_thresh=np.deg2rad(5.0)):
    """ compare lines in rho-theta space """
    r1, t1 = rt1
    r2, t2 = rt2
    return (abs(r1 - r2) < r_thresh) and (abs(t1 - t2) < t_thresh)
    def get_pdf_fig(self, data_frame_object: object):
        """Plot the pdf of the Gaussian mixture on a surface.

        The returned matplotlib.plyplot figure can be shown and saved
        separately.

        Parameters
        ----------
        data_frame_object : DataFrame class object
            The object that contains the processed data and
            information that is used by the algorithms to do the
            fits.

        Returns
        -------
        fig : matplotlib.pyplot figure
            The figure containing the pdf.

        save_string : str
            The recommended file name to use when saving the plot,
            which is done separately.
        """
        if not self.clustered_:
            raise NotImplementedError("Must run a method to cluster the "
                                      "data before visualization.")

        fig = plt.figure()
        ax = fig.gca(projection='3d')
        if self.coordinates == 'Cartesian':
            x1 = data_frame_object.data_array_[:, 0]
            x2 = data_frame_object.data_array_[:, 1]

            x1_range = max(x1) - min(x1)
            x1_grid_min = min(x1) - 0.1 * x1_range
            x1_grid_max = max(x1) + 0.1 * x1_range
            x1_grid_len = np.complex(0, 1000)

            x2_range = max(x2) - min(x2)
            x2_grid_min = min(x2) - 0.1 * x2_range
            x2_grid_max = max(x2) + 0.1 * x2_range
            x2_grid_len = np.complex(0, 1000)

        else:  # if self.coordinates == 'Polar':
            x1 = data_frame_object.data_array_[:, 2]
            x1_range = max(x1) - min(x1)
            x1_grid_min = min(x1) - 0.1 * x1_range
            x1_grid_max = max(x1) + 0.1 * x1_range
            x1_grid_len = np.complex(0, 1000)

            x2_grid_min = 0
            x2_grid_max = 360
            x2_grid_len = np.complex(0, 1000)

        x1_values, x2_values = np.mgrid[
                               x1_grid_min:x1_grid_max:x1_grid_len,
                               x2_grid_min:x2_grid_max:x2_grid_len]
        # Generate meshgrid of x1, x2 values to plot versus the
        # pdf of the GMM fit.
        grid = np.dstack((x1_values, x2_values))
        # Organize into a format usable by the .pdf function
        gmm_values = np.zeros((1000, 1000))
        # Initialize a data array that will hold the values of
        # the GMM for the corresponding x values

        for n in range(0, self.n_comps_found_):
            mean = self.means_[n]
            # Grab both dimensions of the mean from the model
            # result
            if self.cov_type == 'tied':
                cov = self.covariances_
            else:
                cov = self.covariances_[n]
                # Grab the covariance matrix from the model result.
            z_values = scipy.stats.multivariate_normal.pdf(
                grid, mean=mean, cov=cov)
            # Given a mean and covariance matrix, this calculates the pdf of a multivariate
            # Gaussian function for each x,y value.
            z_values *= self.weights_[n]
            # Weight the output by the weights given by the trained model
            gmm_values = np.add(gmm_values, z_values)
            # Add this array element-wise to the GMM array

        if self.coordinates == 'Polar':
            rads = x1_values
            phases = np.deg2rad(x2_values)
            x1_values = np.multiply(rads, np.cos(phases))
            x2_values = np.multiply(rads, np.sin(phases))

        ax.plot_surface(x1_values, x2_values, gmm_values,
                        cmap='viridis')
        title_string = 'GMM PDF (Cov type=%s, ' \
                       'n-components=%i)\n' % (self.cov_type,
                                               self.n_comps_found_)
        title_string += data_frame_object.file[0:-4] + '\n'
        title_string += 'TOF cut=(%.3f,%.3f), ' % \
                        data_frame_object.tof_cut
        title_string += 'Ion cut=%s, ' % (data_frame_object.ion_cut,)
        title_string += 'Rad cut=(%.1f,%.1f)' % \
                        data_frame_object.rad_cut
        title_string += 'Time cut=%s' % (data_frame_object.time_cut,)
        plt.title(title_string)
        ax.set_xlabel('X (mm)')
        ax.set_ylabel('Y (mm)')
        ax.set_zlabel('Probability Density')

        save_string = 'GMM %s %s PDF, %s, %s, %s Clusters, ' \
                      'timecut=%s,radcut=%s,tofcut=%s,' \
                      'ioncut=%s.jpeg' % (
                          self.ic, self.coordinates, data_frame_object.file[0:-4],
                          self.cov_type, self.n_comps_found_,
                          data_frame_object.time_cut, data_frame_object.rad_cut,
                          data_frame_object.tof_cut, data_frame_object.ion_cut)

        return fig, save_string
def main():
    # Input
    lambdas = np.r_[1e0, 1e2].astype(np.float64)
    preconditioners = np.r_[1.0, 1.0, 128.0, 128.0, 128.0].astype(np.float64)

    V, T = box_model(3, 50, 1.0, 1.0)
    N = V.shape[0]

    # Setup no rotations (fixed) except through middle of the bar
    heights = np.unique(V[:,2])
    middle = np.mean(heights)
    delta = 6.0
    I = np.argwhere(((middle - delta) <= V[:,2]) &
                    (V[:,2] < (middle + delta))).flatten()

    M = I.shape[0]

    K = np.zeros((N, 2), dtype=np.int32)
    K[I,0] = -1
    K[I,1] = np.arange(M)

    # Setup variables for solver
    Xg = np.zeros((1, 3), dtype=np.float64)
    s = np.ones((1, 1), dtype=np.float64)

    Xb = np.array([], dtype=np.float64).reshape(0, 3)
    y = np.array([], dtype=np.float64).reshape(0, 1)

    X = np.zeros((M, 3), dtype=np.float64)

    V1 = V.copy()

    # Setup `C`, `P1` and `P2`
    C1 = np.argwhere(V[:,2] >= heights[-3]).flatten()
    L1 = C1.shape[0]
    C2 = np.argwhere(V[:,2] <= heights[2]).flatten()
    L2 = C2.shape[0]
    C = np.r_[C1, C2]

    # Rotate `C1` vertices about their mean (by -30 on the x-axis)
    x = np.r_[np.deg2rad(-30), 0., 0.]
    R = quat.rotationMatrix(quat.quat(x))

    Q = V[C1]
    c = np.mean(Q, axis=0)
    Q = np.dot(Q - c, np.transpose(R)) + c

    Q[:, 0] += 20
    Q[:, 1] += 30
    Q[:, 2] -= 20

    P = np.ascontiguousarray(np.r_[Q, V[C2]])

    # Solve for `V1`
    status = solve_forward_sectioned_arap_proj(
        T, V, Xg, s, Xb, y, X, V1, K, C, P, lambdas, preconditioners, 
        isProjection=False,
        uniformWeights=False,
        fixedScale=True,
        maxIterations=100, 
        verbosenessLevel=1)

    # View
    # vis = VisualiseMesh()
    # vis.add_mesh(V1, T)
    # vis.add_points(V1[I], sphere_radius=0.2, actor_name='I', color=(0., 0., 1.))
    # vis.add_points(V1[C], sphere_radius=0.2, actor_name='C', color=(1., 0., 0.))
    # vis.camera_actions(('SetParallelProjection', (True,)))
    # vis.execute(magnification=4)

    # Rotate `C1` vertices about their mean (by -180 on the x-axis)
    V1 = V.copy()

    x = np.r_[np.deg2rad(+180), 0., 0.]
    R = quat.rotationMatrix(quat.quat(x))

    Q = V[C1]
    c = np.mean(Q, axis=0)
    Q = np.dot(Q - c, np.transpose(R)) + c

    Q[:, 1] += 10
    Q[:, 2] -= Q[-1, 2]

    P = np.ascontiguousarray(np.r_[Q, V[C2]])
    print 'P:'
    print np.around(P, decimals=3)

    # Assign a single rotation to the upper variables
    I1 = np.setdiff1d(np.argwhere(V[:,2] >= (middle + delta)).flatten(), 
                      C1, assume_unique=True)

    K[I1, 0] = -1
    K[I1, 1] = M

    Xg = np.zeros((1, 3), dtype=np.float64)
    X = np.zeros((M+1, 3), dtype=np.float64)

    # Solve for `V1`
    status = solve_forward_sectioned_arap_proj(
        T, V, Xg, s, Xb, y, X, V1, K, C, P, lambdas, preconditioners, 
        isProjection=False,
        uniformWeights=False,
        fixedScale=True,
        maxIterations=100, 
        verbosenessLevel=1)

    print 's:', s
    print 'X[M] (%.2f):' % np.rad2deg(norm(X[M])), np.around(X[M], decimals=3)

    # View
    # vis = VisualiseMesh()
    # vis.add_mesh(V1, T)
    # vis.add_points(V1[I], sphere_radius=0.2, actor_name='I', color=(0., 0., 1.))
    # vis.add_points(V1[C], sphere_radius=0.2, actor_name='C', color=(1., 0., 0.))
    # vis.camera_actions(('SetParallelProjection', (True,)))
    # vis.execute(magnification=4)

    # Deflect absolute positions further
    Q[:, 0] += 20

    # Assign a single basis rotation to the middle variables
    K[I, 0] = np.arange(M) + 1
    K[I, 1] = 0

    # Assign a single free rotation to the upper variables
    K[I1, 0] = -1
    K[I1, 1] = 0

    V1 = V.copy()

    Xg = np.zeros((1, 3), dtype=np.float64)
    X = np.zeros((1, 3), dtype=np.float64)

    Xb = np.zeros((1, 3), dtype=np.float64)
    y = np.ones((M, 1), dtype=np.float64)

    # Solve for `V1`
    status = solve_forward_sectioned_arap_proj(
        T, V, Xg, s, Xb, y, X, V1, K, C, P, lambdas, preconditioners, 
        isProjection=False,
        uniformWeights=False,
        fixedScale=True,
        maxIterations=100, 
        verbosenessLevel=1)

    print 'X:', np.around(X, decimals=3)
    print 'Xb:', np.around(Xb, decimals=3)
    print 'y:', np.around(y, decimals=3)

    vis = VisualiseMesh()
    vis.add_mesh(V1, T)
    vis.add_points(V1[I], sphere_radius=0.2, actor_name='I', color=(0., 0., 1.))
    vis.add_points(V1[C], sphere_radius=0.2, actor_name='C', color=(1., 0., 0.))
    vis.camera_actions(('SetParallelProjection', (True,)))
    vis.execute(magnification=4)
Beispiel #16
0
 def __init__(self, p, r, angle1, angle2):
     Circle.__init__(self, p, r)
     self.angle1 = np.deg2rad(angle1)
     self.angle2 = np.deg2rad(angle2)
Beispiel #17
0
        x_speed_veh = []
        veh_speed_model_estimated = []
        steer = []

        time_list = []
        x_state_vector_ref = []
        x_acc_veh = []
        throttle = []
        brake = []
        yaw_ref = []

        file_p = open(sys.argv[1], 'r')
        line = file_p.readline()

        #prius_parameters = {'steer': 30, 'wheelbase': 2.22455}
        prius_parameters = {'steer': np.deg2rad(80), 'wheelbase': 2.22}
        time_increment = 0

        while len(line) != 0:
            data = eval(line)

            if x_ref == [] or ((data['x'] - x_ref[-1])**2 +
                               (data['y'] - y_ref[-1])**2) > 0.5**2:
                x_ref.append(data['x'])
                y_ref.append(data['y'])
                yaw_ref.append(np.deg2rad(data['yaw']))

                steer.append(data['steer'])
                x_acc_veh.append(data['x_acc'])

                wheelbase.append(data['wheelbase'])
Beispiel #18
0
import numpy as np
import matplotlib.pyplot as plt


def squ(a):
    return (a**2)


Exct = 0.016
R = 1
#diferencia de longitudes
#1
psol = np.sin(np.deg2rad(291.98 - 191.43))  #s-m
pmarte = np.sin(np.deg2rad(191.43 - 155.68))  #m-mo
#2
ssol = np.sin(np.deg2rad(201.92 - 106.62))  #s-m
smarte = np.sin(np.deg2rad(106.62 - 65.21))  #m-mo
#3
tsol = np.sin(np.deg2rad(113.4 - 22.44))
tmarte = np.sin(np.deg2rad(22.44 - 335.03))
#4
csol = np.sin(np.deg2rad(19.77 - 282.83))
cmarte = np.sin(np.deg2rad(282.83 - 241.62))


def rTS(angulo):
    return (R * (1 - squ(Exct)) / 1 + (Exct * np.cos(angulo)))


def d(dia):
    return (365 / 360) * (28 + dia)
Beispiel #19
0
    def __init__(self):
        super(Traffic, self).__init__()

        # Traffic is the toplevel trafficarrays object
        TrafficArrays.SetRoot(self)

        self.ntraf = 0

        self.cond = Condition()  # Conditional commands list
        self.wind = WindSim()
        self.turbulence = Turbulence()
        self.translvl = 5000. * ft  # [m] Default transition level

        with RegisterElementParameters(self):
            # Aircraft Info
            self.id = []  # identifier (string)
            self.type = []  # aircaft type (string)

            # Positions
            self.lat = np.array([])  # latitude [deg]
            self.lon = np.array([])  # longitude [deg]
            self.distflown = np.array([])  # distance travelled [m]
            self.alt = np.array([])  # altitude [m]
            self.hdg = np.array([])  # traffic heading [deg]
            self.trk = np.array([])  # track angle [deg]

            # Velocities
            self.tas = np.array([])  # true airspeed [m/s]
            self.gs = np.array([])  # ground speed [m/s]
            self.gsnorth = np.array([])  # ground speed [m/s]
            self.gseast = np.array([])  # ground speed [m/s]
            self.cas = np.array([])  # calibrated airspeed [m/s]
            self.M = np.array([])  # mach number
            self.vs = np.array([])  # vertical speed [m/s]

            # Atmosphere
            self.p = np.array([])  # air pressure [N/m2]
            self.rho = np.array([])  # air density [kg/m3]
            self.Temp = np.array([])  # air temperature [K]
            self.dtemp = np.array([])  # delta t for non-ISA conditions

            # Wind speeds
            self.windnorth = np.array(
                [])  # wind speed north component a/c pos [m/s]
            self.windeast = np.array(
                [])  # wind speed east component a/c pos [m/s]

            # Traffic autopilot settings
            self.selspd = np.array([])  # selected spd(CAS or Mach) [m/s or -]
            self.aptas = np.array([])  # just for initializing
            self.selalt = np.array([])  # selected alt[m]
            self.selvs = np.array([])  # selected vertical speed [m/s]

            # Whether to perform LNAV and VNAV
            self.swlnav = np.array([], dtype=np.bool)
            self.swvnav = np.array([], dtype=np.bool)
            self.swvnavspd = np.array([], dtype=np.bool)

            # Flight Models
            self.asas = ASAS()
            self.ap = Autopilot()
            self.pilot = Pilot()
            self.adsb = ADSB()
            self.trails = Trails()
            self.actwp = ActiveWaypoint()
            self.perf = Perf()

            # Group Logic
            self.groups = TrafficGroups()

            # Traffic performance data
            self.apvsdef = np.array(
                [])  # [m/s]default vertical speed of autopilot
            self.aphi = np.array([])  # [rad] bank angle setting of autopilot
            self.ax = np.array(
                [])  # [m/s2] absolute value of longitudinal accelleration
            self.bank = np.array([])  # nominal bank angle, [radian]
            self.swhdgsel = np.array(
                [], dtype=np.bool)  # determines whether aircraft is turning

            # limit settings
            self.limspd = np.array([])  # limit speed
            self.limspd_flag = np.array(
                [], dtype=np.bool
            )  # flag for limit spd - we have to test for max and min
            self.limalt = np.array([])  # limit altitude
            self.limalt_flag = np.array(
                [])  # A need to limit altitude has been detected
            self.limvs = np.array(
                [])  # limit vertical speed due to thrust limitation
            self.limvs_flag = np.array([])  # A need to limit V/S detected

            # Display information on label
            self.label = []  # Text and bitmap of traffic label

            # Miscallaneous
            self.coslat = np.array([])  # Cosine of latitude for computations
            self.eps = np.array([])  # Small nonzero numbers

        # Default bank angles per flight phase
        self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))

        self.reset()
Beispiel #20
0
def xy(R, ang):
    return [R * np.cos(np.deg2rad(ang)), R * np.sin(np.deg2rad(ang))]
Beispiel #21
0
# applying function to convert wind direction to degrees
raw_data['DireccionVientoMax_1'] = raw_data['DireccionVientoMax_1'].apply(dir_to_deg)
raw_data['DireccionVientoMax_2'] = raw_data['DireccionVientoMax_2'].apply(dir_to_deg)
raw_data['DireccionVientoMax_3'] = raw_data['DireccionVientoMax_3'].apply(dir_to_deg)
raw_data['DireccionVientoMax_4'] = raw_data['DireccionVientoMax_4'].apply(dir_to_deg)
raw_data['DireccionVientoMax_5'] = raw_data['DireccionVientoMax_5'].apply(dir_to_deg)
raw_data['DireccionVientoMax_6'] = raw_data['DireccionVientoMax_6'].apply(dir_to_deg)
raw_data['DireccionVientoMax_7'] = raw_data['DireccionVientoMax_7'].apply(dir_to_deg)
raw_data['DireccionVientoMax_8'] = raw_data['DireccionVientoMax_8'].apply(dir_to_deg)
raw_data['DireccionVientoMax_9'] = raw_data['DireccionVientoMax_9'].apply(dir_to_deg)
raw_data['DireccionVientoMax_10'] = raw_data['DireccionVientoMax_10'].apply(dir_to_deg)
raw_data['DireccionVientoMax_11'] = raw_data['DireccionVientoMax_11'].apply(dir_to_deg)

# creating new columns with x and y  wind vectors by combining wind direction and speed
raw_data['VientoX_1'] = (raw_data['VelocidadVientoMax_1']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_1']))
raw_data['VientoY_1'] = (raw_data['VelocidadVientoMax_1']) * np.cos(np.deg2rad(raw_data['DireccionVientoMax_1']))

raw_data['VientoX_2'] = (raw_data['VelocidadVientoMax_2']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_2']))
raw_data['VientoY_2'] = (raw_data['VelocidadVientoMax_2']) * np.cos(np.deg2rad(raw_data['DireccionVientoMax_2']))

raw_data['VientoX_3'] = (raw_data['VelocidadVientoMax_3']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_3']))
raw_data['VientoY_3'] = (raw_data['VelocidadVientoMax_3']) * np.cos(np.deg2rad(raw_data['DireccionVientoMax_3']))

raw_data['VientoX_4'] = (raw_data['VelocidadVientoMax_4']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_4']))
raw_data['VientoY_4'] = (raw_data['VelocidadVientoMax_4']) * np.cos(np.deg2rad(raw_data['DireccionVientoMax_4']))

raw_data['VientoX_5'] = (raw_data['VelocidadVientoMax_5']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_5']))
raw_data['VientoY_5'] = (raw_data['VelocidadVientoMax_5']) * np.cos(np.deg2rad(raw_data['DireccionVientoMax_5']))

raw_data['VientoX_6'] = (raw_data['VelocidadVientoMax_6']) * np.sin(np.deg2rad(raw_data['DireccionVientoMax_6']))
Beispiel #22
0
def i(r, ang):
    return (r * np.tan(np.deg2rad(ang)))
Beispiel #23
0
def _latlon_to_tile(lat, lon, zoom):
  n = 2 ** zoom
  x = n * (lon + 180) / 360.
  y = n * (1 - (np.arcsinh(np.tan(np.deg2rad(lat))) / np.pi)) / 2.
  return int(x), int(y)
Beispiel #24
0
import numpy as np

from paraBEM.airfoil.conformal_mapping import VanDeVoorenAirfoil
from paraBEM.vtk_export import VtkWriter
from paraBEM.utils import check_path

####################################################
#      analytic solution of vandevooren-airfoils     #
####################################################

# -inputparameter for the vandevooren airfoil
alpha = np.deg2rad(10)  # alpha is the angle of attack in rad
tau = np.deg2rad(1)  # tau is the angle of the trailing edge
epsilon = 0.10  # epsilon is the thickness-parameter
num_x = 300  # number of plot-points in x-direction
num_y = 300  # number of plot-points in y-direction

# -create joukowsky object
airfoil = VanDeVoorenAirfoil(tau=tau, epsilon=epsilon)

# -helper functions


def complex_to_3vec(z):
    return [z.real, z.imag, 0]


def zeta_velocity(z):
    vel = airfoil.velocity(z, alpha)
    return [vel.real, -vel.imag, 0.]
Beispiel #25
0
import numpy as np
track_offx=80
#track_params = (60,60,60,60,track_offx,0)
track_params = (80,80,80,80,track_offx,0)
stereo_corr_params = {'ws':(128,128),'sxl':250+50+200,'sxr':0,'ofx':80 ,'sxl2':80, 'sxr2':80}
fov=60.97
diff_range_valid=1.0
clear_freqs=5
max_diff_cols=40
ignore_extrema_type=False

pixelwidthx = 960
pixelwidthy = 600  
baseline = 0.122 # (240-100)*.1scale in cm from unreal engine
focal_length=pixelwidthx/( np.tan(np.deg2rad(fov/2)) *2 )
camera_pitch=0

Beispiel #26
0
 def __init__(self, angle=angle.default):
     super(Rotation2D, self).__init__(angle, param_dim=1)
     self._matrix = self._compute_matrix(np.deg2rad(angle))
TARGET_SPEED = 10.0 / 3.6  # [m/s] target speed
N_IND_SEARCH = 10  # Search index number

DT = 0.2  # [s] time tick

# Vehicle parameters
LENGTH = 4.5  # [m]
WIDTH = 2.0  # [m]
BACKTOWHEEL = 1.0  # [m]
WHEEL_LEN = 0.3  # [m]
WHEEL_WIDTH = 0.2  # [m]
TREAD = 0.7  # [m]
WB = 2.5  # [m]

MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad]
MAX_DSTEER = np.deg2rad(30.0)  # maximum steering speed [rad/s]
MAX_SPEED = 55.0 / 3.6  # maximum speed [m/s]
MIN_SPEED = -20.0 / 3.6  # minimum speed [m/s]
MAX_ACCEL = 1.0  # maximum accel [m/ss]

show_animation = True


class State:
    """
    vehicle state class
    """
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
Beispiel #28
0
    def __init__(self):

        freq = 200
        rospy.init_node('convert_tf', anonymous=True)

        self.listener = tf.TransformListener()
        br_svr = tf.TransformBroadcaster()
        br_ee = tf.TransformBroadcaster()
        br_ee_conv = tf.TransformBroadcaster()
        br_robot_base_fixed = tf.TransformBroadcaster()
        self.br_svr_rotated = tf.TransformBroadcaster()
        br_hand_filter = tf.TransformBroadcaster()
        br_ee_svr = tf.TransformBroadcaster()
        br_ee_target = tf.TransformBroadcaster()
        br_ee_finger = tf.TransformBroadcaster()
        self.br_ee_line_target = tf.TransformBroadcaster()
        self.br_ee_line_target2 = tf.TransformBroadcaster()
        self.br_ee_svr_target = tf.TransformBroadcaster()
        br_ee_debug_svr = tf.TransformBroadcaster()
        br_ee_debug_com = tf.TransformBroadcaster()
        br_ee_debug_quat_svr = tf.TransformBroadcaster()
        br_ee_debug_quat_com = tf.TransformBroadcaster()
        br_palm_link = tf.TransformBroadcaster()
        br_thumb_world = tf.TransformBroadcaster()

        tfBuffer = tf2_ros.Buffer()
        listener = tf2_ros.TransformListener(tfBuffer)
        
        # br_robot_base_world_mocap = tf.TransformBroadcaster()

        self.init_topics()
        self.init_params()
        self.load_pointcloud()

        rate = rospy.Rate(freq)
        start = time.time()

        # calibration of the arm orientation
        print('Calibrating the arm orientation')
        delay = 1
        displacement = np.array([0,0,0,0])
        while time.time() < start+delay and not rospy.is_shutdown():
            try:
                trans_arm = self.listener.lookupTransform(
                    '/mocap_hand', '/mocap_shoulder', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            self.trans_arm = trans_arm
            self.p_arm = np.array(
                [trans_arm[0][0], trans_arm[0][1], trans_arm[0][2]])

            self.p_arm = self.p_arm/1.7

            br_svr.sendTransform(self.p_arm, trans_arm[1], rospy.Time.now(),
                                    'mocap_svr', "mocap_hand")

            # freezing the robot_base
            try:
                trans_world_base = self.listener.lookupTransform(
                    '/mocap_world', '/mocap_robot_base', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue

            # assuming that the robot base is similar to the mocap_world (with pi around z-axis)
            q_world_base = tf.transformations.quaternion_about_axis(
                -np.pi/2, (0, 0, 1))
            
            self.pubish_on_point_cloud(self.pointcloud)

            rate.sleep()

        print('Calibration done')

        # while not self.Hand_received:
        # print('Waiting for robot..')
        #     rate.sleep()

        print('Robot reached')
        print('Commencing frame transformation')
        p_hand_filtered = np.array([0, 0, 0])
        q_hand_filtered = np.array([0, 0, 0, 0])
        filter_factor = 1 - (1.0/freq) / (1.0/freq + 1.0/10)

        go_to_init_possition = True
        target_reached = False
        init_point_reached2 = False
        dir_pub_1 = False
        dir_pub_2 = False
        target_str = 'Elbow'
        distance_to_surface = 1
        desired_vel_distance_raw = 1
        target_i = np.size(self.target_vec_y)-1
        # target_i = 0
        desired_vel_combined_old = 0
        line_target = np.array([0, 0, 0])
        self.svr_target = np.array([0, 0, 0])
        gamma_target = 0.01
        limit_real = 0.4
        limit_simul = 0.3
        limit = limit_real
        limit_close = 0.07*1.5  # Change depending on the Passive DS
        limit_close_orig = limit_close
        limit_near=limit_close
        # Offset since line between shoulder and hand is not completely straight
        self.q_svr_rot_orig = tf.transformations.quaternion_about_axis(
            np.deg2rad(4), (0, 1, 0))
        self.q_svr_rot_orig = tf.transformations.quaternion_multiply(
            tf.transformations.quaternion_about_axis(np.deg2rad(-7), (1, 0, 0)), self.q_svr_rot_orig)
        self.q_svr_rot = tf.transformations.quaternion_multiply(
            self.q_svr_rot_orig, trans_arm[1])
        self.q_svr_rot = tf.transformations.quaternion_multiply(
            tf.transformations.quaternion_about_axis(np.deg2rad(90), (0, 0, 1)), self.q_svr_rot)
        self.path = Path()
        self.path_pub = rospy.Publisher('/path', Path, queue_size=10)
        PointCloud_i=0

        #Used for aligning the arm.
        q_tmp3 = tf.transformations.quaternion_about_axis(
                np.deg2rad(180), [0, 1, 0])
        q_tmp2 = tf.transformations.quaternion_multiply(tf.transformations.quaternion_about_axis(
                np.deg2rad(-90), [1, 0, 0]), q_tmp3)  # lower value, facing more down    
        q_tmp = tf.transformations.quaternion_multiply(tf.transformations.quaternion_about_axis(
                np.deg2rad(-5), [0, 1, 0]), q_tmp2)  # lower value, facing more down        
        desired_vel_combined = np.array([0,0,0])
        while not rospy.is_shutdown():
            # Setting palm link at the end of robot

            # world frame is the top tf published by iiwa controller
            br_robot_base_fixed.sendTransform(trans_world_base[0], q_world_base, rospy.Time.now(),
                                                'world', "mocap_world")

            # filtering the mocap_hand frame
            try:
                trans_hand = self.listener.lookupTransform(
                    '/mocap_world', '/mocap_hand', rospy.Time(0))
                if not self.hand_logged:
                    rospy.loginfo("hand transform received")
                    self.hand_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue

            # Filter the hand position
            np_trans_0 = np.array(trans_hand[0])
            np_trans_1 = np.array(trans_hand[1])
            p_hand_filtered = (1-filter_factor) * \
                p_hand_filtered + filter_factor * np_trans_0
            q_hand_filtered = (1-filter_factor) * \
                q_hand_filtered + filter_factor * np_trans_1
            q_hand_filtered = q_hand_filtered / \
                np.linalg.norm(q_hand_filtered)

            br_hand_filter.sendTransform(p_hand_filtered, q_hand_filtered, rospy.Time.now(),
                                            'mocap_hand_filtered', "mocap_world")
            # Rotate the SVR Frame
            self.br_svr_rotated.sendTransform(self.p_arm-[0.0, -0.01, 0.01], self.q_svr_rot, rospy.Time.now(),
                                                'SVR', "mocap_hand_filtered")

            # Broacasting svr fram based on our calibration and filtered hand pose
            br_svr.sendTransform(self.p_arm, trans_arm[1], rospy.Time.now(),
                                    'mocap_svr', "mocap_hand_filtered")

            # Read ee pose in svr frame
            try:
                trans_svr_ee = self.listener.lookupTransform(
                    'SVR', 'iiwa_link_ee', rospy.Time(0))
                if not self.ee_svr_logged:
                    rospy.loginfo("ee_svr transform received")
                    self.ee_svr_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue

            self.trans_svr_ee = trans_svr_ee
            self.publish_svr_ee(trans_svr_ee)

            # desired_vel_svr = -self.desired_end_vec
            # vel_norm = np.linalg.norm(desired_vel_svr)
            # gamma_target = 0.01
            # limit_real = 0.2
            # limit_simul = 0.3
            # limit = limit_real
            # limit_close = 0.07
            # if(vel_norm > limit):
            #     desired_vel_svr = desired_vel_svr / vel_norm * limit
            # if abs(self.gamma_dist-gamma_target) < 0.05:
            #     desired_vel_svr = desired_vel_svr * \
            #         abs(self.gamma_dist-gamma_target)/0.05+[0, 0, 0.03]
            desired_vel_svr_orientation = -self.desired_end_vec

            # rospy.loginfo_throttle(1, str(self.qv_mult(trans_svr_world[1],self.desired_end_vec)))

            # Compute the orientation of the robot
            # q_tf = self.orientation_from_velocity(
            #     desired_vel_svr_orientation)
           
            # Publishing the desired orientation of the end-effector
            try:
                trans_end_svr = self.listener.lookupTransform(
                    '/SVR', '/iiwa_link_ee', rospy.Time(0))
                if not self.svr_ee_logged:
                    rospy.loginfo("svr_ee transform received")
                    self.svr_ee_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            self.trans_end_svr = np.array(trans_end_svr[0])
            # Publishing the desired orientation of the end-effector
            try:
                trans_world_svr = self.listener.lookupTransform(
                    '/world', '/SVR', rospy.Time(0))
                if not self.world_svr_logged:
                    rospy.loginfo("world_svr transform received")
                    self.world_svr_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            self.trans_world_svr = np.array(trans_world_svr[0])
            # br_ee_svr.sendTransform(trans_end_svr[0], q_tf, rospy.Time.now(
            # ), 'e_desired_orientation', "SVR")
            # Reading the desired orientation of the end-effector in the robot base frame
            # try:
            #     trans_ee_desired = self.listener.lookupTransform(
            #         '/world', '/e_desired_orientation', rospy.Time(0))
            #     if not self.ee_world_logged:
            #         rospy.loginfo("ee_world transform received")
            #         self.ee_world_logged = True
            # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            #     continue
            # Sending and reading the target in world frame
            self.br_ee_line_target.sendTransform(line_target, [0, 0, 0, 1], rospy.Time.now(
            ), 'line_target', "SVR")
            try:
                trans_world_line = self.listener.lookupTransform(
                    '/world', '/line_target', rospy.Time(0))
                if not self.world_line_logged:
                    rospy.loginfo("world_line transform received")
                    self.world_line_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            self.trans_world_line = np.array(trans_world_line[0])
            self.br_ee_svr_target.sendTransform(
                self.svr_target, [0, 0, 0, 1], rospy.Time.now(), 'svr_target', 'SVR')
            try:
                trans_world_to_svr_target = np.array(self.listener.lookupTransform(
                    'world', 'svr_target', rospy.Time(0)))
                self.world_to_svr_target = np.array(
                    trans_world_to_svr_target[0])
                if not self.ee_svr_target_logged:
                    rospy.loginfo("ee_svr_target transform received")
                    self.ee_svr_target_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            # self.trans_world_svr=np.array(trans)
            # Getting the end-effecor in the world frame
            try:
                trans_ee_real = self.listener.lookupTransform(
                    '/world', '/iiwa_link_ee', rospy.Time(0))
                if not self.ee_real_logged:
                    rospy.loginfo("ee_real transform received")
                    self.ee_real_logged = True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            # Getting the end-effecor in the mocap_world frame
            self.trans_ee_real = np.array(trans_ee_real[0])
            try:
                trans_ee_mocap = self.listener.lookupTransform(
                    '/mocap_world', '/iiwa_link_ee', rospy.Time(0))
                common_time = self.listener.getLatestCommonTime(
                    '/palm_link', '/link_0')                    
                if not self.ee_mocap_logged:
                    rospy.loginfo("ee_mocap transform received")
                    self.ee_mocap_logged= True
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue  
            self.trans_ee_mocap = np.array(trans_ee_mocap[0])          
            palm_rot = tf.transformations.quaternion_multiply(
                trans_ee_mocap[1], tf.transformations.quaternion_about_axis(np.deg2rad(180), (0, 0, 1)))
            palm_rot2 = tf.transformations.quaternion_multiply(
                palm_rot, tf.transformations.quaternion_about_axis(np.deg2rad(-90), (0, 1, 0)))
            # displacement=np.dot(tf.transformations.quaternion_matrix(palm_rot2),np.array([0,-0.0685,0.0294,0]))
            displacement=np.dot(tf.transformations.quaternion_matrix(palm_rot2),np.array([0.0685,0,-0.0294,0]))
            # print(np.dot(mat_tmp,np.array([0,0,0.0254,0])))
            # test_rot=tf.transformations.rotation_matrix(0,(1,0,0))
            # test_rot2=tf.transformations.rotation_matrix(0,(1,0,0))
            # test_rot[0,0]=0
            # test_rot[2,0]=1
            # test_rot[1,1]=-1
            # test_rot[0,2]=1
            # test_rot[2,2]=0
            # qt1=tf.transformations.quaternion_from_matrix(test_rot)
            # qt2=tf.transformations.quaternion_from_matrix(test_rot2)
            # qt3=tf.transformations.quaternion_multiply(qt1,tf.transformations.quaternion_inverse(qt2))
            # br_palm_link.sendTransform(trans_ee_mocap[0]+np.array([0,-0.0685,0.0294]), palm_rot2,common_time,
            # 'fake_hand_root', "mocap_world")
            # br_palm_link.sendTransform(trans_ee_mocap[0]+np.array([0,-0.0685,0.0254]), palm_rot2,common_time,
            # 'hand_root', "mocap_world")
            # br_palm_link.sendTransform(trans_ee_mocap[0]+np.array([0,-0.0685,0.0294]), palm_rot2,common_time,
            # 'hand_root', "mocap_world")
            br_palm_link.sendTransform(trans_ee_mocap[0]+displacement[:3], palm_rot2,common_time,
            'hand_root', "mocap_world")
            
            try:
                trans_world_tip = self.listener.lookupTransform(
                    '/hand_root', '/link_15_tip', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            self.rot_mat2=np.eye(4)
            qtmps=tf.transformations.quaternion_from_matrix(self.rot_mat2)
            qtmps2=tf.transformations.quaternion_multiply(qtmps,tf.transformations.quaternion_inverse(palm_rot2))
            qtmps3=tf.transformations.quaternion_multiply(qtmps2,palm_rot2)
            # br_palm_link.sendTransform(trans_ee_mocap[0]+displacement[:3], qtmps3,common_time,
            # 'hand_root', "mocap_world")
            # print(tf.transformations.quaternion_matrix(qtmps3))
            trans_world_tip_tmp=np.dot(tf.transformations.quaternion_matrix(qtmps2),np.array([trans_world_tip[0][0],trans_world_tip[0][1],trans_world_tip[0][2],1]))
            br_thumb_world.sendTransform(trans_ee_mocap[0]+displacement[:3]+trans_world_tip_tmp[:3], palm_rot2,rospy.Time.now(),
            'thumb_tip', "mocap_world")     
            try:
                common_time_thumb = self.listener.getLatestCommonTime(
                    '/SVR', '/thumb_tip')                      
                trans_SVR_tip = self.listener.lookupTransform(
                    '/SVR', '/thumb_tip', common_time_thumb)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue                   
            # self.thumbPosePub.position.x=trans_ee_mocap[0][0]+displacement[0]+trans_world_tip_tmp[0]
            # self.thumbPosePub.position.y=trans_ee_mocap[0][1]+displacement[1]+trans_world_tip_tmp[1]
            # self.thumbPosePub.position.z=trans_ee_mocap[0][2]+displacement[2]+trans_world_tip_tmp[2]
            self.thumbPosePub.position.x=trans_SVR_tip[0][0]
            self.thumbPosePub.position.y=trans_SVR_tip[0][1]
            self.thumbPosePub.position.z=trans_SVR_tip[0][2]
            self.ThumbPub.publish(self.thumbPosePub)            
       

            # Frame3: Line algorithm !WORKS OK!
            if not init_point_reached2:
                line_target = np.array([0.27, 0.15, -0.02])
                self.forces_vec = np.array([0, 0, 0, 0, 0, 0])
            if not init_point_reached2 and distance_to_surface < 0.07:
                init_point_reached2 = True
                print('reset')
            # Two points
            # if self.hand_target and init_point_reached2:
            #     line_target = np.array([0.18, 0.25, 0.00])
            # elif not self.hand_target and init_point_reached2:
            #     line_target = np.array([0.18, 0.05, 0.00])
            # if np.linalg.norm(line_target-trans_end_svr[0])<0.05 and target_reached and self.forces_received:
            #     self.hand_target=1-self.hand_target
            #     if self.hand_target:
            #         target_str='Hand'
            #     else:
            #         target_str='Elbow'
            # Interpolated points
            # print(target_i)
            if self.hand_target and init_point_reached2 and self.end_reached == False:
                # if dir_pub_1==False:
                #     counter=0
                #     while counter<50:
                #         self.DirPub.publish(-1)
                #         counter=counter+1
                #     dir_pub_1=True
                #     dir_pub_2=False
                #     print("Shoulder")
                line_target2 = np.array([0.16, 0.27, -0.005])
                line_target = np.array(
                    [self.target_vec_x[target_i], self.target_vec_y[target_i], line_target2[2]])
                self.get_robot_vel(line_target)
                intermediate_dist = np.linalg.norm(
                    (self.world_to_line_target-self.trans_ee_real))
                # intermediate_dist=np.linalg.norm(line_target-trans_end_svr[0])
                if intermediate_dist < 0.05:
                    target_i = target_i-1
                if target_i == 0:
                    self.end_reached = True
            elif not self.hand_target and init_point_reached2 and self.end_reached == False:
                # if dir_pub_2==False:
                #     counter=0
                #     while counter<50:
                #         self.DirPub.publish(1)
                #         counter=counter+1
                #     dir_pub_1=False
                #     dir_pub_2=True
                #     print("Hand")
                line_target2 = np.array([0.18, 0.05, -0.005])
                line_target = np.array(
                    [self.target_vec_x[target_i], self.target_vec_y[target_i], line_target2[2]])
                self.get_robot_vel(line_target)
                intermediate_dist = np.linalg.norm(
                    (self.world_to_line_target-self.trans_ee_real))
                # intermediate_dist=np.linalg.norm(line_target-trans_end_svr[0])
                if intermediate_dist < 0.05:
                    target_i = target_i+1
                if target_i == np.size(self.target_vec_y)-1:
                    self.end_reached = True
            else:
                self.get_robot_vel(line_target)
            if self.end_reached and target_reached and self.forces_received:
                self.hand_target = 1-self.hand_target
                # target_i=np.size(self.target_vec_y)-target_i-1
                if self.hand_target:
                    target_str = 'Hand'
                else:
                    target_str = 'Elbow'
                self.end_reached = False
                # print("change direction")
            ##
            self.get_svm_dir(
                trans_end_svr[0], 0.13-(limit_close_orig-limit_near)/(20*limit_close_orig), init_point_reached2)  # 0.14 used
            # print((limit_close_orig-limit_near)/(10*limit_close_orig))
            # if init_point_reached2:
            #     self.get_svm_dir(trans_end_svr[0],0.13)
            #     # self.svm_dir=np.array([0,0,0])
            # else:
            #     self.svm_dir=np.array([0,0,0])
            # Get target in robot frame
            # print(self.world_to_line_target)
            # line_target2 = np.array([0.15, 0.2, -0.20])

            q_t_ee = tf.transformations.quaternion_multiply(
                q_tmp, self.q_svr_rot)
            br_ee_target.sendTransform(self.p_arm-[0.0, -0.01, 0.01], q_t_ee, rospy.Time.now(
            ), 'target_frame', "mocap_hand_filtered")
            # desired_vel_distance_raw = (line_target-np.array(trans_end_svr[0])) # 7 is good, but makes noises
            # desired_vel_distance = desired_vel_distance_raw*[-1, -1, 1]*6 # 7 is good, but makes noises
            desired_vel_distance_raw = (
                self.world_to_line_target-self.trans_ee_real)
            desired_vel_distance = desired_vel_distance_raw*20  # 6 is good
            distance_to_surface = np.linalg.norm(desired_vel_distance_raw)
            vel_distance_norm = np.linalg.norm(desired_vel_distance)
            limit_near = limit_close*pow((self.forces_vec[1]+1), -3)
            if init_point_reached2:
                if vel_distance_norm > limit_near:
                    desired_vel_distance = desired_vel_distance/vel_distance_norm*limit_near
            else:
                if vel_distance_norm > limit:
                    desired_vel_distance = desired_vel_distance/vel_distance_norm*limit
            # desired_vel_combined_tmp = (beta)*desired_vel_distance+(1-beta)*desired_vel_distance2
            desired_vel_combined_tmp = desired_vel_distance+self.svm_dir

            # desired_vel_combined_tmp = desired_vel_distance
            # Get target frame for robot
            try:
                trans_ee_target = self.listener.lookupTransform(
                    'world', '/target_frame', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue

            # Send end efector to good starting point for svr
            # go_to_init_possition=False
            if go_to_init_possition:
                self.custom_command.data[0:3] = [0, np.deg2rad(90), 0]
                self.custom_command.data[3:6] = [0, 0, 0.03]
                t_start = time.time()
                counter = 0
                while counter < 50:
                    self.RobotCommandPub.publish(self.custom_command)
                    self.GrabPub.publish(0)
                    counter = counter+1
                    rate.sleep()
                go_to_init_possition = False
                raw_input('Waiting to start movement')

            # q_v=self.orientation_from_velocity(desired_vel_distance)
            # trans_ee_desired_combined = beta*np.array(trans_ee_desired[1]) + (1-beta) *q_v
            # trans_ee_desired_combined = tf.transformations.quaternion_slerp(q_v,trans_ee_desired[1],beta)
            # trans_ee_desired_combined=trans_ee_desired[1]
            # q_d = np.array(trans_ee_desired[1])
            # q_d=np.array(trans_ee_desired_combined)
            # q_r=np.array(trans_ee_real[1])

            # if(np.dot(q_d,q_r) < 0.0):
            #     q_d = -q_d

            # angles = tf.transformations.euler_from_quaternion(q_d,)
            # quaternion_matrix : q->R

            # Create desired velocities as a function of distance to the arm
            # desired_vel_svr=desired_vel_svr * \
            #     [-1, -1, 1]*np.sign(self.gamma_dist-gamma_target)

            # rospy.loginfo_throttle(1, [str(trans_end_svr[0])])
                # if line_distance < 0.05:
                #     desired_vel_distance=desired_vel_distance * \
                #         line_distance/0.05 #+[0, 0, 0.03]
            # If hand is close to arm, send grasp pose
            if distance_to_surface <= 0.03 and target_reached == False and (target_i == np.size(self.target_vec_y)-1 or target_i == 0):
                counter = 0
                while counter < 50:
                    self.GrabPub.publish(1)
                    counter = counter+1
                    rate.sleep()
                rospy.loginfo_throttle(1, ['Target reached'])
                target_reached = True
                # Wait for hand to start massaging
            # If hand goes away from target, send open pose
            elif self.gamma_dist > 0.2 and target_reached == True:
                counter = 0
                while counter < 50:
                    self.GrabPub.publish(0)
                    counter = counter+1
                    rate.sleep()
                rospy.loginfo_throttle(1, ['Went away from target'])
                self.forces_vec = np.array([0, 0, 0, 0, 0, 0])
                target_reached = False
                self.forces_received = False
                init_point_reached2 = False
                self.end_reached = False
                self.hand_target = False
                target_i = np.size(self.target_vec_y)-1
            elif init_point_reached2 == True and self.gamma_dist > 0.3:
                init_point_reached2 = False
                target_reached = False
                self.end_reached = False
                print('Start over')
            quat_line_orientation = trans_ee_target[1]
            angles_line_orientation = self.quat_to_direction(
                quat_line_orientation)
            # q_line_tmp=tf.transformations.quaternion_matrix(trans_end_svr[1])
            # q_line_tmp2=tf.transformations.quaternion_matrix(tf.transformations.quaternion_about_axis(np.pi,[0,1,0]))
            # # q_line_tmp3=tf.transformations.quaternion_multiply(q_line_tmp,q_line_tmp2)
            # q_line_tmp3=q_line_tmp
            # angles_line_orientation=self.quat_to_direction(tf.transformations.quaternion_from_matrix(q_line_tmp3))

            # desired_vel_combined_tmp = (beta)*desired_vel_svr+(1-beta)*desired_vel_distance
            # desired_vel_combined_norm=np.linalg.norm(desired_vel_combined_tmp)
            # angles_combined = (beta)*np.array(angles) + \
            #     (1-beta)*np.array([0, 2.5, 0])
            # if desired_vel_combined_norm<limit:
            #     desired_vel_combined = desired_vel_combined_tmp / desired_vel_combined_norm * limit
            # else:
            #     desired_vel_combined = desired_vel_combined_tmp

            desired_vel_combined = desired_vel_combined_tmp
            # if init_point_reached2 and np.linalg.norm((desired_vel_combined-desired_vel_combined_old))>0.1:
            #     desired_vel_combined=(desired_vel_combined+desired_vel_combined_old)/2
            #     print('cutoff')
            # if init_point_reached2 and np.dot(desired_vel_combined,desired_vel_combined_old)<0:
            #     desired_vel_combined=np.array([0,0,0])
            #     print("dot")
            desired_vel_combined = 0.2*desired_vel_combined_tmp+0.8*desired_vel_combined_old
            desired_vel_combined_old = desired_vel_combined

            # rospy.loginfo_throttle(0.5, ['line_dist '+str(np.round(distance_to_surface,7))])
            # rospy.loginfo_throttle(1, ['Beta  '+str(np.round(beta,3))])
            # rospy.loginfo_throttle(0.5, ['Forces  '+str(self.forces_vec)])
            # rospy.loginfo_throttle(0.5, ['gamma  '+str(self.gamma_dist)])
            # rospy.loginfo_throttle(0.5, ['SVM   '+str(self.svm_dir)])
            # rospy.loginfo_throttle(0.5, ['Desired vel '+str(desired_vel_combined)])
            # rospy.loginfo_throttle(0.5, ['Line target '+str(self.world_to_line_target)])
            # rospy.loginfo_throttle(0.5, ['Bools '+str(init_point_reached2)+str(self.end_reached)+str(self.forces_received)])

            # rospy.loginfo_throttle(1, ['SVR'+str(trans_svr_ee[0])])
            br_ee_debug_svr.sendTransform(
                self.svm_dir+self.trans_ee_real, [0, 0, 0, 1], rospy.Time.now(), 'Debug_SVR', 'world')
            br_ee_debug_com.sendTransform(
                desired_vel_combined+self.trans_ee_real, [0, 0, 0, 1], rospy.Time.now(), 'Debug_Com', 'world')

            # rospy.loginfo_throttle(0.5, ['Target   '+target_str+str(target_reached)+ str(self.forces_received)])
            rospy.loginfo_throttle(0.5, ["limit "+str(limit_near)])
            # rospy.loginfo_throttle(0.5, ["dist "+str(desired_vel_distance)])
            # rospy.loginfo_throttle(1, ['Alpha '+str(np.round(alpha,3))])
            # rospy.loginfo_throttle(1, ['Alpha vel '+str((beta*desired_vel_distance+(1-beta)*desired_vel_distance2))])
            # rospy.loginfo_throttle(1, ['Beta  vel '+str(desired_vel_distance3)])

            self.custom_command.data[0:3] = angles_line_orientation
            # self.custom_command.data[0:3] = [0,np.pi,0]#[0,2,0]
            self.custom_command.data[3:6] = desired_vel_combined
            # self.custom_command.data = desired_vel_svr
            self.RobotCommandPub.publish(self.custom_command)

            self.trans_end_svr = trans_end_svr

            # Publish pointcloud of arm
            if PointCloud_i>10:
                self.pubish_on_point_cloud(self.pointcloud)
                PointCloud_i=0
            PointCloud_i=PointCloud_i+1
            # self.publish_path()

            # Publish transformation of Thumb // Not necesseary, published in finger_motion_generator
            # try:
            #     ee_to_finger = self.listener.lookupTransform(
            #         'iiwa_link_ee', 'Finger 3', rospy.Time(0))
            # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            #     continue
            # br_svr.sendTransform(ee_to_finger[0], ee_to_finger[1], rospy.Time.now(),
            #                      'finger_tran', "world")

            # if (np.linalg.norm(self.svm_dir)!=0):
            # q_tf = self.quat_from_Vector(
            #     -self.desired_end_vec)
            # self.quatPosePub.position.x=self.trans_ee_real[0]
            # self.quatPosePub.position.y=self.trans_ee_real[1]
            # self.quatPosePub.position.z=self.trans_ee_real[2]
            # self.quatPosePub.orientation.x=q_tf[0]
            # self.quatPosePub.orientation.y=q_tf[1]
            # self.quatPosePub.orientation.z=q_tf[2]
            # self.quatPosePub.orientation.w=q_tf[3]
            # self.SVRQuatPub.publish(self.quatPosePub)
                # br_ee_debug_quat_com.sendTransform(trans_svr_ee[0],q_tf, rospy.Time.now(),'svr quat','SVR')
            q_tf2=self.quat_from_Vector(desired_vel_combined)
            self.totPosePub.position.x=self.trans_ee_real[0]
            self.totPosePub.position.y=self.trans_ee_real[1]
            self.totPosePub.position.z=self.trans_ee_real[2]
            self.totPosePub.orientation.x=q_tf2[0]
            self.totPosePub.orientation.y=q_tf2[1]
            self.totPosePub.orientation.z=q_tf2[2]
            self.totPosePub.orientation.w=q_tf2[3]
            self.TotQuatPub.publish(self.totPosePub)
            # br_ee_debug_quat_com.sendTransform(self.trans_ee_real,q_tf2, rospy.Time.now(),'tot quat','world')
            rate.sleep()
Beispiel #29
0
 def __init__(self, angle_error=0.1, ang_vel_error=0.1):
     self.noise = np.diag([np.deg2rad(angle_error), np.deg2rad(ang_vel_error)]) ** 2
     self.__yaw = 0.0
     self.__ang_vel = 0.0
Beispiel #30
0
t = time.time()

# Load images
files = sorted(glob.glob('images/*.png'), key=natural_key)
feat = np.empty((len(files), 9720))
labels = np.zeros((len(files), 2))
j = 0
# Loop through roomba images and create feature data
for i in files:
    # Read image
    im = cv2.imread(i)
    # Determine ground truth state
    angle = int(i[i.index('_') + 1:i.index('.')])
    # Create labels
    labels[j][0] = np.sin(np.deg2rad(angle))
    labels[j][1] = np.cos(np.deg2rad(angle))
    # Convert image to HSV colorspace
    imHSV = cv2.cvtColor(im, cv2.COLOR_BGR2HSV)
    # Convert image to grayscale
    gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
    # Compute HOG feature data
    hogFeat = hog(gray,
                  orientations=30,
                  pixels_per_cell=(20, 20),
                  cells_per_block=(2, 2))
    # Compute color histogram data  Removing the color features increased performance
    #colorFeatH = cv2.calcHist([imHSV], [0], None, [179], [0,179], False)
    #colorFeatS = cv2.calcHist([imHSV], [1], None, [256], [0,256], False)
    #colorFeatV = cv2.calcHist([imHSV], [2], None, [256], [0,256], False)
    # Merge feature data`