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()
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
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
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
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],
def sigmoid(phi, a, b, c, d):
return a * np.cos((np.deg2rad(phi) * b + c)) + d
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
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
# 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()
# 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")
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)
def __init__(self, p, r, angle1, angle2):
Circle.__init__(self, p, r)
self.angle1 = np.deg2rad(angle1)
self.angle2 = np.deg2rad(angle2)
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'])
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)
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()
def xy(R, ang):
return [R * np.cos(np.deg2rad(ang)), R * np.sin(np.deg2rad(ang))]
# 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']))
def i(r, ang):
return (r * np.tan(np.deg2rad(ang)))
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)
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.]
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
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
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()
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
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`