def proportion_effectsize(prob1, prob2, method='normal'):
'''effect size for a test comparing two proportions
for use in power function
Parameters
----------
prob1, prob2: float or array_like
Returns
-------
es : float or ndarray
effect size
Notes
-----
only method='normal' is implemented to match pwr.p2.test
see http://www.statmethods.net/stats/power.html
I think other conversions to normality can be used, but I need to check.
'''
if method != 'normal':
raise ValueError('only "normal" is implemented')
es = 2 * (np.arcsin(np.sqrt(prob2)) - np.arcsin(np.sqrt(prob1)))
return es
def to_SQL(self, RAname, DECname):
if self.DECdeg != 90.0 and self.DECdeg != -90.0:
RAmax = self.RAdeg + \
360.0 * np.arcsin(np.sin(0.5*self.radius) / np.cos(self.DEC))/np.pi
RAmin = self.RAdeg - \
360.0 * np.arcsin(np.sin(0.5*self.radius) / np.cos(self.DEC))/np.pi
else:
#just in case, for some reason, we are looking at the poles
RAmax = 360.0
RAmin = 0.0
DECmax = self.DECdeg + self.radiusdeg
DECmin = self.DECdeg - self.radiusdeg
#initially demand that all objects are within a box containing the circle
#set from the DEC1=DEC2 and RA1=RA2 limits of the haversine function
bound = ("%s between %f and %f and %s between %f and %f "
% (RAname, RAmin, RAmax, DECname, DECmin, DECmax))
#then use the Haversine function to constrain the angular distance form boresite to be within
#the desired radius. See http://en.wikipedia.org/wiki/Haversine_formula
bound = bound + ("and 2 * ASIN(SQRT( POWER(SIN(0.5*(%s - %s) * PI() / 180.0),2)" % (DECname,self.DECdeg))
bound = bound + ("+ COS(%s * PI() / 180.0) * COS(%s * PI() / 180.0) " % (DECname, self.DECdeg))
bound = bound + ("* POWER(SIN(0.5 * (%s - %s) * PI() / 180.0),2)))" % (RAname, self.RAdeg))
bound = bound + (" < %s " % self.radius)
return bound
def test_intersect_tube_2points_transformed():
'''inter_local should be the same as interpos,
even if we make the tube longer in z'''
pos = np.array([[50., 0., 0., 1.], [10., .5, 0., 1.], [2, 0, .3, 1.]])
dir = np.array([[-.1, 0., 0., 0], [-0.5, 0, 0., 0.], [-1., 0., 0., 0.]])
exp_inter_loc = np.array([[0., np.arcsin(0.5), 0.],
[0., 0., 0.3]]).T
exp_interpos = np.array([[1., 0., 0.],
[np.sqrt(0.75), 0.5, 0.],
[1, 0., .3]])
circ = Cylinder({'zoom': [1, 1., 5.]})
intersect, interpos, inter_local = circ.intersect(dir, pos)
assert np.all(intersect == True)
assert np.allclose(h2e(interpos), exp_interpos)
assert np.allclose(inter_local, exp_inter_loc)
# Rotate: interpos should be the same as in the unrotated case.
# Beware of rotation direction!
orient = transforms3d.axangles.axangle2mat([0, 0, 1], -.3)
circ = Cylinder({'orientation': orient})
intersect, interpos, inter_local = circ.intersect(dir, pos)
assert np.all(intersect == True)
assert np.allclose(h2e(interpos), exp_interpos)
assert np.allclose(inter_local, np.array([[0.3, 0.3 + np.arcsin(0.5), 0.3],
[0., 0., 0.3]]).T)
# Shift: inter_local is the same, but interpos changes
circ = Cylinder({'position': [-.3, 0, 0]})
intersect, interpos, inter_local = circ.intersect(dir, pos)
assert np.all(intersect == True)
assert np.allclose(h2e(interpos), np.array([[.7, 0, 0],
[np.sqrt(0.75) -.3, 0.5, 0.],
[.7, 0., .3]]))
assert np.allclose(inter_local, exp_inter_loc)
def theta_generate(system, plot = False, realonly = False, useapproximations = False):
global theta_2,theta_3, cos_theta_2, cos_theta_3, tan_theta_2
(n_1,n_2,n_3) = system.indices
theta_1 = system.theta_1
freq = system.freq
cos_theta_2 = (n_2)**-1 * array(n_2**2-((n_1)*sin(theta_1))**2,dtype = complex)
cos_theta_3 = (n_3)**-1 * array(n_3**2-((n_1)*sin(theta_1))**2,dtype = complex)
tan_theta_2 = sqrt((1-cos_theta_2**2)/cos_theta_2)
theta_2 = numpy.arcsin(sin(theta_1) * n_1/ n_2,dtype = complex)
theta_3 = numpy.arcsin(sin(theta_1)* n_1 / n_3,dtype = complex)
if plot == True:
py.plot(freq,numpy.real(cos_theta_2[IR]),'r')
py.plot(freq,numpy.imag(cos_theta_2[IR]),'b')
py.plot(freq,numpy.real(cos_theta_3[IR]),'g')
py.plot(freq,numpy.imag(cos_theta_3[IR]),'k')
py.title("cos Angles of incidence")
py.legend(('real2','imag2','real3','imag3'))
py.show()
system.angles = (theta_2,theta_3, cos_theta_2, cos_theta_3, tan_theta_2)
return 0
def getCurvePoints(self,P0,P1,P2,R,n=20):
"""Generates a curve. P0,P1 and P2 define a corner and R defines the radius of the
tangential circle element. n gives the number of point to approximate the curve.
P1 is the corner itself and P0 and P2 give the tangents.
"""
P0=np.array(P0)
P1=np.array(P1)
P2=np.array(P2)
o1=(P0-P1)/np.sqrt(np.dot(P0-P1,P0-P1))
o2=(P2-P1)/np.sqrt(np.dot(P2-P1,P2-P1))
if np.arcsin(np.cross(o1,o2)) > 0:
a=1.
b=-1.
else:
a=-1.
b=1.
v1=R*np.dot(np.array([[0.,b],[a,0.]]),o1)
v2=R*np.dot(np.array([[0.,a],[b,0.]]),o2)
dv=v2-v1
a=np.array([[o1[0],-o2[0]],[o1[1],-o2[1]]])
b=dv
x=np.linalg.solve(a,b)
circleCenter= P1+x[0]*o1+v1
angle = np.arcsin(np.cross(v2/R,v1/R))
points=[]
for i in range(n+1):
x=-i*angle/n
rot = np.array([[np.cos(x),-np.sin(x)],
[np.sin(x),np.cos(x)]])
points.append(circleCenter+np.dot(rot,-v1))
return points
def zoeppritz_rpp(vp1, vs1, rho1, vp2, vs2, rho2, theta1, terms=False):
"""
Exact Zoeppritz from expression.
This is useful because we can pass arrays to it, which we can't do to
scattering_matrix().
Dvorkin et al. (2014). Seismic Reflections of Rock Properties. Cambridge.
"""
theta1 = np.radians(theta1)
p = np.sin(theta1) / vp1 # Ray parameter
theta2 = np.arcsin(p * vp2)
phi1 = np.arcsin(p * vs1) # Reflected S
phi2 = np.arcsin(p * vs2) # Transmitted S
a = rho2 * (1 - 2 * np.sin(phi2)**2.) - rho1 * (1 - 2 * np.sin(phi1)**2.)
b = rho2 * (1 - 2 * np.sin(phi2)**2.) + 2 * rho1 * np.sin(phi1)**2.
c = rho1 * (1 - 2 * np.sin(phi1)**2.) + 2 * rho2 * np.sin(phi2)**2.
d = 2 * (rho2 * vs2**2 - rho1 * vs1**2)
E = (b * np.cos(theta1) / vp1) + (c * np.cos(theta2) / vp2)
F = (b * np.cos(phi1) / vs1) + (c * np.cos(phi2) / vs2)
G = a - d * np.cos(theta1)/vp1 * np.cos(phi2)/vs2
H = a - d * np.cos(theta2)/vp2 * np.cos(phi1)/vs1
D = E*F + G*H*p**2
rpp = (1/D) * (F*(b*(np.cos(theta1)/vp1) - c*(np.cos(theta2)/vp2)) \
- H*p**2 * (a + d*(np.cos(theta1)/vp1)*(np.cos(phi2)/vs2)))
return rpp
def __arc_segment(self,r,a,b):
"""
Returns length of circle segment in a half-infinite slit.
r ... radius of circle which is centered at the origin
a,b ... borders of slit containing (x,y) with x>a, b>y>0
"""
#print r, a, b
# scalar version
if np.isscalar(r):
if r<a: # circle outside slit
return 0;
elif r**2>a**2+b**2: # circle does not intersect left border at x=a
return r*np.arcsin(b/r);
else: # circle only intersects left border at x=a
return r*np.arccos(a/r);
# parallel version
else:
r = np.atleast_1d(r);
arc = np.zeros(r.size);
index = r>=a; # select radii which intersect half-slit
y2_a = r[index]**2-a**2; # intersection of circle with left border at x=a
ymax = np.where( y2_a > b**2, b, np.sqrt(y2_a) );
# largest y value on circle segment in half-slit
arc[index] = r[index] * np.arcsin( ymax / r[index] );
# calculate arc length
# DEBUG
if self.verbosity>2:
arc2 = [ self.__arc_segment(ir,a,b) for ir in r ];
assert(np.allclose( arc, arc2 ));
return arc;
raise RuntimeError("Should never reach this point");
def calculate_couplings_levine(dt: float, w_jk: Matrix,
w_kj: Matrix) -> Matrix:
"""
Compute the non-adiabatic coupling according to:
`Evaluation of the Time-Derivative Coupling for Accurate Electronic
State Transition Probabilities from Numerical Simulations`.
Garrett A. Meek and Benjamin G. Levine.
dx.doi.org/10.1021/jz5009449 | J. Phys. Chem. Lett. 2014, 5, 2351−2356
"""
# Orthonormalize the Overlap matrices
w_jk = np.linalg.qr(w_jk)[0]
w_kj = np.linalg.qr(w_kj)[0]
# Diagonal matrix
w_jj = np.diag(np.diag(w_jk))
w_kk = np.diag(np.diag(w_kj))
# remove the values from the diagonal
np.fill_diagonal(w_jk, 0)
np.fill_diagonal(w_kj, 0)
# Components A + B
acos_w_jj = np.arccos(w_jj)
asin_w_jk = np.arcsin(w_jk)
a = acos_w_jj - asin_w_jk
b = acos_w_jj + asin_w_jk
A = - np.sin(np.sinc(a))
B = np.sin(np.sinc(b))
# Components C + D
acos_w_kk = np.arccos(w_kk)
asin_w_kj = np.arcsin(w_kj)
c = acos_w_kk - asin_w_kj
d = acos_w_kk + asin_w_kj
C = np.sin(np.sinc(c))
D = np.sin(np.sinc(d))
# Components E
w_lj = np.sqrt(1 - (w_jj ** 2) - (w_kj ** 2))
w_lk = -(w_jk * w_jj + w_kk * w_kj) / w_lj
asin_w_lj = np.arcsin(w_lj)
asin_w_lk = np.arcsin(w_lk)
asin_w_lj2 = asin_w_lj ** 2
asin_w_lk2 = asin_w_lk ** 2
t1 = w_lj * w_lk * asin_w_lj
x1 = np.sqrt((1 - w_lj ** 2) * (1 - w_lk ** 2)) - 1
t2 = x1 * asin_w_lk
t = t1 + t2
E_nonzero = 2 * asin_w_lj * t / (asin_w_lj2 - asin_w_lk2)
# Check whether w_lj is different of zero
E1 = np.where(np.abs(w_lj) > 1e-8, E_nonzero, np.zeros(A.shape))
E = np.where(np.isclose(asin_w_lj2, asin_w_lk2), w_lj ** 2, E1)
cte = 1 / (2 * dt)
return cte * (np.arccos(w_jj) * (A + B) + np.arcsin(w_kj) * (C + D) + E)
def convert_galactic_equa(l, b): '''l and b in degrees''' r = pi / 180.0 dec = arcsin(cos(b * r) * sin((l - 33) * r) * sin(62.6 * r) + sin(b * r) * cos(62.6 * r)) ra = 282.25 + arcsin((cos(b * r) * sin((l - 33) * r) * cos(62.6 * r) - sin(b * r) * sin(62.6 * r)) / cos(dec)) / r dec = dec / r return ra, dec
def raDecFromVec(v):
"""
Taken from
http://www.math.montana.edu/frankw/ccp/multiworld/multipleIVP/spherical/learn.htm
Search for "convert from Cartestion to spherical coordinates"
Adapted because I'm dealing with declination which is defined
with 90degrees at zenith
"""
# Ensure v is a normal vector
v /= np.linalg.norm(v)
ra_deg = 0 # otherwise not in namespace0
dec_rad = np.arcsin(v[2])
s = np.hypot(v[0], v[1])
if s == 0:
ra_rad = 0
else:
ra_rad = np.arcsin(v[1] / s)
ra_deg = np.degrees(ra_rad)
if v[0] >= 0:
if v[1] >= 0:
pass
else:
ra_deg = 360 + ra_deg
else:
if v[1] > 0:
ra_deg = 180 - ra_deg
else:
ra_deg = 180 - ra_deg
raDec = ra_deg, np.degrees(dec_rad)
return np.array(raDec)
def __remove_particles_randomly(self):
grid_pairs = np.array([self.grid[0].ravel(), self.grid[1].ravel()]).T
particles = grid_pairs[
grid_pairs[:, 1]
> (
self.sin_amp * np.sin(self.sin_freq * grid_pairs[:, 0] + np.arcsin(1.0))
+ self.sin_amp
+ self.particle_diameter
)
]
self.driver = grid_pairs[
grid_pairs[:, 1]
<= (
self.sin_amp * np.sin(self.sin_freq * grid_pairs[:, 0] + np.arcsin(1.0))
+ self.sin_amp
+ self.particle_diameter
)
]
self.x = particles[:, 0]
number_of_particles_to_remove = self.__compute_number_of_particles_to_remove()
np.random.shuffle(particles)
self.x = particles[:-number_of_particles_to_remove, 0]
self.y = particles[:-number_of_particles_to_remove, 1]
def test_arcsin_no_warning_on_unscaled_quantity(self):
a = 15 * u.kpc
b = 27 * u.pc
with warnings.catch_warnings():
warnings.filterwarnings('error')
np.arcsin(b/a)
def solve_acos_plus_bsin_plus_c(a, b, c):
"""
Solves a*cos(x) + b*sin(x) + c = 0 for x in [-pi/2, pi/2].
The method used is to change variable by setting t = sin(x).
Args:
a, b, c: coefficients of the equation
Returns:
A solution x if it exists, None in the other cases.
"""
out = None
if a * a + b * b - c * c > 0:
poly = np.poly1d([a * a + b * b, 2 * b * c, c * c - a * a])
if not any([-1 <= t <= 1 for t in poly.r]):
print("no solutions between -1 and 1: ", poly.r)
elif all([-1 <= t <= 1 for t in poly.r]):
i = np.argmin(np.abs(np.array([a * np.sqrt(1 - t * t) + b * t + c for t in poly.r])))
out = np.arcsin(poly.r[i])
else:
if -1 <= poly.r[0] <= 1:
out = np.arcsin(poly.r[0])
else:
np.arcsin(poly.r[1])
# check that the solution is in [-pi/2, pi/2] and satisfies the equation
assert -np.pi / 2 < out < np.pi / 2
np.testing.assert_allclose(a * np.cos(out) + b * np.sin(out) + c, 0, atol=1e-12)
return out
def calc_total_reflection(self,alpha,n1,n2):
beta=90.-(np.arcsin(n1/n2*np.sin(alpha)))*180/np.pi
thetaC=np.arcsin(n1/n2)*180/np.pi
if beta>thetaC:
return True
else:
return False
def test_cartesianToSpherical(self):
"""
Check correct working of the Cartesian to Spherical coordinates transformation.
"""
r, phi, theta = cartesianToSpherical(array([1,0,0]), array([0,1,0]), array([0,0,1]))
assert_array_almost_equal(r,array([1.0,1.0,1.0]))
assert_array_almost_equal(phi,array([0.0,pi/2.0,0.0]))
assert_array_almost_equal(theta,array([0.0,0.0,pi/2.0]))
r, phi, theta = cartesianToSpherical(1.0, 1.0, 0.0)
assert_almost_equal(r, sqrt(2.0))
assert_almost_equal(phi, pi/4.0)
assert_almost_equal(theta, 0.0)
r, phi, theta = cartesianToSpherical(1.0, 1.0, 1.0)
assert_almost_equal(r, sqrt(3.0))
assert_almost_equal(phi, pi/4.0)
assert_almost_equal(theta, arcsin(1.0/sqrt(3.0)))
r, phi, theta = cartesianToSpherical(1.0, 1.0, -1.0)
assert_almost_equal(r, sqrt(3.0))
assert_almost_equal(phi, pi/4.0)
assert_almost_equal(theta, -arcsin(1.0/sqrt(3.0)))
r, phi, theta = cartesianToSpherical(-1.0, -1.0, -1.0)
assert_almost_equal(r, sqrt(3.0))
assert_almost_equal(phi, pi/4.0+pi)
assert_almost_equal(theta, -arcsin(1.0/sqrt(3.0)))
assert_raises(Exception, cartesianToSpherical, 0.0, 0.0, 0.0)
assert_raises(Exception, cartesianToSpherical, array([1,0,0,0]), array([0,1,0,0]), array([0,0,0,1]))
def gfun_apit(Vcorr,theta,R,do,t):
nrv = len(R[0])
Apit = R
g = R
Vcorr = Vcorr+theta
Pt = Vcorr*R*t
for i in range(nrv):
pt = Pt[0][i]
Do = do[0][i]
if pt < Do:
a = 2*pt*(1-(pt*Do**(-1))**2)**(0.5)
Theta1 = 2*np.arcsin(a*Do**(-1))
Theta2 = 2*np.arcsin(a*(2*pt)**(-1))
A1 = 0.5*(Theta1*(Do*0.5)**2-a*np.absolute(Do*0.5-pt**2*Do**(-1)))
A2 = 0.5*(Theta2*pt**2-a*pt**2*Do**(-1))
if pt >= Do:
Apit[0][i] = np.pi*4**(-1)*Do**2
elif pt <= Do*2**(-0.5):
Apit[0][i] = A1+A2
else:
Apit[0][i] = np.pi*4**(-1)*Do**2 - A1 + A2
return Apit
def evalExactφ(sqp,sqpnext,sqpdesired):
a=sqp/sqpdesired
b=sqpnext/sqpdesired
if(abs(a+1) > float("1e-12") and (a*a + b*b - 1)>0):
φ1=2*np.arctan( ( b - (a*a + b*b - 1)**0.5 )/(a+1) )
φ2=2*np.arctan( ( b + (a*a + b*b - 1)**0.5 )/(a+1) )
if(np.isnan(φ1)):
print("Ok nan found, locating butter chicken")
print( (a*a + b*b -1) )
print( (b + (a*a + b*b -1)**0.5 ) )
φ1=np.arcsin(np.sin(φ1))
φ2=np.arcsin(np.sin(φ2))
if(abs(φ1)<abs(φ2)):
print(φ1,φ2)
return φ1
else:
print(φ2,φ1)
return φ2
else:
print("Glaba, something went globular :P")
return 0
def arcsin(x):
try:
val = np.arcsin(x.v)
unc = (1/np.sqrt(1 - x.v*x.v))*x.u
return Measurement(val, np.abs(unc))
except AttributeError:
return np.arcsin(x)
def calc_cercle_visi():
"""Calcul du cercle de visibilité"""
from params import ELEVMIN, h, Re # , a, mu
# Rayon du cercle de visibilité (theta)
phi = np.arcsin(np.cos(ELEVMIN) / (1 + h / Re))
theta = np.pi / 2 - phi - ELEVMIN
## XXX : pas utilisés ici, déjà dans params.py
#T = 2 * np.pi * np.sqrt((a**3) / mu) # Période orbitale (s)
#w = np.sqrt(mu / (a**3)) # vitesse angulaire du satellite (rad.s^(-1))
# Calcul des points extremes du cercle de visibilite
pasR = 0.01
DLAT_VIS = np.arange(-theta, theta, pasR) # np.array([..])
DLON = []
#DLON = np.zeros(len(DLAT_VIS))
for lat_vis in DLAT_VIS:
#for i, lat_vis in enumerate(DLAT_VIS):
dlat = abs(lat_vis)
st2 = np.sin(theta)**2
sdlat = np.sin(dlat)**2 # pas utilisé
DLON.append(np.arcsin(np.sqrt(abs((st2 - sdlat) / (1 - sdlat)))))
#DLON[i] = np.arcsin(np.sqrt(abs((st2 - np.sin(dlat)**2) /
# np.cos(dlat)**2)))
DLON = np.array(DLON)
return DLAT_VIS, DLON
def _transform_parametersraw(self, pars, in_format, out_format):
"""Convert parameter values from in_format to out_format.
Also restores parameters to a preferred range if it permits multiple
values that correspond to the same physical result.
Parameters
pars: Sequence of parameters
in_format: A format defined for this class
out_format: A format defined for this class
Defined Formats
internal: [position, parameterized width-squared, area]
pwa: [position, full width at half maximum, area]
mu_sigma_area: [mu, sigma, area]
"""
temp = np.array(pars)
# Do I need to change anything? The internal parameters may need to be
# placed into the preferred range, even though their interpretation does
# not change.
if in_format == out_format and in_format != "internal":
return pars
# Convert to intermediate format "internal"
if in_format == "internal":
# put the parameter for width in the "physical" quadrant [-pi/2,pi/2],
# where .5*(sin(p)+1) covers fwhm = [0, maxwidth]
n = np.floor((temp[1]+np.pi/2)/np.pi)
if np.mod(n, 2) == 0:
temp[1] = temp[1] - np.pi*n
else:
temp[1] = np.pi*n - temp[1]
temp[2] = np.abs(temp[2]) # map negative area to equivalent positive one
elif in_format == "pwa":
if temp[1] > self.maxwidth:
emsg = "Width %s (FWHM) greater than maximum allowed width %s" %(temp[1], self.maxwidth)
raise SrMiseTransformationError(emsg)
temp[1] = np.arcsin(2.*temp[1]**2/self.maxwidth**2-1.)
elif in_format == "mu_sigma_area":
fwhm = temp[1]*self.sigma2fwhm
if fwhm > self.maxwidth:
emsg = "Width %s (FWHM) greater than maximum allowed width %s" %(fwhm, self.maxwidth)
raise SrMiseTransformationError(emsg)
temp[1] = np.arcsin(2.*fwhm**2/self.maxwidth**2-1.)
else:
raise ValueError("Argument 'in_format' must be one of %s." \
% self.parformats)
# Convert to specified output format from "internal" format.
if out_format == "internal":
pass
elif out_format == "pwa":
temp[1] = np.sqrt(.5*(np.sin(temp[1])+1.)*self.maxwidth**2)
elif out_format == "mu_sigma_area":
temp[1] = np.sqrt(.5*(np.sin(temp[1])+1.)*self.maxwidth**2)/self.sigma2fwhm
else:
raise ValueError("Argument 'out_format' must be one of %s." \
% self.parformats)
return temp
def newton_theta(theta, d_f, d_s, f, n_s, n_a=1, d_w=0, n_w=1):
""" Newton's Method to find theta_1 for a given theta.
Theta_1 as a function of theta is desired, but cannot be solved
analytically. Newton's Method yields a numerical approximation.
Parameters
----------
theta : float
Incident angle (in rads) of a ray in the absence of refraction;
effectively specifies a radial position on the objective lens.
d_f : float
Depth (in m) of hypothetical focal point below sample surface
in the absence of refraction (i.e. focal length minus the distance
between the objective lens and the sample surface).
d_s : float
Intended focal depth (in m) below sample surface (after correction).
f : float
Focal length of objective lens (in m).
n_s : float
Refractive index of the sample.
n_a : float
Refractive index of the ambient.
d_w : float
Thickness of the window (in m), used when irradiating through a window
(e.g. sample inside a heated stage with viewport). Use 0 for no window.
n_w : float
Refractive index of the window.
Returns
----------
ret : float
Numerical approximation of the value of theta_1 for a given theta.
"""
def g_th1(theta_1, d_s, d_w): # separating out a large term from F
return (d_s*n_a*np.sin(theta_1)/np.sqrt(
n_s**2 - n_a**2*np.sin(theta_1)**2) +
d_w*n_a*np.sin(theta_1)/np.sqrt(
n_w**2 - n_a**2*np.sin(theta_1)**2))
iterations = 0
theta_1 = theta # initial 'guess'
F = theta - (theta_1 + np.arcsin((np.cos(theta_1)/f) *
(g_th1(theta_1, d_s, d_w) - (d_f+d_w)*np.tan(theta_1))))
while abs(F) > 1e-10: # precision limit
F = theta - (theta_1 + np.arcsin((np.cos(theta_1)/f) *
(g_th1(theta_1, d_s, d_w) - (d_f+d_w)*np.tan(theta_1))))
theta_1 -= F/diff_theta(theta_1, d_f, d_s, f, n_s, n_a, d_w, n_w)
iterations += 1
return theta_1
def reconstruct_cluster_angle(self, events, index_group):
"""Reconstruct angles from a single event"""
c = 3.00e+8
t = [int(events[u]['ext_timestamp']) for u in index_group]
stations = [events[u]['station_id'] for u in index_group]
dt1 = t[0] - t[1]
dt2 = t[0] - t[2]
r1, phi1 = self.cluster.calc_r_and_phi_for_stations(stations[0], stations[1])
r2, phi2 = self.cluster.calc_r_and_phi_for_stations(stations[0], stations[2])
phi = arctan2((dt2 * r1 * cos(phi1) - dt1 * r2 * cos(phi2)),
(dt2 * r1 * sin(phi1) - dt1 * r2 * sin(phi2)) * -1)
theta1 = arcsin(c * dt1 * 1e-9 / (r1 * cos(phi - phi1)))
theta2 = arcsin(c * dt2 * 1e-9 / (r2 * cos(phi - phi2)))
e1 = sqrt(self.rel_theta1_errorsq(theta1, phi, phi1, phi2, r1, r2))
e2 = sqrt(self.rel_theta2_errorsq(theta2, phi, phi1, phi2, r1, r2))
theta_wgt = (1 / e1 * theta1 + 1 / e2 * theta2) / (1 / e1 + 1 / e2)
if theta_wgt < 0:
theta_wgt *= -1
phi += pi
phi = (phi + pi) % (2 * pi) - pi
return theta_wgt, phi
def tthToRad(twoTheta, unit, wavelength=None, directDist=None):
"""
Convert a two theta angle from original `unit` to radian.
`directDist = ai.getFit2D()["directDist"]`
"""
if isinstance(twoTheta, numpy.ndarray):
pass
elif isinstance(twoTheta, collections.Iterable):
twoTheta = numpy.array(twoTheta)
if unit == units.TTH_RAD:
return twoTheta
elif unit == units.TTH_DEG:
return numpy.deg2rad(twoTheta)
elif unit == units.Q_A:
if wavelength is None:
raise AttributeError("wavelength have to be specified")
return numpy.arcsin((twoTheta * wavelength) / (4.e-10 * numpy.pi)) * 2.0
elif unit == units.Q_NM:
if wavelength is None:
raise AttributeError("wavelength have to be specified")
return numpy.arcsin((twoTheta * wavelength) / (4.e-9 * numpy.pi)) * 2.0
elif unit == units.R_MM:
if directDist is None:
raise AttributeError("directDist have to be specified")
# GF: correct formula?
return numpy.arctan(twoTheta / directDist)
elif unit == units.R_M:
if directDist is None:
raise AttributeError("directDist have to be specified")
# GF: correct formula?
return numpy.arctan(twoTheta / (directDist * 0.001))
else:
raise ValueError("Converting from 2th to unit %s is not supported", unit)
def tpi2k(id, p, dI):
p2brho = 10./2.99792458
deg = pi/180.0
if id not in table.keys():
print( "type identifier is unknown")
print( "known types:")
print( list(table.keys())[:10])
print( list(table.keys())[10:20])
print( list(table.keys())[20:30])
print( list(table.keys())[35:])
return None
magnet_type = table[id]["type"]
a = table[id]["A"]
el = table[id]["EL"]
km = table[id]["KM"]
dk0 = p3(a, dI ) * km/(p*p2brho)
dk = 0.
if magnet_type == "S" or magnet_type == "Q":
#case kQ:
dk = dk0
elif magnet_type == "B":
phi = 2.0*np.arcsin(0.5*el * dk0)
dk = phi/deg
elif magnet_type == "C":
phi = 2.0*np.arcsin(0.5*el * dk0)
dk = phi*1.e3
elif magnet_type == "T":
phi = 2.0*np.arcsin(0.5*el * dk0)
dk = phi*1.e3
return dk
def test_rphi_to_dl_2d():
#This is a tangent point
r,phi= 6., numpy.arccos(0.75)
d,l= bovy_coords.rphi_to_dl_2d(r,phi,degree=False,ro=8.,phio=0.)
l= numpy.arcsin(0.75)
d= 6./numpy.tan(l)
assert numpy.fabs(d-6./numpy.tan(numpy.arcsin(0.75))) < 10.**-10., 'dl_to_rphi_2d conversion did not work as expected'
assert numpy.fabs(l-numpy.arcsin(0.75)) < 10.**-10., 'rphi_to_dl_2d conversion did not work as expected'
#This is another point
r,phi= 2., 55.
d,l= bovy_coords.rphi_to_dl_2d(r,phi,degree=True,ro=2.*numpy.sqrt(2.),
phio=10.)
assert numpy.fabs(d-2.) < 10.**-10., 'rphi_to_dl_2d conversion did not work as expected'
assert numpy.fabs(l-45.) < 10.**-10., 'rphi_to_dl_2d conversion did not work as expected'
#This is another point, for arrays
r,phi= 2., 45.
os= numpy.ones(2)
d,l= bovy_coords.rphi_to_dl_2d(os*r,os*phi,
degree=True,ro=2.*numpy.sqrt(2.),
phio=0.)
assert numpy.all(numpy.fabs(d-2.) < 10.**-10.), 'rphi_to_dl_2d conversion did not work as expected'
assert numpy.all(numpy.fabs(l-45.) < 10.**-10.), 'rphi_to_dl_2d conversion did not work as expected'
#This is another point, for lists, which for some reason I support
r,phi= 2., 45.
d,l= bovy_coords.rphi_to_dl_2d([r,r],[phi,phi],
degree=True,ro=2.*numpy.sqrt(2.),
phio=0.)
d= numpy.array(d)
l= numpy.array(l)
assert numpy.all(numpy.fabs(d-2.) < 10.**-10.), 'rphi_to_dl_2d conversion did not work as expected'
assert numpy.all(numpy.fabs(l-45.) < 10.**-10.), 'rphi_to_dl_2d conversion did not work as expected'
return None
def R2axis(M):
m00 = M[0, 0]
m01 = M[0, 1]
m02 = M[0, 2]
m10 = M[1, 0]
m11 = M[1, 1]
m12 = M[1, 2]
m20 = M[2, 0]
m21 = M[2, 1]
m22 = M[2, 2]
# symmetric matrix K
K = np.array(
[
[m00 - m11 - m22, 0.0, 0.0, 0.0],
[m01 + m10, m11 - m00 - m22, 0.0, 0.0],
[m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
[m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22],
]
)
K /= 3.0
# quaternion is eigenvector of K that corresponds to largest eigenvalue
w, V = np.linalg.eigh(K)
q = V[[3, 0, 1, 2], np.argmax(w)]
# quaternion to Axis
nrm = np.linalg.norm(q[1:4])
if nrm <= 0.0:
a = np.array([0.0, 0.0, 0.0])
elif q[0] < 0.0:
a = (np.pi - np.arcsin(nrm)) * 2 * q[1:4] / nrm
else:
a = np.arcsin(nrm) * 2 * q[1:4] / nrm
return a
def _cal_center(p0,p1,p2):
#p0 is of type up-down
origin=(p1+p2)/2
y_v=f3(np.zeros(3),p1-origin)
x_v=f3(np.zeros(3),p0-origin)
z_v=np.cross(x_v,y_v)
T=f1(x0_v,y0_v,z0_v,x_v,y_v,z_v)
#print T
r=f2(p1,p2)/2*np.tan(np.pi/2-self.open_angle/2.)
phi=0.
L1=f2(p1,p2)/2./np.tan(self.open_angle/2.)
L2=f2(p0,origin)
#look at document#1 in binder for detail
a,b,c=1+np.tan(self.theta_top_down)**2,2*L1/L2*np.tan(self.theta_top_down),(L1/L2)**2-1
sin_list=[(b+(b**2-4*a*c)**0.5)/2./a,(b-(b**2-4*a*c)**0.5)/2./a]
theta=0
if (sin_list[0]<1)&(sin_list[0]>0):
if self.switch==False:
theta=np.pi/2+np.arcsin(sin_list[0])
elif self.switch==True:
theta=np.pi/2-np.arcsin(sin_list[0])
else:
if self.switch==False:
theta=np.pi/2+np.arcsin(sin_list[1])
elif self.switch==True:
theta=np.pi/2-np.arcsin(sin_list[1])
center_point_new=np.array([r*np.cos(phi)*np.sin(theta),r*np.sin(phi)*np.sin(theta),r*np.cos(theta)])
center_point_org=np.dot(inv(T),center_point_new)+origin
return center_point_org
def reconstruct_angle(self, event, offsets=None):
"""Reconstruct angles from a single event"""
c = 3.00e+8
if offsets is not None:
self._correct_offsets(event, offsets)
dt1 = event['t1'] - event['t3']
dt2 = event['t1'] - event['t4']
station = self.cluster.stations[event['station_id']]
r1, phi1 = station.calc_r_and_phi_for_detectors(1, 3)
r2, phi2 = station.calc_r_and_phi_for_detectors(1, 4)
phi = arctan2((dt2 * r1 * cos(phi1) - dt1 * r2 * cos(phi2)),
(dt2 * r1 * sin(phi1) - dt1 * r2 * sin(phi2)) * -1)
theta1 = arcsin(c * dt1 * 1e-9 / (r1 * cos(phi - phi1)))
theta2 = arcsin(c * dt2 * 1e-9 / (r2 * cos(phi - phi2)))
e1 = sqrt(self.rel_theta1_errorsq(theta1, phi, phi1, phi2, r1, r2))
e2 = sqrt(self.rel_theta2_errorsq(theta2, phi, phi1, phi2, r1, r2))
theta_wgt = (1 / e1 * theta1 + 1 / e2 * theta2) / (1 / e1 + 1 / e2)
if theta_wgt < 0:
theta_wgt *= -1
phi += pi
phi = (phi + pi) % (2 * pi) - pi
return theta_wgt, phi
def funDR1R2(x, v1, v2, v3, z1, z2): # def funDR1R2(x, v1, v2, v3, z1, z2): """ Computes arrival time of direct, and two critically refracted waves from three layer models x: offset (array) v1: velocity of 1st layer (float) v2: velocity of 2nd layer (float) v3: velocity of 3rd layer (float) z1: thickness of 1st layer (float) z2: thickness of 2nd layer (float) """ direct = 1./v1*x theta1 = np.arcsin(v1/v2) theta2 = np.arcsin(v2/v3) ti1 = 2*z1*np.cos(theta1)/v1 ti2 = 2*z2*np.cos(theta2)/v2 xc1 = 2*z1*np.tan(theta1) xc2 = 2*z2*np.tan(theta2) act1 = x > xc1 act2 = x > xc2 ref1 = np.ones_like(x)*np.nan ref1[act1] = 1./v2*x[act1] + ti1 ref2 = np.ones_like(x)*np.nan ref2[act2] = 1./v3*x[act2] + ti2 + ti1 t0 = 2.*z1/v1 refl1 = np.sqrt(t0**2+x**2/v1**2) return np.c_[direct, ref1, ref2, refl1]
def sphericalLawOfSines(angle1, side1, angle2, side2=None):
'''Law of sines. 4 possible inputs, angle1 and side1 must be dms arrays and
either angle2 or side2 must also be dms arrays, with the other as None
This function will determing the missing side/angle from the other three
parameters
'''
#if side2 must be found
if side2 == None:
#do math, notice the conversion to/from radians
angle1 = dms2deg(angle1)*np.pi/180
angle2 = dms2deg(angle2)*np.pi/180
side1 = dms2deg( side2)*np.pi/180
side2 = np.arcsin(np.sin(angle2)*np.sin(side1)/np.sin(angle1))*180/np.pi
return deg2dms(side2)
#if angle2 must be found
if angle2 == None:
#do math, notice the conversion to/from radians
angle1 = dms2deg(angle1)*np.pi/180
side1 = dms2deg( side1)*np.pi/180
side2 = dms2deg( side2)*np.pi/180
angle2 = np.arcsin(np.sin(side2)*np.sin(angle1)/np.sin(side1))*180/np.pi
return deg2dms(angle2)
#tell the user that they f****d up
else:
print('Error, law of sines overconstrained, returning 0')
return np.array([0,0,0])
def step(self):
# Unpack the state and actions.
# -----------------------------
# Want to ignore the previous value of omegadd; it could only cause a
# bug if we assign to it.
(theta, thetad, omega, omegad, _, xf, yf, xb, yb, psi) = self.sensors
(T, d) = self.actions
# For recordkeeping.
# ------------------
if self._save_wheel_contact_trajectories:
self.xfhist.append(xf)
self.yfhist.append(yf)
self.xbhist.append(xb)
self.ybhist.append(yb)
# Intermediate time-dependent quantities.
# ---------------------------------------
# Avoid divide-by-zero, just as Randlov did.
if theta == 0:
rf = 1e8
rb = 1e8
rCM = 1e8
else:
rf = self.L / np.abs(sin(theta))
rb = self.L / np.abs(tan(theta))
rCM = sqrt((self.L - self.c)**2 + self.L**2 / tan(theta)**2)
phi = omega + np.arctan(d / self.h)
# Equations of motion.
# --------------------
# Second derivative of angular acceleration:
omegadd = 1 / self.Itot * (
self.M * self.h * self.g * sin(phi) - cos(phi) *
(self.Idc * self.sigmad * thetad + sign(theta) * self.v**2 *
(self.Md * self.r *
(1.0 / rf + 1.0 / rb) + self.M * self.h / rCM)))
thetadd = (T - self.Idv * self.sigmad * omegad) / self.Idl
# Integrate equations of motion using Euler's method.
# ---------------------------------------------------
# yt+1 = yt + yd * dt.
# Must update omega based on PREVIOUS value of omegad.
omegad += omegadd * self.time_step
omega += omegad * self.time_step
thetad += thetadd * self.time_step
theta += thetad * self.time_step
# Handlebars can't be turned more than 80 degrees.
theta = np.clip(theta, -1.3963, 1.3963)
# Wheel ('tyre') contact positions.
# ---------------------------------
# Front wheel contact position.
front_temp = self.v * self.time_step / (2 * rf)
# See Randlov's code.
if front_temp > 1:
front_temp = sign(psi + theta) * 0.5 * np.pi
else:
front_temp = sign(psi + theta) * arcsin(front_temp)
xf += self.v * self.time_step * -sin(psi + theta + front_temp)
yf += self.v * self.time_step * cos(psi + theta + front_temp)
# Rear wheel.
back_temp = self.v * self.time_step / (2 * rb)
# See Randlov's code.
if back_temp > 1:
back_temp = np.sign(psi) * 0.5 * np.pi
else:
back_temp = np.sign(psi) * np.arcsin(back_temp)
xb += self.v * self.time_step * -sin(psi + back_temp)
yb += self.v * self.time_step * cos(psi + back_temp)
# Preventing numerical drift.
# ---------------------------
# Copying what Randlov did.
current_wheelbase = sqrt((xf - xb)**2 + (yf - yb)**2)
if np.abs(current_wheelbase - self.L) > 0.01:
relative_error = self.L / current_wheelbase - 1.0
xb += (xb - xf) * relative_error
yb += (yb - yf) * relative_error
# Update heading, psi.
# --------------------
delta_y = yf - yb
if (xf == xb) and delta_y < 0.0:
psi = np.pi
else:
if delta_y > 0.0:
psi = arctan((xb - xf) / delta_y)
else:
psi = sign(xb - xf) * 0.5 * np.pi - arctan(delta_y / (xb - xf))
self.sensors = np.array(
[theta, thetad, omega, omegad, omegadd, xf, yf, xb, yb, psi])
def readWCSCoeffs(header):
"""
Read distortion coeffs from WCS header keywords and
populate distortion coeffs arrays.
"""
# Read in order for polynomials
_xorder = header['a_order']
_yorder = header['b_order']
order = max(max(_xorder, _yorder), 3)
fx = np.zeros(shape=(order + 1, order + 1), dtype=np.float64)
fy = np.zeros(shape=(order + 1, order + 1), dtype=np.float64)
# Read in CD matrix
_cd11 = header['cd1_1']
_cd12 = header['cd1_2']
_cd21 = header['cd2_1']
_cd22 = header['cd2_2']
_cdmat = np.array([[_cd11, _cd12], [_cd21, _cd22]])
_theta = np.arctan2(-_cd12, _cd22)
_rotmat = np.array([[np.cos(_theta), np.sin(_theta)],
[-np.sin(_theta), np.cos(_theta)]])
_rCD = np.dot(_rotmat, _cdmat)
_skew = np.arcsin(-_rCD[1][0] / _rCD[0][0])
_scale = _rCD[0][0] * np.cos(_skew) * 3600.
_scale2 = _rCD[1][1] * 3600.
# Set up refpix
refpix = {}
refpix['XREF'] = header['crpix1']
refpix['YREF'] = header['crpix2']
refpix['XSIZE'] = header['naxis1']
refpix['YSIZE'] = header['naxis2']
refpix['PSCALE'] = _scale
refpix['V2REF'] = 0.
refpix['V3REF'] = 0.
refpix['THETA'] = np.rad2deg(_theta)
refpix['XDELTA'] = 0.0
refpix['YDELTA'] = 0.0
refpix['DEFAULT_SCALE'] = True
refpix['centered'] = True
# Set up template for coeffs keyword names
cxstr = 'A_'
cystr = 'B_'
# Read coeffs into their own matrix
for i in range(_xorder + 1):
for j in range(i + 1):
xcname = cxstr + str(j) + '_' + str(i - j)
if xcname in header:
fx[i, j] = header[xcname]
# Extract Y coeffs separately as a different order may
# have been used to fit it.
for i in range(_yorder + 1):
for j in range(i + 1):
ycname = cystr + str(j) + '_' + str(i - j)
if ycname in header:
fy[i, j] = header[ycname]
# Now set the linear terms
fx[0][0] = 1.0
fy[0][0] = 1.0
return fx, fy, refpix, order
def beta_calc(a, alpha, b):
return np.arcsin(a * np.sin(alpha) / b)
def _step(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
# max_ang_speed = 0.3
# ang_vel = (action-10)*max_ang_speed*0.1 #from (-0.33 to + 0.33)
vel_cmd = Twist()
# vel_cmd.linear.x = 0.2
# vel_cmd.angular.z = ang_vel
# self.vel_pub.publish(vel_cmd)
if action == 1:
vel_cmd.linear.x = 0.5
elif action == 2:
vel_cmd.angular.z = 0.2
#vel_cmd.linear.x = 0.1
elif action == 3:
vel_cmd.angular.z = -0.2
#vel_cmd.linear.x = 0.1
else:
vel_cmd.linear.x = -0.1
self.vel_pub.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
robot_tf = None
# while robot_tf is None:
# try:
# robot_tf = rospy.wait_for_message("/tf", TFMessage, timeout=5)
# except:
# pass
# rospy.wait_for_service('/gazebo/pause_physics')
# try:
# #resp_pause = pause.call()
# self.pause()
# except (rospy.ServiceException) as e:
# print ("/gazebo/pause_physics service call failed")
state, done = self.calculate_observation(data)
# added code
# targetPosition = self.getmodelstate
# mobilePosition = self.getmodelstate####### ModelState?
targetPositionRe = self.getmodelstate
rospy.wait_for_service("/gazebo/get_model_state")
try:
# targetPosition = self.getmodelstate('Target', 'world')
# mobilePosition = self.getmodelstate('mobile_base', 'world')
targetPositionRe = self.getmodelstate("Target", "chassis")
except (rospy.ServiceException) as e:
print('get the target position failed !')
# x= targetPosition.pose.position.x-mobilePosition.pose.position.x
# y= targetPosition.pose.position.y-mobilePosition.pose.position.y
x = targetPositionRe.pose.position.x
y = targetPositionRe.pose.position.y
# distance,angle= cmath.polar(complex(x,y))
# state = list(state)
# state = state.extend([distance,angle])
distance, theta = cmath.polar(complex(x, y)) #theta (-pi, pi)
theta = np.arcsin(y / distance)
if x < 0 and y < 0:
# angle = -np.pi - np.arcsin(y / distance)
angle = -np.pi - theta
elif x < 0 and y > 0:
angle = np.pi - theta
# angle = np.pi - np.arcsin(y / distance)
else:
# angle = np.arcsin(y / distance)
angle = theta
# quaternion=(robot_tf.transforms[0].transform.rotation.x,
# robot_tf.transforms[0].transform.rotation.y,
# robot_tf.transforms[0].transform.rotation.z,
# robot_tf.transforms[0].transform.rotation.w)
# euler=np.array(tf.transformations.euler_from_quaternion(quaternion))
self.angle = np.round(abs(angle) / np.pi, 2)
self.distance = np.round(distance / 10, 2)
anglereward = -self.angle
# if (abs(targetPosition.pose.position.x - mobilePosition.pose.position.x) and
# abs(targetPosition.pose.position.y - mobilePosition.pose.position.y)) <= 0.3:
if (distance <= 0.3):
target = True
# print("targetPosition.pose.position.y"+str(targetPosition.pose.position.y))
# print("mobilePosition.pose.position.y"+str(mobilePosition.pose.position.y))
else:
target = False
if not done:
# Straight reward = 5, Max angle reward = 0.5
#reward = round(15*(max_ang_speed - abs(ang_vel) +0.0335), 2)
# print ("Action : "+str(action)+" Ang_vel : "+str(ang_vel)+" reward="+str(reward))
if target:
reward = 200
done = True
else:
# distance2 = math.hypot(mobilePosition.pose.position.x - targetPosition.pose.position.x,
# mobilePosition.pose.position.y - targetPosition.pose.position.y)
#distance1 = GazeboMylabTurtlebotLidarNnEnv.store[0]
reward = -self.distance
#GazeboMylabTurtlebotLidarNnEnv.store[0] = distance
else:
reward = -200
#print("reward is :"+str(reward))
#print("distance2:"+str(distance2)+" distance1: "+str(distance1))
# if abs(angle-euler[2])>np.pi:
# anglereward=-abs(angle-euler[2]-2*np.pi)/(np.pi)
# else:
# anglereward=-abs(angle-euler[2])/(np.pi)
# print("anglereward is: "+str(math.degrees(-abs(angle)))+"reward is :"+str(reward))
reward = reward + anglereward
# self.angle=np.round(angle-euler[2],2)/np.pi
state = np.hstack([state, self.distance, self.angle])
return state, reward, done, {}
def single_channel_analysis(self, beta0, beta1):
beta0=beta0*np.pi/180
beta1=beta1*np.pi/180
self.alpha0=180/np.pi*(np.arcsin(self.D0**0.5*np.sin(beta0))-beta0)
self.alpha1=180/np.pi*(-np.arcsin(self.D1**0.5*np.sin(beta1))+beta1)
filename_plots=projfolder+'Plots/AGNCore/Stacking/'
## Input my commandline parameters ##
parser = OptionParser ( usage = '%prog [options]')
parser.add_option ('--dec', dest = 'dec', type = float,
default = 0., metavar = 'DEC',
help = 'sin of the source declination.')
parser.add_option ('--years', dest = 'years', type = int,
default = 2, metavar = 'YEARS',
help = 'Number of years of data')
opts,args = parser.parse_args ()
dec_deg = np.arcsin(opts.dec) * 180./np.pi
years = opts.years
##Not sure if this background folder needs to be redone... but it worked for single source so I don't think so.
datafolder='/data/user/brelethford/Output/all_sky_sensitivity/results/single_stacked/multi_year/{0}/dec{1:+010.5}/'.format(str(years),dec_deg)
files = [cache.load(datafolder+file) for file in os.listdir(datafolder) if file.endswith('.array')]
n_inj=[]
nsources=[]
TS=[]
beta=(0.5) #For background ts
TS_beta=[] #Calculated from the total TS median after we get all the TS.
beta_err=[]
gamma=[]
for file in files:
for item in range(len(file['n_inj'])):
def do_range_projection(self):
""" Project a pointcloud into a spherical projection image.projection.
Function takes no arguments because it can be also called externally
if the value of the constructor was not set (in case you change your
mind about wanting the projection)
"""
# JLLIU x, y, z, i, h, w
# return_to_bin_table = np.zeros((64, 2048, 7), dtype=np.float32)
# print(return_to_bin_table.shape)
# laser parameters
fov_up = self.proj_fov_up / 180.0 * np.pi # field of view up in rad
fov_down = self.proj_fov_down / 180.0 * np.pi # field of view down in rad
fov = abs(fov_down) + abs(fov_up) # get field of view total in rad
# get depth of all points
depth = np.linalg.norm(self.points, 2, axis=1)
# get scan components
scan_x = self.points[:, 0]
scan_y = self.points[:, 1]
scan_z = self.points[:, 2]
# print(scan_x[0])
# get angles of all points
yaw = -np.arctan2(scan_y, scan_x)
pitch = np.arcsin(scan_z / depth)
# get projections in image coords
proj_x = 0.5 * (yaw / np.pi + 1.0) # in [0.0, 1.0]
proj_y = 1.0 - (pitch + abs(fov_down)) / fov # in [0.0, 1.0]
# scale to image size using angular resolution
proj_x *= self.proj_W # in [0.0, W]
proj_y *= self.proj_H # in [0.0, H]
# round and clamp for use as index
proj_x = np.floor(proj_x)
proj_x = np.minimum(self.proj_W - 1, proj_x)
proj_x = np.maximum(0, proj_x).astype(np.int32) # in [0,W-1]
self.proj_x = np.copy(proj_x) # store a copy in orig order
proj_y = np.floor(proj_y)
proj_y = np.minimum(self.proj_H - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
self.proj_y = np.copy(proj_y) # stope a copy in original order
# copy of depth in original order
self.unproj_range = np.copy(depth)
# order in decreasing depth
indices = np.arange(depth.shape[0])
order = np.argsort(depth)[::-1]
depth = depth[order]
indices = indices[order]
points = self.points[order]
remission = self.remissions[order]
proj_y = proj_y[order]
proj_x = proj_x[order]
#print("\npoints.shape: ",self.points.shape)
#print("yaw.shape: ",yaw.shape)
#print("pitch.shape: ",pitch.shape)
#print("proj_x.shape: ",proj_x.shape)
#print("proj_y.shape: ",proj_y.shape)
#for doggy in range(0,100):
#print("proj_x[%d]: "%doggy, proj_x[doggy])
#print("proj_y[%d]: "%doggy, proj_y[doggy])
# assing to images
self.proj_range[proj_y, proj_x] = depth
self.proj_xyz[proj_y, proj_x] = points
self.proj_remission[proj_y, proj_x] = remission
self.proj_idx[proj_y, proj_x] = indices
self.proj_mask = (self.proj_idx > 0).astype(np.int32)
def align_beamline(beamLine,
hDiv=1.5e-3,
vDiv=2.5e-4,
nameVCMstripe='Rh',
pitch=None,
nameDCMcrystal='Si111',
bragg=None,
energy=9000.,
fixedExit=20.,
nameDiagnFoil='top-edge',
nameVFMcylinder='Rh',
heightVFM=None):
beamLine.feMovableMaskLT.set_divergence(beamLine.sources[0],
[-hDiv / 2, vDiv / 2])
beamLine.feMovableMaskRB.set_divergence(beamLine.sources[0],
[hDiv / 2, -vDiv / 2])
print('for {0:.2f}h x {1:.2f}v mrad^2 divergence:'.format(
hDiv * 1e3, vDiv * 1e3))
print('full feMovableMaskLT.opening = [{0[0]:.3f}, {0[1]:.3f}]'.format(
beamLine.feMovableMaskLT.opening))
print('full feMovableMaskRB.opening = [{0[0]:.3f}, {0[1]:.3f}]'.format(
beamLine.feMovableMaskRB.opening))
print('half feMovableMaskLT.opening = [{0[0]:.3f}, {0[1]:.3f}]'.format(
[op / 2 for op in beamLine.feMovableMaskLT.opening]))
print('half feMovableMaskRB.opening = [{0[0]:.3f}, {0[1]:.3f}]'.format(
[op / 2 for op in beamLine.feMovableMaskRB.opening]))
beamLine.vfm.select_surface(nameVFMcylinder)
p = raycing.distance_xy(beamLine.vfm.center, beamLine.sources[0].center)
if pitch is None:
sinPitch = beamLine.vfm.r * 1.5 / p
pitch = float(np.arcsin(sinPitch))
else:
sinPitch = np.sin(pitch)
beamLine.vfm.R = p / sinPitch
beamLine.vcm.select_surface(nameVCMstripe)
beamLine.vcm.pitch = pitch
beamLine.vcm.set_jacks()
p = raycing.distance_xy(beamLine.vcm.center, beamLine.sources[0].center)
beamLine.vcm.R = 2. * p / sinPitch
beamLine.vcm.get_orientation()
print('VCM.p = {0:.1f}'.format(p))
print('VCM.pitch = {0:.6f} mrad'.format(beamLine.vcm.pitch * 1e3))
print('VCM.roll = {0:.6f} mrad'.format(beamLine.vcm.roll * 1e3))
print('VCM.yaw = {0:.6f} mrad'.format(beamLine.vcm.yaw * 1e3))
print('VCM.z = {0:.3f}'.format(beamLine.vcm.center[2]))
print('VCM.R = {0:.0f}'.format(beamLine.vcm.R))
print('VCM.dx = {0:.3f}'.format(beamLine.vcm.dx))
print('VCM.jack1.zCalib = {0:.3f}'.format(beamLine.vcm.jack1Calib))
print('VCM.jack2.zCalib = {0:.3f}'.format(beamLine.vcm.jack2Calib))
print('VCM.jack3.zCalib = {0:.3f}'.format(beamLine.vcm.jack3Calib))
print('VCM.tx1 = {0:.3f}'.format(beamLine.vcm.tx1[0]))
print('VCM.tx2 = {0:.3f}'.format(beamLine.vcm.tx2[0]))
p = raycing.distance_xy(beamLine.fsm2.center, beamLine.vcm.center)
fsm2height = beamLine.height + p * np.tan(2 * pitch)
beamLine.dcm.select_surface(nameDCMcrystal)
p = raycing.distance_xy(beamLine.dcm.center, beamLine.vcm.center)
beamLine.dcm.center[2] = beamLine.height + p * np.tan(2 * pitch)
beamLine.dcm.set_jacks()
aCrystal = beamLine.dcm.material[beamLine.dcm.surface.index(
nameDCMcrystal)]
dSpacing = aCrystal.d
print('DCM.crystal = {0}'.format(nameDCMcrystal))
print('DCM.dSpacing = {0:.6f} angsrom'.format(dSpacing))
if bragg is None:
theta = float(np.arcsin(rm.ch / (2 * dSpacing * energy)))
bragg = theta + 2 * pitch
else:
theta = bragg - 2 * pitch
energy = rm.ch / (2 * dSpacing * np.sin(theta))
print('DCM.energy = {0:.3f} eV'.format(energy))
print('DCM.bragg = {0:.6f} deg'.format(np.degrees(bragg)))
print('DCM.realThetaAngle = DCM.bragg - 2*VCM.pitch = {0:.6f} deg'.format(
np.degrees(theta)))
beamLine.dcm.energy = energy
beamLine.dcm.bragg = bragg
p = raycing.distance_xy(beamLine.vfm.center, beamLine.vcm.center)
if heightVFM is not None:
fixedExit = (heightVFM - beamLine.height -
p * np.tan(2 * pitch)) * np.cos(2 * pitch)
else:
heightVFM = fixedExit / np.cos(2 * pitch) + \
beamLine.height + p * np.tan(2 * pitch) + 0.5
beamLine.heightVFM = heightVFM
beamLine.dcm.fixedExit = fixedExit
beamLine.dcm.cryst2perpTransl = \
beamLine.dcm.fixedExit/2./np.cos(beamLine.dcm.bragg)
beamLine.dcm.get_orientation()
print('DCM.pitch = {0:.6f} mrad'.format(beamLine.dcm.pitch * 1e3))
print('DCM.roll = {0:.6f} mrad'.format(beamLine.dcm.roll * 1e3))
print('DCM.yaw = {0:.6f} mrad'.format(beamLine.dcm.yaw * 1e3))
print('DCM.z = {0:.3f}'.format(beamLine.dcm.center[2]))
print('DCM.fixedExit = {0:.3f}'.format(fixedExit))
print('DCM.cryst2perpTransl = {0:.3f}'.format(
beamLine.dcm.cryst2perpTransl))
print('DCM.dx = {0:.3f}'.format(beamLine.dcm.dx))
print('DCM.jack1.zCalib = {0:.3f}'.format(beamLine.dcm.jack1Calib))
print('DCM.jack2.zCalib = {0:.3f}'.format(beamLine.dcm.jack2Calib))
print('DCM.jack3.zCalib = {0:.3f}'.format(beamLine.dcm.jack3Calib))
p = raycing.distance_xy(beamLine.vfm.center, beamLine.fsm3.center)
fsm3height = heightVFM - p * np.tan(2 * pitch)
p = raycing.distance_xy(
beamLine.vfm.center,
(beamLine.xbpm4foils.center[0], beamLine.xbpm4foils.center[1]))
heightXBPM4 = heightVFM - p * np.tan(2 * pitch)
beamLine.xbpm4foils.select_aperture(nameDiagnFoil, heightXBPM4)
print('beamLine.xbpm4foils.zActuator = {0:.3f}'.format(
beamLine.xbpm4foils.zActuator))
beamLine.vfm.pitch = -pitch
beamLine.vfm.center[2] = heightVFM - beamLine.vfm.hCylinder
beamLine.vfm.set_jacks()
# beamLine.vfm.yaw = 50e-6
beamLine.vfm.set_x_stages()
beamLine.vfm.get_orientation()
print('VFM.pitch = {0:.6f} mrad'.format(beamLine.vfm.pitch * 1e3))
print('VFM.roll = {0:.6f} mrad'.format(beamLine.vfm.roll * 1e3))
print('VFM.yaw = {0:.6f} mrad'.format(beamLine.vfm.yaw * 1e3))
print('VFM.z = {0:.3f}'.format(beamLine.vfm.center[2]))
print('VFM.R = {0:.0f}'.format(beamLine.vfm.R))
print('VFM.r = {0:.3f}'.format(beamLine.vfm.r))
print('VFM.dx = {0:.3f}'.format(beamLine.vfm.dx))
print('VFM.jack1.zCalib = {0:.3f}'.format(beamLine.vfm.jack1Calib))
print('VFM.jack2.zCalib = {0:.3f}'.format(beamLine.vfm.jack2Calib))
print('VFM.jack3.zCalib = {0:.3f}'.format(beamLine.vfm.jack3Calib))
print('VFM.tx1 = {0:.3f}'.format(beamLine.vfm.tx1[0]))
print('VFM.tx2 = {0:.3f}'.format(beamLine.vfm.tx2[0]))
beamLine.eh100To40Flange.center[2] = heightVFM
print('eh100To40Flange.center[2] = {0:.3f}'.format(
beamLine.eh100To40Flange.center[2]))
beamLine.slitEH.opening[2] += heightVFM - beamLine.height
beamLine.slitEH.opening[3] += heightVFM - beamLine.height
beamLine.slitEH.set_optical_limits()
print('fsm1.z = {0:.3f}'.format(beamLine.height))
print('fsm2.z = {0:.3f}'.format(fsm2height))
print('fsm3.z = {0:.3f}'.format(fsm3height))
print('fsm4.z = {0:.3f}'.format(heightVFM))
from subprocess import check_output
import numpy as np
from astropy.table import QTable
from astropy.coordinates import SkyCoord, Angle
from astropy.time import Time
from astropy import units as u
np.random.seed(12345)
N = 100
tab = QTable()
target_lon = np.random.uniform(0, 360, N) * u.deg
target_lat = np.degrees(np.arcsin(np.random.uniform(-1, 1, N))) * u.deg
tab['target'] = SkyCoord(target_lon, target_lat, frame='fk5')
tab['obstime'] = Time(np.random.uniform(
Time('1997-01-01').mjd,
Time('2017-12-31').mjd, N),
format='mjd',
scale='utc')
tab['obslon'] = Angle(np.random.uniform(-180, 180, N) * u.deg)
tab['obslat'] = Angle(np.arcsin(np.random.uniform(-1, 1, N)) * u.deg)
tab['geocent'] = 0.
tab['heliocent'] = 0.
tab['lsrk'] = 0.
tab['lsrd'] = 0.
tab['galactoc'] = 0.
tab['localgrp'] = 0.
def chipDrw_1(c, chip_resonator_length, chip_coupler_length, wiggle_length, d=None, inductive_launcher=False):
### Chip Init
# gapw=calculate_gap_width(eps_eff,50,pinw)
print(d.__dict__.keys())
if d == None: d = MaskMaker.ChipDefaults()
gap_ratio = d.gapw / d.pinw
c.frequency = 10
c.pinw = 1.5 # d.pinw
c.gapw = 1.
c.center_gapw = 1
c.radius = 40
c.frequency = 5 # GHz
# c.Q = 1000000 #one of the couplers
c.imp_rsn = 80
launch_pin_width = 150
launcher_width = (1 + 2 * gap_ratio) * launch_pin_width
c.launcher_length = 500
c.inside_padding = 1160
c.left_inside_padding = 180
c.C = 0.5e-15 # (factor of 2 smaller than actual value from simulation.)
c.cap1 = mask_utils.sapphire_capacitor_by_C_Channels(c.C)
# c.cap1 = sapphire_capacitor_by_Q_Channels(c.frequency, c.Q, 50, resonator_type=0.5)
### Calculating the interior length of the resonator
# c.interior_length = calculate_interior_length(c.frequency, d.phase_velocity,
# c.imp_rsn, resonator_type=0.5,
# harmonic=0, Ckin=c.cap1, Ckout=c.cap1)
c.interior_length = 17631 / 2. # unit is micron, devide by 2 to get the half wavelength
# This gives 6.88GHz in reality.
c.meander_length = c.interior_length # (c.interior_length - (c.inside_padding)) - c.left_inside_padding
print('meander_length', c.meander_length)
c.num_wiggles = 7
c.meander_horizontal_length = (c.num_wiggles + 1) * 2 * c.radius
launcher_straight = 645
chipInit(c, defaults=d)
### Components
#### MaskMaker.Launcher
two_layer = c.two_layer
c.s5.pinw = 4.
c.s5.gapw = 2.5
c.s7.pinw = 4.
c.s7.gapw = 2.5
c.s3.pinw2 = d.pinw_rsn
c.s4.pinw2 = d.pinw_rsn
if two_layer:
c.s5.chip.two_layer = True
c.s7.chip.two_layer = True
# resonator_length = 3000 # for 10GHz
length_before_turning = 20 # 220 + 116
turn_radius = 100
turn_radius_2 = 10
coupler_width_1 = 5
length_2 = 750 / 2. - turn_radius - turn_radius_2 - coupler_width_1
### Left launchers and the drive resonator
s = c.sl1
MaskMaker.Launcher(s, pinw=2, gapw=2)
MaskMaker.CPWStraight(s, length_before_turning)
MaskMaker.CPWBend(s, -90, radius=turn_radius, segments=10)
MaskMaker.CPWStraight(s, length_2)
MaskMaker.CPWBend(s, 90, radius=turn_radius_2, segments=10)
# MaskMaker.CPWTaper(s, 10, 0.09, stop_pinw=1, stop_gapw=.5)
# Interaction driven programming: encourage trying code out and look at the output, instead of just reading code.
# such encouragements unlock new behaviors that positively re-enforce itself.
# this positive re-enforcement is what makes this kind of philosophical change significant.
s = c.sl2
MaskMaker.Launcher(s, pinw=2, gapw=2)
MaskMaker.CPWStraight(s, length_before_turning)
MaskMaker.CPWBend(s, 90, radius=turn_radius, segments=10)
MaskMaker.CPWStraight(s, length_2)
MaskMaker.CPWBend(s, -90, radius=turn_radius_2, segments=10)
# MaskMaker.CPWTaper(s, 10, 0.09, stop_pinw=1, stop_gapw=.5)
# c.sl1.last = MaskMaker.middle(c.sl1.last, c.sl2.last)
# CPWWiggles(c.sl1, 1, 12, 1, start_up=True, radius=(1.5 + 2) / 2., segments=10)
# drive resonator from the left
drive_resonator_start_pt = MaskMaker.middle(c.sl1.last, c.sl2.last)
c.s_drive = MaskMaker.Structure(c, start=drive_resonator_start_pt, direction=0)
MaskMaker.CoupledTaper(c.s_drive, 6, pinw=c.sl1.pinw, gapw=c.sl1.gapw, center_gapw=8, stop_pinw=1.2, stop_gapw=1,
stop_center_gapw=1)
drive_pinw = channel_W - 2 * res_gp_gap_W
drive_gapw = res_gp_gap_W
padding_to_connect_to_ellipse = trap_gap
three_pin_L = trap_guard_L + trap_gap + padding_to_connect_to_ellipse
MaskMaker.CPWTaper(c.s_drive, 50, pinw=3.4, gapw=1.0, stop_pinw=drive_pinw, stop_gapw=drive_gapw)
MaskMaker.CPWStraight(c.s_drive,
2500 + 302.910 - 300 + 9.32 + 0.09 - 1.68 - 0.140 + 0.75 + 2 * guard_gap + trap_guard_L)
# trap area
# gapw is always res_gp_gap_W
MaskMaker.CPWStraight(c.s_drive, trap_gap, pinw=trap_pin_W, gapw=(channel_W - trap_pin_W) / 2)
MaskMaker.ThreePinTaper(c.s_drive, three_pin_L, pinw=res_pin_W, center_pinw=trap_pin_W, gapw=res_gp_gap_W,
center_gapw=trap_gap)
MaskMaker.CPWStraight(c.s_drive, trap_L - padding_to_connect_to_ellipse * 2, pinw=trap_pin_W,
gapw=(channel_W - trap_pin_W) / 2)
MaskMaker.ThreePinTaper(c.s_drive, three_pin_L, pinw=res_pin_W, center_pinw=trap_pin_W, gapw=res_gp_gap_W,
center_gapw=trap_gap)
MaskMaker.CoupledStraight(c.s_drive, trap_gap, center_gapw=trap_gap * 2 + trap_pin_W)
### Right launchers and the readout resonator
s = c.s4
s.chip.two_layer = True
pinw = res_pin_W * 2 + res_center_gap_W
gapw = 1.5
if inductive_launcher:
MaskMaker.Inductive_Launcher(s, pinw=25, gapw=20, padw=200, padl=300, num_loops=4)
else:
MaskMaker.Launcher(s, pinw=1.5, gapw=1.5)
MaskMaker.CPWStraight(s, 245)
right_launcher_straight = 2000 + wiggle_length - 80 - 245. - chip_resonator_length + 280 + 229.890 + 300 + 0.75 + guard_gap * 2 + trap_guard_L
# cprint("right_launcher_straight: {}".format(right_launcher_straight), color="red")
MaskMaker.CPWTaper(s, 80, stop_pinw=pinw, stop_gapw=gapw)
MaskMaker.CPWStraight(s, right_launcher_straight)
MaskMaker.CPWTaper(s, 1, stop_pinw=res_pin_W * 2, stop_gapw=res_center_gap_W / 2 + res_gp_gap_W)
MaskMaker.CPWStraight(s, 4.5)
MaskMaker.CPWTaper(s, 1) ##, stop_pinw=1.5, stop_gapw=(channel_W - 1.5) / 2)
MaskMaker.CPWStraight(s, 10)
MaskMaker.CPWTaper(s, 3, stop_pinw=res_pin_W * 2 + res_center_gap_W, stop_gapw=res_gp_gap_W)
# resonator (on the right)
readout_resonator_start_pt = MaskMaker.translate_pt(c.center, (
chip_resonator_length - 529.390 - 300 + 1000 - wiggle_length - 0.75 - 2 * guard_gap - trap_guard_L, 0))
c.s_readout = MaskMaker.Structure(c, start=readout_resonator_start_pt, direction=180)
s = c.s_readout
s.pinw = res_pin_W
s.gapw = res_gp_gap_W
s.center_gapw = res_center_gap_W
MaskMaker.CoupledStraight(s, 50)
wiggle_right_length = resonator_length - wiggle_length - 1 - 1.16 + 1.27 - 1.86 - 0.1400
MaskMaker.CoupledWiggles(s, 6, wiggle_length, 0, start_up=True, radius=30,
segments=30)
MaskMaker.CoupledStraight(s, wiggle_right_length)
# if not hasattr(s, 'gap_layer') and not hasattr(s, 'pin_layer'):
# MaskMaker.CPWStraight(s, .1225, pinw=0, gapw=.22 / 2.)
# Center Guard
length_to_trap = 450 - 5 - 10
guard_straight = 185.51 - 1.351 - 0.28
center_guard_taper = 5
guard_middle_pinch_length = 4 + 0.371
guardSHorizontal = 445. - 300
s = c.s14
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=1.5, gapw=1.5)
MaskMaker.CPWStraight(s, length_to_trap - 400)
MaskMaker.CPWSturn(s, 20, 90, 90, guardSHorizontal, -90, 90, 20, segments=10)
MaskMaker.CPWStraight(s, guard_straight)
MaskMaker.CPWTaper(s, center_guard_taper, stop_pinw=trap_L, stop_gapw=trap_gap)
MaskMaker.CPWStraight(s, guard_middle_pinch_length)
s = c.s18
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=1.5, gapw=1.5)
MaskMaker.CPWStraight(s, length_to_trap - 400)
MaskMaker.CPWSturn(s, 20, -90, 90, guardSHorizontal, 90, 90, 20, segments=10)
MaskMaker.CPWStraight(s, guard_straight)
MaskMaker.CPWTaper(s, center_guard_taper, stop_pinw=trap_L, stop_gapw=trap_gap)
MaskMaker.CPWStraight(s, guard_middle_pinch_length)
mid_pt = MaskMaker.translate_pt(MaskMaker.middle(c.s1.last, c.s2.last), (-300, 0))
s.last = mid_pt
theta2 = np.arcsin(0.70 / (2 * res_pin_trap_outer_A))
theta3 = np.arcsin(0.70 / (2 * res_pin_trap_inner_A))
outer_arc = MaskMaker.ellipse_arcpts(mid_pt, res_pin_trap_outer_A * res_pin_trap_outer_ratio, res_pin_trap_outer_A,
angle_start=theta2, angle_stop=np.pi - theta2, angle=0, segments=15)
inner_arc = MaskMaker.ellipse_arcpts(mid_pt, res_pin_trap_inner_A * res_pin_trap_inner_ratio, res_pin_trap_inner_A,
angle_start=np.pi - theta3, angle_stop=theta3, angle=0, segments=15)
for ia in inner_arc:
outer_arc.append(ia)
outer_arc.append(outer_arc[0])
s.pin_layer.append(sdxf.PolyLine(outer_arc))
# And mirror the trap
mirrored_outer_arc = MaskMaker.mirror_pts(outer_arc, axis_angle=0, axis_pt=mid_pt)
s.pin_layer.append(sdxf.PolyLine(mirrored_outer_arc))
MaskMaker.Ellipses(s.gap_layer, mid_pt, trap_L / 2, trap_W / 2, angle=0, segments=30)
MaskMaker.Ellipses(s.pin_layer, mid_pt, trap_pin_A * trap_pin_ratio, trap_pin_A, angle=0, segments=30)
s.chip.two_layer = True
middleGuardL = 1.2
sideGuardL = 0.2
guardGap = 0.2
guardLauncherStraight = 145 + 1.3759 + 0.3750 - 5.5461
guardSHorizontal = 360 + 12.68 + 1.277 + 0.0707 - 0.220 - 0.0670 - 0.290 - 0.07
guardEndStraight = 120 + 20.7 + 2 + 2
sideGuardEndStraight = 1.0 + 3.9897 + 0.255 - 0.12 - 0.25 - 1.350 + 0.37 - 0.28
### Microwave Feed Couplers
coupler_offset = 15
R1 = 60
L1 = 0
coupler_offset_h = 00 + 39.6475 - 0.8
L3 = 199.5 - 0.0013 - 120 - 40 - 40
L5 = 27.2513 + 150 + 150 - coupler_offset # to make it `coupler_offset` um away from the resonator
s = c.s15
s.chip.two_layer = False
s.pinw = 3
s.gapw = 1
MaskMaker.Launcher(s)
MaskMaker.CPWStraight(s, L1 + R1 * 2 + L3)
# MaskMaker.CPWSturn(s, L1, -90, R1, coupler_offset_h, 90, R1, L3, segments=5)
MaskMaker.CPWStraight(s, L5)
MaskMaker.CPWStraight(s, 1.5, pinw=0, gapw=2.5)
s = c.s19
s.chip.two_layer = False
s.pinw = 3
s.gapw = 1
MaskMaker.Launcher(s)
MaskMaker.CPWStraight(s, L1 + R1 * 2 + L3)
# MaskMaker.CPWSturn(s, L1, 90, R1, coupler_offset_h, -90, R1, L3, segments=5)
MaskMaker.CPWStraight(s, L5)
MaskMaker.CPWStraight(s, 1.5, pinw=0, gapw=2.5)
### Resonator Couplers
c.two_layer = True
coupler_spacing = 0.8
coupler_offset_v = 1.5 + 1.2 - 1. - 0.73
coupler_offset_h = 462.4025 - coupler_spacing + 159.6475 - coupler_spacing
launch_pt = MaskMaker.translate_pt(c.center, (coupler_offset_h, coupler_offset_v))
setattr(c, 'resonator_coupler_1', MaskMaker.Structure(c, start=launch_pt, direction=90))
launch_pt = MaskMaker.translate_pt(c.center, (coupler_offset_h, -coupler_offset_v))
setattr(c, 'resonator_coupler_2', MaskMaker.Structure(c, start=launch_pt, direction=-90))
coupler_1 = c.resonator_coupler_1
res_coupler_gap = 0.65
end_cap_gap = 1.2 / 2 + res_coupler_gap # 1.25 um
coupler_1.pinw = 1.2
coupler_1.gapw = res_coupler_gap
coupler_2 = c.resonator_coupler_2
coupler_2.pinw = 1.2
coupler_2.gapw = res_coupler_gap
MaskMaker.CPWStraight(coupler_1, coupler_offset + chip_coupler_length)
MaskMaker.CPWStraight(coupler_1, 1.5, pinw=0, gapw=end_cap_gap)
MaskMaker.CPWStraight(coupler_2, coupler_offset + chip_coupler_length)
MaskMaker.CPWStraight(coupler_2, 1.5, pinw=0, gapw=end_cap_gap)
### DC Guards
guard_pin_w = trap_guard_L
launcher_pin_w = 3
launcher_gap_w = 2.5
guard_radius = 1.2
guard_taper_length = 6
guard_horizontal_2 = guardSHorizontal + 1876 - 300 - 1.24 - 1.4118 - 1.4980 - 0.1840 + (guard_pin_w / 2)
s = c.s5
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=launcher_pin_w, gapw=launcher_gap_w)
MaskMaker.CPWStraight(s, guardLauncherStraight)
MaskMaker.CPWSturn(s, 20, 90, 90, guard_horizontal_2, -60, 90, 20,
segments=10)
MaskMaker.CPWStraight(s, guardEndStraight)
MaskMaker.CPWTaper(s, guard_taper_length, stop_pinw=guard_pin_w, stop_gapw=trap_gap)
MaskMaker.CPWBend(s, -30, radius=guard_radius, segments=1)
MaskMaker.CPWStraight(s, sideGuardEndStraight)
s = c.s7
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=launcher_pin_w, gapw=launcher_gap_w)
MaskMaker.CPWStraight(s, guardLauncherStraight)
MaskMaker.CPWSturn(s, 20, -90, 90, guard_horizontal_2, 60, 90, 20,
segments=10)
MaskMaker.CPWStraight(s, guardEndStraight)
MaskMaker.CPWTaper(s, guard_taper_length, stop_pinw=guard_pin_w, stop_gapw=trap_gap)
MaskMaker.CPWBend(s, 30, radius=guard_radius, segments=1)
MaskMaker.CPWStraight(s, sideGuardEndStraight)
# Resonator Side Guards
s = c.s1
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=launcher_pin_w, gapw=launcher_gap_w)
MaskMaker.CPWStraight(s, guardLauncherStraight)
MaskMaker.CPWSturn(s, 20, -90, 90, guard_horizontal_2 - 1900, 60, 90, 20,
segments=10)
MaskMaker.CPWStraight(s, guardEndStraight)
MaskMaker.CPWTaper(s, guard_taper_length, stop_pinw=guard_pin_w, stop_gapw=trap_gap)
MaskMaker.CPWBend(s, 30, radius=guard_radius, segments=1)
MaskMaker.CPWStraight(s, sideGuardEndStraight)
s = c.s2
s.chip.two_layer = False
MaskMaker.Launcher(s, pinw=launcher_pin_w, gapw=launcher_gap_w)
MaskMaker.CPWStraight(s, guardLauncherStraight)
MaskMaker.CPWSturn(s, 20, 90, 90, guard_horizontal_2 - 1900, -60, 90, 20,
segments=10)
MaskMaker.CPWStraight(s, guardEndStraight)
MaskMaker.CPWTaper(s, guard_taper_length, stop_pinw=guard_pin_w, stop_gapw=trap_gap)
MaskMaker.CPWBend(s, -30, radius=guard_radius, segments=1)
MaskMaker.CPWStraight(s, sideGuardEndStraight)
# Resonator DC Bias pinch electrodes
L1 = 20
R = 20
L3 = - (resonator_length - 900 - wiggle_length) + 1450 - 40 - 36.86 + 0.15 + 1.5 + - 0.15 + guard_gap
L5 = 4
L6 = 2
L4 = 450 - 40 - L1 - 0.4 - 1.070 - L5 - L6 - 0.28
s = c.s6
MaskMaker.Launcher(s, pinw=1.5, gapw=1.5)
MaskMaker.CPWSturn(s, L1, -90, R, L3, 90, R, L4, segments=3)
MaskMaker.CPWTaper(s, L5, stop_pinw=3.5, stop_gapw=0.5)
MaskMaker.CPWStraight(s, L6)
s = c.s8
MaskMaker.Launcher(s, pinw=1.5, gapw=1.5)
MaskMaker.CPWSturn(s, L1, 90, R, L3, -90, R, L4, segments=3)
MaskMaker.CPWTaper(s, L5, stop_pinw=3.5, stop_gapw=0.5)
MaskMaker.CPWStraight(s, L6)
s.chip.two_layer = True
def rhs(self, Core):
lref = self.bref
# wind interpolation bit
alt = Core.lla[2]
wind_v = self.wind_speed
wind_angle = self.wind_heading
wind_angle = wind_angle / 180 * np.pi
# find the aerodynamic frame velocity
_VN = wind_v * np.cos(wind_angle)
_VE = wind_v * np.sin(wind_angle)
Vgust_b = np.dot(Core.DCMbe, np.array([_VN, _VE, 0.]))
# Vb = Core.DCMbc @ Core.DCMci @ Core.vel
Vb = Core.DCMbc @ Core.vel
Va = Vgust_b + Vb
Va_norm = np.linalg.norm(Va)
# compute stuff for aero
mach = Va_norm / Core.a
q = 0.5 * Core.rho * Va_norm**2
alpha = np.arctan2(Va[2], Va[0])
if Va_norm == 0.:
beta = 0.
else:
beta = np.arcsin(Va[1] / Va_norm)
if alpha > MAX_ALPHA:
alpha = MAX_ALPHA
if alpha < -MAX_ALPHA:
alpha = -MAX_ALPHA
# compute p_hat, q_hat and r_hat
if np.sqrt(Va_norm**2) <= 1e-8:
p_hat = 0.
q_hat = 0.
r_hat = 0.
else:
p_hat = self.bref * Core.omega[0] / (2 * Va_norm)
q_hat = self.cref * Core.omega[1] / (2 * Va_norm)
r_hat = self.bref * Core.omega[2] / (2 * Va_norm)
# compute the coefficient along axis
c_x = self.c_x_0 + self.c_x_a2 * alpha**2 + self.c_x_p * p_hat + self.c_x_q * q_hat + self.c_x_r * r_hat
c_y = self.c_y_b * beta + self.c_y_p * p_hat + self.c_y_q * q_hat + self.c_y_r * r_hat
c_z = self.c_z_a * alpha + self.c_z_p * p_hat + self.c_z_q * q_hat + self.c_z_r * r_hat
c_ll = self.c_ll_0 + self.c_ll_p * p_hat
c_m = self.c_m_a * alpha + self.c_m_p * p_hat + self.c_m_q * q_hat + self.c_m_r * r_hat
c_ln = self.c_ln_b * beta + self.c_ln_p * p_hat + self.c_ln_q * q_hat + self.c_ln_r * r_hat
# compute force and moment coefficients
c_f = np.array([c_x, c_y, c_z])
# l = -Core.cg+np.array([x_cp, 0, 0])
length = Core.cg
c_m = np.array([c_ll, c_m, c_ln]) + np.cross(length, c_f) / np.array(
[self.bref, self.cref, self.bref])
# compute actual forces and moments
aero_force = q * c_f * self.sref * self.scale
aero_force_c = aero_force
aero_moment = q * c_m * self.sref * np.array(
[self.bref, self.cref, self.bref]) * self.scale
Core.force[0] += aero_force_c[0]
Core.moment[0] += aero_moment[0]
Core.force[1] += aero_force_c[1]
Core.moment[1] += aero_moment[1]
Core.force[2] += aero_force_c[2]
Core.moment[2] += aero_moment[2]
def rotation_to_euler_angle(self, R):
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arcsin(-R[2, 0])
yaw = np.arctan2(R[1, 0], R[0, 0])
return roll, pitch, yaw
def load_config_commands(rob_controller, config):
"""Load the position commands to the robot controller.
Uses the method determined by the command type
selected in the configuration file. Currently, the
options are `read_from_file`, `sample_from_file`, or
`parametric_sample`. `read_from_file` takes the
commands directly from the command file.
`sample_from_file` generates a sequence of commands by
sampling rows from the command file with replacement.
For both of these options, the command file is assumed
to have three columns ordered as `reach distance`,
`azimuth', `elevation`. `parametric_sample` does not
use the command file. Rather, it samples commands
uniformly from the `reach volume` determined by the
user-selected inverse kinematics parameters.
Todo:
* Further functionalize for better clarity.
Parameters
----------
rob_controller : serial.serialposix.Serial
The serial interface to the robot controller.
config : dict
The currently loaded configuration file.
Returns
-------
config : dict
The configuration file possibly updated to include
the command values that were loaded.
"""
#extract robot kinematic settings
ygimbal_to_joint = config['RobotSettings']['ygimbal_to_joint']
zgimbal_to_joint = config['RobotSettings']['zgimbal_to_joint']
xgimbal_xoffset = config['RobotSettings']['xgimbal_xoffset']
ygimbal_yoffset = config['RobotSettings']['ygimbal_yoffset']
zgimbal_zoffset = config['RobotSettings']['zgimbal_zoffset']
x_origin = config['RobotSettings']['x_origin']
y_origin = config['RobotSettings']['y_origin']
z_origin = config['RobotSettings']['z_origin']
reach_dist_min = config['RobotSettings']['reach_dist_min']
reach_dist_max = config['RobotSettings']['reach_dist_max']
reach_angle_max = config['RobotSettings']['reach_angle_max']
#generate commands according to selected command type
n = 100
if config['RobotSettings']['command_type'] == "parametric_sample":
r = (reach_dist_min + (reach_dist_max - reach_dist_min) *
np.random.uniform(low=0.0, high=1.0, size=500 * n)**(1.0 / 3.0))
theta_y = reach_angle_max * np.random.uniform(
low=-1, high=1, size=500 * n)
theta_z = reach_angle_max * np.random.uniform(
low=-1, high=1, size=500 * n)
theta = np.sqrt(theta_y**2 + theta_z**2)
r = r[theta <= reach_angle_max][0:n]
theta_y = theta_y[theta <= reach_angle_max][0:n]
theta_z = theta_z[theta <= reach_angle_max][0:n]
elif config['RobotSettings']['command_type'] == "sample_from_file":
r_set, theta_y_set, theta_z_set = np.loadtxt(
config['RobotSettings']['command_file'],
skiprows=1,
delimiter=',',
unpack=True,
usecols=(1, 2, 3))
rand_sample = np.random.choice(range(len(r_set)), replace=True, size=n)
r = r_set[rand_sample]
theta_y = theta_y_set[rand_sample]
theta_z = theta_z_set[rand_sample]
elif config['RobotSettings']['command_type'] == "read_from_file":
r, theta_y, theta_z = np.loadtxt(
config['RobotSettings']['command_file'],
skiprows=1,
delimiter=',',
unpack=True,
usecols=(1, 2, 3))
else:
raise Exception("Invalid command type.")
#pass generated commands though inverse kinematic transformation
Ax = np.sqrt(xgimbal_xoffset**2 + r**2 -
2 * xgimbal_xoffset * r * np.cos(theta_y) * np.cos(theta_z))
gammay = -np.arcsin(
np.sin(theta_y) * np.sqrt((r * np.cos(theta_y) * np.cos(theta_z))**2 +
(r * np.sin(theta_y) * np.cos(theta_z))**2) /
np.sqrt((xgimbal_xoffset - r * np.cos(theta_y) * np.cos(theta_z))**2 +
(r * np.sin(theta_y) * np.cos(theta_z))**2))
gammaz = -np.arcsin(r * np.sin(theta_z) / Ax)
Ay = np.sqrt((ygimbal_to_joint -
ygimbal_to_joint * np.cos(gammay) * np.cos(gammaz))**2 +
(ygimbal_yoffset -
ygimbal_to_joint * np.sin(gammay) * np.cos(gammaz))**2 +
(ygimbal_to_joint * np.sin(gammaz))**2)
Az = np.sqrt((zgimbal_to_joint -
zgimbal_to_joint * np.cos(gammay) * np.cos(gammaz))**2 +
(zgimbal_to_joint * np.sin(gammay) * np.cos(gammaz))**2 +
(zgimbal_zoffset - zgimbal_to_joint * np.sin(gammaz))**2)
Ax = np.round((Ax - xgimbal_xoffset) / 50 * 1024 + x_origin, decimals=1)
Ay = np.round((Ay - ygimbal_yoffset) / 50 * 1024 + y_origin, decimals=1)
Az = np.round((Az - zgimbal_zoffset) / 50 * 1024 + z_origin, decimals=1)
#convert tranformed commands to appropriate data types/format
x = np.array2string(Ax, formatter={'float_kind': lambda Ax: "%.1f" % Ax})
y = np.array2string(Ay, formatter={'float_kind': lambda Ay: "%.1f" % Ay})
z = np.array2string(Az, formatter={'float_kind': lambda Az: "%.1f" % Az})
r = np.array2string(r, formatter={'float_kind': lambda r: "%.1f" % r})
theta_y = np.array2string(
theta_y, formatter={'float_kind': lambda theta_y: "%.1f" % theta_y})
theta_z = np.array2string(
theta_z, formatter={'float_kind': lambda theta_z: "%.1f" % theta_z})
x = x[1:-1] + ' '
y = y[1:-1] + ' '
z = z[1:-1] + ' '
r = r[1:-1] + ' '
theta_y = theta_y[1:-1] + ' '
theta_z = theta_z[1:-1] + ' '
#load commands to robot
try:
_load_command_variable(rob_controller, 'x_command_pos', x)
_load_command_variable(rob_controller, 'y_command_pos', y)
_load_command_variable(rob_controller, 'z_command_pos', z)
except Exception as varname:
raise Exception("Failed to load: " + varname)
#record the loaded commands to the config
config['RobotSettings']['x'] = x
config['RobotSettings']['y'] = y
config['RobotSettings']['z'] = z
config['RobotSettings']['r'] = r
config['RobotSettings']['theta_y'] = theta_y
config['RobotSettings']['theta_z'] = theta_z
return config
def rhs(self, Core):
# wind interpolation bit
alt = Core.lla[2]
_VN, _VE = interp_one(alt, self.wind_alt, self.wind_speed)
Vgust_b = np.dot(Core.DCMbe, np.array([_VN, _VE, 0.]))
Vb = Core.DCMbc @ Core.vel
Va = Vgust_b + Vb
Va_norm = np.linalg.norm(Va)
# compute stuff for aero
mach = Va_norm / Core.a
q = 0.5 * Core.rho * Va_norm**2
alpha = np.arctan2(Va[2], Va[0])
if Va_norm == 0.:
beta = 0.
else:
beta = np.arcsin(Va[1] / Va_norm)
alpha_raw, beta_raw = alpha, beta
if alpha > MAX_ALPHA:
alpha = MAX_ALPHA
if alpha < -MAX_ALPHA:
alpha = -MAX_ALPHA
if beta > MAX_ALPHA:
beta = MAX_ALPHA
if beta < -MAX_ALPHA:
beta = -MAX_ALPHA
# compute p_hat, q_hat and r_hat
if np.sqrt(Va_norm**2) == 0.:
p_hat = 0.
q_hat = 0.
r_hat = 0.
else:
p_hat = self.lref * Core.omega[0] / (2 * Va_norm)
q_hat = self.lref * Core.omega[1] / (2 * Va_norm)
r_hat = self.lref * Core.omega[2] / (2 * Va_norm)
# make sure mach number is within the bounds of interpolation
if mach < self.machs[0]:
mach = self.machs[0]
if mach > self.machs[-1]:
mach = self.machs[-1]
# interpolate the coefficients
c_x_0 = interp_one(mach, self.machs, self.c_x_0)
c_x_a = interp_one(mach, self.machs, self.c_x_a)
c_x_a2 = interp_one(mach, self.machs, self.c_x_a2)
c_x_p = interp_one(mach, self.machs, self.c_x_p)
c_x_q = interp_one(mach, self.machs, self.c_x_q)
c_x_r = interp_one(mach, self.machs, self.c_x_r)
c_y_b = interp_one(mach, self.machs, self.c_y_b)
c_y_p = interp_one(mach, self.machs, self.c_y_p)
c_y_q = interp_one(mach, self.machs, self.c_y_q)
c_y_r = interp_one(mach, self.machs, self.c_y_r)
c_z_a = interp_one(mach, self.machs, self.c_z_a)
c_z_p = interp_one(mach, self.machs, self.c_z_p)
c_z_q = interp_one(mach, self.machs, self.c_z_q)
c_z_r = interp_one(mach, self.machs, self.c_z_r)
c_ll_0 = interp_one(mach, self.machs, self.c_ll_0)
c_ll_p = interp_one(mach, self.machs, self.c_ll_p)
c_m_a = interp_one(mach, self.machs, self.c_m_a)
c_m_p = interp_one(mach, self.machs, self.c_m_p)
c_m_q = interp_one(mach, self.machs, self.c_m_q)
c_m_r = interp_one(mach, self.machs, self.c_m_r)
c_ln_b = interp_one(mach, self.machs, self.c_ln_b)
c_ln_p = interp_one(mach, self.machs, self.c_ln_p)
c_ln_q = interp_one(mach, self.machs, self.c_ln_q)
c_ln_r = interp_one(mach, self.machs, self.c_ln_r)
x_cp = interp_one(mach, self.machs, self.x_cp)
# compute the coefficient along axis
c_x = c_x_0 + c_x_a2 * alpha**2 + c_x_p * p_hat + c_x_q * q_hat + c_x_r * r_hat
# c_x = c_x_0 + c_x_a * alpha + c_x_a2 * alpha**2 + c_x_p * p_hat + c_x_q * q_hat + c_x_r * r_hat
c_y = c_y_b * beta + c_y_p * p_hat + c_y_q * q_hat + c_y_r * r_hat
c_z = c_z_a * alpha + c_z_p * p_hat + c_z_q * q_hat + c_z_r * r_hat
c_ll = c_ll_0 + c_ll_p * p_hat
c_m = c_m_a * alpha + c_m_p * p_hat + c_m_q * q_hat + c_m_r * r_hat
c_ln = c_ln_b * beta + c_ln_p * p_hat + c_ln_q * q_hat + c_ln_r * r_hat
# compute force and moment coefficients
c_f = np.array([c_x, c_y, c_z])
length = Core.cg
c_m = np.array([c_ll, c_m, c_ln
]) + np.cross(-1 * length, c_f) / self.lref
# compute actual forces and moments
# self.scale = np.ones(6)*1.3
aero_force = q * c_f * self.sref * self.scale[:3]
aero_moment = q * c_m * self.sref * self.lref * self.scale[3:]
# print(Va)
# print(alpha, beta)
# print(aero_force, aero_moment)
Core.force[0] += aero_force[0]
Core.moment[0] += aero_moment[0]
Core.force[1] += aero_force[1]
Core.moment[1] += aero_moment[1]
Core.force[2] += aero_force[2]
Core.moment[2] += aero_moment[2]
if Core.logging:
self.history["f_a[0]"] = np.append(self.history["f_a[0]"],
aero_force[0])
self.history["f_a[1]"] = np.append(self.history["f_a[1]"],
aero_force[1])
self.history["f_a[2]"] = np.append(self.history["f_a[2]"],
aero_force[2])
self.history["m_a[0]"] = np.append(self.history["m_a[0]"],
aero_moment[0])
self.history["m_a[1]"] = np.append(self.history["m_a[1]"],
aero_moment[1])
self.history["m_a[2]"] = np.append(self.history["m_a[2]"],
aero_moment[2])
self.history["M"] = np.append(self.history["M"], mach)
self.history["q"] = np.append(self.history["q"], q)
self.history["alpha"] = np.append(self.history["alpha"], alpha_raw)
self.history["beta"] = np.append(self.history["beta"], beta_raw)
def euler(self, order='xyz'):
q = self.normalized().qs
q0 = q[..., 0]
q1 = q[..., 1]
q2 = q[..., 2]
q3 = q[..., 3]
es = np.zeros(self.shape + (3,))
if order == 'xyz':
es[..., 0] = np.arctan2(
2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
es[..., 1] = np.arcsin((2 * (q0 * q2 - q3 * q1)).clip(-1, 1))
es[..., 2] = np.arctan2(
2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
elif order == 'yzx':
es[..., 0] = np.arctan2(
2 * (q1 * q0 - q2 * q3), -q1 * q1 + q2 * q2 - q3 * q3 + q0 * q0)
es[..., 1] = np.arctan2(
2 * (q2 * q0 - q1 * q3), q1 * q1 - q2 * q2 - q3 * q3 + q0 * q0)
es[..., 2] = np.arcsin((2 * (q1 * q2 + q3 * q0)).clip(-1, 1))
else:
raise NotImplementedError(
'Cannot convert from ordering %s' % order)
"""
# These conversion don't appear to work correctly for Maya.
# http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
if order == 'xyz':
es[fa + (0,)] = np.arctan2(2 * (q0 * q3 - q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q1 * q3 + q0 * q2)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q0 * q1 - q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
elif order == 'yzx':
es[fa + (0,)] = np.arctan2(2 * (q0 * q1 - q2 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q1 * q2 + q0 * q3)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q0 * q2 - q1 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
elif order == 'zxy':
es[fa + (0,)] = np.arctan2(2 * (q0 * q2 - q1 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q0 * q1 + q2 * q3)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q0 * q3 - q1 * q2), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
elif order == 'xzy':
es[fa + (0,)] = np.arctan2(2 * (q0 * q2 + q1 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q0 * q3 - q1 * q2)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
elif order == 'yxz':
es[fa + (0,)] = np.arctan2(2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q0 * q1 - q2 * q3)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
elif order == 'zyx':
es[fa + (0,)] = np.arctan2(2 * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
es[fa + (1,)] = np.arcsin((2 * (q0 * q2 - q1 * q3)).clip(-1,1))
es[fa + (2,)] = np.arctan2(2 * (q0 * q3 + q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
else:
raise KeyError('Unknown ordering %s' % order)
"""
# https://github.com/ehsan/ogre/blob/master/OgreMain/src/OgreMatrix3.cpp
# Use this class and convert from matrix
return es
def _visualize_output():
last_frame_index = 0
last_frame_time = time.time()
fps_history = []
all_gaze_histories = []
if args.fullscreen:
cv.namedWindow('vis', cv.WND_PROP_FULLSCREEN)
cv.setWindowProperty('vis', cv.WND_PROP_FULLSCREEN, cv.WINDOW_FULLSCREEN)
while True:
# If no output to visualize, show unannotated frame
if inferred_stuff_queue.empty():
next_frame_index = last_frame_index + 1
if next_frame_index in data_source._frames:
next_frame = data_source._frames[next_frame_index]
if 'faces' in next_frame and len(next_frame['faces']) == 0:
if not args.headless:
cv.imshow('vis', next_frame['bgr'])
if args.record_video:
video_out_queue.put_nowait(next_frame_index)
last_frame_index = next_frame_index
if cv.waitKey(1) & 0xFF == ord('q'):
return
continue
# Get output from neural network and visualize
output = inferred_stuff_queue.get()
bgr = None
for j in range(batch_size):
frame_index = output['frame_index'][j]
if frame_index not in data_source._frames:
continue
frame = data_source._frames[frame_index]
# Decide which landmarks are usable
heatmaps_amax = np.amax(output['heatmaps'][j, :].reshape(-1, 18), axis=0)
can_use_eye = np.all(heatmaps_amax > 0.7)
can_use_eyelid = np.all(heatmaps_amax[0:8] > 0.75)
can_use_iris = np.all(heatmaps_amax[8:16] > 0.8)
start_time = time.time()
eye_index = output['eye_index'][j]
bgr = frame['bgr']
eye = frame['eyes'][eye_index]
eye_image = eye['image']
eye_side = eye['side']
eye_landmarks = output['landmarks'][j, :]
eye_radius = output['radius'][j][0]
if eye_side == 'left':
eye_landmarks[:, 0] = eye_image.shape[1] - eye_landmarks[:, 0]
eye_image = np.fliplr(eye_image)
# Embed eye image and annotate for picture-in-picture
eye_upscale = 2
eye_image_raw = cv.cvtColor(cv.equalizeHist(eye_image), cv.COLOR_GRAY2BGR)
eye_image_raw = cv.resize(eye_image_raw, (0, 0), fx=eye_upscale, fy=eye_upscale)
eye_image_annotated = np.copy(eye_image_raw)
if can_use_eyelid:
cv.polylines(
eye_image_annotated,
[np.round(eye_upscale*eye_landmarks[0:8]).astype(np.int32)
.reshape(-1, 1, 2)],
isClosed=True, color=(255, 255, 0), thickness=1, lineType=cv.LINE_AA,
)
if can_use_iris:
cv.polylines(
eye_image_annotated,
[np.round(eye_upscale*eye_landmarks[8:16]).astype(np.int32)
.reshape(-1, 1, 2)],
isClosed=True, color=(0, 255, 255), thickness=1, lineType=cv.LINE_AA,
)
cv.drawMarker(
eye_image_annotated,
tuple(np.round(eye_upscale*eye_landmarks[16, :]).astype(np.int32)),
color=(0, 255, 255), markerType=cv.MARKER_CROSS, markerSize=4,
thickness=1, line_type=cv.LINE_AA,
)
face_index = int(eye_index / 2)
eh, ew, _ = eye_image_raw.shape
v0 = face_index * 2 * eh
v1 = v0 + eh
v2 = v1 + eh
u0 = 0 if eye_side == 'left' else ew
u1 = u0 + ew
bgr[v0:v1, u0:u1] = eye_image_raw
bgr[v1:v2, u0:u1] = eye_image_annotated
# Visualize preprocessing results
frame_landmarks = (frame['smoothed_landmarks']
if 'smoothed_landmarks' in frame
else frame['landmarks'])
for f, face in enumerate(frame['faces']):
for landmark in frame_landmarks[f][:-1]:
cv.drawMarker(bgr, tuple(np.round(landmark).astype(np.int32)),
color=(0, 0, 255), markerType=cv.MARKER_STAR,
markerSize=2, thickness=1, line_type=cv.LINE_AA)
cv.rectangle(
bgr, tuple(np.round(face[:2]).astype(np.int32)),
tuple(np.round(np.add(face[:2], face[2:])).astype(np.int32)),
color=(0, 255, 255), thickness=1, lineType=cv.LINE_AA,
)
# Transform predictions
eye_landmarks = np.concatenate([eye_landmarks,
[[eye_landmarks[-1, 0] + eye_radius,
eye_landmarks[-1, 1]]]])
eye_landmarks = np.asmatrix(np.pad(eye_landmarks, ((0, 0), (0, 1)),
'constant', constant_values=1.0))
eye_landmarks = (eye_landmarks *
eye['inv_landmarks_transform_mat'].T)[:, :2]
eye_landmarks = np.asarray(eye_landmarks)
eyelid_landmarks = eye_landmarks[0:8, :]
iris_landmarks = eye_landmarks[8:16, :]
iris_centre = eye_landmarks[16, :]
eyeball_centre = eye_landmarks[17, :]
eyeball_radius = np.linalg.norm(eye_landmarks[18, :] -
eye_landmarks[17, :])
# Smooth and visualize gaze direction
num_total_eyes_in_frame = len(frame['eyes'])
if len(all_gaze_histories) != num_total_eyes_in_frame:
all_gaze_histories = [list() for _ in range(num_total_eyes_in_frame)]
gaze_history = all_gaze_histories[eye_index]
if can_use_eye:
# Visualize landmarks
cv.drawMarker( # Eyeball centre
bgr, tuple(np.round(eyeball_centre).astype(np.int32)),
color=(0, 255, 0), markerType=cv.MARKER_CROSS, markerSize=4,
thickness=1, line_type=cv.LINE_AA,
)
# cv.circle( # Eyeball outline
# bgr, tuple(np.round(eyeball_centre).astype(np.int32)),
# int(np.round(eyeball_radius)), color=(0, 255, 0),
# thickness=1, lineType=cv.LINE_AA,
# )
# Draw "gaze"
# from models.elg import estimate_gaze_from_landmarks
# current_gaze = estimate_gaze_from_landmarks(
# iris_landmarks, iris_centre, eyeball_centre, eyeball_radius)
i_x0, i_y0 = iris_centre
e_x0, e_y0 = eyeball_centre
theta = -np.arcsin(np.clip((i_y0 - e_y0) / eyeball_radius, -1.0, 1.0))
phi = np.arcsin(np.clip((i_x0 - e_x0) / (eyeball_radius * -np.cos(theta)),
-1.0, 1.0))
current_gaze = np.array([theta, phi])
gaze_history.append(current_gaze)
gaze_history_max_len = 10
if len(gaze_history) > gaze_history_max_len:
gaze_history = gaze_history[-gaze_history_max_len:]
util.gaze.draw_gaze(bgr, iris_centre, np.mean(gaze_history, axis=0),
length=120.0, thickness=1)
else:
gaze_history.clear()
if can_use_eyelid:
cv.polylines(
bgr, [np.round(eyelid_landmarks).astype(np.int32).reshape(-1, 1, 2)],
isClosed=True, color=(255, 255, 0), thickness=1, lineType=cv.LINE_AA,
)
if can_use_iris:
cv.polylines(
bgr, [np.round(iris_landmarks).astype(np.int32).reshape(-1, 1, 2)],
isClosed=True, color=(0, 255, 255), thickness=1, lineType=cv.LINE_AA,
)
cv.drawMarker(
bgr, tuple(np.round(iris_centre).astype(np.int32)),
color=(0, 255, 255), markerType=cv.MARKER_CROSS, markerSize=4,
thickness=1, line_type=cv.LINE_AA,
)
dtime = 1e3*(time.time() - start_time)
if 'visualization' not in frame['time']:
frame['time']['visualization'] = dtime
else:
frame['time']['visualization'] += dtime
def _dtime(before_id, after_id):
return int(1e3 * (frame['time'][after_id] - frame['time'][before_id]))
def _dstr(title, before_id, after_id):
return '%s: %dms' % (title, _dtime(before_id, after_id))
if eye_index == len(frame['eyes']) - 1:
# Calculate timings
frame['time']['after_visualization'] = time.time()
fps = int(np.round(1.0 / (time.time() - last_frame_time)))
fps_history.append(fps)
if len(fps_history) > 60:
fps_history = fps_history[-60:]
fps_str = '%d FPS' % np.mean(fps_history)
last_frame_time = time.time()
fh, fw, _ = bgr.shape
cv.putText(bgr, fps_str, org=(fw - 110, fh - 20),
fontFace=cv.FONT_HERSHEY_DUPLEX, fontScale=0.8,
color=(0, 0, 0), thickness=1, lineType=cv.LINE_AA)
cv.putText(bgr, fps_str, org=(fw - 111, fh - 21),
fontFace=cv.FONT_HERSHEY_DUPLEX, fontScale=0.79,
color=(255, 255, 255), thickness=1, lineType=cv.LINE_AA)
if not args.headless:
cv.imshow('vis', bgr)
last_frame_index = frame_index
# Record frame?
if args.record_video:
video_out_queue.put_nowait(frame_index)
# Quit?
if cv.waitKey(1) & 0xFF == ord('q'):
return
# Print timings
if frame_index % 60 == 0:
latency = _dtime('before_frame_read', 'after_visualization')
processing = _dtime('after_frame_read', 'after_visualization')
timing_string = ', '.join([
_dstr('read', 'before_frame_read', 'after_frame_read'),
_dstr('preproc', 'after_frame_read', 'after_preprocessing'),
'infer: %dms' % int(frame['time']['inference']),
'vis: %dms' % int(frame['time']['visualization']),
'proc: %dms' % processing,
'latency: %dms' % latency,
])
print('%08d [%s] %s' % (frame_index, fps_str, timing_string))
def update(self):
# diam - diamond thickness
# ds - seat thickness
# r1 - small radius
# r2 - large radius
# tilt - tilting angle of DAC
dtor = np.pi / 180.0
diam = self._diamond_thickness
ds = self._seat_thickness
r1 = self._small_cbn_seat_radius
r2 = self._large_cbn_seat_radius
tilt = -self._tilt * dtor
tilt_rotation = self._tilt_rotation * dtor + np.pi / 2
center_offset_angle = self._center_offset_angle * dtor
two_theta = self._tth_array * dtor
azi = self._azi_array * dtor
# calculate radius of the cone for each pixel specific to a center_offset and rotation angle
if self._center_offset != 0:
beta = azi - np.arcsin(self._center_offset * np.sin(
(np.pi -
(azi + center_offset_angle))) / r1) + center_offset_angle
r1 = np.sqrt(r1**2 + self._center_offset**2 -
2 * r1 * self._center_offset * np.cos(beta))
r2 = np.sqrt(r2**2 + self._center_offset**2 -
2 * r2 * self._center_offset * np.cos(beta))
# defining rotation matrices for the diamond anvil cell
Rx = np.matrix([[1, 0, 0],
[0, np.cos(tilt_rotation), -np.sin(tilt_rotation)],
[0, np.sin(tilt_rotation),
np.cos(tilt_rotation)]])
Ry = np.matrix([[np.cos(tilt), 0, np.sin(tilt)], [0, 1, 0],
[-np.sin(tilt), 0, np.cos(tilt)]])
dac_vector = np.array(Rx * Ry * np.matrix([1, 0, 0]).T)
# calculating a diffraction vector for each pixel
diffraction_vec = np.array([
np.cos(two_theta),
np.cos(azi) * np.sin(two_theta),
np.sin(azi) * np.sin(two_theta)
])
# angle between diffraction vector and diamond anvil cell vector based on dot product:
tt = np.arccos(dot_product(dac_vector, diffraction_vec) / \
(vector_len(dac_vector) * vector_len(diffraction_vec)))
# calculate path through diamond its absorption
path_diamond = diam / np.cos(tt)
abs_diamond = np.exp(-path_diamond / self._diamond_abs_length)
# define the different regions for the absorption in the seat
# region 2 is partial absorption (in the cone) and region 3 is complete absorbtion
ts1 = np.arctan(r1 / diam)
ts2 = np.arctan(r2 / (diam + ds))
tseat = np.arctan((r2 - r1) / ds)
region2 = np.logical_and(tt > ts1, tt < ts2)
region3 = tt >= ts2
# calculate the paths through each region
path_seat = np.zeros(tt.shape)
if self._center_offset != 0:
deltar = diam * np.tan(tt[region2]) - r1[region2]
alpha = np.pi / 2. - tseat[region2]
gamma = np.pi - (alpha + tt[region2] + np.pi / 2)
else:
deltar = diam * np.tan(tt[region2]) - r1
alpha = np.pi / 2. - tseat
gamma = np.pi - (alpha + tt[region2] + np.pi / 2)
path_seat[region2] = deltar * np.sin(alpha) / np.sin(gamma)
path_seat[region3] = ds / np.cos(tt[region3])
abs_seat = np.exp(-path_seat / self._seat_abs_length)
# combine both, diamond and seat absorption correction
self._data = abs_diamond * abs_seat
angle = -40
k_0 = 10
n = 4
k_y = k_0 * np.sin(np.deg2rad(angle))
k_z1 = k_0 * np.cos(np.deg2rad(angle))
k_z2 = np.sqrt((k_0 * n)**2 - k_y**2)
frames = 100
list = []
amp = 10
r_coef = 0.5
t_coef = 0.5
ref_angle = np.rad2deg(np.arcsin(np.sin(np.deg2rad(angle)) / n))
print(ref_angle)
def myFun(x, y, i):
if y > 0:
if y < -1 / np.sin(np.deg2rad(-angle)) * (x - 0.5) and y < 1 / np.sin(
np.deg2rad(-angle)) * (x + 1.5):
return amp * np.real(np.exp(
1j * (k_y * x + k_z1 * y + 0.1 * i))) + amp * r_coef * np.real(
np.exp(1j * (k_y * x - k_z1 * y + 0.1 * i)))
if y < -1 / np.sin(np.deg2rad(-angle)) * (x - 0.5):
return amp * np.real(np.exp(
1j * (k_y * x + k_z1 * y + 0.1 * i))) + amp * np.real(
np.exp(1j *
# Centre frequency of modelled antenna
f = 1.5e9 # GSSI 1.5GHz antenna model
# Largest dimension of antenna transmitting element
D = 0.060 # GSSI 1.5GHz antenna model
# Traces to plot for sanity checking
traceno = np.s_[:] # All traces
########################################
# Critical angle and velocity
if epsr:
mr = 1
z1 = np.sqrt(mr / epsr) * z0
v1 = c / np.sqrt(epsr)
thetac = np.round(np.arcsin(v1 / c) * (180 / np.pi))
wavelength = v1 / f
# Print some useful information
print('Centre frequency: {} GHz'.format(f / 1e9))
if epsr:
print('Critical angle for Er {} is {} degrees'.format(epsr, thetac))
print('Wavelength: {:.3f} m'.format(wavelength))
print(
'Observation distance(s) from {:.3f} m ({:.1f} wavelengths) to {:.3f} m ({:.1f} wavelengths)'
.format(radii[0], radii[0] / wavelength, radii[-1],
radii[-1] / wavelength))
print(
'Theoretical boundary between reactive & radiating near-field (0.62*sqrt((D^3/wavelength): {:.3f} m'
.format(0.62 * np.sqrt((D**3) / wavelength)))
print(
#Excentricidad(e)
#e=np.sqrt((1-(np.linalg.norm(h)**2))/mu*a)
e = np.sqrt((1 - (np.linalg.norm(h)**2)) / a)
#Inclinación(i)
hz = h[3]
i = np.arccos(hz / np.linalg.norm(h)) #entre 0 y 90°
#Longitud del nodo ascendente(O-omega)
hx = h[0]
hy = -h[1]
O1 = np.arcsin(hx / np.linalg.norm(h) * np.sin(i))
O2 = np.arccos(-hy / np.linalg.norm(h) * np.sin(i))
O = com(O1, O2)
#Perihelio(w-omega)
#hallar v True anomaly
v1 = np.arccos(((a * (1 - squ(e)) / Norma_rm) - 1) * 1 / e)
#v2=np.arcsin((np.dot(rm,rp)*a*(1-squ(e)))/np.linalg.norm(h)*Norma_rm)
v2 = np.arcsin(((np.dot(rm, rp) * a *
(1 - squ(e))) / np.linalg.norm(h) * Norma_rm) * 1 / e)
v = np.rad2deg(com(v1, v2))
#hallar U--revisar x,y,z
U1 = np.arccos(np.dot(rm, np.cos(O) + np.sin(O)) / Norma_rm)
def bvn_cdf(x,
y,
mu_x=0.0,
mu_y=0.0,
sigma_xx=1.0,
sigma_yy=1.0,
sigma_xy=0.0):
""" Bivariate normal cumulative distribution function with specified mean and covariance matrix.
Parameters
----------
x : float or numpy.ndarray of floats
x-coordinate(s) at which to evaluate the CDF (upper limit).
y : float or numpy.ndarray of floats
y-coordinate(s) at which to evaluate the CDF (upper limit).
mu_x : float
x-coordinate of the mean.
mu_y : float
y-coordinate of the mean.
sigma_x : float
Variance in x.
sigma_y : float
Variance in y.
sigma_xy : float
Covariance of x and y.
Returns
-------
float
Value of joint CDF at (x, y), i.e., P(X <= birth, Y <= pers).
Notes
-----
Based on the Matlab implementations by Thomas H. Jørgensen (http://www.tjeconomics.com/code/) and Alan Genz (http://www.math.wsu.edu/math/faculty/genz/software/matlab/bvnl.m) using the approach described by Drezner and Wesolowsky (https://doi.org/10.1080/00949659008811236).
"""
dh = -(x - mu_x) / np.sqrt(sigma_xx)
dk = -(y - mu_y) / np.sqrt(sigma_yy)
hk = np.multiply(dh, dk)
r = sigma_xy / np.sqrt(sigma_xx * sigma_yy)
lg, w, x = gauss_legendre_quad(r)
dim1 = np.ones((len(dh), ), dtype=np.float64)
dim2 = np.ones((lg, ), dtype=np.float64)
bvn = np.zeros((len(dh), ), dtype=np.float64)
if abs(r) < 0.925:
hs = (np.multiply(dh, dh) + np.multiply(dk, dk)) / 2.0
asr = np.arcsin(r)
sn1 = np.sin(asr * (1.0 - x) / 2.0)
sn2 = np.sin(asr * (1.0 + x) / 2.0)
dim1w = np.outer(dim1, w)
hkdim2 = np.outer(hk, dim2)
hsdim2 = np.outer(hs, dim2)
dim1sn1 = np.outer(dim1, sn1)
dim1sn2 = np.outer(dim1, sn2)
sn12 = np.multiply(sn1, sn1)
sn22 = np.multiply(sn2, sn2)
bvn = asr * np.sum(np.multiply(dim1w, np.exp(np.divide(np.multiply(dim1sn1, hkdim2) - hsdim2,
(1 - np.outer(dim1, sn12))))) +
np.multiply(dim1w, np.exp(np.divide(np.multiply(dim1sn2, hkdim2) - hsdim2,
(1 - np.outer(dim1, sn22))))), axis=1) / (4 * np.pi) \
+ np.multiply(norm_cdf(-dh), norm_cdf(-dk))
else:
if r < 0:
dk = -dk
hk = -hk
if abs(r) < 1:
opmr = (1.0 - r) * (1.0 + r)
sopmr = np.sqrt(opmr)
xmy2 = np.multiply(dh - dk, dh - dk)
xmy = np.sqrt(xmy2)
rhk8 = (4.0 - hk) / 8.0
rhk16 = (12.0 - hk) / 16.0
asr = -1.0 * (np.divide(xmy2, opmr) + hk) / 2.0
ind = asr > 100
bvn[ind] = sopmr * np.multiply(
np.exp(asr[ind]), 1.0 - np.multiply(
np.multiply(rhk8[ind], xmy2[ind] - opmr),
(1.0 - np.multiply(rhk16[ind], xmy2[ind]) / 5.0) / 3.0) +
np.multiply(rhk8[ind], rhk16[ind]) * opmr * opmr / 5.0)
ind = hk > -100
ncdfxmyt = np.sqrt(2.0 * np.pi) * norm_cdf(-xmy / sopmr)
bvn[ind] = bvn[ind] - np.multiply(
np.multiply(np.multiply(np.exp(-hk[ind] / 2.0), ncdfxmyt[ind]),
xmy[ind]),
1.0 - np.multiply(
np.multiply(rhk8[ind], xmy2[ind]),
(1.0 - np.multiply(rhk16[ind], xmy2[ind]) / 5.0) / 3.0))
sopmr = sopmr / 2
for ix in [-1, 1]:
xs = np.multiply(sopmr + sopmr * ix * x,
sopmr + sopmr * ix * x)
rs = np.sqrt(1 - xs)
xmy2dim2 = np.outer(xmy2, dim2)
dim1xs = np.outer(dim1, xs)
dim1rs = np.outer(dim1, rs)
dim1w = np.outer(dim1, w)
rhk16dim2 = np.outer(rhk16, dim2)
hkdim2 = np.outer(hk, dim2)
asr1 = -1.0 * (np.divide(xmy2dim2, dim1xs) + hkdim2) / 2.0
ind1 = asr1 > -100
cdim2 = np.outer(rhk8, dim2)
sp1 = 1.0 + np.multiply(np.multiply(cdim2, dim1xs),
1.0 + np.multiply(rhk16dim2, dim1xs))
ep1 = np.divide(
np.exp(
np.divide(-np.multiply(hkdim2, (1.0 - dim1rs)),
2.0 * (1.0 + dim1rs))), dim1rs)
bvn = bvn + np.sum(np.multiply(
np.multiply(np.multiply(sopmr, dim1w),
np.exp(np.multiply(asr1, ind1))),
np.multiply(ep1, ind1) - np.multiply(sp1, ind1)),
axis=1)
bvn = -bvn / (2.0 * np.pi)
if r > 0:
bvn = bvn + norm_cdf(-np.maximum(dh, dk))
elif r < 0:
bvn = -bvn + np.maximum(0, norm_cdf(-dh) - norm_cdf(-dk))
return bvn
def controller(states, states_load, states_d, parameters):
# mass of vehicles (kg)
m = parameters.m
ml = .136
# is there some onboard thrust control based on the vertical acceleration?
ml = .01
# acceleration due to gravity (m/s^2)
g = parameters.g
rospy.logwarn('g: ' + str(g))
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0, 0.0, 1.0])
#--------------------------------------#
# transported mass: position and velocity
p = states[0:3]
v = states[3:6]
# thrust unit vector and its angular velocity
R = states[6:15]
R = numpy.reshape(R, (3, 3))
r3 = R.dot(e3)
# load
pl = states_load[0:3]
vl = states_load[3:6]
Lmeas = norm(p - pl)
rospy.logwarn('rope length: ' + str(Lmeas))
L = 0.66
#rospy.logwarn('param Throttle_neutral: '+str(parameters.Throttle_neutral))
rl = (p - pl) / Lmeas
omegal = skew(rl).dot(v - vl) / Lmeas
omegal = zeros(3)
#--------------------------------------#
# desired quad trajectory
pd = states_d[0:3] - L * e3
vd = states_d[3:6]
ad = states_d[6:9]
jd = states_d[9:12]
sd = states_d[12:15]
# rospy.logwarn(numpy.concatenate((v,vl,vd)))
#--------------------------------------#
# position error and velocity error
ep = pl - pd
ev = vl - vd
rospy.logwarn('ep: ' + str(ep))
#--------------------------------------#
gstar = g * e3 + ad
d_gstar = jd
dd_gstar = sd
#rospy.logwarn('rl: '+str(rl))
#rospy.logwarn('omegal: '+str(omegal))
#rospy.logwarn('ev: '+str(ev))
# temp override
#rl = array([0.,0.,1.])
#omegal = zeros(3)
#L = 1
#--------------------------------------#
# rospy.logwarn('ep='+str(ep)+' ev='+str(ev)+' rl='+str(rl)+' omegal='+str(omegal))
Tl, tau, _, _ = backstep_ctrl(ep, ev, rl, omegal, gstar, d_gstar, dd_gstar)
rospy.logwarn('g: ' + str(g))
U = (eye(3) - outer(rl, rl)).dot(tau * m * L) + rl * (
# -m/L*inner(v-vl,v-vl)
+Tl * (m + ml))
U = R.T.dot(U)
n = rl
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Full_actuation = m*(ad + u + g*e3 + self.d_est)
Full_actuation = U
# -----------------------------------------------------------------------------#
U = numpy.zeros(4)
Throttle = numpy.dot(Full_actuation, r3)
# this decreases the throtle, which will be increased
Throttle = Throttle * numpy.dot(n, r3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0]
theta = euler[1]
# current psi
psi = euler[2]
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = r3_des
# desired roll and pitch angles
r3_des = Full_actuation / numpy.linalg.norm(Full_actuation)
r3_des_rot = Rz(-psi).dot(r3_des)
sin_phi = -r3_des_rot[1]
sin_phi = bound(sin_phi, 1, -1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = r3_des_rot[0] / c(phi)
sin_theta = bound(sin_theta, 1, -1)
cos_theta = r3_des_rot[2] / c(phi)
cos_theta = bound(cos_theta, 1, -1)
pitch = numpy.arctan2(sin_theta, cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw
# desired yaw: psi_star
psi_star = parameters.psi_star
psi_star_dot = 0
psi_dot = psi_star_dot - k_yaw * s(psi - psi_star)
U[3] = 1 / c(phi) * (c(theta) * psi_dot - s(phi) * U[2])
U = Cmd_Converter(U, parameters)
return U
ax.view_init(hoshi_chihei.alt.value, hoshi_chihei.az.value)
# 天頂から天体までの曲線
tenchou_chihei = SkyCoord(az=hoshi_chihei.az,
alt=np.linspace(hoshi_chihei.alt.value, 90, 101),
unit='deg',
frame='altaz')
tenchou_xyz = tenchou_chihei.cartesian.xyz
ax.plot(*tenchou_xyz, color='#9999ff')
# 時間と方位角と仰俯角と視差角を書く
az = hoshi_chihei.az.to_string(unit=u.degree, sep=['° ', '′ ', '″'])
alt = hoshi_chihei.alt.to_string(unit=u.degree, sep=['° ', '′ ', '″'])
parang = np.degrees(
np.arcsin(
np.sin(hoshi_chihei.az.radian) * np.cos(koko.lat.radian) /
np.cos(hoshi_sekidou.dec.radian)))
ax.text(
0,
0,
1.2,
f'{(ima+9*u.hour).datetime}\naz = {az}\nalt = {alt}\nparang= {parang:.3f}',
color='#ccffcc',
size=18,
ha='center')
plt.axis('off')
fig.canvas.draw()
gif.append(np.array(fig.canvas.renderer._renderer))
plt.close()
lis_parang.append(parang)
def read(humfile, sonpath, cs2cs_args, c, draft, doplot, t, bedpick, flip_lr,
model, calc_bearing, filt_bearing, chunk): #cog = 1,
'''
Read a .DAT and associated set of .SON files recorded by a Humminbird(R)
instrument.
Parse the data into a set of memory mapped files that will
subsequently be used by the other functions of the PyHum module.
Export time-series data and metadata in other formats.
Create a kml file for visualising boat track
Syntax
----------
[] = PyHum.read(humfile, sonpath, cs2cs_args, c, draft, doplot, t, bedpick, flip_lr, chunksize, model, calc_bearing, filt_bearing, chunk)
Parameters
------------
humfile : str
path to the .DAT file
sonpath : str
path where the *.SON files are
cs2cs_args : int, *optional* [Default="epsg:26949"]
arguments to create coordinates in a projected coordinate system
this argument gets given to pyproj to turn wgs84 (lat/lon) coordinates
into any projection supported by the proj.4 libraries
c : float, *optional* [Default=1450.0]
speed of sound in water (m/s). Defaults to a value of freshwater
draft : float, *optional* [Default=0.3]
draft from water surface to transducer face (m)
doplot : float, *optional* [Default=1]
if 1, plots will be made
t : float, *optional* [Default=0.108]
length of transducer array (m).
Default value is that of the 998 series Humminbird(R)
bedpick : int, *optional* [Default=1]
if 1, bedpicking with be carried out automatically
if 0, user will be prompted to pick the bed location on screen
flip_lr : int, *optional* [Default=0]
if 1, port and starboard scans will be flipped
(for situations where the transducer is flipped 180 degrees)
model: int, *optional* [Default=998]
A 3 or 4 number code indicating the model number
Examples: 998, 997, 1198, 1199
calc_bearing : float, *optional* [Default=0]
if 1, bearing will be calculated from coordinates
filt_bearing : float, *optional* [Default=0]
if 1, bearing will be filtered
chunk : str, *optional* [Default='d100' (distance, 100 m)]
letter, followed by a number.
There are the following letter options:
'd' - parse chunks based on distance, then number which is distance in m
'p' - parse chunks based on number of pings, then number which is number of pings
'h' - parse chunks based on change in heading, then number which is the change in heading in degrees
'1' - process just 1 chunk
Returns
---------
sonpath+base+'_data_port.dat': memory-mapped file
contains the raw echogram from the port side
sidescan sonar (where present)
sonpath+base+'_data_port.dat': memory-mapped file
contains the raw echogram from the starboard side
sidescan sonar (where present)
sonpath+base+'_data_dwnhi.dat': memory-mapped file
contains the raw echogram from the high-frequency
echosounder (where present)
sonpath+base+'_data_dwnlow.dat': memory-mapped file
contains the raw echogram from the low-frequency
echosounder (where present)
sonpath+base+"trackline.kml": google-earth kml file
contains the trackline of the vessel during data
acquisition
sonpath+base+'rawdat.csv': comma separated value file
contains time-series data. columns corresponding to
longitude
latitude
easting (m)
northing (m)
depth to bed (m)
alongtrack cumulative distance (m)
vessel heading (deg.)
sonpath+base+'meta.mat': .mat file
matlab format file containing a dictionary object
holding metadata information. Fields are:
e : ndarray, easting (m)
n : ndarray, northing (m)
es : ndarray, low-pass filtered easting (m)
ns : ndarray, low-pass filtered northing (m)
lat : ndarray, latitude
lon : ndarray, longitude
shape_port : tuple, shape of port scans in memory mapped file
shape_star : tuple, shape of starboard scans in memory mapped file
shape_hi : tuple, shape of high-freq. scans in memory mapped file
shape_low : tuple, shape of low-freq. scans in memory mapped file
dep_m : ndarray, depth to bed (m)
dist_m : ndarray, distance along track (m)
heading : ndarray, heading of vessel (deg. N)
pix_m: float, size of 1 pixel in across-track dimension (m)
bed : ndarray, depth to bed (m)
c : float, speed of sound in water (m/s)
t : length of sidescan transducer array (m)
spd : ndarray, vessel speed (m/s)
time_s : ndarray, time elapsed (s)
caltime : ndarray, unix epoch time (s)
'''
# prompt user to supply file if no input file given
if not humfile:
print('An input file is required!!!!!!')
Tk().withdraw(
) # we don't want a full GUI, so keep the root window from appearing
humfile = askopenfilename(filetypes=[("DAT files", "*.DAT")])
# prompt user to supply directory if no input sonpath is given
if not sonpath:
print('A *.SON directory is required!!!!!!')
Tk().withdraw(
) # we don't want a full GUI, so keep the root window from appearing
sonpath = askdirectory()
# print given arguments to screen and convert data type where necessary
if humfile:
print('Input file is %s' % (humfile))
if sonpath:
print('Son files are in %s' % (sonpath))
if cs2cs_args:
print('cs2cs arguments are %s' % (cs2cs_args))
if draft:
draft = float(draft)
print('Draft: %s' % (str(draft)))
if c:
c = float(c)
print('Celerity of sound: %s m/s' % (str(c)))
if doplot:
doplot = int(doplot)
if doplot == 0:
print("Plots will not be made")
if flip_lr:
flip_lr = int(flip_lr)
if flip_lr == 1:
print("Port and starboard will be flipped")
if t:
t = np.asarray(t, float)
print('Transducer length is %s m' % (str(t)))
if bedpick:
bedpick = np.asarray(bedpick, int)
if bedpick == 1:
print('Bed picking is auto')
elif bedpick == 0:
print('Bed picking is manual')
else:
print('User will be prompted per chunk about bed picking method')
if chunk:
chunk = str(chunk)
if chunk[0] == 'd':
chunkmode = 1
chunkval = int(chunk[1:])
print('Chunks based on distance of %s m' % (str(chunkval)))
elif chunk[0] == 'p':
chunkmode = 2
chunkval = int(chunk[1:])
print('Chunks based on %s pings' % (str(chunkval)))
elif chunk[0] == 'h':
chunkmode = 3
chunkval = int(chunk[1:])
print('Chunks based on heading devation of %s degrees' %
(str(chunkval)))
elif chunk[0] == '1':
chunkmode = 4
chunkval = 1
print('Only 1 chunk will be produced')
else:
print(
"Chunk mode not understood - should be 'd', 'p', or 'h' - using defaults"
)
chunkmode = 1
chunkval = 100
print('Chunks based on distance of %s m' % (str(chunkval)))
if model:
try:
model = int(model)
print("Data is from the %s series" % (str(model)))
except:
if model == 'onix':
model = 0
print("Data is from the ONIX series")
elif model == 'helix':
model = 1
print("Data is from the HELIX series")
elif model == 'mega':
model = 2
print("Data is from the MEGA series")
# if cog:
# cog = int(cog)
# if cog==1:
# print "Heading based on course-over-ground"
if calc_bearing:
calc_bearing = int(calc_bearing)
if calc_bearing == 1:
print("Bearing will be calculated from coordinates")
if filt_bearing:
filt_bearing = int(filt_bearing)
if filt_bearing == 1:
print("Bearing will be filtered")
## for debugging
#humfile = r"test.DAT"; sonpath = "test_data"
#cs2cs_args = "epsg:26949"; doplot = 1; draft = 0
#c=1450; bedpick=1; fliplr=1; chunk = 'd100'
#model=998; cog=1; calc_bearing=0; filt_bearing=0
#if model==2:
# f = 1000
#else:
f = 455
try:
print(
"Checking the epsg code you have chosen for compatibility with Basemap ... "
)
from mpl_toolkits.basemap import Basemap
m = Basemap(projection='merc',
epsg=cs2cs_args.split(':')[1],
resolution='i',
llcrnrlon=10,
llcrnrlat=10,
urcrnrlon=30,
urcrnrlat=30)
del m
print("... epsg code compatible")
except (ValueError):
print(
"Error: the epsg code you have chosen is not compatible with Basemap"
)
print(
"please choose a different epsg code (http://spatialreference.org/)"
)
print("program will now close")
sys.exit()
# start timer
if os.name == 'posix': # true if linux/mac or cygwin on windows
start = time.time()
else: # windows
start = time.clock()
# if son path name supplied has no separator at end, put one on
if sonpath[-1] != os.sep:
sonpath = sonpath + os.sep
# get the SON files from this directory
sonfiles = glob.glob(sonpath + '*.SON')
if not sonfiles:
sonfiles = glob.glob(os.getcwd() + os.sep + sonpath + '*.SON')
base = humfile.split('.DAT') # get base of file name for output
base = base[0].split(os.sep)[-1]
# remove underscores, negatives and spaces from basename
base = humutils.strip_base(base)
print("WARNING: Because files have to be read in byte by byte,")
print("this could take a very long time ...")
#reading each sonfile in parallel should be faster ...
try:
o = Parallel(n_jobs=np.min([len(sonfiles), cpu_count()]), verbose=0)(
delayed(getscans)(sonfiles[k], humfile, c, model, cs2cs_args)
for k in range(len(sonfiles)))
X, Y, A, B = zip(*o)
for k in range(len(Y)):
if Y[k] == 'sidescan_port':
dat = A[k] #data.gethumdat()
metadat = B[k] #data.getmetadata()
if flip_lr == 0:
data_port = X[k].astype('int16')
else:
data_star = X[k].astype('int16')
elif Y[k] == 'sidescan_starboard':
if flip_lr == 0:
data_star = X[k].astype('int16')
else:
data_port = X[k].astype('int16')
elif Y[k] == 'down_lowfreq':
data_dwnlow = X[k].astype('int16')
elif Y[k] == 'down_highfreq':
data_dwnhi = X[k].astype('int16')
elif Y[k] == 'down_vhighfreq': #hopefully this only applies to mega systems
data_dwnhi = X[k].astype('int16')
del X, Y, A, B, o
old_pyread = 0
if 'data_port' not in locals():
data_port = ''
print("portside scan not available")
if 'data_star' not in locals():
data_star = ''
print("starboardside scan not available")
if 'data_dwnhi' not in locals():
data_dwnlow = ''
print("high-frq. downward scan not available")
if 'data_dwnlow' not in locals():
data_dwnlow = ''
print("low-frq. downward scan not available")
except: # revert back to older version if paralleleised version fails
print(
"something went wrong with the parallelised version of pyread ...")
try:
import pyread
except:
from . import pyread
data = pyread.pyread(sonfiles, humfile, c, model, cs2cs_args)
dat = data.gethumdat()
metadat = data.getmetadata()
old_pyread = 1
nrec = len(metadat['n'])
metadat['instr_heading'] = metadat['heading'][:nrec]
#metadat['heading'] = humutils.get_bearing(calc_bearing, filt_bearing, cog, metadat['lat'], metadat['lon'], metadat['instr_heading'])
try:
es = humutils.runningMeanFast(metadat['e'][:nrec],
len(metadat['e'][:nrec]) / 100)
ns = humutils.runningMeanFast(metadat['n'][:nrec],
len(metadat['n'][:nrec]) / 100)
except:
es = metadat['e'][:nrec]
ns = metadat['n'][:nrec]
metadat['es'] = es
metadat['ns'] = ns
try:
trans = pyproj.Proj(init=cs2cs_args)
except:
trans = pyproj.Proj(cs2cs_args.lstrip(), inverse=True)
lon, lat = trans(es, ns, inverse=True)
metadat['lon'] = lon
metadat['lat'] = lat
metadat['heading'] = humutils.get_bearing(calc_bearing, filt_bearing,
metadat['lat'], metadat['lon'],
metadat['instr_heading']) #cog
dist_m = humutils.get_dist(lat, lon)
metadat['dist_m'] = dist_m
if calc_bearing == 1: # recalculate speed, m/s
ds = np.gradient(np.squeeze(metadat['time_s']))
dx = np.gradient(np.squeeze(metadat['dist_m']))
metadat['spd'] = dx[:nrec] / ds[:nrec]
# theta at 3dB in the horizontal
theta3dB = np.arcsin(c / (t * (f * 1000)))
#resolution of 1 sidescan pixel to nadir
ft = (np.pi / 2) * (1 / theta3dB) #/ (f/455)
dep_m = humutils.get_depth(metadat['dep_m'][:nrec])
if old_pyread == 1: #older pyread version
# port scan
try:
if flip_lr == 0:
data_port = data.getportscans().astype('int16')
else:
data_port = data.getstarscans().astype('int16')
except:
data_port = ''
print("portside scan not available")
if data_port != '':
Zt, ind_port = makechunks_scan(chunkmode, chunkval, metadat, data_port,
0)
del data_port
## create memory mapped file for Z
shape_port = io.set_mmap_data(sonpath, base, '_data_port.dat', 'int16',
Zt)
##we are only going to access the portion of memory required
port_fp = io.get_mmap_data(sonpath, base, '_data_port.dat', 'int16',
shape_port)
if old_pyread == 1: #older pyread version
# starboard scan
try:
if flip_lr == 0:
data_star = data.getstarscans().astype('int16')
else:
data_star = data.getportscans().astype('int16')
except:
data_star = ''
print("starboardside scan not available")
if data_star != '':
Zt, ind_star = makechunks_scan(chunkmode, chunkval, metadat, data_star,
1)
del data_star
# create memory mapped file for Z
shape_star = io.set_mmap_data(sonpath, base, '_data_star.dat', 'int16',
Zt)
star_fp = io.get_mmap_data(sonpath, base, '_data_star.dat', 'int16',
shape_star)
if 'star_fp' in locals() and 'port_fp' in locals():
# check that port and starboard are same size
# and trim if not
if np.shape(star_fp) != np.shape(port_fp):
print(
"port and starboard scans are different sizes ... rectifying")
if np.shape(port_fp[0])[1] > np.shape(star_fp[0])[1]:
tmp = port_fp.copy()
tmp2 = np.empty_like(star_fp)
for k in range(len(tmp)):
tmp2[k] = tmp[k][:, :np.shape(star_fp[k])[1]]
del tmp
# create memory mapped file for Z
shape_port = io.set_mmap_data(sonpath, base, '_data_port2.dat',
'int16', tmp2)
#shape_star = shape_port.copy()
shape_star = tuple(np.asarray(shape_port).copy())
##we are only going to access the portion of memory required
port_fp = io.get_mmap_data(sonpath, base, '_data_port2.dat',
'int16', shape_port)
ind_port = list(ind_port)
ind_port[-1] = np.shape(star_fp[0])[1]
ind_port = tuple(ind_port)
elif np.shape(port_fp[0])[1] < np.shape(star_fp[0])[1]:
tmp = star_fp.copy()
tmp2 = np.empty_like(port_fp)
for k in range(len(tmp)):
tmp2[k] = tmp[k][:, :np.shape(port_fp[k])[1]]
del tmp
# create memory mapped file for Z
shape_port = io.set_mmap_data(sonpath, base, '_data_star2.dat',
'int16', tmp2)
#shape_star = shape_port.copy()
shape_star = tuple(np.asarray(shape_port).copy())
#we are only going to access the portion of memory required
star_fp = io.get_mmap_data(sonpath, base, '_data_star2.dat',
'int16', shape_star)
ind_star = list(ind_star)
ind_star[-1] = np.shape(port_fp[0])[1]
ind_star = tuple(ind_star)
if old_pyread == 1: #older pyread version
# low-freq. sonar
try:
data_dwnlow = data.getlowscans().astype('int16')
except:
data_dwnlow = ''
print("low-freq. scan not available")
if data_dwnlow != '':
Zt, ind_low = makechunks_scan(chunkmode, chunkval, metadat,
data_dwnlow, 2)
del data_dwnlow
# create memory mapped file for Z
shape_low = io.set_mmap_data(sonpath, base, '_data_dwnlow.dat',
'int16', Zt)
##we are only going to access the portion of memory required
dwnlow_fp = io.get_mmap_data(sonpath, base, '_data_dwnlow.dat',
'int16', shape_low)
if old_pyread == 1: #older pyread version
# hi-freq. sonar
try:
data_dwnhi = data.gethiscans().astype('int16')
except:
data_dwnhi = ''
print("high-freq. scan not available")
if data_dwnhi != '':
Zt, ind_hi = makechunks_scan(chunkmode, chunkval, metadat, data_dwnhi,
3)
del data_dwnhi
# create memory mapped file for Z
shape_hi = io.set_mmap_data(sonpath, base, '_data_dwnhi.dat', 'int16',
Zt)
dwnhi_fp = io.get_mmap_data(sonpath, base, '_data_dwnhi.dat', 'int16',
shape_hi)
if 'dwnhi_fp' in locals() and 'dwnlow_fp' in locals():
# check that low and high are same size
# and trim if not
if (np.shape(dwnhi_fp) != np.shape(dwnlow_fp)) and (chunkmode != 4):
print("dwnhi and dwnlow are different sizes ... rectifying")
if np.shape(dwnhi_fp[0])[1] > np.shape(dwnlow_fp[0])[1]:
tmp = dwnhi_fp.copy()
tmp2 = np.empty_like(dwnlow_fp)
for k in range(len(tmp)):
tmp2[k] = tmp[k][:, :np.shape(dwnlow_fp[k])[1]]
del tmp
# create memory mapped file for Z
shape_low = io.set_mmap_data(sonpath, base, '_data_dwnhi2.dat',
'int16', tmp2)
#shape_hi = shape_low.copy()
shape_hi = tuple(np.asarray(shape_low).copy())
##we are only going to access the portion of memory required
dwnhi_fp = io.get_mmap_data(sonpath, base, '_data_dwnhi2.dat',
'int16', shape_hi)
ind_hi = list(ind_hi)
ind_hi[-1] = np.shape(dwnlow_fp[0])[1]
ind_hi = tuple(ind_hi)
elif np.shape(dwnhi_fp[0])[1] < np.shape(dwnlow_fp[0])[1]:
tmp = dwnlow_fp.copy()
tmp2 = np.empty_like(dwnhi_fp)
for k in range(len(tmp)):
tmp2[k] = tmp[k][:, :np.shape(dwnhi_fp[k])[1]]
del tmp
# create memory mapped file for Z
shape_low = io.set_mmap_data(sonpath, base,
'_data_dwnlow2.dat', 'int16',
tmp2)
#shape_hi = shape_low.copy()
shape_hi = tuple(np.asarray(shape_low).copy())
##we are only going to access the portion of memory required
dwnlow_fp = io.get_mmap_data(sonpath, base,
'_data_dwnlow2.dat', 'int16',
shape_low)
ind_low = list(ind_low)
ind_low[-1] = np.shape(dwnhi_fp[0])[1]
ind_low = tuple(ind_low)
if old_pyread == 1: #older pyread version
del data
if ('shape_port' in locals()) and (chunkmode != 4):
metadat['shape_port'] = shape_port
nrec = metadat['shape_port'][0] * metadat['shape_port'][2]
elif ('shape_port' in locals()) and (chunkmode == 4):
metadat['shape_port'] = shape_port
nrec = metadat['shape_port'][1]
else:
metadat['shape_port'] = ''
if ('shape_star' in locals()) and (chunkmode != 4):
metadat['shape_star'] = shape_star
nrec = metadat['shape_star'][0] * metadat['shape_star'][2]
elif ('shape_star' in locals()) and (chunkmode == 4):
metadat['shape_star'] = shape_star
nrec = metadat['shape_star'][1]
else:
metadat['shape_star'] = ''
if ('shape_hi' in locals()) and (chunkmode != 4):
metadat['shape_hi'] = shape_hi
#nrec = metadat['shape_hi'][0] * metadat['shape_hi'][2] * 2
elif ('shape_hi' in locals()) and (chunkmode == 4):
metadat['shape_hi'] = shape_hi
else:
metadat['shape_hi'] = ''
if ('shape_low' in locals()) and (chunkmode != 4):
metadat['shape_low'] = shape_low
#nrec = metadat['shape_low'][0] * metadat['shape_low'][2] * 2
elif ('shape_low' in locals()) and (chunkmode == 4):
metadat['shape_low'] = shape_low
else:
metadat['shape_low'] = ''
#make kml boat trackline
humutils.make_trackline(lon, lat, sonpath, base)
if 'port_fp' in locals() and 'star_fp' in locals():
#if not os.path.isfile(os.path.normpath(os.path.join(sonpath,base+'meta.mat'))):
if 2 > 1:
if bedpick == 1: # auto
x, bed = humutils.auto_bedpick(ft, dep_m, chunkmode, port_fp,
c)
if len(dist_m) < len(bed):
dist_m = np.append(
dist_m, dist_m[-1] * np.ones(len(bed) - len(dist_m)))
if doplot == 1:
if chunkmode != 4:
for k in range(len(star_fp)):
plot_2bedpicks(
port_fp[k], star_fp[k],
bed[ind_port[-1] * k:ind_port[-1] * (k + 1)],
dist_m[ind_port[-1] * k:ind_port[-1] *
(k + 1)],
x[ind_port[-1] * k:ind_port[-1] * (k + 1)], ft,
shape_port, sonpath, k, chunkmode)
else:
plot_2bedpicks(port_fp, star_fp, bed, dist_m, x, ft,
shape_port, sonpath, 0, chunkmode)
# 'real' bed is estimated to be the minimum of the two
bed = np.min(np.vstack((bed[:nrec], np.squeeze(x[:nrec]))),
axis=0)
bed = humutils.runningMeanFast(bed, 3)
elif bedpick > 1: # user prompt
x, bed = humutils.auto_bedpick(ft, dep_m, chunkmode, port_fp,
c)
if len(dist_m) < len(bed):
dist_m = np.append(
dist_m, dist_m[-1] * np.ones(len(bed) - len(dist_m)))
# 'real' bed is estimated to be the minimum of the two
bed = np.min(np.vstack((bed[:nrec], np.squeeze(x[:nrec]))),
axis=0)
bed = humutils.runningMeanFast(bed, 3)
# manually intervene
fig = plt.figure()
ax = plt.gca()
if chunkmode != 4:
im = ax.imshow(np.hstack(port_fp),
cmap='gray',
origin='upper')
else:
im = ax.imshow(port_fp, cmap='gray', origin='upper')
plt.plot(bed, 'r')
plt.axis('normal')
plt.axis('tight')
pts1 = plt.ginput(
n=300,
timeout=30) # it will wait for 200 clicks or 60 seconds
x1 = map(lambda x: x[0],
pts1) # map applies the function passed as
y1 = map(lambda x: x[1],
pts1) # first parameter to each element of pts
plt.close()
del fig
if x1 != []: # if x1 is not empty
tree = KDTree(zip(np.arange(1, len(bed)), bed))
try:
dist, inds = tree.query(zip(x1, y1),
k=100,
eps=5,
n_jobs=-1)
except:
dist, inds = tree.query(zip(x1, y1), k=100, eps=5)
b = np.interp(inds, x1, y1)
bed2 = bed.copy()
bed2[inds] = b
bed = bed2
if doplot == 1:
if chunkmode != 4:
for k in range(len(star_fp)):
plot_2bedpicks(
port_fp[k], star_fp[k],
bed[ind_port[-1] * k:ind_port[-1] * (k + 1)],
dist_m[ind_port[-1] * k:ind_port[-1] *
(k + 1)],
x[ind_port[-1] * k:ind_port[-1] * (k + 1)], ft,
shape_port, sonpath, k, chunkmode)
else:
plot_2bedpicks(port_fp, star_fp, bed, dist_m, x, ft,
shape_port, sonpath, 0, chunkmode)
else: #manual
beds = []
if chunkmode != 4:
for k in range(len(port_fp)):
raw_input(
"Bed picking " + str(k + 1) + " of " +
str(len(port_fp)) +
", are you ready? 30 seconds. Press Enter to continue..."
)
bed = {}
fig = plt.figure()
ax = plt.gca()
im = ax.imshow(port_fp[k], cmap='gray', origin='upper')
pts1 = plt.ginput(
n=300, timeout=30
) # it will wait for 200 clicks or 60 seconds
x1 = map(lambda x: x[0],
pts1) # map applies the function passed as
y1 = map(
lambda x: x[1],
pts1) # first parameter to each element of pts
bed = np.interp(np.r_[:ind_port[-1]], x1, y1)
plt.close()
del fig
beds.append(bed)
extent = np.shape(port_fp[k])[0]
bed = np.asarray(np.hstack(beds), 'float')
else:
raw_input(
"Bed picking - are you ready? 30 seconds. Press Enter to continue..."
)
bed = {}
fig = plt.figure()
ax = plt.gca()
im = ax.imshow(port_fp, cmap='gray', origin='upper')
pts1 = plt.ginput(
n=300, timeout=30
) # it will wait for 200 clicks or 60 seconds
x1 = map(lambda x: x[0],
pts1) # map applies the function passed as
y1 = map(lambda x: x[1],
pts1) # first parameter to each element of pts
bed = np.interp(np.r_[:ind_port[-1]], x1, y1)
plt.close()
del fig
beds.append(bed)
extent = np.shape(port_fp)[1]
bed = np.asarray(np.hstack(beds), 'float')
# now revise the depth in metres
dep_m = (1 / ft) * bed
if doplot == 1:
if chunkmode != 4:
for k in range(len(star_fp)):
plot_bedpick(
port_fp[k], star_fp[k], (1 / ft) *
bed[ind_port[-1] * k:ind_port[-1] * (k + 1)],
dist_m[ind_port[-1] * k:ind_port[-1] * (k + 1)],
ft, shape_port, sonpath, k, chunkmode)
else:
plot_bedpick(port_fp, star_fp, (1 / ft) * bed, dist_m, ft,
shape_port, sonpath, 0, chunkmode)
metadat['bed'] = bed[:nrec]
else:
metadat['bed'] = dep_m[:nrec] * ft
metadat['heading'] = metadat['heading'][:nrec]
metadat['lon'] = lon[:nrec]
metadat['lat'] = lat[:nrec]
metadat['dist_m'] = dist_m[:nrec]
metadat['dep_m'] = dep_m[:nrec]
metadat['pix_m'] = 1 / ft
metadat['bed'] = metadat['bed'][:nrec]
metadat['c'] = c
metadat['t'] = t
if model == 2:
metadat['f'] = f * 2
else:
metadat['f'] = f
metadat['spd'] = metadat['spd'][:nrec]
metadat['time_s'] = metadat['time_s'][:nrec]
metadat['e'] = metadat['e'][:nrec]
metadat['n'] = metadat['n'][:nrec]
metadat['es'] = metadat['es'][:nrec]
metadat['ns'] = metadat['ns'][:nrec]
try:
metadat['caltime'] = metadat['caltime'][:nrec]
except:
metadat['caltime'] = metadat['caltime']
savemat(os.path.normpath(os.path.join(sonpath, base + 'meta.mat')),
metadat,
oned_as='row')
f = open(os.path.normpath(os.path.join(sonpath, base + 'rawdat.csv')),
'wt')
writer = csv.writer(f)
writer.writerow(
('longitude', 'latitude', 'easting', 'northing', 'depth (m)',
'distance (m)', 'instr. heading (deg)', 'heading (deg.)'))
for i in range(0, nrec):
writer.writerow(
(float(lon[i]), float(lat[i]), float(es[i]), float(ns[i]),
float(dep_m[i]), float(dist_m[i]),
float(metadat['instr_heading'][i]), float(metadat['heading'][i])))
f.close()
del lat, lon, dep_m #, dist_m
if doplot == 1:
plot_pos(sonpath, metadat, es, ns)
if 'dwnlow_fp' in locals():
plot_dwnlow(dwnlow_fp, chunkmode, sonpath)
if 'dwnhi_fp' in locals():
plot_dwnhi(dwnhi_fp, chunkmode, sonpath)
if os.name == 'posix': # true if linux/mac
elapsed = (time.time() - start)
else: # windows
elapsed = (time.clock() - start)
print("Processing took " + str(elapsed) + "seconds to analyse")
print("Done!")
print("===================================================")
def compute_invariants(self):
"""
Computes the invariants according to Weaver et al., [2000, 2003]
Mostly used to plot Mohr's circles
In a 1D case: rho = mu (inv1**2+inv2**2)/w & phi = arctan(inv2/inv1)
Sets the invariants as attributes:
**inv1** : real off diaganol part normalizing factor
**inv2** : imaginary off diaganol normalizing factor
**inv3** : real anisotropy factor (range from [0,1])
**inv4** : imaginary anisotropy factor (range from [0,1])
**inv5** : suggests electric field twist
**inv6** : suggests in phase small scale distortion
**inv7** : suggests 3D structure
**strike** : strike angle (deg) assuming positive clockwise 0=N
**strike_err** : strike angle error (deg)
**q** : dependent variable suggesting dimensionality
"""
# get the length of z to initialize some empty arrays
nz = self.z.shape[0]
# set some empty arrays to put stuff into
self.inv1 = np.zeros(nz)
self.inv2 = np.zeros(nz)
self.inv3 = np.zeros(nz)
self.inv4 = np.zeros(nz)
self.inv5 = np.zeros(nz)
self.inv6 = np.zeros(nz)
self.inv7 = np.zeros(nz)
self.q = np.zeros(nz)
self.strike = np.zeros(nz)
self.strike_err = np.zeros(nz)
c_tf = self.z.all() == 0.0
if c_tf == True:
return
# loop over each freq
for ii in range(nz):
# compute the mathematical invariants
x1 = .5 * (self.z[ii, 0, 0].real + self.z[ii, 1, 1].real) # trace
x2 = .5 * (self.z[ii, 0, 1].real + self.z[ii, 1, 0].real)
x3 = .5 * (self.z[ii, 0, 0].real - self.z[ii, 1, 1].real)
x4 = .5 * (self.z[ii, 0, 1].real - self.z[ii, 1, 0].real) # berd
e1 = .5 * (self.z[ii, 0, 0].imag + self.z[ii, 1, 1].imag) # trace
e2 = .5 * (self.z[ii, 0, 1].imag + self.z[ii, 1, 0].imag)
e3 = .5 * (self.z[ii, 0, 0].imag - self.z[ii, 1, 1].imag)
e4 = .5 * (self.z[ii, 0, 1].imag - self.z[ii, 1, 0].imag) # berd
ex = x1 * e1 - x2 * e2 - x3 * e3 + x4 * e4
if ex == 0.0:
print 'Could not compute invariants for {0:5e} Hz'.format(
self.freq[ii])
self.inv1[ii] = np.nan
self.inv2[ii] = np.nan
self.inv3[ii] = np.nan
self.inv4[ii] = np.nan
self.inv5[ii] = np.nan
self.inv6[ii] = np.nan
self.inv7[ii] = np.nan
self.q[ii] = np.nan
self.strike[ii] = np.nan
self.strike_err[ii] = np.nan
else:
d12 = (x1 * e2 - x2 * e1) / ex
d34 = (x3 * e4 - x4 * e3) / ex
d13 = (x1 * e3 - x3 * e1) / ex
d24 = (x2 * e4 - x4 * e2) / ex
d41 = (x4 * e1 - x1 * e4) / ex
d23 = (x2 * e3 - x3 * e2) / ex
inv1 = np.sqrt(x4**2 + x1**2)
inv2 = np.sqrt(e4**2 + e1**2)
inv3 = np.sqrt(x2**2 + x3**2) / inv1
inv4 = np.sqrt(e2**2 + e3**2) / inv2
s41 = (x4 * e1 + x1 * e4) / ex
inv5 = s41 * ex / (inv1 * inv2)
inv6 = d41 * ex / (inv1 * inv2)
q = np.sqrt((d12 - d34)**2 + (d13 + d24)**2)
inv7 = (d41 - d23) / q
# if abs(inv7)>1.0:
# print("debug value inv7=", inv7)
strikeang = .5 * np.arctan2(d12 - d34,
d13 + d24) * (180 / np.pi)
strikeangerr = abs(.5 * np.arcsin(inv7)) * (180 / np.pi)
self.inv1[ii] = inv1
self.inv2[ii] = inv2
self.inv3[ii] = inv3
self.inv4[ii] = inv4
self.inv5[ii] = inv5
self.inv6[ii] = inv6
self.inv7[ii] = inv7
self.q[ii] = q
self.strike[ii] = strikeang
self.strike_err[ii] = strikeangerr
# *************************************
# - Physics -
# All measures are nondimensionalized at last instance with crankpin length.
# Measures are irrelevant, so crankpink length has been taken as unitary.
# Step between axis.
theta = np.array([i * 2. * np.pi / n for i in range(1, n)])
# Secondary rods connection points radius.
e = (1. - m) / (1. - (l * np.sin(theta)) ** 2 / 2.)
# Angle between primary rod and primary piston to secondary rods connection
# points line.
delta = np.arcsin(np.sin(theta) * np.sqrt(1. - 2. * np.cos(theta) /
e + e ** -2) ** -1)
# The same mission as l for secondary pistons.
lb = l * np.sin(theta) * (m * np.sin(theta + delta)) ** -1
# Special point needed for secondary pistons position calculation.
OM = np.sin(theta) * (np.sin(theta + delta)) ** -1
# Cranckshaft rotation angle.
alpha = np.linspace(0., 2. * np.pi, frames // loops)
# Angle between primary rod and vertical axis.
betha = np.arcsin(l * np.sin(alpha))
def Bp(alpha):
def elevation(times, latitudes, longitudes):
"""
Elevation angle of the sun in local solar time. Zero at horizon.
Parameters
----------
times: pd.DatetimeIndex
Corresponding timestamps, must be localized or UTC will be assumed.
latitudes: numeric
Latitude(s) in radians.
longitudes: numeric
Longitude(s) in radians.
Returns
-------
elevation: numeric
Elevation angle in local solar time in radians.
References
----------
pvlib.solarposition.ephemeris
"""
# ===== COPY PASTED =====
Abber = 20 / 3600.
# The SPA algorithm needs time to be expressed in terms of
# decimal UTC hours of the day of the year.
# If localized, convert to UTC. otherwise, assume UTC.
try:
time_utc = times.tz_convert('UTC')
except TypeError:
time_utc = times
# Strip out the day of the year and calculate the decimal hour
DayOfYear = time_utc.dayofyear
DecHours = (time_utc.hour + time_utc.minute / 60. +
time_utc.second / 3600. + time_utc.microsecond / 3600.e6)
# np.array needed for pandas > 0.20
UnivDate = np.array(DayOfYear)
UnivHr = np.array(DecHours)
Yr = np.array(time_utc.year) - 1900
YrBegin = 365 * Yr + np.floor((Yr - 1) / 4.) - 0.5
Ezero = YrBegin + UnivDate
T = Ezero / 36525.
# Calculate Greenwich Mean Sidereal Time (GMST)
GMST0 = 6 / 24. + 38 / 1440. + (45.836 + 8640184.542 * T +
0.0929 * T**2) / 86400.
GMST0 = 360 * (GMST0 - np.floor(GMST0))
GMSTi = np.mod(GMST0 + 360 * (1.0027379093 * UnivHr / 24.), 360)
# =======================
# Local apparent sidereal time
LocAST = (360 + GMSTi.reshape(-1, 1) +
np.degrees(longitudes).reshape(1, -1)) % 360
# ===== COPY PASTED =====
EpochDate = Ezero + UnivHr / 24.
T1 = EpochDate / 36525.
ObliquityR = np.radians(23.452294 - 0.0130125 * T1 - 1.64e-06 * T1**2 +
5.03e-07 * T1**3)
MlPerigee = 281.22083 + 4.70684e-05 * EpochDate + 0.000453 * T1**2 + (
3e-06 * T1**3)
MeanAnom = np.mod((358.47583 + 0.985600267 * EpochDate - 0.00015 * T1**2 -
3e-06 * T1**3), 360)
Eccen = 0.01675104 - 4.18e-05 * T1 - 1.26e-07 * T1**2
EccenAnom = MeanAnom
E = 0
while np.max(abs(EccenAnom - E)) > 0.0001:
E = EccenAnom
EccenAnom = MeanAnom + np.degrees(Eccen) * np.sin(np.radians(E))
TrueAnom = (2 * np.mod(
np.degrees(
np.arctan2(((1 + Eccen) / (1 - Eccen))**0.5 *
np.tan(np.radians(EccenAnom) / 2.), 1)), 360))
EcLon = np.mod(MlPerigee + TrueAnom, 360) - Abber
EcLonR = np.radians(EcLon)
DecR = np.arcsin(np.sin(ObliquityR) * np.sin(EcLonR))
RtAscen = np.degrees(
np.arctan2(np.cos(ObliquityR) * np.sin(EcLonR), np.cos(EcLonR)))
# =======================
HrAngleR = np.radians(LocAST - RtAscen.reshape(-1, 1))
DecR = DecR.reshape(-1, 1)
LatR = latitudes.reshape(1, -1)
return np.arcsin(
np.cos(DecR) * np.cos(HrAngleR) * np.cos(LatR) +
np.sin(DecR) * np.sin(LatR))
def propagate(dt):
global state, inp, wind, param, forces, moments, gusts, num1, num2, num3, den1, den2, den3, t0, V_a, abp, beta,\
alpha, pub_h, pub_va, pub_phi, pub_chi
s = np.copy(state)
t0 += dt
# print state, "state starting dynamics"
g = 9.81
m = param['m']
b = param['b']
c = param['c']
S = param['S']
rho = param['rho']
p_n = s[0][0]
p_e = s[1][0]
p_d = s[2][0]
u = s[3][0]
v = s[4][0]
w = s[5][0]
phi = s[6][0]
theta = s[7][0]
psi = s[8][0]
p = s[9][0]
q = s[10][0]
r = s[11][0]
origin, xaxis, yaxis, zaxis = (0., 0., 0.), (1., 0., 0.), (0., 1.,
0.), (0., 0.,
1.)
Rx = rotation_matrix(phi, xaxis)
Ry = rotation_matrix(theta, yaxis)
Rz = rotation_matrix(psi, zaxis)
R_V_B = concatenate_matrices(Rx, Ry, Rz)
R_V_B = R_V_B[0:3, 0:3]
wind_body = np.transpose(R_V_B) * np.matrix(np.copy(wind))
###
# low alt, light turb
alt = 50.0
Lu = 200.0
Lv = 200.0
Lw = 50.0
sigma_u = 1.06
sigma_v = 1.06
sigma_w = 0.7
num1 = sigma_u * np.sqrt(2.0 * V_a / Lu)
den1 = [1.0, V_a / Lu]
num2 = [
sigma_v * np.sqrt(3.0 * V_a / Lv),
sigma_v * np.sqrt(3.0 * V_a / Lv) * (V_a / np.sqrt(3.0 * Lv))
]
den2 = [1.0, 2.0 * V_a / Lv, (V_a / Lv)**2.0]
num3 = [
sigma_w * np.sqrt(3.0 * V_a / Lw),
sigma_w * np.sqrt(3.0 * V_a / Lw) * (V_a / np.sqrt(3.0 * Lw))
]
den3 = [1.0, 2.0 * V_a / Lw, (V_a / Lw)**2.0]
time_series = np.linspace(0, t0)
#print np.shape(time_series)
input_series = np.random.randn(np.size(time_series), 1) * 0.1
#print time_series, input_series
_, y1, _ = signal.lsim((num1, den1), input_series, time_series)
_, y2, _ = signal.lsim((num2, den2), input_series, time_series)
_, y3, _ = signal.lsim((num3, den3), input_series, time_series)
gusts[0][0] = y1[-1] / 10000.0
gusts[1][0] = y2[-1] / 10000.0
gusts[2][0] = y3[-1] / 10000.0
#print gusts
# if p_d > -alt:
# gusts = [[0.0],[0.0],[0.0]]
V_r = [[u - wind_body[0, 0] + gusts[0][0]],
[v - wind_body[1, 0] + gusts[1][0]],
[w - wind_body[2, 0] + gusts[2][0]]]
# V_r = [[u],[v],[w]]
# V_a = 25.0
V_a = np.sqrt((V_r[0][0])**2.0 + V_r[1][0]**2.0 + V_r[2][0]**2.0)
# print V_a, "printing V_a in propagation"
alpha = np.arctan2(V_r[2][0], V_r[0][0])
# alpha,beta,_ = abp
beta = np.arcsin(V_r[1][0] / V_a)
# alpha = np.arctan2(w,u)
# beta = np.arcsin(v/V_a)
Vn = V_a * np.cos(psi) + wind[0][0]
Ve = V_a * np.sin(psi) + wind[1][0]
course = np.arctan2(Ve, Vn)
sphi = np.sin(phi)
cphi = np.cos(phi)
stheta = np.sin(theta)
ctheta = np.cos(theta)
spsi = np.sin(psi)
cpsi = np.cos(psi)
ttheta = np.tan(theta)
secth = 1.0 / ctheta
calpha = np.cos(alpha)
salpha = np.sin(alpha)
C_D_alpha = param['C_D_0'] + param['C_D_alpha'] * alpha
C_L_alpha = param['C_L_0'] + param['C_L_alpha'] * alpha
C_X = -C_D_alpha * calpha + C_L_alpha * salpha
C_X_q = -param['C_D_q'] * calpha + param['C_L_q'] * salpha
C_X_delta_e = -param['C_D_delta_e'] * calpha + param['C_L_delta_e'] * salpha
C_Z = -C_D_alpha * salpha - C_L_alpha * calpha
C_Z_q = -param['C_D_q'] * salpha - param['C_L_q'] * calpha
C_Z_delta_e = -param['C_D_delta_e'] * salpha - param['C_L_delta_e'] * calpha
forces = np.copy(np.matrix([[-m*g*stheta],[m*g*ctheta*sphi],[m*g*ctheta*cphi]]) + \
0.5*rho*(V_a**2.0)*S*np.matrix([[C_X + C_X_q*c*q/(2.0*V_a)+ C_X_delta_e*inp[1][0]],\
[param['C_Y_0'] + param['C_Y_beta']*beta + param['C_Y_p']*b*p/(2.0*V_a) + param['C_Y_r']*b*r/(2.0*V_a) + param['C_Y_delta_a']*inp[0][0] + param['C_Y_delta_r']*inp[2][0]],\
[C_Z + C_Z_q*c*q/(2.0*V_a) + C_Z_delta_e*inp[1][0]]])\
+ 0.5*rho*param['S_prop']*param['C_prop']*np.matrix([[(param['k_motor']*inp[3][0])**2.0 - V_a**2.0],[0.],[0.]]))
moments = np.copy(0.5*rho*(V_a**2.0)*S*np.matrix([[b*(param['C_l_0'] + param['C_l_p']*b*p/(2.0*V_a) + param['C_l_r']*b*r/(2*V_a) + param['C_l_delta_a']*inp[0][0] +\
param['C_l_delta_r']*inp[2][0])],\
[c*(param['C_m_0'] + param['C_m_alpha']*alpha + param['C_m_q']*c*q/(2.0*V_a) + param['C_m_delta_e']*inp[1][0])],\
[b*(param['C_n_0'] + param['C_n_beta']*beta + param['C_n_p']*b*p/(2.0*V_a) + param['C_n_r']*b*r/(2.0*V_a) + param['C_n_delta_a']*inp[0][0] + \
param['C_n_delta_r']*inp[2][0])]]) +\
np.matrix([[-param['k_T_p']*((param['k_Omega']*inp[3][0])**2.0)],[0.0],[0.0]]))
# RK4 integration
k1 = dynamics(s)
k2 = dynamics(s + dt / 2.0 * k1)
k3 = dynamics(s + dt / 2.0 * k2)
k4 = dynamics(s + dt / 2.0 * k3)
temp = k1 + 2.0 * k2 + 2.0 * k3 + k4
# temp[6][0] = wrap(temp[6][0])
# temp[7][0] = wrap(temp[7][0])
# temp[8][0] = wrap(temp[8][0])
s += dt / 6.0 * temp
# s[6][0] = wrap(s[6][0])
# s[7][0] = wrap(s[7][0])
# s[8][0] = wrap(s[8][0])
print s[6][0], s[2][0]
pub_h.publish(s[2][0])
pub_va.publish(V_a)
pub_phi.publish(s[6][0])
pub_chi.publish(course)
state = np.copy(s)
def beam_grid(nbi,
rstart,
nx=None,
ny=None,
nz=None,
dv=8.0,
length=100.0,
width=80.0,
height=80.0):
"""
#+#beam_grid
#+ Calculates settings for a grid that aligns with the neutral beam.
#+***
#+##Arguments
#+ **nbi**: [Neutral beam geometry structure](|url|/page/03_technical/01_prefida_inputs.html#neutral-beam-geometry-structure)
#+
#+ **rstart**: Radial start position of beam grid [cm]
#+
#+##Keyword Arguments
#+ **dV**: Cell volume [\(cm^3\)]: Defaults to 8.0
#+
#+ **nx**: Number of cells in length: Default determined by `dV`
#+
#+ **ny**: Number of cells in width: Default determined by `dV`
#+
#+ **nz**: Number of cells in height: Default determined by `dV`
#+
#+ **length**: Length of grid along beam sightline. [cm]: Defaults to 100 cm
#+
#+ **width**: Width of grid [cm]: Defaults to 100 cm
#+
#+ **height**: Height of grid [cm]: Defaults 80 cm
#+
#+##Return Value
#+ Structure containing beam grid settings suitable for the Namelist File
#+
#+##Example Usage
#+```python
#+>>> grid = beam_grid(nbi,200.0,nx=100,ny=50,nz=50,length=100,width=50,height=50)
#+```
"""
if width < nbi['widy']:
warn("Grid width is smaller then the source width")
print("width: {}".format(width))
print("source width: {}".format(nbi['widy']))
if height < nbi['widz']:
warn("Grid height is smaller then the source height")
print("height: {}".format(height))
print("source height: {}".format(nbi['widz']))
dv3 = dv**(1. / 3.)
if nx is None:
nx = round(length / dv3)
if ny is None:
ny = round(width / dv3)
if nz is None:
nz = round(height / dv3)
xmin = 0.
xmax = length
ymin = -width / 2.
ymax = width / 2.
zmin = -height / 2.
zmax = height / 2.
src = nbi['src']
axis = nbi['axis'] / np.sqrt(np.sum(nbi['axis']**2))
pos = src + 100. * axis
if np.sqrt(src[0]**2 + src[1]**2) < rstart:
error("Source radius cannot be less then rstart", halt=True)
dis = np.sqrt(np.sum((src - pos)**2.0))
beta = np.arcsin((src[2] - pos[2]) / dis)
alpha = np.arctan2((pos[1] - src[1]), (pos[0] - src[0]))
gamma = 0.
a = axis[0]**2 + axis[1]**2
b = 2. * (src[0] * axis[0] + src[1] * axis[1])
c = src[0]**2 + src[1]**2 - rstart**2
t = (-b - np.sqrt(b**2 - 4. * a * c)) / (2. * a)
origin = src + t * axis
beam_grid = {
'nx': nx,
'ny': ny,
'nz': nz,
'xmin': xmin,
'xmax': xmax,
'ymin': ymin,
'ymax': ymax,
'zmin': zmin,
'zmax': zmax,
'alpha': alpha,
'beta': beta,
'gamma': gamma,
'origin': origin
}
return beam_grid
#30? different angles are used --->x
energy = 9.5 #10.72 #keV
wavelength = 1.23984E-9 / energy
pixel_det = 55E-6 # Pixel ctr to ctr distance (w) [m] #Pilatus
z = 1
#def calculate_dq3():
# lattice constant Ga(0.51In(0.49)P
#lattice_constant_a = 5.653E-10
# lattice constant InP
lattice_constant_a = 5.8687E-10
# distance between planes (111), correct?
d = lattice_constant_a / np.sqrt(3)
q_abs = 2 * np.pi / d
#q_abs_alt = 4*np.pi *np.sin(theta)/wavelength
theta = np.arcsin(q_abs * wavelength / (4 * np.pi))
theta = theta * 180 / np.pi
np.disp('Theta [o]:')
np.disp(theta)
dtheta = wavelength / (4 * (500E-9) * np.sin(theta * np.pi / 180))
dtheta = dtheta * 180 / np.pi
print 'dtheta [o]'
np.disp(dtheta)
# reconvert to radians for later calcultations
theta = theta * np.pi / 180
dtheta = dtheta * np.pi / 180
# return dq
# define pixel sizes in reciprocal space
dq1 = 2 * np.pi * pixel_det / (wavelength * z)