def distance(start, end, point):
""" Return the distance from a point to a line in meters """
lat1, lng1 = start
lat2, lng2 = end
lat3, lng3 = point
R = 6371000.0
y = np.sin(lng3 - lng1) * np.cos(lat3)
x = np.cos(lat1) * np.sin(lat3) - np.sin(lat1) * np.cos(lat3) * np.cos(lat3 - lat1)
bearing1 = np.rad2deg(np.arctan2(y,x))
bearing1 = 360 - (bearing1 - 360 % 360)
y2 = np.sin(lng2 - lng1) * np.cos(lat2)
x2 = np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(lat2 - lat1)
bearing2 = np.rad2deg(np.arctan2(y2,x2))
bearing2 = 360 - (bearing2 - 360 % 360)
lat1Rad = np.deg2rad(lat1)
lat3Rad = np.deg2rad(lat3)
dLong = np.deg2rad(lng3 - lng1)
distance13 = np.arccos(np.sin(lat1Rad) * np.sin(lat3Rad) + np.cos(lat1Rad)*np.cos(lat3Rad)*np.cos(dLong)) * R
min_distance = np.fabs(np.arcsin(np.sin(distance13/R) * np.sin(np.deg2rad(bearing1) - np.deg2rad(bearing2))) * R)
return min_distance
def dummy_rotation(self, r_x, r_y, r_z):
vel = np.deg2rad(1)
acc_x = acc_y = acc_z = 0
while r_x > 0 or r_y > 0 or r_z > 0:
if r_x > 0:
acc_x += vel
r_x -= np.rad2deg(vel)
if r_y > 0:
acc_y += vel
r_y -= np.rad2deg(vel)
if r_z > 0:
acc_z += vel
r_z -= np.rad2deg(vel)
self.move_object(0, 0, 0, acc_x, acc_y, acc_z)
self.read_data()
beta = self.compute_pose()
res = 'beta '
self.check_position_error()
for num in beta:
res += '{:0.3f}'.format(num) + ' '
print(res)
self.project_pose()
def angle_between(v1, v2):
"""
Calcualates the angle between two wind vecotrs. Utilizes the cos equation:
cos(theta) = (u dot v)/(magnitude(u) dot magnitude(v))
Input:
v1 = vector 1. A numpy array, list, or tuple with
u in the first index and v in the second
v2 = vector 2. A numpy array, list, or tuple with
u in the first index and v in the second
Output:
Returns the angle in radians between vectors 'v1' and 'v2'::
>>> angle_between((1, 0, 0), (0, 1, 0))
1.5707963267948966
>>> angle_between((1, 0, 0), (1, 0, 0))
0.0
>>> angle_between((1, 0, 0), (-1, 0, 0))
3.141592653589793
"""
v1_u = unit_vector(v1)
v2_u = unit_vector(v2)
angle = np.arccos(np.dot(v1_u, v2_u))
if np.isnan(angle):
if (v1_u == v2_u).all():
return np.rad2deg(0.0)
else:
return np.rad2deg(np.pi)
return np.rad2deg(angle)
def xyz2eq(xin,yin,zin, units='deg'):
"""
Convert x,y,z on the unit sphere to RA DEC.
parameters
----------
x,y,z:
scalars or arrays as given by eq2xyz
units: string, optional
'deg' if the output is to be degrees, 'rad' if it is to be radians.
Default is degrees.
Notes:
This follows the same convention as the STOMP package.
"""
x = numpy.array(xin, ndmin=1, copy=False)
y = numpy.array(yin, ndmin=1, copy=False)
z = numpy.array(zin, ndmin=1, copy=False)
theta,phi = _xyz2thetaphi(x,y,z)
theta += _sdsspar['node']
if units == 'deg':
numpy.rad2deg(theta,theta)
numpy.rad2deg(phi,phi)
atbound(theta, 0.0, 360.0)
# theta->ra, phi->dec
return theta,phi
def plot_poses_vels_theta_omega(pylab, poses, vels):
thetas = np.array([angle_from_SE2(p) for p in poses])
omegas = np.array([linear_angular_from_se2(vel)[1] for vel in vels])
positive = omegas > 0
negative = np.logical_not(positive)
pylab.plot(np.rad2deg(thetas)[positive], omegas[positive], 'r.')
pylab.plot(np.rad2deg(thetas)[negative], omegas[negative], 'b.')
def mso_r_lat_lon_position(time, mso=False, sza=False, **kwargs):
"""Returns position in MSO spherical polar coordinates.
With `mso' set, return [r/lat/lon], [mso x/y/z [km]].
With `sza' set, return [r/lat/lon], [sza [deg]].
With both, return return [r/lat/lon], [mso x/y/z [km]], [sza [deg]]."""
if sza:
pos = position(time, frame = 'MAVEN_MSO', **kwargs)
sza = np.rad2deg(np.arctan2(np.sqrt(pos[1]**2 + pos[2]**2), pos[0]))
if isinstance(sza, np.ndarray):
inx = sza < 0.
if np.any(inx):
sza[inx] = 180. + sza[inx]
elif sza < 0.0:
sza = 180. + sza
tmp = reclat(pos)
tmp_out = np.empty_like(tmp)
tmp_out[0] = tmp[0]
tmp_out[1] = np.rad2deg(tmp[2])
tmp_out[2] = np.rad2deg(tmp[1])
if mso:
return tmp_out, pos, sza
return tmp_out, sza
else:
pos = position(time, frame = 'MAVEN_MSO', **kwargs)
tmp = reclat(pos)
tmp_out = np.empty_like(tmp)
tmp_out[0] = tmp[0]
tmp_out[1] = np.rad2deg(tmp[2])
tmp_out[2] = np.rad2deg(tmp[1])
if mso:
return tmp_out, pos
return tmp_out
def load_star_data(self, name):
# stetson_df = pandas.read_pickle('stetson.pd')
files = glob.glob('cam1/*.fits')
for thisFile in files:
thisFits = pf.open(thisFile)
fibreTable = thisFits['FIBRES'].data
idx = fibreTable.field('NAME').strip()==name
if fibreTable[idx].shape[0] >0: #if there is any star in this file...
starInfo = fibreTable[idx][0]
self.name = starInfo.field('NAME').strip()
self.RA = np.rad2deg(starInfo.field('RA'))
self.Dec = np.rad2deg(starInfo.field('DEC'))
self.RA_deg, self.RA_min, self.RA_sec = toolbox.dec2sex(self.RA)
self.Dec_deg, self.Dec_min, self.Dec_sec = toolbox.dec2sex(self.Dec)
self.Vmag = starInfo.field('MAGNITUDE')
# self.B = stetson_df[stetson_df.target == self.name].B.values[0]
# self.I = stetson_df[stetson_df.target == self.name].I.values[0]
# self.R = stetson_df[stetson_df.target == self.name].R.values[0]
# self.U = stetson_df[stetson_df.target == self.name].U.values[0]
# self.V = stetson_df[stetson_df.target == self.name].mag.values[0]
# self.BV = stetson_df[stetson_df.target == self.name].BV.values[0]
print self.name,'star created'
break
def torus(obj, display, theta0=0., thetaarc=2*np.pi, phi0=0., phiarc=np.pi * 2):
'''Describe a (possibly incomplete) torus.
The plot range for theta and phi is taken from the values for ``coo1`` and ``coo2``
in the ``display`` dictionary. There entries should be a list of value if less then
the full torus (ranging from 0 to 2 pi in each coordinate) is desired for the plot.
'''
theta = display.get('coo1', [0, 2 * np.pi])
phi = display.get('coo2', [0, 2 * np.pi])
out = {}
out['n'] = 1
out['name'] = str(obj.name)
out['material'] = 'MeshStandardMaterial'
out['materialproperties'] = threejs.materialdict(display, out['material'])
out['geometry'] = 'ModifiedTorusBufferGeometry'
out['geometrypars'] = (obj.R, obj.r,
int(np.rad2deg(theta[1])), int(np.rad2deg(phi[1])),
theta[1], theta[0], phi[1], phi[0])
out['pos4d'] = [obj.pos4d.flatten().tolist()]
if not ('side' in display):
out['materialproperties']['side'] = 2
return out
def test_degrees(self):
# the following doesn't make much sense in terms of the name of the
# routine, but we check it gives the correct result.
q1 = np.rad2deg(60. * u.degree)
assert_allclose(q1.value, 60.)
assert q1.unit == u.degree
q2 = np.degrees(60. * u.degree)
assert_allclose(q2.value, 60.)
assert q2.unit == u.degree
q3 = np.rad2deg(np.pi * u.radian)
assert_allclose(q3.value, 180.)
assert q3.unit == u.degree
q4 = np.degrees(np.pi * u.radian)
assert_allclose(q4.value, 180.)
assert q4.unit == u.degree
with pytest.raises(TypeError):
np.rad2deg(3. * u.m)
with pytest.raises(TypeError):
np.degrees(3. * u.m)
def calc(self, observer): self.compute(observer) self.ze = np.pi/2 - self.alt self.nsr = nsr(self.az, self.ze) self.ewr = ewr(self.az, self.ze) self.nsd = np.rad2deg(self.nsr) self.ewd = np.rad2deg(self.ewr)
def __reverse_lambert(self,x,y):
p = math.pi
ln = np.log
power = np.power
sin = np.sin
cos = np.cos
tan = np.tan
cot = lambda x: 1./tan(x)
sec = lambda x: 1./cos(x)
ra0 = np.deg2rad(self.center[0])
dec0 = np.deg2rad(self.center[1])
dec1 = np.deg2rad(self.ref_dec1)
dec2 = np.deg2rad(self.ref_dec2)
n = ln(cos(dec1)*sec(dec2))/ln(tan(p/4+dec2/2)*cot(p/4+dec1/2))
F = cos(dec1)*power(tan(p/4+dec1/2),n)/n
rho0 = F*power(cos(p/4+dec0/2)/sin(p/4+dec0/2),n)
rho = np.sign(n)*np.sqrt(x**2+(rho0-y)**2)
theta = np.arctan(x/(rho0-y))
dec = 2.*np.arctan(np.power((F/rho),(1./n))) - p/2
ra = ra0 + theta/n
return np.rad2deg(ra), np.rad2deg(dec)
def solarcalculator_test():
import ephem
import pytz
from datetime import datetime, timedelta
import numpy as np
from solarcalculator import SolarCalculator
print("Testing SolarCalculator interface.")
sc = SolarCalculator(("65:01", "25:28")) # Passing coordinates of Oulu to constructor
assert sc is not None, 'SolarCalculator initialization returned None'
print(" Initialization ok.")
hel = ephem.city('Helsinki')
sc.observer = hel
assert (int(sc.observer.lat/3.14159*180*1e5)/1e5) == 60.16986, 'Observer coordinates (lat) incorrect: {}'.format(sc.observer.lat/3.14159*180)
assert (int(sc.observer.long/3.14159*180*1e5)/1e5) == 24.93826, 'Observer coordinates (long) incorrect: {}'.format(sc.observer.long/3.14159*180)
print(" Observer setting ok.")
sc.coords = ('37.8044', '-122.2697', 3.0)
assert np.round(np.rad2deg(sc.observer.lat),4) == 37.8044, 'Changing observer (lat) coordinates failed: {}'.format(np.rad2deg(sc.observer.lat))
assert np.round(np.rad2deg(sc.observer.long),4) == -122.2697, 'Changing observer (long) coordinates failed: {}'.format(np.rad2deg(sc.observer.long))
print(" Coordinate setting ok.")
time_str = '2012/11/16 20:34:56'
time_fmt = '%Y/%m/%d %H:%M:%S'
dt = datetime.strptime(time_str, time_fmt).replace(tzinfo=pytz.utc)
assert dt.strftime(time_fmt) == time_str, 'Initializing a datetime object failed: {}'.format(dt.strftime(time_fmt))
print(" Initialized datetime object ok.")
sc.date = dt
assert sc.localized_date(tz=pytz.utc).strftime(time_fmt) == time_str, 'Initializing date to solarcalculator failed: {}'.format(sc.localized_date(tz=pytz.utc).strftime(time_fmt))
print(" Changed observer date ok.")
tz = sc.timezone
assert str(tz) == 'America/Los_Angeles', 'Timezone is incorrect: {}'.format(tz)
print(" Timezone found ok.")
assert sc.date.strftime(time_fmt) == '2012/11/16 12:34:56', 'Observer date did not return a proper result: {}'.format(sc.date.strftime(time_fmt))
print(" Conversion to local timezone ok.")
sdt = sc.solartime
assert sdt.strftime(time_fmt) == '2012/11/16 12:40:55', 'Solartime did not return proper result: {}'.format(sdt.strftime(time_fmt))
assert sdt.strftime("%z") == '-0754', 'Timezone offset incorrect: {}'.format(sdt.strftime("%z"))
obs = ephem.city('Helsinki')
sc.observer = obs
assert sc.coords == (1.0501613384326405, 0.43525439939787997), 'Coordinates were not set properly: {}'.format(sc.coords)
sc.coords = ('60:10:11.3', '24.9')
dt = pytz.timezone('Europe/Stockholm').localize(datetime(2016, 1, 6, 20, 41, 31, 0))
sc.date = dt
assert sc.date.strftime(time_fmt) == '2016/01/06 21:41:31', 'Setting time from wrong local timezone failed: {}'.format(sc.date.strftime(time_fmt))
print(" Date and time changed from wrongly localized time ok.")
assert sc.solartime.strftime(time_fmt) == '2016/01/06 21:15:22', 'Calculating solar time in east failed: {}'.format(sc.solartime.strftime(time_fmt))
print(" Solar time calculated ok.")
tz = sc.timezone
assert str(tz) == 'Europe/Helsinki', 'Timezone location failed: {}'.format(tz)
print(" Timezone located ok.")
def main():
if args["<date>"]:
date = args["<date>"]
time = args["<timeutc>"]
else:
date, time = enter_datetime()
valid = False
while not valid:
try:
date = datetime.strptime(date + " " + time, "%Y-%m-%d %H:%M")
valid = True
except ValueError:
print("Could not parse date/time, please use the given notation\n")
date, time = enter_datetime()
fact = fact_setup(date)
stars = create_dataframe(fact)
az, alt, ra, dec = dark_spot_gridsearch(fact, stars, min_altitude, 0.25, 2)
stars_fov = get_stars_in_fov(az, alt, stars, fact)
print(u"best ratescan position:")
print(u"RA: {:1.3f} h".format(np.rad2deg(ra) * 24 / 360))
print(u"DEC: {:1.3f}°".format(np.rad2deg(dec)))
print(u"Az: {:1.3f}°".format(np.rad2deg(az)))
print(u"Alt: {:1.3f}°".format(np.rad2deg(alt)))
print(u"Brightest star in FOV: {:1.2f} mag".format(stars_fov.vmag.min()))
if args["--plot"]:
plot_dark_spot(stars, az, alt, min_altitude)
def evaluate(cls, phi_N, theta_N, phi, theta, psi):
"""
Evaluate ZXZ rotation into celestial coordinates.
Note currently the parameter values are passed in as degrees so we need
to convert all inputs to radians and all outputs back to degrees.
"""
# TODO: Unfortunately right now this superfluously converts from
# radians to degrees back to radians again--this will be addressed in a
# future change
phi_N = np.deg2rad(phi_N)
theta_N = np.deg2rad(theta_N)
phi = np.deg2rad(phi)
theta = np.deg2rad(theta)
psi = np.deg2rad(psi)
alpha_C, delta_C = cls._rotate_zxz(phi_N, theta_N, phi, theta, psi)
alpha_C = np.rad2deg(alpha_C)
delta_C = np.rad2deg(delta_C)
mask = alpha_C < 0
if isinstance(mask, np.ndarray):
alpha_C[mask] += 360
elif mask:
alpha_C += 360
return alpha_C, delta_C
def __call__(self, phi, theta):
phi = np.deg2rad(phi)
theta = np.deg2rad(theta)
rtheta = self._compute_rtheta(theta)
x = np.rad2deg(rtheta * np.sin(phi))
y = - np.rad2deg(rtheta * np.cos(phi))
return x, y
def get_mean_center(x, y, z):
"""
parameters
----------
x: array
y: array
z: array
returns
-------
ramean, decmean
"""
xmean = x.mean()
ymean = y.mean()
zmean = z.mean()
rmean = sqrt(xmean ** 2 + ymean ** 2 + zmean ** 2)
thetamean = arccos(zmean / rmean)
phimean = arctan2(ymean, xmean)
ramean = rad2deg(phimean)
decmean = rad2deg(pi / 2.0 - thetamean)
ramean = atbound1(ramean, 0.0, 360.0)
return ramean, decmean
def evaluate(cls, alpha_C, delta_C, phi, theta, psi):
"""
Evaluate ZXZ rotation into native coordinates.
This is like RotateNative2Celestial.evaluate except phi and psi are
swapped in ZXZ rotation.
Note currently the parameter values are passed in as degrees so we need
to convert all inputs to radians and all outputs back to degrees.
"""
alpha_C = np.deg2rad(alpha_C)
delta_C = np.deg2rad(delta_C)
phi = np.deg2rad(phi)
theta = np.deg2rad(theta)
psi = np.deg2rad(psi)
phi_N, theta_N = cls._rotate_zxz(alpha_C, delta_C, psi, theta, phi)
phi_N = np.rad2deg(phi_N)
theta_N = np.rad2deg(theta_N)
mask = phi_N > 180
if isinstance(mask, np.ndarray):
phi_N[mask] -= 360
elif mask:
phi_N -= 360
return phi_N, theta_N
def __call__(self, x, y):
x = np.asarray(x) + 0.
y = np.asarray(y) + 0.
rtheta = self._compute_rtheta(x, y)
theta = np.rad2deg(np.arccos(rtheta / self.r0))
phi = np.rad2deg(np.arctan2(x, -y))
return phi, theta
def ecef2sky( x0, xS ):
"""Converts user and satellite ecef coordinates to azimuth and elevation
from user on Earth.
Parameters
----------
x0 : ndarray of shape (3,)
observer coordinate
xs : ndarray of shape(N,3)
object coordinates
Returns
-------
output : ndarray of shape(N,3)
The objects' sky coordinatescoordinates
Notes
-----
"""
enu = ecef2enu(x0, xS)
e = enu[0]; n = enu[1]; u = enu[2]
az = np.arctan2(e[0],n[0])
el = np.arcsin(u[0]/np.sqrt(e[0]**2 + n[0]**2 + u[0]**2))
sky = np.column_stack((np.rad2deg(az), np.rad2deg(el)))
return sky
def calc_geoinc(trace,metric=True):
"""docstring for calc_geoinc"""
stla=trace.stats.sac.stla
stlo=trace.stats.sac.stlo
stdp=trace.stats.sac.stdp
evla=trace.stats.sac.evla
evlo=trace.stats.sac.evlo
evdp=trace.stats.sac.evdp
if metric:
baz=np.rad2deg(np.arctan2((evlo-stlo),(evla-stla)))
EpiDist = np.sqrt((evlo-stlo)**2. + (evla-stla)**2.)
inc = np.rad2deg(np.arctan(EpiDist/ (evdp-stdp)))
HypoDist = np.sqrt((evdp-stdp)**2. + EpiDist**2)
if baz<0.:
baz=baz+360.
azi=np.mod(baz+180.0,360.0)
inc=np.mod(inc+180.0,180.)
return azi,inc,baz,HypoDist,EpiDist,stdp
def __call__(self, x, y):
x = np.asarray(x) + 0.
y = np.asarray(y) + 0.
phi = np.rad2deg(np.arctan2(x, -y))
rtheta = self._compute_rtheta(x, y)
theta = 90 - np.rad2deg(2 * np.arctan(rtheta / (2 * self.r0)))
return phi, theta
def get_lonlatalt(self, utc_time):
"""Calculate sublon, sublat and altitude of satellite.
http://celestrak.com/columns/v02n03/
"""
(pos_x, pos_y, pos_z), (vel_x, vel_y, vel_z) = self.get_position(
utc_time, normalize=True)
lon = ((np.arctan2(pos_y * XKMPER, pos_x * XKMPER) - astronomy.gmst(utc_time))
% (2 * np.pi))
lon = np.where(lon > np.pi, lon - np.pi * 2, lon)
lon = np.where(lon <= -np.pi, lon + np.pi * 2, lon)
r = np.sqrt(pos_x ** 2 + pos_y ** 2)
lat = np.arctan2(pos_z, r)
e2 = F * (2 - F)
while True:
lat2 = lat
c = 1 / (np.sqrt(1 - e2 * (np.sin(lat2) ** 2)))
lat = np.arctan2(pos_z + c * e2 * np.sin(lat2), r)
if np.all(abs(lat - lat2) < 1e-10):
break
alt = r / np.cos(lat) - c
alt *= A
return np.rad2deg(lon), np.rad2deg(lat), alt
def get_lonlatalt(pos, utc_time):
"""Calculate sublon, sublat and altitude of satellite, considering the
earth an ellipsoid.
http://celestrak.com/columns/v02n03/
"""
(pos_x, pos_y, pos_z) = pos / XKMPER
lon = ((np.arctan2(pos_y * XKMPER, pos_x * XKMPER) - astronomy.gmst(utc_time))
% (2 * np.pi))
lon = np.where(lon > np.pi, lon - np.pi * 2, lon)
lon = np.where(lon <= -np.pi, lon + np.pi * 2, lon)
r = np.sqrt(pos_x ** 2 + pos_y ** 2)
lat = np.arctan2(pos_z, r)
e2 = F * (2 - F)
while True:
lat2 = lat
c = 1 / (np.sqrt(1 - e2 * (np.sin(lat2) ** 2)))
lat = np.arctan2(pos_z + c * e2 * np.sin(lat2), r)
if np.all(abs(lat - lat2) < 1e-10):
break
alt = r / np.cos(lat) - c
alt *= A
return np.rad2deg(lon), np.rad2deg(lat), alt
def evaluate(cls, x, y, mu, gamma):
phi = np.arctan2(x / np.cos(gamma), -y)
r = cls._compute_r_theta(x, y, gamma)
pho = r / (cls.r0 * (mu + 1) + y * np.sin(gamma))
psi = np.arctan2(1, pho)
omega = np.arcsin((pho * mu) / np.sqrt(pho ** 2 + 1))
theta1 = np.rad2deg(psi - omega)
theta2 = np.rad2deg(psi + omega) + 180
if np.abs(mu) < 1:
if theta1 < 90 and theta1 > -90:
theta = theta1
else:
theta = theta2
else:
# theta1dif = 90 - theta1
# theta2dif = 90 - theta2
if theta1 < theta2:
theta = theta1
else:
theta = theta2
phi = np.rad2deg(phi)
return phi, theta
def getJointLimits(self, input_urdf, use_deg=True):
import xml.etree.ElementTree as ET
tree = ET.parse(input_urdf)
limits = {}
for j in tree.findall('joint'):
name = j.attrib['name']
torque = 0
lower = 0
upper = 0
velocity = 0
if j.attrib['type'] == 'revolute':
l = j.find('limit')
if l != None:
torque = l.attrib['effort'] #this is not really the physical limit but controller limit, but well
lower = l.attrib['lower']
upper = l.attrib['upper']
velocity = l.attrib['velocity']
limits[name] = {}
limits[name]['torque'] = float(torque)
if use_deg:
limits[name]['lower'] = np.rad2deg(float(lower))
limits[name]['upper'] = np.rad2deg(float(upper))
limits[name]['velocity'] = np.rad2deg(float(velocity))
else:
limits[name]['lower'] = float(lower)
limits[name]['upper'] = float(upper)
limits[name]['velocity'] = float(velocity)
return limits
def check_visible(src,aa,t=None,check27m=True,check2m=True):
''' Check whether a source is observable from the location of aa
given the limits on az/el of the 2-m's and HA/dec of the 27-m's, at
date/time dt. Returns True if visible, False otherwise.
If no time is given, it checks for the current time.
By default, returns True only if it is visible from both the 2-m's and the
27-m's; use check27m=False or check2m=False to ignore either the 27-m's or
the 2-m's, respectively.
'''
if t is None:
t = util.Time.now()
# get alt, az, ha, dec at given date/time
aa.set_jultime(t.jd)
src.compute(aa)
alt = rad2deg(src.alt)
az = rad2deg(src.az)
ha = rad2deg(eovsa_ha(src,t))
dec = rad2deg(src.dec)
if check27m and check2m:
return check_27m_visible(ha,dec) and check_2m_visible(az,alt) and check_2meq_visible(ha,dec)
elif check27m:
return check_27m_visible(ha,dec)
elif check2m:
return check_2m_visible(az,alt) and check_2meq_visible(ha,dec)
else:
return True
def compute_fiducial(wcslist, domain=None):
"""
For a celestial footprint this is the center.
For a spectral footprint, it is the beginning of the range.
This function assumes all WCSs have the same output coordinate frame.
"""
output_frame = getattr(wcslist[0], 'output_frame')
axes_types = getattr(wcslist[0], output_frame).axes_type
spatial_axes = np.array(axes_types) == 'SPATIAL'
spectral_axes = np.array(axes_types) == 'SPECTRAL'
footprints = np.hstack([w.footprint(domain=domain) for w in wcslist])
spatial_footprint = footprints[spatial_axes]
spectral_footprint = footprints[spectral_axes]
fiducial = np.empty(len(axes_types))
if (spatial_footprint).any():
lon, lat = spatial_footprint
lon, lat = np.deg2rad(lon), np.deg2rad(lat)
x_mean = np.mean(np.cos(lat) * np.cos(lon))
y_mean = np.mean(np.cos(lat) * np.sin(lon))
z_mean = np.mean(np.sin(lat))
lon_fiducial = np.rad2deg(np.arctan2(y_mean, x_mean)) % 360.0
lat_fiducial = np.rad2deg(np.arctan2(z_mean, np.sqrt(x_mean**2 + y_mean\
** 2)))
fiducial[spatial_axes] = lon_fiducial, lat_fiducial
if (spectral_footprint).any():
fiducial[spectral_axes] = spectral_footprint.min()
return fiducial
def interpolate_trajectory(self, t, return_degrees=True, **kw):
lat = self.interpolate_timeseries('latitude', t, **kw)
lon = self.interpolate_timeseries('longitude', t, **kw)
dep = self.interpolate_timeseries('depth', t, **kw)
if return_degrees and self.units('latitude') is 'radian':
lat, lon = np.rad2deg(lat), np.rad2deg(lon)
return lon, lat, dep
def sphdist(ra1, dec1, ra2, dec2, units=['deg','deg']):
"""
Get the arc length between two points on the unit sphere
parameters
----------
ra1,dec1,ra2,dec2: scalar or array
Coordinates of two points or sets of points.
Must be the same length.
units: sequence
A sequence containing the units of the input and output. Default
['deg',deg'], which means inputs and outputs are in degrees. Units
can be 'deg' or 'rad'
"""
units_in,units_out = units
# note x,y,z from eq2xyz always returns 8-byte float
x1,y1,z1 = eq2xyz(ra1, dec1, units=units_in)
x2,y2,z2 = eq2xyz(ra2, dec2, units=units_in)
costheta = x1*x2 + y1*y2 + z1*z2
costheta.clip(-1.0,1.0,out=costheta)
theta = arccos(costheta)
if units_out == 'deg':
numpy.rad2deg(theta,theta)
return theta
def get_observer_look(sat_lon, sat_lat, sat_alt, utc_time, lon, lat, alt):
"""Calculate observers look angle to a satellite.
http://celestrak.com/columns/v02n02/
utc_time: Observation time (datetime object)
lon: Longitude of observer position on ground in degrees east
lat: Latitude of observer position on ground in degrees north
alt: Altitude above sea-level (geoid) of observer position on ground in km
Return: (Azimuth, Elevation)
"""
(pos_x, pos_y, pos_z), (vel_x, vel_y, vel_z) = astronomy.observer_position(
utc_time, sat_lon, sat_lat, sat_alt)
(opos_x, opos_y, opos_z), (ovel_x, ovel_y, ovel_z) = \
astronomy.observer_position(utc_time, lon, lat, alt)
lon = np.deg2rad(lon)
lat = np.deg2rad(lat)
theta = (astronomy.gmst(utc_time) + lon) % (2 * np.pi)
rx = pos_x - opos_x
ry = pos_y - opos_y
rz = pos_z - opos_z
sin_lat = np.sin(lat)
cos_lat = np.cos(lat)
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
top_s = sin_lat * cos_theta * rx + \
sin_lat * sin_theta * ry - cos_lat * rz
top_e = -sin_theta * rx + cos_theta * ry
top_z = cos_lat * cos_theta * rx + \
cos_lat * sin_theta * ry + sin_lat * rz
az_ = np.arctan(-top_e / top_s)
if has_xarray and isinstance(az_, xr.DataArray):
az_data = az_.data
else:
az_data = az_
if has_dask and isinstance(az_data, da.Array):
az_data = da.where(top_s > 0, az_data + np.pi, az_data)
az_data = da.where(az_data < 0, az_data + 2 * np.pi, az_data)
else:
az_data[np.where(top_s > 0)] += np.pi
az_data[np.where(az_data < 0)] += 2 * np.pi
if has_xarray and isinstance(az_, xr.DataArray):
az_.data = az_data
else:
az_ = az_data
rg_ = np.sqrt(rx * rx + ry * ry + rz * rz)
el_ = np.arcsin(top_z / rg_)
return np.rad2deg(az_), np.rad2deg(el_)
def lc_to_lonlat(l, c, resolution, lambda_D=104.7):
"""转换行列号到地理经纬度
Parameters
----------
l (int): FY-4 行号
c (int): FY-4 列号
resolution (str): FY-4数据分辨率，例如： '4000' 代表 4km
lambda_D (float): 卫星星下点所在经度，默认：104.7
Returns
-------
(float, float): 经度，纬度
"""
# 地球的半长轴，短半轴
ea, eb = 6378.137, 6356.7523
# 地心到卫星质心的距离
h = 42164
# 卫星星下点所在经度
lambda_D = np.deg2rad(lambda_D)
# 列偏移
COFFs = {
'500': 10991.5,
'1000': 5495.5,
'2000': 2747.5,
'4000': 1373.5
}
# 行偏移
LOFFs = {
'500': 10991.5,
'1000': 5495.5,
'2000': 2747.5,
'4000': 1373.5
}
# 列比例因子
CFACs = {
'500': 81865099,
'1000': 40932549,
'2000': 20466274,
'4000': 10233137
}
# 行比例因子
LFACs = {
'500': 81865099,
'1000': 40932549,
'2000': 20466274,
'4000': 10233137
}
# 参数检查
if resolution not in ("500", "1000", "2000", "4000"):
raise Exception('resolution must in "500", "1000", "2000", "4000"')
if np.any(l < 0) or np.any(c < 0):
raise Exception("l and c must be greater than 0")
if np.any(l > loff * 2) or np.any(c > coff * 2):
raise Exception("l>" + str(int(loff * 2)) + " or c>" +
str(int(coff * 2)))
coff, loff, cfac, lfac = COFFs[resolution], LOFFs[resolution], CFACs[
resolution], LFACs[resolution]
x = np.deg2rad((c - coff) / ((2**-16) * cfac))
y = np.deg2rad((l - loff) / ((2**-16) * lfac))
sd = np.sqrt((h * np.cos(x) * np.cos(y))**2 -
(np.cos(y)**2 + (ea**2) /
(eb**2) * np.sin(y)**2) * (h**2 - ea**2))
sn = (h * np.cos(x) * np.cos(y) - sd) / (np.cos(y)**2 + (ea**2) /
(eb**2) * np.sin(y)**2)
s1 = h - sn * np.cos(x) * np.cos(y)
s2 = sn * np.sin(x) * np.cos(y)
s3 = -sn * np.sin(y)
sxy = np.sqrt(s1**2 + s2**2)
lon = np.rad2deg(np.arctan(s2 / s1) + lambda_D)
lat = np.rad2deg(np.arctan((ea**2 / eb**2) * (s3 / sxy)))
return lon, lat
def build_fit(P, Q, fitgeom):
# Build fit matrix:
fit_matrix = np.dstack((P[:2], Q[:2]))[0]
# determinant of the transformation
det = P[0] * Q[1] - P[1] * Q[0]
sdet = np.sign(det)
proper = (sdet >= 0)
# Create a working copy (no reflections) for computing transformation
# parameters (scale, rotation angle, skew):
wfit = fit_matrix.copy()
# Default skew:
skew = 0.0
if fitgeom == 'shift':
return {
'offset': (P[2], Q[2]),
'fit_matrix': fit_matrix,
'rot': 0.0,
'rotxy': (0.0, 0.0, 0.0, skew),
'scale': (1.0, 1.0, 1.0),
'coeffs': (P, Q),
'skew': skew,
'proper': proper,
'fitgeom': fitgeom
}
# Compute average scale:
s = np.sqrt(np.abs(det))
# Compute scales for each axis:
if fitgeom == 'general':
sx = np.sqrt(P[0]**2 + Q[0]**2)
sy = np.sqrt(P[1]**2 + Q[1]**2)
else:
sx = s
sy = s
# Remove scale from the transformation matrix:
wfit[0, :] /= sx
wfit[1, :] /= sy
# Compute rotation angle as if we have a proper rotation.
# This will also act as *some sort* of "average rotation" even for
# transformations with different rot_x and rot_y:
prop_rot = np.rad2deg(
np.arctan2(wfit[1, 0] - sdet * wfit[0, 1],
wfit[0, 0] + sdet * wfit[1, 1])) % 360.0
if proper and fitgeom == 'rscale':
rotx = prop_rot
roty = prop_rot
rot = prop_rot
skew = 0.0
else:
rotx = np.rad2deg(np.arctan2(-wfit[0, 1], wfit[0, 0])) % 360.0
roty = np.rad2deg(np.arctan2(wfit[1, 0], wfit[1, 1])) % 360.0
rot = 0.5 * (rotx + roty)
skew = roty - rotx
return {
'offset': (P[2], Q[2]),
'fit_matrix': fit_matrix,
'rot': prop_rot,
'rotxy': (rotx, roty, rot, skew),
'scale': (s, sx, sy),
'coeffs': (P, Q),
'skew': skew,
'proper': proper,
'fitgeom': fitgeom
}
def fit_rscale(xyin, xyref):
"""
Set up the products used for computing the fit derived using the code from
lib/geofit.x for the function 'geo_fmagnify()'. Comparisons with results
from geomap (no additional clipping) were made and produced the same
results out to 5 decimal places.
Output
------
fit: dict
Dictionary containing full solution for fit.
"""
if len(xyin) < 2:
raise ValueError("At least two points are required to find "
"shifts, rotation, and scale.")
dx = xyref[:, 0].astype(ndfloat128)
dy = xyref[:, 1].astype(ndfloat128)
du = xyin[:, 0].astype(ndfloat128)
dv = xyin[:, 1].astype(ndfloat128)
n = xyref.shape[0]
Sx = dx.sum()
Sy = dy.sum()
Su = du.sum()
Sv = dv.sum()
xr0 = Sx / n
yr0 = Sy / n
xi0 = Su / n
yi0 = Sv / n
Sxrxr = np.power((dx - xr0), 2).sum()
Syryr = np.power((dy - yr0), 2).sum()
Syrxi = ((dy - yr0) * (du - xi0)).sum()
Sxryi = ((dx - xr0) * (dv - yi0)).sum()
Sxrxi = ((dx - xr0) * (du - xi0)).sum()
Syryi = ((dy - yr0) * (dv - yi0)).sum()
rot_num = Sxrxi * Syryi
rot_denom = Syrxi * Sxryi
if rot_num == rot_denom:
det = 0.0
else:
det = rot_num - rot_denom
if (det < 0):
rot_num = Syrxi + Sxryi
rot_denom = Sxrxi - Syryi
else:
rot_num = Syrxi - Sxryi
rot_denom = Sxrxi + Syryi
if rot_num == rot_denom:
theta = 0.0
else:
theta = np.rad2deg(np.arctan2(rot_num, rot_denom))
if theta < 0:
theta += 360.0
ctheta = np.cos(np.deg2rad(theta))
stheta = np.sin(np.deg2rad(theta))
s_num = rot_denom * ctheta + rot_num * stheta
s_denom = Sxrxr + Syryr
if s_denom < 0.0:
mag = 1.0
elif s_denom > 0.0:
mag = s_num / s_denom
else:
raise ArithmeticError("Singular matrix.")
if det < 0:
# "flip" y-axis (reflection about x-axis *after* rotation)
# NOTE: keep in mind that 'fit_matrix'
# is the transposed rotation matrix.
sthetax = -mag * stheta
cthetay = -mag * ctheta
else:
sthetax = mag * stheta
cthetay = mag * ctheta
cthetax = mag * ctheta
sthetay = mag * stheta
sdet = np.sign(det)
xshift = (xi0 - (xr0 * cthetax + sdet * yr0 * sthetax)).astype(np.float64)
yshift = (yi0 - (-sdet * xr0 * sthetay + yr0 * cthetay)).astype(np.float64)
P = np.array([cthetax, sthetay, xshift], dtype=np.float64)
Q = np.array([-sthetax, cthetay, yshift], dtype=np.float64)
# Return the shift, rotation, and scale changes
result = build_fit(P, Q, fitgeom='rscale')
resids = xyin - np.dot((xyref), result['fit_matrix']) - result['offset']
rms = [
resids[:, 0].std(dtype=np.float64), resids[:, 1].std(dtype=np.float64)
]
result['rms'] = rms
result['resids'] = resids
return result
# -*- coding: utf-8 -*-
from __future__ import division
import numpy as np
# lambdas
sind = lambda x: np.sin(np.deg2rad(x))
cosd = lambda x: np.cos(np.deg2rad(x))
tand = lambda x: np.tan(np.deg2rad(x))
asind = lambda x: np.rad2deg(np.arcsin(x))
acosd = lambda x: np.rad2deg(np.arccos(x))
atand = lambda x: np.rad2deg(np.arctan(x))
atan2d = lambda x1, x2: np.rad2deg(np.arctan2(x1, x2))
getldd = lambda x, y: (atan2d(x, y) % 360, 90 - 2 * asind(
np.sqrt((x * x + y * y) / 2)))
getfdd = lambda x, y: (atan2d(-x, -y) % 360, 2 * asind(
np.sqrt((x * x + y * y) / 2)))
l2v = lambda azi, inc: np.array([
np.atleast_1d(cosd(azi) * cosd(inc)),
np.atleast_1d(sind(azi) * cosd(inc)),
np.atleast_1d(sind(inc))
])
p2v = lambda azi, inc: np.array([
np.atleast_1d(-cosd(azi) * sind(inc)),
np.atleast_1d(-sind(azi) * sind(inc)),
np.atleast_1d(cosd(inc))
])
class YeadonGUI(HasTraits):
"""A GUI for the yeadon module, implemented using the traits package."""
# Input.
measurement_file_name = File()
# Drawing options.
show_mass_center = Bool(False)
show_inertia_ellipsoid = Bool(False)
# Configuration variables.
opts = {'enter_set': True, 'auto_set': True, 'mode': 'slider'}
for name, bounds in zip(Human.CFGnames, Human.CFGbounds):
# TODO : Find a better way than using locals here, it may not be a good
# idea, but I don't know the consequences.
locals()[name] = Range(float(rad2deg(bounds[0])),
float(rad2deg(bounds[1])), 0.0, **opts)
reset_configuration = Button()
# Display of Human object properties.
Ixx = Property(Float, depends_on=sliders)
Ixy = Property(Float, depends_on=sliders)
Ixz = Property(Float, depends_on=sliders)
Iyx = Property(Float, depends_on=sliders)
Iyy = Property(Float, depends_on=sliders)
Iyz = Property(Float, depends_on=sliders)
Izx = Property(Float, depends_on=sliders)
Izy = Property(Float, depends_on=sliders)
Izz = Property(Float, depends_on=sliders)
x = Property(Float, depends_on=sliders)
y = Property(Float, depends_on=sliders)
z = Property(Float, depends_on=sliders)
scene = Instance(MlabSceneModel, args=())
input_group = Group(Item('measurement_file_name'))
vis_group = Group(Item('scene',
editor=SceneEditor(scene_class=MayaviScene), height=580, width=430,
show_label=False))
config_first_group = Group(
Item('somersault'),
Item('tilt'),
Item('twist'),
Item('PTsagittalFlexion', label='PT sagittal flexion'),
Item('PTbending', label='PT bending'),
Item('TCspinalTorsion', label='TC spinal torsion'),
Item('TCsagittalSpinalFlexion',
label='TC sagittal spinal flexion'),
label='Whole-body, pelvis, torso',
dock='tab',
)
config_upper_group = Group(
Item('CA1extension', label='CA1 extension'),
Item('CA1adduction', label='CA1 adduction'),
Item('CA1rotation', label='CA1 rotation'),
Item('CB1extension', label='CB1 extension'),
Item('CB1abduction', label='CB1 abduction'),
Item('CB1rotation', label='CB1 rotation'),
Item('A1A2extension', label='A1A2 extension'),
Item('B1B2extension', label='B1B2 extension'),
label='Upper limbs',
dock='tab',
)
config_lower_group = Group(
Item('PJ1extension', label='PJ1 extension'),
Item('PJ1adduction', label='PJ1 adduction'),
Item('PK1extension', label='PK1 extension'),
Item('PK1abduction', label='PK1 abduction'),
Item('J1J2flexion', label='J1J2 flexion'),
Item('K1K2flexion', label='K1K2 flexion'),
label='Lower limbs',
dock='tab',
)
config_group = VGroup(
Label('Configuration'),
Group(config_first_group, config_upper_group, config_lower_group,
layout='tabbed',
),
Item('reset_configuration', show_label=False),
Label('P: pelvis (red); T: thorax (orange); C: chest-head (yellow)'),
Label('A1/A2: left upper arm/forearm-hand; B1/B2: right arm'),
Label('J1/J2: left thigh/shank-foot; K1/K2: right leg'),
show_border=True,
)
inertia_prop = VGroup(
Label('Mass center (from origin of coord. sys.) (m):'),
HGroup(
Item('x', style='readonly', format_func=format_func),
Item('y', style='readonly', format_func=format_func),
Item('z', style='readonly', format_func=format_func)
),
Label('Inertia tensor (about origin, in basis shown) (kg-m^2):'),
HSplit( # HSplit 2
Group(
Item('Ixx', style='readonly', format_func=format_func),
Item('Iyx', style='readonly', format_func=format_func),
Item('Izx', style='readonly', format_func=format_func),
),
Group(
Item('Ixy', style='readonly', format_func=format_func),
Item('Iyy', style='readonly', format_func=format_func),
Item('Izy', style='readonly', format_func=format_func),
),
Group(
Item('Ixz', style='readonly', format_func=format_func),
Item('Iyz', style='readonly', format_func=format_func),
Item('Izz', style='readonly', format_func=format_func)
),
), # end HSplit 2
Label('X, Y, Z axes drawn as red, green, blue arrows, respectively.'),
show_border=True,
) # end VGroup
view = View(
VSplit(
input_group,
HSplit(vis_group,
VSplit(
config_group,
Item('show_mass_center'),
Item('show_inertia_ellipsoid'),
inertia_prop
)
),
),
resizable=True,
title='Yeadon human inertia model'
) # end View
measPreload = { 'Ls5L' : 0.545, 'Lb2p' : 0.278, 'La5p' : 0.24, 'Ls4L' :
0.493, 'La5w' : 0.0975, 'Ls4w' : 0.343, 'La5L' : 0.049, 'Lb2L' : 0.2995,
'Ls4d' : 0.215, 'Lj2p' : 0.581, 'Lb5p' : 0.24, 'Lb5w' : 0.0975, 'Lk8p' :
0.245, 'Lk8w' : 0.1015, 'Lj5L' : 0.878, 'La6w' : 0.0975, 'Lk1L' : 0.062,
'La6p' : 0.2025, 'Lk1p' : 0.617, 'La6L' : 0.0805, 'Ls5p' : 0.375, 'Lj5p' :
0.2475, 'Lk8L' : 0.1535, 'Lb5L' : 0.049, 'La3p' : 0.283, 'Lj9w' : 0.0965,
'La4w' : 0.055, 'Ls6L' : 0.152, 'Lb0p' : 0.337, 'Lj8w' : 0.1015, 'Lk2p' :
0.581, 'Ls6p' : 0.53, 'Lj9L' : 0.218, 'La3L' : 0.35, 'Lj8p' : 0.245, 'Lj3L'
: 0.449, 'La4p' : 0.1685, 'Lk3L' : 0.449, 'Lb3p' : 0.283, 'Ls7L' : 0.208,
'Ls7p' : 0.6, 'Lb3L' : 0.35, 'Lk3p' : 0.3915, 'La4L' : 0.564, 'Lj8L' :
0.1535, 'Lj3p' : 0.3915, 'Lk4L' : 0.559, 'La1p' : 0.2915, 'Lb6p' : 0.2025,
'Lj6L' : 0.05, 'Lb6w' : 0.0975, 'Lj6p' : 0.345, 'Lb6L' : 0.0805, 'Ls0p' :
0.97, 'Ls0w' : 0.347, 'Lj6d' : 0.122, 'Ls8L' : 0.308, 'Lk5L' : 0.878,
'La2p' : 0.278, 'Lj9p' : 0.215, 'Ls1L' : 0.176, 'Lj1L' : 0.062, 'Lb1p' :
0.2915, 'Lj1p' : 0.617, 'Ls1p' : 0.865, 'Ls1w' : 0.317, 'Lk4p' : 0.34,
'Lk5p' : 0.2475, 'La2L' : 0.2995, 'Lb4w' : 0.055, 'Lb4p' : 0.1685, 'Lk9p' :
0.215, 'Lk9w' : 0.0965, 'Ls2p' : 0.845, 'Lj4L' : 0.559, 'Ls2w' : 0.285,
'Lk6L' : 0.05, 'La7w' : 0.047, 'La7p' : 0.1205, 'La7L' : 0.1545, 'Lk6p' :
0.345, 'Ls2L' : 0.277, 'Lj4p' : 0.34, 'Lk6d' : 0.122, 'Lk9L' : 0.218,
'Lb4L' : 0.564, 'La0p' : 0.337, 'Ls3w' : 0.296, 'Ls3p' : 0.905, 'Lb7p' :
0.1205, 'Lb7w' : 0.047, 'Lj7p' : 0.252, 'Lb7L' : 0.1545, 'Ls3L' : 0.388,
'Lk7p' : 0.252 }
def __init__(self, meas_in=None):
HasTraits.__init__(self, trait_value=True)
if meas_in:
measurement_file_name = meas_in
else:
measurement_file_name = 'Path to measurement input text file.'
self.H = Human(meas_in if meas_in else self.measPreload)
self._init_draw_human()
def _init_draw_human(self):
self.H.draw(self.scene.mlab, True)
if self.show_mass_center:
self.H._draw_mayavi_mass_center_sphere(self.scene.mlab)
if self.show_inertia_ellipsoid:
self.H._draw_mayavi_inertia_ellipsoid(self.scene.mlab)
@on_trait_change('scene.activated')
def set_view(self):
"""Sets a reasonable camera angle for the intial view."""
self.scene.mlab.view(azimuth=90.0, elevation=-90.0)
def _get_Ixx(self):
return self.H.inertia[0, 0]
def _get_Ixy(self):
return self.H.inertia[0, 1]
def _get_Ixz(self):
return self.H.inertia[0, 2]
def _get_Iyx(self):
return self.H.inertia[1, 0]
def _get_Iyy(self):
return self.H.inertia[1, 1]
def _get_Iyz(self):
return self.H.inertia[1, 2]
def _get_Izx(self):
return self.H.inertia[2, 0]
def _get_Izy(self):
return self.H.inertia[2, 1]
def _get_Izz(self):
return self.H.inertia[2, 2]
def _get_x(self):
return self.H.center_of_mass[0, 0]
def _get_y(self):
return self.H.center_of_mass[1, 0]
def _get_z(self):
return self.H.center_of_mass[2, 0]
@on_trait_change('measurement_file_name')
def _update_measurement_file_name(self):
# Must convert to str (from unicode), because Human parses it
# differently depending on its type, and there's no consideration for
# it being unicode.
self.H = Human(str(self.measurement_file_name))
self.scene.mlab.clf()
self._init_draw_human()
@on_trait_change('show_inertia_ellipsoid')
def _update_show_inertia_ellipsoid(self):
if self.show_inertia_ellipsoid:
self.H._draw_mayavi_inertia_ellipsoid(self.scene.mlab)
else:
self.H._ellipsoid_mesh.remove()
def _maybe_update_inertia_ellipsoid(self):
if self.show_inertia_ellipsoid:
self.H._update_mayavi_inertia_ellipsoid()
@on_trait_change('show_mass_center')
def _update_show_mass_center(self):
if self.show_mass_center:
self.H._draw_mayavi_mass_center_sphere(self.scene.mlab)
else:
self.H._mass_center_sphere.remove()
def _maybe_update_mass_center(self):
if self.show_mass_center:
self.H._update_mayavi_mass_center_sphere()
@on_trait_change('reset_configuration')
def _update_reset_configuration(self):
# TODO: This is really slow because it sets every trait one by one. It
# would be nice to set them all to zero and only call the redraw once.
for cfg in sliders:
setattr(self, cfg, self.trait(cfg).default_value()[1])
@on_trait_change('somersault')
def _update_somersault(self):
self.H.set_CFG('somersault', deg2rad(self.somersault))
self._update_mayavi(['P', 'T', 'C', 'A1', 'A2', 'B1', 'B2', 'J1', 'J2',
'K1', 'K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('tilt')
def _update_tilt(self):
self.H.set_CFG('tilt', deg2rad(self.tilt))
self._update_mayavi(['P', 'T', 'C', 'A1', 'A2', 'B1', 'B2', 'J1', 'J2',
'K1', 'K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('twist')
def _update_twist(self):
self.H.set_CFG('twist', deg2rad(self.twist))
self._update_mayavi(['P', 'T', 'C', 'A1', 'A2', 'B1', 'B2', 'J1', 'J2',
'K1', 'K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PTsagittalFlexion')
def _update_PTsagittalFlexion(self):
self.H.set_CFG('PTsagittalFlexion', deg2rad(self.PTsagittalFlexion))
self._update_mayavi(['T', 'C', 'A1', 'A2', 'B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PTbending')
def _update_PTFrontalFlexion(self):
self.H.set_CFG('PTbending', deg2rad(self.PTbending))
self._update_mayavi(['T', 'C', 'A1', 'A2', 'B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('TCspinalTorsion')
def _update_TCSpinalTorsion(self):
self.H.set_CFG('TCspinalTorsion', deg2rad(self.TCspinalTorsion))
self._update_mayavi(['C', 'A1', 'A2', 'B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('TCsagittalSpinalFlexion')
def _update_TCLateralSpinalFlexion(self):
self.H.set_CFG('TCsagittalSpinalFlexion',
deg2rad(self.TCsagittalSpinalFlexion))
self._update_mayavi(['C', 'A1', 'A2', 'B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CA1extension')
def _update_CA1extension(self):
self.H.set_CFG('CA1extension', deg2rad(self.CA1extension))
self._update_mayavi(['A1', 'A2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CA1adduction')
def _update_CA1adduction(self):
self.H.set_CFG('CA1adduction', deg2rad(self.CA1adduction))
self._update_mayavi(['A1', 'A2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CA1rotation')
def _update_CA1rotation(self):
self.H.set_CFG('CA1rotation', deg2rad(self.CA1rotation))
self._update_mayavi(['A1', 'A2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CB1extension')
def _update_CB1extension(self):
self.H.set_CFG('CB1extension', deg2rad(self.CB1extension))
self._update_mayavi(['B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CB1abduction')
def _update_CB1abduction(self):
self.H.set_CFG('CB1abduction', deg2rad(self.CB1abduction))
self._update_mayavi(['B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('CB1rotation')
def _update_CB1rotation(self):
self.H.set_CFG('CB1rotation', deg2rad(self.CB1rotation))
self._update_mayavi(['B1', 'B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('A1A2extension')
def _update_A1A2extension(self):
self.H.set_CFG('A1A2extension', deg2rad(self.A1A2extension))
self._update_mayavi(['A2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('B1B2extension')
def _update_B1B2extension(self):
self.H.set_CFG('B1B2extension', deg2rad(self.B1B2extension))
self._update_mayavi(['B2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PJ1extension')
def _update_PJ1extension(self):
self.H.set_CFG('PJ1extension', deg2rad(self.PJ1extension))
self._update_mayavi(['J1', 'J2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PJ1adduction')
def _update_PJ1adduction(self):
self.H.set_CFG('PJ1adduction', deg2rad(self.PJ1adduction))
self._update_mayavi(['J1', 'J2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PK1extension')
def _update_PK1extension(self):
self.H.set_CFG('PK1extension', deg2rad(self.PK1extension))
self._update_mayavi(['K1', 'K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('PK1abduction')
def _update_PK1abduction(self):
self.H.set_CFG('PK1abduction', deg2rad(self.PK1abduction))
self._update_mayavi(['K1', 'K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('J1J2flexion')
def _update_J1J2flexion(self):
self.H.set_CFG('J1J2flexion', deg2rad(self.J1J2flexion))
self._update_mayavi(['J2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
@on_trait_change('K1K2flexion')
def _update_K1K2flexion(self):
self.H.set_CFG('K1K2flexion', deg2rad(self.K1K2flexion))
self._update_mayavi(['K2'])
self._maybe_update_mass_center()
self._maybe_update_inertia_ellipsoid()
def _update_mayavi(self, segments):
"""Updates all of the segments and solids."""
for affected in segments:
seg = self.H.get_segment_by_name(affected)
for solid in seg.solids:
solid._mesh.scene.disable_render = True
for affected in segments:
self.H.get_segment_by_name(affected)._update_mayavi()
for affected in segments:
seg = self.H.get_segment_by_name(affected)
for solid in seg.solids:
solid._mesh.scene.disable_render = False
def offsetTesting(acqStatic,model,display = False, unitTesting=False):
# Display
# tibial torsion
ltt_vicon = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LTibialTorsion").value().GetInfo().ToDouble()[0])
rtt_vicon =np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RTibialTorsion").value().GetInfo().ToDouble()[0])
# shank abAdd offset
abdAdd_l = model.getViconAnkleAbAddOffset("Left")
abdAdd_r = model.getViconAnkleAbAddOffset("Right")
# thigh and shank Offsets
lto = model.getViconThighOffset("Left")
lso = model.getViconShankOffset("Left")
rto = model.getViconThighOffset("Right")
rso = model.getViconShankOffset("Right")
# foot offsets
spf_l,sro_l= model.getViconFootOffset("Left")
spf_r,sro_r= model.getViconFootOffset("Right")
vicon_abdAdd_l = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LAnkleAbAdd").value().GetInfo().ToDouble()[0])
vicon_abdAdd_r = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RAnkleAbAdd").value().GetInfo().ToDouble()[0])
lto_vicon = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LThighRotation").value().GetInfo().ToDouble()[0])
lso_vicon = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LShankRotation").value().GetInfo().ToDouble()[0])
rto_vicon = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RThighRotation").value().GetInfo().ToDouble()[0])
rso_vicon = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RShankRotation").value().GetInfo().ToDouble()[0])
vicon_spf_l = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LStaticPlantFlex").value().GetInfo().ToDouble()[0])
vicon_spf_r = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RStaticPlantFlex").value().GetInfo().ToDouble()[0])
vicon_sro_l = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LStaticRotOff").value().GetInfo().ToDouble()[0])
vicon_sro_r = np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RStaticRotOff").value().GetInfo().ToDouble()[0])
if display:
logging.info(" LTibialTorsion : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(ltt_vicon,model.mp_computed["LeftTibialTorsionOffset"]))
logging.info(" RTibialTorsion : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(rtt_vicon,model.mp_computed["RightTibialTorsionOffset"]))
logging.info(" LAbdAddRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_abdAdd_l,abdAdd_l))
logging.info(" RAbdAddRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_abdAdd_r,abdAdd_r))
logging.info(" LThighRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(lto_vicon,lto))
logging.info(" LShankRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(lso_vicon,lso))
logging.info(" RThighRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(rto_vicon,rto))
logging.info(" RShankRotation : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(rso_vicon,rso))
logging.info(" LStaticPlantFlex : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_spf_l, spf_l))
logging.info(" RStaticPlantFlex : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_spf_r, spf_r))
logging.info(" LStaticRotOff : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_sro_l, sro_l))
logging.info(" RStaticRotOff : Vicon (%.6f) Vs pyCGM2 (%.6f)" %(vicon_sro_r ,sro_r))
if unitTesting:
# tibial torsion
np.testing.assert_almost_equal(-ltt_vicon,model.mp_computed["LeftTibialTorsionOffset"] , decimal = 3)
np.testing.assert_almost_equal(rtt_vicon,model.mp_computed["RightTibialTorsionOffset"] , decimal = 3)
# thigh and shank Offsets
np.testing.assert_almost_equal(lto, np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LThighRotation").value().GetInfo().ToDouble()[0]) , decimal = 3)
np.testing.assert_almost_equal(lso, np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("LShankRotation").value().GetInfo().ToDouble()[0]) , decimal = 3)
np.testing.assert_almost_equal(rto, np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RThighRotation").value().GetInfo().ToDouble()[0]) , decimal = 3)
np.testing.assert_almost_equal(rso, np.rad2deg(acqStatic.GetMetaData().FindChild("PROCESSING").value().FindChild("RShankRotation").value().GetInfo().ToDouble()[0]) , decimal = 3)
# shank abAdd offset
np.testing.assert_almost_equal(abdAdd_l ,vicon_abdAdd_l , decimal = 3)
np.testing.assert_almost_equal(abdAdd_r ,vicon_abdAdd_r , decimal = 3)
# foot offsets
np.testing.assert_almost_equal(spf_l,vicon_spf_l , decimal = 3)
np.testing.assert_almost_equal(spf_r,vicon_spf_r , decimal = 3)
np.testing.assert_almost_equal(sro_l,vicon_sro_l , decimal = 3)
np.testing.assert_almost_equal(sro_r,vicon_sro_r , decimal = 3)
def max_distance_deg(self):
return np.rad2deg(self.max_distance)
def polar_stuff(fig, telescope):
# PolarAxes.PolarTransform takes radian. However, we want our coordinate
# system in degree
tr = Affine2D().scale(np.pi / 180., 1.).translate(
+np.pi / 2., 0) + PolarAxes.PolarTransform()
# polar projection, which involves cycle, and also has limits in
# its coordinates, needs a special method to find the extremes
# (min, max of the coordinate within the view).
# 20, 20 : number of sampling points along x, y direction
n = 1
extreme_finder = angle_helper.ExtremeFinderCycle(
n,
n,
lon_cycle=360,
lat_cycle=None,
lon_minmax=None,
lat_minmax=(-90, 90),
)
grid_locator1 = angle_helper.LocatorDMS(12)
# Find a grid values appropriate for the coordinate (degree,
# minute, second).
tick_formatter1 = angle_helper.FormatterDMS()
# And also uses an appropriate formatter. Note that,the
# acceptable Locator and Formatter class is a bit different than
# that of mpl's, and you cannot directly use mpl's Locator and
# Formatter here (but may be possible in the future).
grid_helper = GridHelperCurveLinear(tr,
extreme_finder=extreme_finder,
grid_locator1=grid_locator1,
tick_formatter1=tick_formatter1)
ax1 = SubplotHost(fig, 1, 1, 1, grid_helper=grid_helper)
# make ticklabels of right and top axis visible.
ax1.axis["right"].major_ticklabels.set_visible(True)
ax1.axis["top"].major_ticklabels.set_visible(True)
# let right axis shows ticklabels for 1st coordinate (angle)
ax1.axis["right"].get_helper().nth_coord_ticks = 0
# let bottom axis shows ticklabels for 2nd coordinate (radius)
ax1.axis["bottom"].get_helper().nth_coord_ticks = 1
fig.add_subplot(ax1)
# A parasite axes with given transform
ax2 = ParasiteAxesAuxTrans(ax1, tr, "equal")
# note that ax2.transData == tr + ax1.transData
# Anything you draw in ax2 will match the ticks and grids of ax1.
ax1.parasites.append(ax2)
# intp = cbook.simple_linear_interpolation
#ax2.plot(intp(np.array([0, 30]), 50),
# intp(np.array([10., 10.]), 50),
# linewidth=2.0)
x = np.rad2deg(telescope.az.value) * np.cos(telescope.alt.value)
y = np.rad2deg(telescope.alt.value)
circle = plt.Circle(
(np.rad2deg(telescope.az.value - np.pi) * np.sin(telescope.alt.value),
np.rad2deg(-telescope.alt.value * np.cos(
(telescope.az.value - np.pi)))),
radius=7.7 / 2,
color="red",
alpha=0.2,
)
circle = plt.Circle(
(x, y),
radius=7.7 / 2,
color="red",
alpha=0.2,
)
ax1.add_artist(circle)
# point = ax1.scatter(x, y, c="b", s=20, zorder=10, transform=ax2.transData)
ax2.annotate(1, (x, y),
fontsize=15,
xytext=(4, 4),
textcoords='offset pixels')
ax1.set_xlim(-180, 180)
ax1.set_ylim(0, 90)
ax1.set_aspect(1.)
ax1.grid(True, zorder=0)
ax1.set_xlabel("Azimuth in degrees", fontsize=20)
ax1.set_ylabel("Zenith in degrees", fontsize=20)
plt.show()
return fig
def get_theta(self, vec):
vec_in_pixels = self.ax.transData.transform(vec) - self._center
return np.rad2deg(np.arctan2(vec_in_pixels[1], vec_in_pixels[0]))
def lonlat_to_lc(lon, lat, resolution, lambda_D=104.7):
"""转换地理经纬度到FY-4卫星行列号
Parameters
----------
lon (float): 地理经度
lat (float): 地理纬度
resolution (str): FY-4数据分辨率，例如： '4000' 代表 4km
lambda_D (float): 卫星星下点所在经度，默认：104.7
Returns
-------
(float, float): 行号，列号
"""
# 地球的半长轴，短半轴
ea, eb = 6378.137, 6356.7523
# 地心到卫星质心的距离
h = 42164
# 卫星星下点所在经度
lambda_D = np.deg2rad(lambda_D)
# 列偏移
COFFs = {
'500': 10991.5,
'1000': 5495.5,
'2000': 2747.5,
'4000': 1373.5
}
# 行偏移
LOFFs = {
'500': 10991.5,
'1000': 5495.5,
'2000': 2747.5,
'4000': 1373.5
}
# 列比例因子
CFACs = {
'500': 81865099,
'1000': 40932549,
'2000': 20466274,
'4000': 10233137
}
# 行比例因子
LFACs = {
'500': 81865099,
'1000': 40932549,
'2000': 20466274,
'4000': 10233137
}
coff, loff, cfac, lfac = COFFs[resolution], LOFFs[resolution], CFACs[
resolution], LFACs[resolution]
# 转换经度为负值时加上360
if np.ndim(lon) > 0:
lon[lon < 0] += 360
else:
lon = 360 + lon if lon < 0 else lon
# 经纬度检查
valid_extent = 24.11662309, 360 - 174.71662309, -80.56672132, 80.56672132
if (np.nanmin(lon) < valid_extent[0]) | (np.nanmax(lon) >
valid_extent[1]):
raise Exception(
"`lon` must be in range of ({},{})".format(*valid_extent[:2]))
if (np.nanmin(lat) < valid_extent[2]) | (np.nanmax(lat) >
valid_extent[3]):
raise Exception(
"`lat` must be in range of ({},{})".format(*valid_extent[2:]))
lon = np.deg2rad(lon)
lat = np.deg2rad(lat)
lambda_e = lon
phi_e = np.arctan((eb**2 / ea**2) * np.tan(lat))
radius_e = eb / np.sqrt(1 - (ea**2 - eb**2) /
(ea**2) * np.cos(phi_e)**2)
r1 = h - radius_e * np.cos(phi_e) * np.cos(lambda_e - lambda_D)
r2 = -radius_e * np.cos(phi_e) * np.sin(lambda_e - lambda_D)
r3 = radius_e * np.sin(phi_e)
rn = np.sqrt(r1**2 + r2**2 + r3**2)
x = np.rad2deg(np.arctan(-r2 / r1))
y = np.rad2deg(np.arcsin(-r3 / rn))
c = coff + x * (2**-16) * cfac
l = loff + y * (2**-16) * lfac
return l, c
from bob_robotics import navigation
import cv2
import matplotlib.pyplot as plt
import numpy as np
import sys
from time import time
dbPath = sys.argv[1]
img = cv2.imread(dbPath + '/image_00100.png')
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
print('Loaded image')
algo = navigation.PerfectMemory((180, 45))
algo.train_route(dbPath)
print('Training complete')
t0 = time()
ang = algo.get_heading(img)
print(time() - t0)
# Alternatively you can use algo.get_heading(img) to just get the heading,
# which should be faster
(heading, best_snap, best_min, diffs) = algo.get_ridf_data(img)
print('Best snapshot: %d at %g° (diff: %g)' % (best_snap, np.rad2deg(heading), best_min))
xs = np.linspace(0, 360, diffs.shape[1])
ridf = diffs[best_snap, :]
plt.plot(xs, ridf / 255.0)
plt.show()
def cartesian2spherical(x, y, z):
h = np.hypot(x, y)
alpha = np.rad2deg(np.arctan2(y, x))
delta = np.rad2deg(np.arctan2(z, h))
return alpha, delta
def mat2euler_deg(M):
ee = mat2euler(M)
return np.rad2deg(ee)
def main(arg):
rospy.init_node("Sawyer_DMP")
limb = intera_interface.Limb('right')
position_dmp = DMP(arg.gain, arg.num_gaussians, arg.stabilization,
arg.obstacle_avoidance)
orientation_dmp = DMP(arg.gain, arg.num_gaussians, arg.stabilization,
False) # no obstacle avoidance on orientation
data = load_demo(arg.input_file)
t, q = parse_demo(data)
s = position_dmp.phase(t)
psv = position_dmp.distributions(s)
t = normalize_vector(t)
print('Calculating forward kinematics...')
robot = Sawyer()
Te = sp.lambdify(robot.q, robot.get_T_f()[-1])
ht = np.array([Te(*robot.JointOffset(qi)) for qi in q])
position, orientation = np.zeros((len(t), 3)), np.zeros((len(t), 3))
for i, h in enumerate(ht):
c = h[:3, 3]
p = np.arctan2(-h[2, 0], np.sqrt(h[0, 0]**2 + h[1, 0]**2))
y = np.arctan2(h[1, 0] / np.cos(p), h[0, 0] / np.cos(p))
r = np.arctan2(h[2, 1] / np.cos(p), h[2, 2] / np.cos(p))
if i > 0:
y = fix_angle(orientation[i - 1, 2], y)
p = fix_angle(orientation[i - 1, 1], p)
r = fix_angle(orientation[i - 1, 0], r)
position[i] = c
orientation[i] = np.stack((r, p, y))
# Set static obstacles
#obstacles = np.array([np.concatenate((position[len(position)//3], orientation[len(position)//3]))])#,:3]])
#obstacles = np.array([np.random.uniform(l,h,size=(2,)) for l,h in [(-0.2,0.6),(-0.6,0.2),(-0.1,0.2),(0,0),(0,0),(0,0)]]).T
ob_angles = [
-1.931447265625, -0.3394013671875, 0.4098779296875, 1.296037109375,
-0.550625, 0.7161005859375, 0.5338125
]
obstacles = np.array([
np.concatenate((Te(*robot.JointOffset(ob_angles))[:3, 3], [0, 0, 0]))
])
#obstacles = np.array([[0.4134, -0.5456, 0.109, 0, 0, 0]])
obstacles = np.array([[0.2, -0.69431369, -0.05, 0, 0,
0]]) #-0.08348392, 0,0,0]]
print('Smoothing and filtering cartesian trajectory...')
dposition, ddposition = np.zeros((2, position.shape[0], position.shape[1]))
for i in range(position.shape[1]):
position[:, i] = smooth_trajectory(position[:, i], arg.window)
dposition[:, i] = vel(position[:, i], t)
ddposition[:, i] = vel(dposition[:, i], t)
dorientation, ddorientation = np.zeros(
(2, orientation.shape[0], orientation.shape[1]))
for i in range(position.shape[1]):
orientation[:, i] = smooth_trajectory(orientation[:, i], arg.window)
dorientation[:, i] = vel(orientation[:, i], t)
ddorientation[:, i] = vel(dorientation[:, i], t)
# Filter the position velocity and acceleration signals
f_position, f_dposition, f_ddposition = np.zeros(
(3, position.shape[0], position.shape[1]))
for i in range(position.shape[1]):
f_position[:, i] = blend_trajectory(position[:, i], dposition[:, i], t,
arg.blends)
f_dposition[:, i] = vel(f_position[:, i], t)
f_ddposition[:, i] = vel(f_dposition[:, i], t)
f_orientation, f_dorientation, f_ddorientation = np.zeros(
(3, orientation.shape[0], orientation.shape[1]))
for i in range(position.shape[1]):
f_orientation[:, i] = blend_trajectory(orientation[:, i],
dorientation[:,
i], t, arg.blends)
f_dorientation[:, i] = vel(f_orientation[:, i], t)
f_ddorientation[:, i] = vel(f_dorientation[:, i], t)
print('DMP: Imitating trajectory...')
ftarget_position, w_position = position_dmp.imitate(
position, dposition, ddposition, t, s, psv)
ftarget_orientation, w_orientation = orientation_dmp.imitate(
orientation, dorientation, ddorientation, t, s, psv)
print('DMP: Generating trajectory...')
ddxp, dxp, xp = position_dmp.generate(w_position, f_position[0],
f_position[-1], t, s, psv, obstacles)
ddxo, dxo, xo = position_dmp.generate(w_orientation, f_orientation[0],
f_orientation[-1], t, s, psv,
obstacles)
print('Calculating inverse kinematics from DMP-cartesian trajectory...')
xc = np.zeros(q.shape)
prev_sol = q[0]
for i, xi in enumerate(xp):
if i > 0:
prev_sol = xc[i - 1]
ik = robot.Inverse_Kinematics(limb, xi, np.rad2deg(xo[i]), prev_sol)
xc[i] = [ik[j] for j in limb.joint_names()]
real_sol = np.concatenate((xp, xo), axis=1)
gen_sol = np.concatenate((position, orientation), axis=1)
if arg.show_plots:
#plot.comparison(t, q, xc, None, labels=['Original q', 'cartesian-DMP', 'None'])
#plot.position(t, xo, position[:,3:], title='Orientation')
plot.cartesian_history([real_sol, gen_sol, obstacles],
[0.2, 0.2, 100.0])
plot.show_all()
print('Saving joint angle solutions to: %s' %
(arg.output_file + '_trajectory.txt'))
traj_final = np.concatenate(
(xc, np.multiply(np.ones((xc.shape[0], 1)), 0.0402075604203)), axis=1)
time = np.linspace(0, t[-1], xc.shape[0]).reshape((xc.shape[0], 1))
traj_final = np.concatenate((t.reshape((-1, 1)), traj_final), axis=1)
header = 'time,right_j0,right_j1,right_j2,right_j3,right_j4,right_j5,right_j6,right_gripper'
np.savetxt(arg.output_file + '_trajectory.txt',
traj_final,
delimiter=',',
header=header,
comments='',
fmt="%1.12f")
def fft_rotate(in_frame, alpha, pad=4, return_full=False):
"""
3 FFT shear based rotation, following Larkin et al 1997
Copied from the GRAPHIC exoplanet direct imaging pipeline
in_frame: the numpy array which has to be rotated
alpha: the rotation alpha in degrees
pad: the padding factor
return_full: If True, return the padded array
One side effect of this program is that the image gains two columns and two rows.
This is necessary to avoid problems with the different choice of centre between
GRAPHIC and numpy. Numpy rotates around the boundary between 4 pixels, whereas this
program rotates around the centre of a pixel.
Return the rotated array
"""
#################################################
# Check alpha validity and correcting if needed
#################################################
alpha = 1. * alpha - 360 * np.floor(alpha / 360)
# We need to add some extra rows since np.rot90 has a different definition of the centre
temp = np.zeros((in_frame.shape[0] + 3, in_frame.shape[1] + 3)) + np.nan
temp[1:in_frame.shape[0] + 1, 1:in_frame.shape[1] + 1] = in_frame
in_frame = temp
# FFT rotation only work in the -45:+45 range
if alpha > 45 and alpha <= 135:
in_frame = np.rot90(in_frame, k=1)
alpha_rad = -np.deg2rad(alpha - 90)
elif alpha > 135 and alpha <= 225:
in_frame = np.rot90(in_frame, k=2)
alpha_rad = -np.deg2rad(alpha - 180)
elif alpha > 225 and alpha <= 315:
in_frame = np.rot90(in_frame, k=3)
alpha_rad = -np.deg2rad(alpha - 270)
else:
alpha_rad = -np.deg2rad(alpha)
# Remove one extra row
in_frame = in_frame[:-1, :-1]
###################################
# Preparing the frame for rotation
###################################
# Calculate the position that the input array will be in the padded array to simplify
# some lines of code later
px1 = np.int(((pad - 1) / 2.) * in_frame.shape[0])
px2 = np.int(((pad + 1) / 2.) * in_frame.shape[0])
py1 = np.int(((pad - 1) / 2.) * in_frame.shape[1])
py2 = np.int(((pad + 1) / 2.) * in_frame.shape[1])
# Make the padded array
pad_frame = np.ones(
(in_frame.shape[0] * pad, in_frame.shape[1] * pad)) * np.NaN
pad_mask = np.ones((pad_frame.shape), dtype=bool)
pad_frame[px1:px2, py1:py2] = in_frame
pad_mask[px1:px2, py1:py2] = np.where(np.isnan(in_frame), True, False)
# Rotate the mask, to know what part is actually the image
pad_mask = ndimage.interpolation.rotate(pad_mask,
np.rad2deg(-alpha_rad),
reshape=False,
order=0,
mode='constant',
cval=True,
prefilter=False)
# Replace part outside the image which are NaN by 0, and go into Fourier space.
pad_frame = np.where(np.isnan(pad_frame), 0., pad_frame)
###############################
# Rotation in Fourier space
###############################
a = np.tan(alpha_rad / 2.)
b = -np.sin(alpha_rad)
M = -2j * np.pi * np.ones(pad_frame.shape)
N = fftpack.fftfreq(pad_frame.shape[0])
X = np.arange(-pad_frame.shape[0] / 2.,
pad_frame.shape[0] / 2.) #/pad_frame.shape[0]
pad_x = fftpack.ifft((fftpack.fft(pad_frame, axis=0, overwrite_x=True).T *
np.exp(a * ((M * N).T * X).T)).T,
axis=0,
overwrite_x=True)
pad_xy = fftpack.ifft(fftpack.fft(pad_x, axis=1, overwrite_x=True) *
np.exp(b * (M * X).T * N),
axis=1,
overwrite_x=True)
pad_xyx = fftpack.ifft((fftpack.fft(pad_xy, axis=0, overwrite_x=True).T *
np.exp(a * ((M * N).T * X).T)).T,
axis=0,
overwrite_x=True)
# Go back to real space
# Put back to NaN pixels outside the image.
pad_xyx[pad_mask] = np.NaN
if return_full:
return np.abs(pad_xyx).copy()
else:
return np.abs(pad_xyx[px1:px2, py1:py2]).copy()
def cartesian_to_geographic_aeqd(data, R=6370997.):
"""
Azimuthal equidistant Cartesian to geographic coordinate transform.
Transform a set of Cartesian/Cartographic coordinates (x, y) to
geographic coordinate system (lat, lon) using a azimuthal equidistant
map projection [1]_.
.. math::
lat = \\arcsin(\\cos(c) * \\sin(lat_0) +
(y * \\sin(c) * \\cos(lat_0) / \\rho))
lon = lon_0 + \\arctan2(
x * \\sin(c),
\\rho * \\cos(lat_0) * \\cos(c) - y * \\sin(lat_0) * \\sin(c))
\\rho = \\sqrt(x^2 + y^2)
c = \\rho / R
Where x, y are the Cartesian position from the center of projection;
lat, lon the corresponding latitude and longitude; lat_0, lon_0 are the
latitude and longitude of the center of the projection; R is the radius of
the earth (defaults to ~6371 km). lon is adjusted to be between -180 and
180.
Parameters
----------
x, y : array-like
Cartesian coordinates in the same units as R, typically meters.
lon_0, lat_0 : float
Longitude and latitude, in degrees, of the center of the projection.
R : float, optional
Earth radius in the same units as x and y. The default value is in
units of meters.
Returns
-------
lon, lat : array
Longitude and latitude of Cartesian coordinates in degrees.
References
----------
.. [1] Snyder, J. P. Map Projections--A Working Manual. U. S. Geological
Survey Professional Paper 1395, 1987, pp. 191-202.
"""
x = np.atleast_1d(np.asarray(data['x']))
y = np.atleast_1d(np.asarray(data['y']))
lat_0_rad = np.deg2rad(data['radar_lat'])
lon_0_rad = np.deg2rad(data['radar_lon'])
rho = np.sqrt(x * x + y * y)
c = rho / R
#with warnings.catch_warnings():
# division by zero may occur here but is properly addressed below so
# the warnings can be ignored
#warnings.simplefilter("ignore", RuntimeWarning)
lat_rad = np.arcsin(
np.cos(c) * np.sin(lat_0_rad) +
y * np.sin(c) * np.cos(lat_0_rad) / rho)
data['lat_gate'] = np.rad2deg(lat_rad)
# fix cases where the distance from the center of the projection is zero
data['lat_gate'][rho == 0] = data['radar_lat']
x1 = x * np.sin(c)
x2 = rho * np.cos(lat_0_rad) * np.cos(c) - y * np.sin(lat_0_rad) * np.sin(
c)
lon_rad = lon_0_rad + np.arctan2(x1, x2)
data['lon_gate'] = np.rad2deg(lon_rad)
# Longitudes should be from -180 to 180 degrees
data['lon_gate'][data['lon_gate'] > 180] -= 360.
data['lon_gate'][data['lon_gate'] < -180] += 360.
return data
def cut_object(img, mask, padding, use_mask=False, bg_color=None, allow_rotate=True):
""" cut an object from image according binary object segmentation
:param ndarray img: inout image
:param ndarray mask: segmentation
:param int padding: set padding around segmented object
:param bool use_mask: fill BG values also outside the mask
:param bg_color: set as default values outside bounding box
:param bool allow_rotate: allowing rotate object to get minimal bbox
:return:
>>> img = np.ones((10, 20), dtype=int)
>>> img[3:7, 4:16] = 2
>>> mask = np.zeros((10, 20), dtype=int)
>>> mask[4:6, 5:15] = 1
>>> cut_object(img, mask, 2)
array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1],
[1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1],
[1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1],
[1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]])
>>> cut_object(img, mask, 2, use_mask=True, allow_rotate=False)
array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1],
[1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]])
"""
assert mask.shape[:2] == img.shape[:2]
# prepare a simple mask with one horizontal segment
prop = measure.regionprops(np.array([[0] * 20, [1] * 20, [0] * 20], dtype=int))[0]
#: according to skimage version the major axis are swapped
PROP_ROTATION_OFFSET = prop.orientation
prop = measure.regionprops(mask.astype(int))[0]
bg_pixels = np.hstack([mask[0, :], mask[:, 0], mask[-1, :], mask[:, -1]])
bg_mask = np.argmax(np.bincount(bg_pixels))
if not bg_color:
bg_color = get_image2d_boundary_color(img)
if allow_rotate:
rotate = np.rad2deg(prop.orientation - PROP_ROTATION_OFFSET)
shift = prop.centroid - (np.array(mask.shape) / 2.)
shift = np.append(shift, np.zeros(img.ndim - mask.ndim))
mask = ndimage.interpolation.shift(mask, -shift[:mask.ndim], order=0)
mask = ndimage.rotate(mask, -rotate, order=0, mode='constant', cval=np.nan)
img = ndimage.interpolation.shift(img, -shift[:img.ndim], order=0)
img = ndimage.rotate(img, -rotate, order=0, mode='constant', cval=np.nan)
img_cut = img.copy()
img_cut[np.isnan(mask), ...] = bg_color
mask[np.isnan(mask)] = bg_mask
prop = measure.regionprops(mask.astype(int))[0]
min_row, min_col, max_row, max_col = add_padding(img_cut.shape, padding, *prop.bbox)
img_cut = img_cut[min_row:max_row, min_col:max_col, ...]
if use_mask:
use_mask = mask[min_row:max_row, min_col:max_col, ...].astype(bool)
img_cut[~use_mask, ...] = bg_color
return img_cut
def test_bird():
"""Test Bird/Hulstrom Clearsky Model"""
times = pd.date_range(start='1/1/2015 0:00',
end='12/31/2015 23:00',
freq='H')
tz = -7 # test timezone
gmt_tz = pytz.timezone('Etc/GMT%+d' % -(tz))
times = times.tz_localize(gmt_tz) # set timezone
# match test data from BIRD_08_16_2012.xls
latitude = 40.
longitude = -105.
press_mB = 840.
o3_cm = 0.3
h2o_cm = 1.5
aod_500nm = 0.1
aod_380nm = 0.15
b_a = 0.85
alb = 0.2
eot = solarposition.equation_of_time_spencer71(times.dayofyear)
hour_angle = solarposition.hour_angle(times, longitude, eot) - 0.5 * 15.
declination = solarposition.declination_spencer71(times.dayofyear)
zenith = solarposition.solar_zenith_analytical(np.deg2rad(latitude),
np.deg2rad(hour_angle),
declination)
zenith = np.rad2deg(zenith)
airmass = atmosphere.get_relative_airmass(zenith, model='kasten1966')
etr = irradiance.get_extra_radiation(times)
# test Bird with time series data
field_names = ('dni', 'direct_horizontal', 'ghi', 'dhi')
irrads = clearsky.bird(zenith, airmass, aod_380nm, aod_500nm, h2o_cm,
o3_cm, press_mB * 100., etr, b_a, alb)
Eb, Ebh, Gh, Dh = (irrads[_] for _ in field_names)
data_path = DATA_DIR / 'BIRD_08_16_2012.csv'
testdata = pd.read_csv(data_path, usecols=range(1, 26), header=1).dropna()
testdata.index = times[1:48]
assert np.allclose(testdata['DEC'], np.rad2deg(declination[1:48]))
assert np.allclose(testdata['EQT'], eot[1:48], rtol=1e-4)
assert np.allclose(testdata['Hour Angle'], hour_angle[1:48])
assert np.allclose(testdata['Zenith Ang'], zenith[1:48])
dawn = zenith < 88.
dusk = testdata['Zenith Ang'] < 88.
am = pd.Series(np.where(dawn, airmass, 0.), index=times).fillna(0.0)
assert np.allclose(testdata['Air Mass'].where(dusk, 0.),
am[1:48],
rtol=1e-3)
direct_beam = pd.Series(np.where(dawn, Eb, 0.), index=times).fillna(0.)
assert np.allclose(testdata['Direct Beam'].where(dusk, 0.),
direct_beam[1:48],
rtol=1e-3)
direct_horz = pd.Series(np.where(dawn, Ebh, 0.), index=times).fillna(0.)
assert np.allclose(testdata['Direct Hz'].where(dusk, 0.),
direct_horz[1:48],
rtol=1e-3)
global_horz = pd.Series(np.where(dawn, Gh, 0.), index=times).fillna(0.)
assert np.allclose(testdata['Global Hz'].where(dusk, 0.),
global_horz[1:48],
rtol=1e-3)
diffuse_horz = pd.Series(np.where(dawn, Dh, 0.), index=times).fillna(0.)
assert np.allclose(testdata['Dif Hz'].where(dusk, 0.),
diffuse_horz[1:48],
rtol=1e-3)
# test keyword parameters
irrads2 = clearsky.bird(zenith,
airmass,
aod_380nm,
aod_500nm,
h2o_cm,
dni_extra=etr)
Eb2, Ebh2, Gh2, Dh2 = (irrads2[_] for _ in field_names)
data_path = DATA_DIR / 'BIRD_08_16_2012_patm.csv'
testdata2 = pd.read_csv(data_path, usecols=range(1, 26), header=1).dropna()
testdata2.index = times[1:48]
direct_beam2 = pd.Series(np.where(dawn, Eb2, 0.), index=times).fillna(0.)
assert np.allclose(testdata2['Direct Beam'].where(dusk, 0.),
direct_beam2[1:48],
rtol=1e-3)
direct_horz2 = pd.Series(np.where(dawn, Ebh2, 0.), index=times).fillna(0.)
assert np.allclose(testdata2['Direct Hz'].where(dusk, 0.),
direct_horz2[1:48],
rtol=1e-3)
global_horz2 = pd.Series(np.where(dawn, Gh2, 0.), index=times).fillna(0.)
assert np.allclose(testdata2['Global Hz'].where(dusk, 0.),
global_horz2[1:48],
rtol=1e-3)
diffuse_horz2 = pd.Series(np.where(dawn, Dh2, 0.), index=times).fillna(0.)
assert np.allclose(testdata2['Dif Hz'].where(dusk, 0.),
diffuse_horz2[1:48],
rtol=1e-3)
# test scalars just at noon
# XXX: calculations start at 12am so noon is at index = 12
irrads3 = clearsky.bird(zenith[12],
airmass[12],
aod_380nm,
aod_500nm,
h2o_cm,
dni_extra=etr[12])
Eb3, Ebh3, Gh3, Dh3 = (irrads3[_] for _ in field_names)
# XXX: testdata starts at 1am so noon is at index = 11
np.allclose([Eb3, Ebh3, Gh3, Dh3],
testdata2[['Direct Beam', 'Direct Hz', 'Global Hz',
'Dif Hz']].iloc[11],
rtol=1e-3)
return pd.DataFrame({
'Eb': Eb,
'Ebh': Ebh,
'Gh': Gh,
'Dh': Dh
},
index=times)
# Maximm area rectangle
screenCnt = cnts[0]
# show the contour (outline) of the piece of paper
cv2.drawContours(image, [screenCnt], -1, (0, 255, 0), 2)
cv2.imshow("Outline", image)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Calculate angles
xs = [num[0] for num in screenCnt]
ys = [num[1] for num in screenCnt]
angles = np.arctan([
(ys[0] - ys[1]) / (xs[0] - xs[1]),
(ys[1] - ys[2]) / (xs[1] - xs[2]),
])
# rotation angle
rotation_angle = abs(np.rad2deg(angles[0]))
print('rotation angle: ', rotation_angle)
# tilted angle
titled_angle = abs(np.rad2deg(angles[0] - angles[1]))
print('tilted angle: ', titled_angle)
# image area ratio
area = cv2.contourArea(screenCnt)
image_area = image.shape[0] * image.shape[1]
print(image_area)
print('area ratio: ', area / image_area)
def train():
config_path = sys.argv[1]
with open(config_path, 'r') as f:
config = yaml.load(f)
root_log_dir = config['root_log_dir']
data_path = config['data_path']
experiment_name = '_'.join([config['experiment_name'], get_experiment_id()])
if not os.path.exists(root_log_dir):
os.mkdir(root_log_dir)
experiment_dir = os.path.join(root_log_dir, experiment_name)
os.mkdir(experiment_dir)
shutil.copy(config_path, experiment_dir)
(xtr, ptr_rad, ttr_rad, rtr_rad, names_tr), \
(xval, pval_rad, tval_rad, rval_rad, names_val), \
(xte, pte_rad, tte_rad, rte_rad, names_te) = load_idiap(config['data_path'])
image_height, image_width = xtr.shape[1], xtr.shape[2]
net_output = config['net_output']
if net_output == 'pan':
ytr = rad2bit(ptr_rad)
yval = rad2bit(pval_rad)
yte = rad2bit(pte_rad)
ytr_deg = np.rad2deg(ptr_rad)
yval_deg = np.rad2deg(pval_rad)
yte_deg = np.rad2deg(pte_rad)
elif net_output == 'tilt':
ytr = rad2bit(ttr_rad)
yval = rad2bit(tval_rad)
yte = rad2bit(tte_rad)
ytr_deg = np.rad2deg(ttr_rad)
yval_deg = np.rad2deg(tval_rad)
yte_deg = np.rad2deg(tte_rad)
elif net_output == 'roll':
ytr = rad2bit(rtr_rad)
yval = rad2bit(rval_rad)
yte = rad2bit(rte_rad)
ytr_deg = np.rad2deg(rtr_rad)
yval_deg = np.rad2deg(rval_rad)
yte_deg = np.rad2deg(rte_rad)
else:
raise ValueError("net_output should be 'pan', 'tilt' or 'roll'")
net_output = config['net_output']
predict_kappa = config['predict_kappa']
fixed_kappa_value = config['fixed_kappa_value']
if config['loss'] == 'cosine':
print("using cosine loss..")
loss_te = cosine_loss_tf
elif config['loss'] == 'von_mises':
print("using von-mises loss..")
loss_te = von_mises_loss_tf
elif config['loss'] == 'mad':
print("using mad loss..")
loss_te = mad_loss_tf
elif config['loss'] == 'vm_likelihood':
print("using likelihood loss..")
if predict_kappa:
loss_te = von_mises_neg_log_likelihood_keras
else:
def _von_mises_neg_log_likelihood_keras_fixed(y_true, y_pred):
mu_pred = y_pred[:, 0:2]
kappa_pred = tf.ones([tf.shape(y_pred[:, 2:])[0], 1])*fixed_kappa_value
return -K.mean(von_mises_log_likelihood_tf(y_true, mu_pred, kappa_pred))
loss_te = _von_mises_neg_log_likelihood_keras_fixed
else:
raise ValueError("loss should be 'mad','cosine','von_mises' or 'vm_likelihood'")
best_trial_id = 0
n_trials = config['n_trials']
batch_sizes = config['batch_sizes']
learning_rates = config['learning_rates']
params_grid = list(itertools.product(learning_rates, batch_sizes))*n_trials
results = dict()
res_cols = ['trial_id', 'batch_size', 'learning_rate', 'val_maad', 'val_likelihood', 'test_maad', 'test_likelihood']
results_df = pd.DataFrame(columns=res_cols)
results_csv = os.path.join(experiment_dir, 'results.csv')
# for tid in range(0, n_trials):
for tid, params in enumerate(params_grid):
learning_rate = params[0]
batch_size = params[1]
print("TRIAL %d // %d" % (tid, len(params_grid)))
print("batch_size: %d" % batch_size)
print("learning_rate: %f" % learning_rate)
trial_dir = os.path.join(experiment_dir, str(tid))
os.mkdir(trial_dir)
print("logs could be found at %s" % trial_dir)
vgg_model = vgg.BiternionVGG(image_height=image_height,
image_width=image_width,
n_channels=3,
predict_kappa=predict_kappa,
fixed_kappa_value=fixed_kappa_value)
optimizer = keras.optimizers.Adam(epsilon=1.0e-07,
lr=learning_rate,
decay=0.0)
vgg_model.model.compile(loss=loss_te, optimizer=optimizer)
tensorboard_callback = keras.callbacks.TensorBoard(log_dir=trial_dir)
train_csv_log = os.path.join(trial_dir, 'train.csv')
csv_callback = keras.callbacks.CSVLogger(train_csv_log, separator=',', append=False)
best_model_weights_file = os.path.join(trial_dir, 'vgg_bit_' + config['loss'] + '_town.best.weights.h5')
model_ckpt_callback = keras.callbacks.ModelCheckpoint(best_model_weights_file,
monitor='val_loss',
mode='min',
save_best_only=True,
save_weights_only=True,
period=1,
verbose=1)
vgg_model.model.save_weights(best_model_weights_file)
vgg_model.model.fit(x=xtr, y=ytr,
batch_size=batch_size,
epochs=config['n_epochs'],
verbose=1,
validation_data=(xval, yval),
callbacks=[tensorboard_callback, csv_callback, model_ckpt_callback])
best_model = vgg.BiternionVGG(image_height=image_height,
image_width=image_width,
n_channels=3,
predict_kappa=predict_kappa,
fixed_kappa_value=fixed_kappa_value)
best_model.model.load_weights(best_model_weights_file)
trial_results = dict()
trial_results['learning_rate'] = float(learning_rate)
trial_results['batch_size'] = float(batch_size)
trial_results['ckpt_path'] = best_model_weights_file
trial_results['train'] = best_model.evaluate(xtr, ytr_deg, 'train')
trial_results['validation'] = best_model.evaluate(xval, yval_deg, 'validation')
trial_results['test'] = best_model.evaluate(xte, yte_deg, 'test')
results[tid] = trial_results
results_np = np.asarray([tid, batch_size, learning_rate,
trial_results['validation']['maad_loss'],
trial_results['validation']['log_likelihood_mean'],
trial_results['test']['maad_loss'],
trial_results['test']['log_likelihood_mean']]).reshape([1, 7])
trial_res_df = pd.DataFrame(results_np, columns=res_cols)
results_df = results_df.append(trial_res_df)
results_df.to_csv(results_csv)
if tid > 0:
if trial_results['validation']['log_likelihood_mean'] > \
results[best_trial_id]['validation']['log_likelihood_mean']:
best_trial_id = tid
print("Better validation loss achieved, current best trial: %d" % best_trial_id)
print("loading best model..")
best_ckpt_path = results[best_trial_id]['ckpt_path']
overall_best_ckpt_path = os.path.join(experiment_dir, 'vgg.full_model.overall_best.weights.hdf5')
shutil.copy(best_ckpt_path, overall_best_ckpt_path)
best_model = vgg.BiternionVGG(image_height=image_height,
image_width=image_width,
n_channels=3,
predict_kappa=predict_kappa,
fixed_kappa_value=fixed_kappa_value)
best_model.model.load_weights(overall_best_ckpt_path)
print("finetuning kappa values..")
best_kappa = fixed_kappa_value
if not predict_kappa:
best_kappa = finetune_kappa(xval, yval, best_model)
print("best kappa: %f" % best_kappa)
best_model = vgg.BiternionVGG(image_height=image_height,
image_width=image_width,
n_channels=3,
predict_kappa=predict_kappa,
fixed_kappa_value=best_kappa)
best_model.model.load_weights(overall_best_ckpt_path)
best_results = dict()
best_results['learning_rate'] = results[best_trial_id]['learning_rate']
best_results['batch_size'] = results[best_trial_id]['batch_size']
print("evaluating best model..")
best_results['train'] = best_model.evaluate(xtr, ytr_deg, 'train')
best_results['validation'] = best_model.evaluate(xval, yval_deg, 'validation')
best_results['test'] = best_model.evaluate(xte, yte_deg, 'test')
results['best'] = best_results
results_yml_file = os.path.join(experiment_dir, 'results.yml')
with open(results_yml_file, 'w') as results_yml_file:
yaml.dump(results, results_yml_file, default_flow_style=False)
results_df.to_csv(results_csv)
return
def ephemeris_passes(opt, st0, et0):
"""
DESCRIPTION:
Finds passes from the start time to the end time given the options. Will
implement a bash script or execute on the command line.
USAGE:
ephemeris_passes(opt, st0, et0)
INPUTS:
opt command line arguments
st0 unix time start time
et0 unix time end time
RETURNS:
None
AFFECTS:
Prints all the passes
EXCEPTIONS:
None
DEPENDENCIES:
ephem
"""
passes = {}
objects = __read_config__(opt.config)
site = __read_config__(opt.site)
if opt.verbose:
print("# got objects ", objects)
print("# got radio site ", site)
print("\n")
ctime = st0
etime = et0
last_sat_rise = ctime
while ctime < etime:
obj = get_next_object(opt, site, objects, ctime)
obj_id = obj
obj_info = objects[obj]
if opt.debug:
print("# object ", obj_id, " @ ", ctime)
print("# obj_info", obj_info)
site_name = site["site"]["name"]
site_tag = site["site"]["tag"]
obs_lat = site["site"]["latitude"]
obs_long = site["site"]["longitude"]
obs_elev = float(site["site"]["elevation"])
obj_name = obj_info["name"]
obj_tle1 = obj_info["tle1"][1:-1]
obj_tle2 = obj_info["tle2"][1:-1]
obj_freqs = np.array(string.split(obj_info["frequencies"], ","),
np.float32)
c_dtime = datetime.datetime.utcfromtimestamp(ctime)
c_ephem_time = ephem.Date(c_dtime)
try:
(sat_rise, sat_transit, sat_set) = satellite_rise_and_set(
opt,
obs_lat,
obs_long,
obs_elev,
obj_name,
obj_tle1,
obj_tle2,
c_ephem_time,
)
if sat_set <= sat_rise or sat_transit <= sat_rise or sat_set <= sat_transit:
continue
if not last_sat_rise == sat_rise:
(
sub_lat,
sub_long,
sat_range,
sat_velocity,
az,
el,
ra,
dec,
alt,
) = satellite_values_at_time(
opt,
obs_lat,
obs_long,
obs_elev,
obj_name,
obj_tle1,
obj_tle2,
sat_transit,
)
(obj_bandwidth, obj_doppler) = satellite_bandwidth(
opt,
obs_lat,
obs_long,
obs_elev,
obj_name,
obj_tle1,
obj_tle2,
sat_rise,
sat_set,
op.interval,
obj_freqs,
)
last_sat_rise = sat_rise
if opt.debug:
print(
"time : ",
c_ephem_time,
sat_set,
(sat_set - c_ephem_time) * 60 * 60 * 24,
)
ctime = ctime + (sat_set - c_ephem_time) * 60 * 60 * 24
if opt.el_mask:
el_val = np.rad2deg(el)
el_mask = np.float(opt.el_mask)
if opt.debug:
print("# el_val ", el_val, " el_mask ", el_mask)
if el_val < el_mask: # check mask here!
continue
# This should really go out as digital metadata into the recording location
print("# Site : %s " % (site_name))
print("# Site tag : %s " % (site_tag))
print("# Object Name: %s" % (obj_name))
print(
"# observer @ latitude : %s, longitude : %s, elevation : %s m"
% (obs_lat, obs_long, obs_elev))
print(
"# GMT -- Rise Time: %s, Transit Time: %s, Set Time: %s" %
(sat_rise, sat_transit, sat_set))
print("# Azimuth: %f deg, Elevation: %f deg, Altitude: %g km" %
(np.rad2deg(az), np.rad2deg(el), alt / 1000.0))
print("# Frequencies: %s MHz, Bandwidth: %s kHz" % (
obj_freqs / 1.0e6,
obj_bandwidth[np.argmax(obj_bandwidth)] / 1.0e3,
))
pass_md = {
"obj_id": obj_id,
"rise_time": sat_rise,
"transit_time": sat_transit,
"set_time": sat_set,
"azimuth": np.rad2deg(az),
"elevation": np.rad2deg(el),
"altitude": alt,
"doppler_frequency": obj_doppler,
"doppler_bandwidth": obj_bandwidth,
}
if opt.schedule:
d = sat_rise.tuple()
rise_time = "%04d%02d%02d_%02d%02d" % (d[0], d[1], d[2],
d[3], d[4])
offset_rise = ephem.date(sat_rise - ephem.minute)
d = offset_rise.tuple()
offset_rise_time = "%04d-%02d-%02dT%02d:%02d:%02dZ" % (
d[0],
d[1],
d[2],
d[3],
d[4],
int(d[5]),
)
offset_set = ephem.date(sat_set + ephem.minute)
d = offset_set.tuple()
offset_set_time = "%04d-%02d-%02dT%02d:%02d:%02dZ" % (
d[0],
d[1],
d[2],
d[3],
d[4],
int(d[5]),
)
cmd_lines = []
radio_channel = string.split(
site["radio"]["channel"][1:-1], ",")
radio_gain = string.split(site["radio"]["gain"][1:-1], ",")
radio_address = string.split(
site["radio"]["address"][1:-1], ",")
recorder_channels = string.split(
site["recorder"]["channels"][1:-1], ",")
radio_sample_rate = site["radio"]["sample_rate"]
cmd_line0 = "%s " % (site["recorder"]["command"])
if site["radio"]["type"] == "b210":
# just record a fixed frequency, needs a dual radio Thor3 script. This can be done!
idx = 0
freq = obj_freqs[1]
cmd_line1 = '-r %s -d "%s" -s %s -e %s -c %s -f %4.3f ' % (
radio_sample_rate,
radio_channel[idx],
offset_rise_time,
offset_set_time,
recorder_channels[idx],
freq,
)
log_file_name = "%s_%s_%s_%dMHz.log" % (
site_tag,
obj_id,
offset_rise_time,
int(freq / 1.0e6),
)
cmd_fname = "%s_%s_%s_%dMHz" % (
site_tag,
obj_id,
rise_time,
int(freq / 1.0e6),
)
cmd_line2 = " -g %s -m %s --devargs num_recv_frames=1024 --devargs master_clock_rate=24.0e6 -o %s/%s" % (
radio_gain[idx],
radio_address[idx],
site["recorder"]["data_path"],
cmd_fname,
)
cmd_line2 += " {0}".format(site["radio"].get(
"extra_args", "")).rstrip()
if not opt.foreground:
cmd_line0 = "nohup " + cmd_line0
cmd_line2 = cmd_line2 + " 2>&1 &"
else:
cmd_line2 = cmd_line2
if opt.debug:
print(cmd_line0, cmd_line1, cmd_line2, cmd_fname)
cmd_lines.append((
cmd_line0 + cmd_line1 + cmd_line2,
cmd_fname,
pass_md,
obj_info,
))
print("\n")
elif site["radio"]["type"] == "n200_tvrx2":
cmd_line1 = (
' -r %s -d "%s %s" -s %s -e %s -c %s,%s -f %4.3f,%4.3f '
% (
radio_sample_rate,
radio_channel[0],
radio_channel[1],
offset_rise_time,
offset_set_time,
recorder_channels[0],
recorder_channels[1],
obj_freqs[0],
obj_freqs[1],
))
log_file_name = "%s_%s_%s_combined.log" % (
site_tag,
obj_id,
offset_rise_time,
)
cmd_fname = "%s_%s_%s_combined" % (site_tag, obj_id,
rise_time)
cmd_line2 = " -g %s,%s -m %s -o %s/%s" % (
radio_gain[0],
radio_gain[1],
radio_address[0],
site["recorder"]["data_path"],
cmd_fname,
)
cmd_line2 += " {0}".format(site["radio"].get(
"extra_args", "")).rstrip()
if not opt.foreground:
cmd_line0 = "nohup " + cmd_line0
cmd_line2 = cmd_line2 + " 2>&1 &"
else:
cmd_line2 = cmd_line2
if opt.debug:
print(cmd_line0, cmd_line1, cmd_line2, cmd_fname)
cmd_lines.append((
cmd_line0 + cmd_line1 + cmd_line2,
cmd_fname,
pass_md,
obj_info,
))
print("\n")
if opt.foreground:
dtstart0 = dateutil.parser.parse(offset_rise_time)
dtstop0 = dateutil.parser.parse(offset_set_time)
start0 = int((dtstart0 - datetime.datetime(
1970, 1, 1, tzinfo=pytz.utc)).total_seconds())
stop0 = int((dtstop0 - datetime.datetime(
1970, 1, 1, tzinfo=pytz.utc)).total_seconds())
if opt.verbose:
print("# waiting for %s @ %s " %
(obj_id, offset_rise_time))
while time.time() < start0 - 30:
time.sleep(op.interval)
if opt.verbose:
print("# %d sec" % (start0 - time.time()))
for cmd_tuple in cmd_lines:
cmd, cmd_fname, pass_md, info_md = cmd_tuple
print("# Executing command %s " % (cmd))
# write the digital metadata
start_idx = int(start0)
mdata_dir = (site["recorder"]["metadata_path"] + "/" +
cmd_fname + "/metadata")
# site metadata
# note we use directory structure for the dictionary here
# eventually we will add this feature to digital metadata
for k in site:
try:
os.makedirs(mdata_dir + "/config/%s" % (k))
except:
pass
md_site_obj = DigitalMetadataWriter(
mdata_dir + "/config/%s" % (k), 3600, 60, 1, 1,
k)
if opt.debug:
print(site[k])
if opt.verbose:
print("# writing metadata config / %s " % (k))
md_site_obj.write(start_idx, site[k])
# info metadata
try:
os.makedirs(mdata_dir + "/info")
except:
pass
md_info_obj = DigitalMetadataWriter(
mdata_dir + "/info", 3600, 60, 1, 1, "info")
if opt.verbose:
print("# writing metadata info")
if opt.debug:
print(info_md)
md_info_obj.write(start_idx, info_md)
# pass metadata
try:
os.makedirs(mdata_dir + "/pass")
except:
pass
md_pass_obj = DigitalMetadataWriter(
mdata_dir + "/pass", 3600, 60, 1, 1, "pass")
if opt.verbose:
print("# writing metadata pass")
if opt.debug:
print(pass_md)
md_pass_obj.write(start_idx, pass_md)
# sys.exit(1)
# call the command
try:
subprocess.call(cmd, shell=True)
except Exception as eobj:
exp_str = str(ExceptionString(eobj))
print("exception: %s." % (exp_str))
exc_type, exc_value, exc_traceback = sys.exc_info()
lines = traceback.format_exception(
exc_type, exc_value, exc_traceback)
print(lines)
print("# wait...")
while time.time() < stop0 + 1:
time.sleep(op.interval)
if opt.verbose:
print("# complete in %d sec" %
(stop0 - time.time()))
except Exception as eobj:
exp_str = str(ExceptionString(eobj))
print("exception: %s." % (exp_str))
exc_type, exc_value, exc_traceback = sys.exc_info()
lines = traceback.format_exception(exc_type, exc_value,
exc_traceback)
print(lines)
# sys.exit(1)
# advance 10 minutes
ctime = ctime + 60 * op.interval
continue
# Add data from OB161195
datasets = []
file_names = ['KCT01I.dat', 'KCT41I.dat', 'KCT42I.dat', 'KSA01I.dat',
'KSA41I.dat', 'KSA42I.dat', 'KSS01I.dat', 'KSS41I.dat',
'KSS42I.dat', 'spitzer_b12.dat']
dir_ = os.path.join(MulensModel.DATA_PATH, "photometry_files", "OB161195")
for file_name in file_names:
file_ = os.path.join(dir_, file_name)
datasets.append(MulensModel.MulensData(file_name=file_, add_2450000=True))
# Close-- model
model = MulensModel.Model(
{'t_0': 2457568.7692, 'u_0': -0.05321, 't_E': 9.96, 'rho': 0.00290,
'pi_E_N': -0.2154, 'pi_E_E': -0.380,
'alpha': np.rad2deg(-0.9684), 's': 0.9842, 'q': 0.0000543})
methods = [7560., 'VBBL', 7580.]
model.set_magnification_methods(methods)
model.set_default_magnification_method('point_source_point_lens')
event = MulensModel.Event(datasets=datasets, model=model,
coords="17:55:23.50 -30:12:26.1")
# Which parameters we want to fit?
parameters_to_fit = ["t_0", "u_0", "t_E", "pi_E_N", "pi_E_E", "fsKCT01", "fsSpitzer"]
# And remember to provide dispersions to draw starting set of points
sigmas = [0.01, 0.001, 0.1, 0.01, 0.01, 0.01, 0.01]
# Initializations for EMCEE
image[int(round((x - .2) * 100)), int(round((y + 1) * 100))] += 1
angle = angle + angle_increment
img = np.uint8(image / np.max(image) * 255)
#
lines = cv2.HoughLinesP(img, 1, np.pi / 180, 1, 5, 3)
line_img = np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
k = []
try:
for line in lines:
for x1, y1, x2, y2 in line:
k = np.append(k,
(y2.astype(np.float32) - y1.astype(np.float32)) /
(x2.astype(np.float32) - x1.astype(np.float32)))
cv2.line(line_img, (x1, y1), (x2, y2), [0, 255, 255], 1)
theta_h = np.arctan(np.mean(k))
#theta_h=np.arctan(np.max(y2a)-np.max(y1a))/(np.max(x2a)-np.max(x1a))
plt.imshow(line_img)
#plt.imshow(img,cmap='Reds')
plt.pause(0.001)
except:
plt.imshow(img, cmap='Reds')
break
print("{0:.4f}".format(np.rad2deg(theta_h)))
theta_all = np.append(theta_all, theta_h)
#break
plt.show()
bag.close()
def angle_between(p1, p2)->float: # 두점 사이의 각도:(getAngle3P 계산용) 시계 방향으로 계산한다. P1-(0,0)-P2의 각도를 시계방향으로 ang1 = np.arctan2(*p1[::-1]) ang2 = np.arctan2(*p2[::-1]) res = np.rad2deg((ang1 - ang2) % (2 * np.pi)) return res
def quat2angle(q0, qvec, output_unit='rad', rotation_sequence='ZYX'):
"""
Convert a unit quaternion to the equivalent sequence of angles of rotation
about the rotation_sequence axes.
This function can take inputs in either degree or radians, and can also
batch process a series of rotations (e.g., time series of quaternions).
By default this function assumes aerospace rotation sequence but can be
changed using the ``rotation_sequence`` keyword argument.
Parameters
----------
q0 : {(N,), (N,1), or (1,N)} array_like
Scalar componenet of the quaternion
qvec : {(N,3),(3,N)} array_like
Vector component of the quaternion
rotation_sequence : {'ZYX'}, optional
Rotation sequences. Default is 'ZYX'.
Returns
-------
rotAngle1, rotAngle2, rotAngle3 : {(N,), (N,1), or (1,N)} array_like
They are a sequence of angles about successive axes described by
rotation_sequence.
output_unit : {'rad', 'deg'}, optional
Rotation angles. Default is 'rad'.
Notes
-----
Convert rotation angles to unit quaternion that transforms a vector in F1 to
F2 according to
:math:`v_q^{F2} = q^{-1} \otimes v_q^{F1} \otimes q`
where :math:`\otimes` indicates the quaternion multiplcation and :math:`v_q^F`
is a pure quaternion representation of the vector :math:`v_q^F`. The scalar
componenet of :math:`v_q^F` is zero.
For aerospace sequence ('ZYX'): rotAngle1 = psi, rotAngle2 = the,
and rotAngle3 = phi
Examples
--------
>>> import numpy as np
>>> from navpy import quat2angle
>>> q0 = 0.800103145191266
>>> qvec = np.array([0.4619398,0.3314136,-0.1913417])
>>> psi, theta, phi = quat2angle(q0,qvec)
>>> psi
1.0217702360987295e-07
>>> theta
0.7853982192745731
>>> phi
1.0471976051067484
>>> psi, theta, phi = quat2angle(q0,qvec,output_unit='deg')
>>> psi
5.8543122160542875e-06
>>> theta
45.00000320152342
>>> phi
60.000003088824108
>>> q0 = [ 0.96225019, 0.92712639, 0.88162808]
>>> qvec = np.array([[-0.02255757, 0.25783416, 0.08418598],\
[-0.01896854, 0.34362114, 0.14832854],\
[-0.03266701, 0.4271086 , 0.19809857]])
>>> psi, theta, phi = quat2angle(q0,qvec,output_unit='deg')
>>> psi
array([ 9.99999941, 19.99999997, 29.9999993 ])
>>> theta
array([ 30.00000008, 39.99999971, 50.00000025])
>>> phi
array([ -6.06200867e-07, 5.00000036e+00, 1.00000001e+01])
"""
q0, N0 = _input_check_Nx1(q0)
qvec, Nvec = _input_check_Nx3(qvec)
if (N0 != Nvec):
raise ValueError('Inputs are not of same dimensions')
if (N0 == 1):
q1 = qvec[0]
q2 = qvec[1]
q3 = qvec[2]
else:
q1 = qvec[:, 0]
q2 = qvec[:, 1]
q3 = qvec[:, 2]
rotAngle1 = np.zeros(N0)
rotAngle2 = np.zeros(N0)
rotAngle3 = np.zeros(N0)
if (rotation_sequence == 'ZYX'):
m11 = 2 * q0**2 + 2 * q1**2 - 1
m12 = 2 * q1 * q2 + 2 * q0 * q3
m13 = 2 * q1 * q3 - 2 * q0 * q2
m23 = 2 * q2 * q3 + 2 * q0 * q1
m33 = 2 * q0**2 + 2 * q3**2 - 1
rotAngle1 = np.arctan2(m12, m11)
rotAngle2 = np.arcsin(-m13)
rotAngle3 = np.arctan2(m23, m33)
else:
raise ValueError('rotation_sequence unknown')
if (output_unit == 'deg'):
rotAngle1 = np.rad2deg(rotAngle1)
rotAngle2 = np.rad2deg(rotAngle2)
rotAngle3 = np.rad2deg(rotAngle3)
return rotAngle1, rotAngle2, rotAngle3
# Unless required by applicable law or agreed to in writing, software distributed
# under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
# CONDITIONS OF ANY KIND, either express or implied. See the License for the
# specific language governing permissions and limitations under the License.
import numpy as np
import sys
import datetime
from wisdem.ccblade import CCAirfoil, CCBlade
from scipy import interpolate, gradient, integrate
# Some useful constants
now = datetime.datetime.now()
pi = np.pi
rad2deg = np.rad2deg(1)
deg2rad = np.deg2rad(1)
rpm2RadSec = 2.0*(np.pi)/60.0
RadSec2rpm = 60/(2.0 * np.pi)
class Controller():
"""
Class Controller used to calculate controller tunings parameters
Methods:
-------
tune_controller
Parameters:
-----------
def derotate3(image):
num_peaks = 20
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# t = time.time()
edges = cv2.Canny(gray, 25, 255)
# print("canny: {:.3f}ms".format((time.time() - t) * 1000))
# t = time.time()
h, a, d = transform.hough_line(edges)
# print("hough_line: {:.3f}ms".format((time.time() - t) * 1000))
# t = time.time()
_, ap, _ = transform.hough_line_peaks(h, a, d, num_peaks=num_peaks)
# print("hough_line_peaks: {:.3f}ms".format((time.time() - t) * 1000))
if len(ap) == 0:
raise RuntimeError("Bad Quality image")
absolute_deviations = [calculate_deviation(k) for k in ap]
average_deviation = np.mean(np.rad2deg(absolute_deviations))
ap_deg = [np.rad2deg(x) for x in ap]
bin_0_45 = []
bin_45_90 = []
bin_0_45n = []
bin_45_90n = []
for ang in ap_deg:
deviation_sum = int(90 - ang + average_deviation)
if compare_sum(deviation_sum):
bin_45_90.append(ang)
continue
deviation_sum = int(ang + average_deviation)
if compare_sum(deviation_sum):
bin_0_45.append(ang)
continue
deviation_sum = int(-ang + average_deviation)
if compare_sum(deviation_sum):
bin_0_45n.append(ang)
continue
deviation_sum = int(90 + ang + average_deviation)
if compare_sum(deviation_sum):
bin_45_90n.append(ang)
angles = [bin_0_45, bin_45_90, bin_0_45n, bin_45_90n]
lmax = 0
for j in range(len(angles)):
l = len(angles[j])
if l > lmax:
lmax = l
maxi = j
if lmax:
ans_arr = get_max_freq_elem(angles[maxi])
ans_res = np.mean(ans_arr)
else:
ans_arr = get_max_freq_elem(ap_deg)
ans_res = np.mean(ans_arr)
angle = ans_res
# if 0 <= angle <= 90:
# rot_angle = angle - 90 + r_angle
if -45 <= angle < 0:
angle = angle - 90
if -90 <= angle < -45:
angle = 90 + angle
if 90 <= angle <= 180:
angle -= 90
if angle>45:
angle = angle-90
rotated = rotate2(image, angle)
return rotated, angle
def llarate(VN, VE, VD, lat, alt, lat_unit='deg', alt_unit='m'):
"""
Calculate Latitude, Longitude, Altitude Rate given locally tangent velocity
Parameters
----------
VN : {(N,)} array like earth relative velocity in the North direction, m/s
VE : {(N,)} array like earth relative velocity in the East direction, m/s
VD : {(N,)} array like earth relative velocity in the Down direction, m/s
lat : {(N,)} array like latitudes, unit specified in lat_unit, default deg
alt : {(N,)} array like altitudes, unit specified in alt_unit, default m
Returns
-------
lla_dot : {(N,3)} np.array of latitude rate, longitude rate, altitude rate.
The unit of latitude and longitude rate will be the same as the
unit specified by lat_unit and the unit of altitude rate will be
the same as alt_unit
See Also
--------
earthrad : called by this method
Examples
--------
>>> import numpy as np
>>> from navpy import llarate
>>> llarate(100,0,0,45.0,0) # Moving North at 100 m/s, location is at N45.0
array([ 0.00089983, 0. , 0. ])
>>> # That output was in deg/sec
>>> lat = [np.pi/4, -np.pi/6]
>>> alt = [100.0, 50]
>>> VN = [100, 0]
>>> VE = [0, 100]
>>> VD = [0, -5]
>>> llarate(VN,VE,VD,lat,alt,lat_unit='rad')
array([[ 1.57047955e-05, 0.00000000e+00, 0.00000000e+00],\
[ 0.00000000e+00, 1.80887436e-05, 5.00000000e+00]])
>>> # That output was in rad/sec
"""
dim_check = 1
VN, N1 = _input_check_Nx1(VN)
VE, N2 = _input_check_Nx1(VE)
if (N2 != N1):
dim_check *= 0
VD, N2 = _input_check_Nx1(VD)
if (N2 != N1):
dim_check *= 0
lat, N2 = _input_check_Nx1(lat)
if (N2 != N1):
dim_check *= 0
alt, N2 = _input_check_Nx1(alt)
if (N2 != N1):
dim_check *= 0
if (dim_check == 0):
raise ValueError('Inputs are not of the same dimension')
Rew, Rns = earthrad(lat, lat_unit=lat_unit)
lla_dot = np.zeros((N1, 3))
if (lat_unit == 'deg'):
lla_dot[:, 0] = np.rad2deg(VN / (Rns + alt))
lla_dot[:, 1] = np.rad2deg(VE / (Rew + alt) / np.cos(np.deg2rad(lat)))
lla_dot[:, 2] = -VD
elif (lat_unit == 'rad'):
lla_dot[:, 0] = VN / (Rns + alt)
lla_dot[:, 1] = VE / (Rew + alt) / np.cos(lat)
lla_dot[:, 2] = -VD
if (N1 == 1):
lla_dot = lla_dot.reshape(3)
return lla_dot
def equatorial_coordinates_rotation(ra,
dec,
ra_center,
dec_center,
angle,
units='deg'):
'''
Rotation in spherical coordinates using Rodrigues formula.
Parameters
----------
ra, dec, ra_center, dec_center : Array or float
Right ascension and declination of the points to rotate
and their pivot centers.
units : String indicating ra, dec and angle units.
Options are: 'rad', 'deg'. Default: 'deg'.
Returns
-------
ra_rot, dec_rot : Array or float
Right ascencion and declination after the desired rotation
in units defined by 'units'.
Notes
-----
See this equations: https://math.stackexchange.com/a/1404353
In the prevoius link, the angle definition is not explicit
but they are like this:
> phi is longitude, or right ascension.
> theta is the zenith angle, or 90 - declination
'''
if units not in ['rad', 'deg']:
raise ValueError('Argument units="{}" not recognized. '
'Options are: "rad", "deg".'.format(units))
if units == 'deg':
ra, dec = np.deg2rad(ra), np.deg2rad(dec)
ra_center, dec_center = np.deg2rad(ra_center), np.deg2rad(dec_center)
angle = np.deg2rad(angle)
phi = ra
theta = dec
theta = np.pi / 2. - theta # theta is measured from the zenith
ax_phi = ra_center
ax_theta = dec_center
ax_theta = np.pi / 2. - ax_theta # ax_theta is measured from the zenith
ax = np.sin(theta) * np.cos(phi)
ay = np.sin(theta) * np.sin(phi)
az = np.cos(theta)
kx = np.sin(ax_theta) * np.cos(ax_phi)
ky = np.sin(ax_theta) * np.sin(ax_phi)
kz = np.cos(ax_theta)
a = np.stack((ax, ay, az), axis=1)
k = np.stack((kx, ky, kz), axis=1)
dot = np.sum(k * a, axis=1).reshape((-1, 1))
cross = np.cross(k, a, axisa=1, axisb=1)
b = a * np.cos(angle) + cross * np.sin(angle) + k * dot * (1 -
np.cos(angle))
new_phi = np.arctan2(b[:, 1], b[:, 0])
new_theta = np.arctan2(np.sqrt(b[:, 0]**2 + b[:, 1]**2), b[:, 2])
new_theta = np.pi / 2 - new_theta # get back to measure theta from equator
if units == 'deg':
ra_rot, dec_rot = np.rad2deg(new_phi), np.rad2deg(new_theta)
else:
ra_rot, dec_rot = new_phi, new_theta
return ra_rot, dec_rot
def RV2COE(r_vector, v_vector, mu):
"Pulls the whole system together into on Function"
h_vector = np.cross(r_vector, v_vector)
h_hat = h_vector / (np.linalg.norm(h_vector))
e_vector = np.divide(np.cross(v_vector, h_vector), mu) - np.divide(
r_vector, np.linalg.norm(r_vector))
e = np.linalg.norm(e_vector)
k_hat = [0, 0, 1]
n_vector = np.cross(k_hat, h_vector)
p = ((np.linalg.norm(h_vector))**2) / mu
theta = np.arctan2(np.dot(h_vector, np.cross(e_vector, r_vector)),
np.linalg.norm(h_vector) * np.dot(e_vector, r_vector))
i = np.arccos(np.dot(k_hat, h_hat))
a = p / (1 - e**2)
i_hat = [1, 0, 0]
j_hat = [0, 1, 0]
raan = np.arctan2(np.dot(j_hat, n_vector), np.dot(i_hat, n_vector))
w = np.arctan2(np.dot(h_vector, np.cross(n_vector, e_vector)),
np.linalg.norm(h_vector) * np.dot(n_vector, e_vector))
r_p = p / (1 + e)
r_a = p / (1 - e)
fpa = np.arctan((e * np.sin(theta)) / (1 + (e * np.cos(theta))))
period = 2 * np.pi * np.sqrt((a * a * a) / mu)
if (theta < 0):
theta = 2 * np.pi + theta
else:
theta = theta
if (raan < 0):
raan = 2 * np.pi + raan
else:
raan = raan
if (w < 0):
w = 2 * np.pi + w
else:
w = w
if (i < 0):
i = 2 * np.pi + i
else:
i = i
print("Semi Major Axis in Kilometers:\n\t", str(a), "\n")
print("Eccentricity:\n\t", str(np.linalg.norm(e_vector)), "\n")
print("Eccentricity Vector:\n\t", str(e_vector), "\n")
print("Inclination in Degrees:\n\t", str(np.rad2deg(i)), "\n")
print("RAAN in Degrees:\n\t", str(np.rad2deg(raan)), "\n")
print("Argument of Periapsis in Degrees:\n\t", str(np.rad2deg(w)), "\n")
print("True Anomaly in Degrees:\n\t", str(np.rad2deg(theta)), "\n")
print("Semi Latus Rectum in Kilometers:\n\t", str(p), "\n")
print("Radius of Periapsis in Kilometers:\n\t", str(r_p), "\n")
print("Radius of Apoapsis in Kilometers:\n\t", str(r_a), "\n")
print("Line of Nodes Vector in Kilometers:\n\t", str(n_vector), "\n")
print("Orbit Period in Seconds:\n\t", str(period), "\n")
return a, e, e_vector, i, raan, w, theta, p, r_p, r_a, n_vector, period
