def ICeuklid_to_ICcircle(IC):
"""
converts from IC in euklidean space to IC in circle parameters (rotational invariant).
The formats are:
IC_euklid: [x, y, z, vx, vy, vz]
IC_circle: [y, vy, |v|, |l|, phiv], where |v| is the magnitude of CoM velocity, |l|
is the distance from leg1 (assumed to be at [0,0,0]) to CoM, and phiv the angle
of the velocity in horizontal plane wrt x-axis
*NOTE* for re-conversion, the leg position is additionally required
:args:
IC (6x float): the initial conditions in euklidean space
:returns:
IC (5x float): the initial conditions in circular coordinates
"""
x, y, z, vx, vy, vz = IC
v = sqrt(vx**2 + vy**2 + vz**2)
l = sqrt(x**2 + y**2 + z**2)
#phiv = arctan2(vz, vx)
#phiv = arctan2(-vz, vx)
phiv = -arctan2(-vz, vx)
#phix = arctan2(-z, -x)
phix = arctan2(z, -x)
# warnings.warn('TODO: fix phi_x (add)')
# print "phix:", phix * 180 / pi
return [y, vy, v, l, phiv + phix]
def haversine (latlong1, latlong2, r):
deltaLatlong = latlong1 - latlong2
dLat = deltaLatlong[0]
dLon = deltaLatlong[1]
lat1 = latlong1[0]
lat2 = latlong2[0]
a = (sin (dLat/2) * sin (dLat/2) +
sin (dLon/2) * sin (dLon/2) * cos (lat1) * cos (lat2))
c = 2 * arctan2 (sqrt (a), sqrt (1-a))
d = r * c
# initial bearing
y = sin (dLon) * cos (lat2)
x = (cos (lat1)*sin (lat2) -
sin (lat1)*cos (lat2)*cos (dLon))
b1 = arctan2 (y, x);
# final bearing
dLon = -dLon
dLat = -dLat
tmp = lat1
lat1 = lat2
lat2 = tmp
y = sin (dLon) * cos (lat2)
x = (cos (lat1) * sin (lat2) -
sin (lat1) * cos (lat2) * cos (dLon))
b2 = arctan2 (y, x)
b2 = mod ((b2 + pi), 2*pi)
return (d, b1, b2)
def ICeuklid_to_ICcircle(IC):
"""
converts from IC in euklidean space to IC in circle parameters (rotational invariant).
The formats are:
IC_euklid: [x, y, z, vx, vy, vz]
IC_circle: [y, vy, |v|, |l|, phiv], where |v| is the magnitude of CoM velocity, |l|
is the distance from leg1 (assumed to be at [0,0,0]) to CoM, and phiv the angle
of the velocity in horizontal plane wrt x-axis
*NOTE* for re-conversion, the leg position is additionally required
:args:
IC (6x float): the initial conditions in euklidean space
:returns:
IC (5x float): the initial conditions in circular coordinates
"""
x,y,z,vx,vy,vz = IC
v = sqrt(vx**2 + vy**2 + vz**2)
l = sqrt(x**2 + y**2 + z**2)
#phiv = arctan2(vz, vx)
#phiv = arctan2(-vz, vx)
phiv = -arctan2(-vz, vx)
#phix = arctan2(-z, -x)
phix = arctan2(z, -x)
# warnings.warn('TODO: fix phi_x (add)')
# print "phix:", phix * 180 / pi
return [y, vy, v, l, phiv + phix]
def haversine(latlong1, latlong2, r):
deltaLatlong = latlong1 - latlong2
dLat = deltaLatlong[0]
dLon = deltaLatlong[1]
lat1 = latlong1[0]
lat2 = latlong2[0]
a = (sin(dLat / 2) * sin(dLat / 2) +
sin(dLon / 2) * sin(dLon / 2) * cos(lat1) * cos(lat2))
c = 2 * arctan2(sqrt(a), sqrt(1 - a))
d = r * c
# initial bearing
y = sin(dLon) * cos(lat2)
x = (cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon))
b1 = arctan2(y, x)
# final bearing
dLon = -dLon
dLat = -dLat
tmp = lat1
lat1 = lat2
lat2 = tmp
y = sin(dLon) * cos(lat2)
x = (cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon))
b2 = arctan2(y, x)
b2 = mod((b2 + pi), 2 * pi)
return (d, b1, b2)
def eq_to_hor_wikipedia(ra, dec, lat, lst):
H = lst - ra
sin_a = sin(lat) * sin(dec) + cos(lat) * cos(dec) * cos(H)
cos_A_cos_a = cos(lat) * sin(dec) - sin(lat) * cos(dec) * cos(H)
sin_A_cos_a = -cos(dec) * sin(H)
az = arctan2(sin_A_cos_a, cos_A_cos_a)
r = sqrt(sin_A_cos_a**2 + cos_A_cos_a**2)
el = arctan2(sin_a, r)
return az, el
def pos2angles(self, armPos, armLen):
x = armPos[0]
y = armPos[1]
l1 = armLen[0]
l2 = armLen[1]
elang = abs(2*arctan(sqrt(((l1 + l2)**2 - (x**2 + y**2))/((x**2 + y**2) - (l1 - l2)**2))));
phi = arctan2(y,x);
psi = arctan2(l2 * sin(elang), l1 + (l2 * cos(elang)));
shang = phi - psi;
return [shang,elang]
def haversine(location1, location2=None): # calculates great circle distance
__doc__ = """Returns the great circle distance of the given
coordinates.
INPUT: location1 = ((lat1, lon1), ..., n(lat1, lon1))
*location2 = ((lat2, lon2), ..., n(lat2, lon2))
*if location2 is not given a square matrix of distances
for location1 will be put out
OUTPUT: distance in km
(dist1 ... ndist
: :
ndist1 ... ndist)
shape will depend on the input
METHOD: a = sin(dLat / 2) * sin(dLat / 2) +
sin(dLon / 2) * sin(dLon / 2) *
cos(lat1) * cos(lat2)
c = 2 * arctan2(sqrt(a), sqrt(1 - a))
d = R * c
where R is the earth's radius (6371 km)
and d is the distance in km"""
from itertools import product, combinations
from pylab import deg2rad, sin, cos, arctan2, \
meshgrid, sqrt, array, arange
if location2:
location1 = array(location1, ndmin=2)
location2 = array(location2, ndmin=2)
elif location2 is None:
location1 = array(location1, ndmin=2)
location2 = location1.copy()
# get all combinations using indicies
ind1 = arange(location1.shape[0])
ind2 = arange(location2.shape[0])
ind = array(list(product(ind1, ind2)))
# using combination inds to get lats and lons
lat1, lon1 = location1[ind[:,0]].T
lat2, lon2 = location2[ind[:,1]].T
# setting up variables for haversine
R = 6371.
dLat = deg2rad(lat2 - lat1)
dLon = deg2rad(lon2 - lon1)
lat1 = deg2rad(lat1)
lat2 = deg2rad(lat2)
# haversine formula
a = sin(dLat / 2) * sin(dLat / 2) + \
sin(dLon / 2) * sin(dLon / 2) * \
cos(lat1) * cos(lat2)
c = 2 * arctan2(sqrt(a), sqrt(1 - a))
d = R * c
# reshape accodring to the input
D = d.reshape(location1.shape[0], location2.shape[0])
return D
def haversine(location1, location2=None): # calculates great circle distance
__doc__ = """Returns the great circle distance of the given
coordinates.
INPUT: location1 = ((lat1, lon1), ..., n(lat1, lon1))
*location2 = ((lat2, lon2), ..., n(lat2, lon2))
*if location2 is not given a square matrix of distances
for location1 will be put out
OUTPUT: distance in km
(dist1 ... ndist
: :
ndist1 ... ndist)
shape will depend on the input
METHOD: a = sin(dLat / 2) * sin(dLat / 2) +
sin(dLon / 2) * sin(dLon / 2) *
cos(lat1) * cos(lat2)
c = 2 * arctan2(sqrt(a), sqrt(1 - a))
d = R * c
where R is the earth's radius (6371 km)
and d is the distance in km"""
from itertools import product, combinations
from pylab import deg2rad, sin, cos, arctan2, \
meshgrid, sqrt, array, arange
if location2:
location1 = array(location1, ndmin=2)
location2 = array(location2, ndmin=2)
elif location2 is None:
location1 = array(location1, ndmin=2)
location2 = location1.copy()
# get all combinations using indicies
ind1 = arange(location1.shape[0])
ind2 = arange(location2.shape[0])
ind = array(list(product(ind1, ind2)))
# using combination inds to get lats and lons
lat1, lon1 = location1[ind[:, 0]].T
lat2, lon2 = location2[ind[:, 1]].T
# setting up variables for haversine
R = 6371.
dLat = deg2rad(lat2 - lat1)
dLon = deg2rad(lon2 - lon1)
lat1 = deg2rad(lat1)
lat2 = deg2rad(lat2)
# haversine formula
a = sin(dLat / 2) * sin(dLat / 2) + \
sin(dLon / 2) * sin(dLon / 2) * \
cos(lat1) * cos(lat2)
c = 2 * arctan2(sqrt(a), sqrt(1 - a))
d = R * c
# reshape accodring to the input
D = d.reshape(location1.shape[0], location2.shape[0])
return D
def anamorphosis(Im, r_in):
dpi = 72
r_cylinder = int(r_in * dpi)
(sy, sx) = Im.shape
Im = np.pad(Im, (0, r_cylinder), mode="constant")[:, :sx]
(sy, sx) = Im.shape
th_range = (pyl.pi) * 2
max_r = sy
fy = 2 * max_r
fx = 2 * max_r
#Mid
ys = fy / 2 - pyl.arange(fy)
xs = fx / 2 - pyl.arange(fx)
xq, yq = np.meshgrid(ys, xs)
rs = pyl.sqrt(yq * yq + xq * xq)
rsmx = rs < max_r
ths = pyl.arctan2(yq, xq)
thsmx = ths < th_range
inx = abs(sx - ths * (sx / th_range)) % (sx - 1)
inx[~thsmx] = sx * 2
iny = abs(sy - rs)
iny[~rsmx] = sy * 2
X = np.vstack((iny.flatten(), inx.flatten()))
Final = ndimage.interpolation.map_coordinates(Im, X).reshape(fy,
fx)[::-1, :]
return Final
def get_touchdown(self, t, y, params):
"""
Compute the touchdown position of the leg w.r.t. CoM velocity
:args:
t (float): time
y (6x float): state of the CoM
params (4x float): leg parameter: stiffness, l0, alpha, beta
:returns:
[xFoot, yFoot, zFoot] the position of the leg tip
"""
k, l0, alpha, beta = params
vx, vz = y[3], y[5]
a_v_com = -arctan2(vz, vx) # correct with our coordinate system
#for debugging
#print "v_com_angle:", a_v_com * 180. / pi
xf = y[0] + l0 * cos(alpha) * cos(beta + a_v_com)
yf = y[1] - l0 * sin(alpha)
zf = y[2] - l0 * cos(alpha) * sin(beta + a_v_com)
#for debugging
#print "foot: %2.3f,%2.3f,%2.3f," % ( xf,yf, zf)
return array([xf, yf, zf])
def get_touchdown(self, t, y, params):
"""
Compute the touchdown position of the leg w.r.t. CoM velocity
:args:
t (float): time
y (6x float): state of the CoM
params (4x float): leg parameter: stiffness, l0, alpha, beta
:returns:
[xFoot, yFoot, zFoot] the position of the leg tip
"""
k, l0, alpha, beta = params
vx, vz = y[3], y[5]
a_v_com = -arctan2(vz, vx) # correct with our coordinate system
#for debugging
#print "v_com_angle:", a_v_com * 180. / pi
xf = y[0] + l0 * cos(alpha) * cos(beta + a_v_com)
yf = y[1] - l0 * sin(alpha)
zf = y[2] - l0 * cos(alpha) * sin(beta + a_v_com)
#for debugging
#print "foot: %2.3f,%2.3f,%2.3f," % ( xf,yf, zf)
return array([xf, yf, zf])
def angular_distance(ra0, dec0, ra1, dec1):
dra = ra1 - ra0
numerator = sqrt(
pow(cos(dec1) * sin(dra), 2.0) +
pow(cos(dec0) * sin(dec1) - sin(dec0) * cos(dec1) * cos(dra), 2.0))
denomenator = sin(dec0) * sin(dec1) + cos(dec0) * cos(dec1) * cos(dra)
return arctan2(numerator, denomenator)
def hillshade(data,scale=10.0,azdeg=165.0,altdeg=45.0):
'''
This code thanks to Ran Novitsky Nof
http://rnovitsky.blogspot.co.uk/2010/04/using-hillshade-image-as-intensity.html
Repeated here to make my cyclopean uk_map code prettier.
convert data to hillshade based on matplotlib.colors.LightSource class.
input:
data - a 2-d array of data
scale - scaling value of the data. higher number = lower gradient
azdeg - where the light comes from: 0 south ; 90 east ; 180 north ;
270 west
altdeg - where the light comes from: 0 horison ; 90 zenith
output: a 2-d array of normalized hilshade
'''
from pylab import pi, gradient, arctan, hypot, arctan2, sin, cos
# convert alt, az to radians
az = azdeg*pi/180.0
alt = altdeg*pi/180.0
# gradient in x and y directions
dx, dy = gradient(data/float(scale))
slope = 0.5*pi - arctan(hypot(dx, dy))
aspect = arctan2(dx, dy)
intensity = sin(alt)*sin(slope) + cos(alt)*cos(slope)*cos(-az - aspect - 0.5*pi)
intensity = (intensity - intensity.min())/(intensity.max() - intensity.min())
return intensity
def D(c,f,X):
'''
The derived elastica energy as in sharon
E= a**2 etc.
'''
# either c or f can be an array! not both
# c: current bar orientation
# f: flanker orientation
# x: relative flanker distance and position orientation
x = X[0]
y = X[1]
## 'Affinity' D
# D = Ba^2 + Bb^2 -BaBb
# Here Ba is the angle of the flanker with the line connecting it with the center
# and Bb is the reverse for the center
# See figure 5 in Leung & Malik (1998) for intuitive figure
# flanker positional angles
theta = pl.arctan2(x,y)
# if theta > pi/2: theta-=pi
# if theta < -pi/2: theta+=pi
# B values normalized within -pi to pi
Ba = pl.arctan(pl.tan(0.5*(-f+theta)))*2
Bb = pl.arctan(pl.tan(0.5*(c-theta)))*2
D = 4*(Ba**2 + Bb**2 - Ba*Bb)
return D
def loop(self):
while not rospy.is_shutdown():
# update TF
if self.tl.canTransform('base_link', 'tool0', rospy.Time(0)):
tr = self.tl.lookupTransform('base_link', 'tool0',
rospy.Time(0))
d = False
# angle-axis from quaternion
s2 = pl.sqrt(tr[1][0]**2 + tr[1][1]**2 + tr[1][2]**2)
tu = [0, 0, 0]
if s2 > 1e-6:
t = pl.arctan2(s2, tr[1][3]) * 2
if t > pl.pi:
t -= 2 * pl.pi
tu = [t * tr[1][i] / s2 for i in range(3)]
if len(self.des_pose):
d = self.lin_ax.update(tr[0], self.des_pose[:3])
self.ang_ax.update(tu, self.des_pose[3:])
else:
d = self.lin_ax.update(tr[0])
self.ang_ax.update(tu)
if d:
self.canvas.draw()
rospy.sleep(0.1)
def mfreqz(self, b,a=1):
'''
plotting freqz of filter , like matlab representation.
:param b: nominator
:param a: denominator
default: a = 1
'''
from matplotlib import pyplot as plt
from pylab import unwrap, arctan2, imag, real, log10
w, h = signal.freqz(b,a)
h_dB = 20 * log10(abs(h))
plt.subplot(211)
plt.plot(w/max(w), h_dB)
plt.grid()
plt.ylim(-150, 5)
plt.ylabel('Magnitude (db)')
plt.xlabel(r'Normalized Frequency (x$\pi$rad/sample)')
plt.title(r'Frequency response')
plt.subplot(212)
h_Phase = unwrap(arctan2(imag(h),real(h)))
plt.plot(w/max(w),h_Phase)
plt.grid()
plt.ylabel('Phase (radians)')
plt.xlabel(r'Normalized Frequency (x$\pi$rad/sample)')
plt.title(r'Phase response')
plt.subplots_adjust(hspace=0.5)
plt.show(block=False)
def hillshade(data, scale=10.0, azdeg=165.0, altdeg=45.0):
'''
Convert data to hillshade based on matplotlib.colors.LightSource class.
Args:
data - a 2-d array of data
scale - scaling value of the data. higher number = lower gradient
azdeg - where the light comes from: 0 south ; 90 east ; 180 north ;
270 west
altdeg - where the light comes from: 0 horison ; 90 zenith
Returns:
a 2-d array of normalized hilshade
'''
# convert alt, az to radians
az = azdeg*pi/180.0
alt = altdeg*pi/180.0
# gradient in x and y directions
dx, dy = gradient(data/float(scale))
slope = 0.5*pi - arctan(hypot(dx, dy))
aspect = arctan2(dx, dy)
az = -az - aspect - 0.5*pi
intensity = sin(alt)*sin(slope) + cos(alt)*cos(slope)*cos(az)
mi, ma = intensity.min(), intensity.max()
intensity = (intensity - mi)/(ma - mi)
return intensity
def rot2rph (R):
"""
Decompose a 3x3 rotation matrix R into roll, pitch, and yaw angles
"""
h = arctan2 (R[1,0], R[0,0])
ch = cos (h)
sh = sin (h)
p = arctan2 (-R[2,0], R[0,0]*ch + R[1,0]*sh)
r = arctan2 (R[0,2]*sh - R[1,2]*ch, -R[0,1]*sh + R[1,1]*ch);
return array ([r,p,h])
def _matrix2State(self, R, T):
'''
calculate euler angle from rotation matrix
and form the vector state from angle and w_T_L
'''
if R[2,0]!=1 or R[2,0]!=-1:
pitch=-pl.arcsin(R[2,0])#another solution is pi-pitch
roll=pl.arctan2(R[2,1]/cos(pitch), R[2,2]/cos(pitch))
yaw=pl.arctan2(R[1,0]/cos(pitch), R[0,0]/cos(pitch))
else:
yaw=0
if R[2,0]==-1:
pitch=pi/2
roll=yaw+pl.arctan2(R[0,1],R[0,2])
else:
pitch=-pi/2
roll=-yaw+pl.arctan2(-R[0,1],-R[0,2])
state = pl.array([T[0],T[1],T[2],roll*RAD2DEG,pitch*RAD2DEG,yaw*RAD2DEG])
return state
def plot_upd_mfreqz(self, fig, taps, a=1):
if not signal:
return
self.plot_init_mfreqz(fig)
ax1, ax2 = fig.get_axes()
w, h = signal.freqz(taps, a)
if sum(abs(h)) == 0:
return
h_dB = 20 * pylab.log10(abs(h))
ax1.plot(w / max(w), h_dB)
h_Phase = pylab.unwrap(pylab.arctan2(pylab.imag(h), pylab.real(h)))
ax2.plot(w / max(w), h_Phase)
def scatter_1():
count = 5
for x in range(count):
x = plb.rand(1, 2, 1500)
y = plb.rand(1, 2, 1500)
plb.axes([0.075, 0.075, .88, .88])
plb.cla()
plb.scatter(x, y, s=65, alpha=.75, linewidths=.125, c=plb.arctan2(x, y))
plb.grid(True)
plb.xlim(-0.085, 1.085), plb.ylim(-0.085, 1.085)
plb.pause(1)
def AnglesFromRot(R):
# Hypothesis 1
h2a = arcsin(R[0, 2])
cos2 = cos(h2a)
if abs(cos2) < 1e-8:
cos2 += 1e-8
h1a = arctan2(-R[1, 2] / cos2, R[2, 2] / cos2)
h3a = arctan2(-R[0, 1] / cos2, R[0, 0] / cos2)
# Hypothesis 2
h2b = pi - h2a
cos2 = cos(h2b)
if abs(cos2) < 1e-8:
cos2 += 1e-8
h1b = arctan2(-R[1, 2] / cos2, R[2, 2] / cos2)
h3b = arctan2(-R[0, 1] / cos2, R[0, 0] / cos2)
# Testing
resa = abs(sum(R - RotFromAngles([h1a, h2a, h3a])))
resb = abs(sum(R - RotFromAngles([h1b, h2b, h3b])))
if resa < resb:
return [h1a, h2a, h3a]
else:
return [h1b, h2b, h3b]
def AnglesFromRot(R):
# Hypothesis 1
h2a = arcsin(R[0, 2])
cos2 = cos(h2a)
if abs(cos2) < 1e-8:
cos2 += 1e-8
h1a = arctan2(-R[1, 2] / cos2, R[2, 2] / cos2)
h3a = arctan2(-R[0, 1] / cos2, R[0, 0] / cos2)
# Hypothesis 2
h2b = pi - h2a
cos2 = cos(h2b)
if abs(cos2) < 1e-8:
cos2 += 1e-8
h1b = arctan2(-R[1, 2] / cos2, R[2, 2] / cos2)
h3b = arctan2(-R[0, 1] / cos2, R[0, 0] / cos2)
# Testing
resa = abs(sum(R - RotFromAngles([h1a, h2a, h3a])))
resb = abs(sum(R - RotFromAngles([h1b, h2b, h3b])))
if resa < resb:
return [h1a, h2a, h3a]
else:
return [h1b, h2b, h3b]
def det_rotationangles2(cell, segment1, segment2):
'''return rotationangles around x- and y-axis, so the line between two
chosen segments is aligned in parallell to the z-axis, corrected to soma.'''
R = pl.sqrt((cell.xmid[segment1]-cell.xmid[segment2])**2 \
+ (cell.ymid[segment1]-cell.ymid[segment2])**2 \
+ (cell.zmid[segment1]-cell.zmid[segment2])**2)
rot_x = pl.pi+pl.arctan2(cell.ymid[segment1]-cell.ymid[segment2],\
cell.zmid[segment1]-cell.zmid[segment2])
rot_y = -pl.pi + pl.arcsin((cell.xmid[segment1] - cell.xmid[segment2]) / R)
rotation = {'x': rot_x, 'y': rot_y}
return rotation
def _matrix2State(self, R, T):
'''
calculate euler angle from rotation matrix
and form the vector state from angle and w_T_L
'''
if R[2, 0] != 1 or R[2, 0] != -1:
pitch = -pl.arcsin(R[2, 0]) #another solution is pi-pitch
roll = pl.arctan2(R[2, 1] / cos(pitch), R[2, 2] / cos(pitch))
yaw = pl.arctan2(R[1, 0] / cos(pitch), R[0, 0] / cos(pitch))
else:
yaw = 0
if R[2, 0] == -1:
pitch = pi / 2
roll = yaw + pl.arctan2(R[0, 1], R[0, 2])
else:
pitch = -pi / 2
roll = -yaw + pl.arctan2(-R[0, 1], -R[0, 2])
state = pl.array(
[T[0], T[1], T[2], roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG])
return state
def angles(h,k,l):
if h is not 0:
phi = pylab.arctan2(k,h)*180/numpy.pi
elif k<0:
phi = -90
elif k>0:
phi = 90
elif k==0:
phi = 0
theta = numpy.arccos(l/numpy.sqrt(h**2+k**2+l**2))*180/numpy.pi
return phi, theta
def det_rotationangles2(cell, segment1, segment2):
"""return rotationangles around x- and y-axis, so the line between two
chosen segments is aligned in parallell to the z-axis, corrected to soma."""
R = pl.sqrt(
(cell.xmid[segment1] - cell.xmid[segment2]) ** 2
+ (cell.ymid[segment1] - cell.ymid[segment2]) ** 2
+ (cell.zmid[segment1] - cell.zmid[segment2]) ** 2
)
rot_x = pl.pi + pl.arctan2(cell.ymid[segment1] - cell.ymid[segment2], cell.zmid[segment1] - cell.zmid[segment2])
rot_y = -pl.pi + pl.arcsin((cell.xmid[segment1] - cell.xmid[segment2]) / R)
rotation = {"x": rot_x, "y": rot_y}
return rotation
def GetOrientation(CentralMoments):
# Corrige le cadrant et permet d'avoir l'orientation de l'ellipse !
mu = CentralMoments
a = mu[2, 0] / mu[0, 0]
b = mu[1, 1] / mu[0, 0]
c = mu[0, 2] / mu[0, 0]
if a - c == 0:
Orientation = m.pi / 2.0
else:
Orientation = -0.5 * m.arctan2(2 * b, (a - c))
return Orientation
def eq_to_gal(ra, dec):
"""
expects dec between -pi/2 and pi/2
compares to pyephem within 0.45 arcsec
returns lon, lat in radians
"""
alpha = galactic_north_equatorial[0]
delta = galactic_north_equatorial[1]
la = angles.from_degrees(122.932 - 90.0)
b = arcsin(sin(dec) * sin(delta) + cos(dec) * cos(delta) * cos(ra - alpha))
l = arctan2(
sin(dec) * cos(delta) - cos(dec) * sin(delta) * cos(ra - alpha),
cos(dec) * sin(ra - alpha)) + la
l += 2.0 * pylab.pi * (l < 0)
l = l % (2.0 * pylab.pi)
return l, b
def popvec(self,X):
''' Population vector for the set of responses X, with each value in
the vector X corresponding to an angle in self.ang
X is a 1D vector of length len(self.ang)
Returns the angle of the population vector.
'''
# define vector coordinates
v = pl.zeros((2,self.N))
v[0,:] = cos(2*self.ang)
v[1,:] = sin(2*self.ang)
# find population vector
vest0 = pl.sum(((X-min(X))/max(X))*v,1)
# return the angle of the population vector
return 0.5*pl.arctan2(vest0[1],vest0[0])
def on_key(event):
#print 'you pressed', event.key, event.xdata, event.ydata
if (event.key == 'q'):
sys.exit(0)
#
elif (event.key == 'w'):
pylab.close(event.canvas.figure)
#
elif (event.key == 'd'):
print "##############################################################"
print "Please click two points to get the distance and slope."
from matplotlib.widgets import Cursor
cursor = Cursor(event.inaxes, useblit=False, color='red', linewidth=1)
points = pylab.ginput(n=2, show_clicks=True, timeout=0)
xs = pylab.array([points[0][0], points[1][0]])
ys = pylab.array([points[0][1], points[1][1]])
dx = xs[1] - xs[0]
dy = ys[1] - ys[0]
dy_log = pylab.log10(ys[0]) - pylab.log10(ys[1])
dy_ln = pylab.log(ys[0]) - pylab.log(ys[1])
angle = pylab.arctan2(dy, dx)
print "points = ", points
print "distance (x) =", dx
print "distance (y) =", dy
print "distance =", pylab.sqrt( dx**2 + dy**2 )
print "Ratio: x0/x1 =", xs[0] / xs[1], " x1/x0 =", xs[1] / xs[0], " y0/y1 =", ys[0] / ys[1], " y1/y0 =", ys[1] / ys[0]
print "dy/y0 = ", dy/ys[0]
print "dx/x0 = ", dx/xs[0]
print "Angle: theta = atan2(y/x) =", angle, "rad =", angle*180.0/pylab.pi, "deg"
print "Slope: ", dy/dx, " 1/slope =", dx/dy
print "Slope (log10 scale):", dy_log/dx
print "Slope (ln scale):", dy_ln/dx
event.inaxes.plot([points[0][0], points[1][0]], [points[0][1], points[1][1]], '--r', lw=1.0)
event.inaxes.plot([points[0][0], points[1][0]], [points[0][1], points[1][1]], '+r', lw=1.0)
pylab.draw()
#
elif (event.key == 'a'):
print "##############################################################"
print "Please click a point to get the position."
from matplotlib.widgets import Cursor
cursor = Cursor(event.inaxes, useblit=False, color='red', linewidth=1)
point = pylab.ginput(n=1, show_clicks=False, timeout=0)
print "point = ", point
event.inaxes.plot(point[0][0], point[0][1], '+r', lw=1.0)
pylab.draw()
def test(x_offset, y_offset, angle, min_distance = 0.25, max_distance = 3.6):
max_dist = (max_distance*scale) ** 2
min_dist = (min_distance*scale) ** 2
xs = Xs - x_offset
ys = Ys - y_offset
angles = pylab.arctan2( xs, ys )
angles = (angles - radians(angle)) % (2*pi)
angles -= 2*pi * (angles > pi)
evidence = wide_angle_function(angles)
dist = xs**2 + ys**2
evidence *= pylab.logical_and( dist < max_dist, dist > min_dist)
return evidence
def test(x_offset, y_offset, angle, min_distance=0.25, max_distance=3.6):
max_dist = (max_distance * scale)**2
min_dist = (min_distance * scale)**2
xs = Xs - x_offset
ys = Ys - y_offset
angles = pylab.arctan2(xs, ys)
angles = (angles - radians(angle)) % (2 * pi)
angles -= 2 * pi * (angles > pi)
evidence = wide_angle_function(angles)
dist = xs**2 + ys**2
evidence *= pylab.logical_and(dist < max_dist, dist > min_dist)
return evidence
def gal_to_eq(l, b):
"""
Inputs:
l: galactic longitude
b: galactic latitude
Returns:
ra, dec
compares to pyephem within 0.45 arcsec
"""
alpha = galactic_north_equatorial[0]
delta = galactic_north_equatorial[1]
la = angles.from_degrees(122.932 - 90.0)
dec = arcsin(sin(b) * sin(delta) + cos(b) * cos(delta) * sin(l - la))
ra = arctan2(
cos(b) * cos(l - la),
sin(b) * cos(delta) - cos(b) * sin(delta) * sin(l - la)) + alpha
ra += 2.0 * pylab.pi * (ra < 0)
ra = ra % (2.0 * pylab.pi)
return ra, dec
def run_one_bolo(self, bolo, boresight_pointing, maps, times):
'''
Function: run_one_bolo
Purpose: runs simulation for one bolometer at a time, in the event that multiple bolos are
used. The number of bolos can be accessed/changed in default_settings.
This function: * uses the boresight pointing (generated by new_pointing file)
* rotates it according to the number of bolos
* converts to lat/lon
* reads healpy map and gets corresponding I, Q, and U values
* demodulates the signal
* adds noise/non-linearity/HWPSS
* plots the signal
* can also make and write a healpy map if developed further.
Inputs:
-> bolo (int): bolometer number
-> boresight_pointing (float): array of coordinates in ra/dec
-> maps (healpy map): map being used. This can be accessed/changed in default_settings.
-> times (float): array of times
Outputs: none
'''
detector_pointing = self.rotate_boresight_pointing(
boresight_pointing, bolo)
lat, lon = coordinates.eq_to_gal(detector_pointing[0],
detector_pointing[1])
bolo_i = healpy.get_interp_val(maps[0], pl.pi / 2.0 - lat, lon)
bolo_q = healpy.get_interp_val(maps[1], pl.pi / 2.0 - lat, lon)
bolo_u = healpy.get_interp_val(maps[2], pl.pi / 2.0 - lat, lon)
bolo_alpha = 1 / 2. * pl.arctan2(bolo_u, bolo_q)
bolo_p = pl.sqrt(bolo_q**2 + bolo_u**2) / bolo_i
#hwp_angle = np.sin(2*pl.pi * self.settings.f_hwp * self.hwp_angles)
#hwp_angle = 2*pl.pi * self.settings.f_hwp * self.hwp_rotation()
data = 1 / 2. * (
bolo_i + bolo_p * pl.cos(4 * self.hwp_rotation() - 2 * bolo_alpha))
data = self.add_hwpss(times, data, self.hwp_rotation())
#data = self.add_nonlinearity(data)
#data = self.add_noise(data, self.settings.add_white_noise, self.settings.add_1f_noise)
self.plot_data(times, data)
def rotateData(names, pos, name='Poz', name2='fWAR', vbose=0):
flipX = flipY = +1
for i, v in enumerate(names):
if v==name:
xpos, ypos = pos[i][:]
theta = pylab.arctan2(xpos, ypos)
for i, v in enumerate(names):
xpos, ypos = pos[i][:]
nxpos = xpos*pylab.cos(theta) - ypos*pylab.sin(theta)
nypos = xpos*pylab.sin(theta) + ypos*pylab.cos(theta)
pos[i][0] = nxpos
pos[i][1] = nypos
if vbose>=1:
print v, 'oldpos', xpos, ypos, 'newpos(rota)', nxpos, nypos
for i, v in enumerate(names):
if v==name2:
x2, y2 = pos[i][:]
if x2<0:
flipX = -1
if y2>0:
flixY = -1
for i, v in enumerate(names):
xpos, ypos = pos[i][:]
nxpos = xpos*flipX
nypos = ypos*flipY
pos[i][0] = nxpos
pos[i][1] = nypos
if vbose>=1:
print v, 'oldpos', xpos, ypos, 'newpos(flip)', nxpos, nypos
return
def _xy2pt(x,y,oh=False):
"""
Convert x,y coordinates to phi,theta, assuming a spherical surface with
radius equal to 1.
x,y are assumed to have been rotated to be consistent with the desired/
expected POV. However, there still exists the issue of whether x,y are
for the nearer hemisphere, or the "opposite hemisphere". If oh==True,
adjust theta accordingly.
In actuality, floating point round-off error leads to a grid of phi,theta
that is not regular (i.e., it is not a perfect meshgrid). This error does
not hurt numerics much, but can cause issues with plotting algorithms, so
we choose to enforce a regular meshgrid in phi,theta based on the first
columns,rows of x and y. THIS IS ANOTHER CONSEQUENCE OF STRANGE DESIGN
CHOICES FOR THE MIX FILE FORMAT.
"""
phis = p.arctan2(y[:,-1],x[:,-1])
phis[phis<0] = phis[phis<0] + 2*p.pi
if oh:
phis[0] = 2*p.pi
phis[-1] = 0
else:
phis[0] = 0
phis[-1] = 2*p.pi
thetas = p.arcsin(x[0,:]) # assumes x[0,:] corresponds to y==0
phi,theta = p.meshgrid(phis, thetas, indexing='ij')
if oh:
theta = p.pi - theta
return (phi, theta)
def gradient(image):
''' Given an image, this computes its gradient using
sobel filters::
[-1 0 1] [ -1 -2 -1 ]
Dx = [-2 0 2] Dy = [ 0 0 0 ]
[-1 0 1] [ 1 2 1 ]
The prewitt filters can also be used::
[-1 0 1] [ -1 -1 -1 ]
Dx = [-1 0 1] Dy = [ 0 0 0 ]
[-1 0 1] [ 1 1 1 ]
'''
im_x = pl.zeros(image.shape)
im_y = pl.zeros(image.shape)
filters.sobel(image, 1, im_x)
filters.sobel(image, 0, im_y)
magnitude = pl.sqrt(im_x**2 + im_y**2)
angle = pl.arctan2(im_y, im_x)
return magnitude, angle
def on_key(event):
#print 'you pressed', event.key, event.xdata, event.ydata
if (event.key == 'q'):
sys.exit(0)
#
elif (event.key == 'w'):
pylab.close(pylab.gcf())
#
elif (event.key == 'd'):
print "##############################################################"
print "Please click two points to get the distance and slope."
points = pylab.ginput(n=2, show_clicks=True, timeout=0)
dx = points[1][0] - points[0][0]
dy = points[1][1] - points[0][1]
dy_log = pylab.log10(points[0][1]) - pylab.log10(points[1][1])
dy_ln = pylab.log(points[0][1]) - pylab.log(points[1][1])
angle = pylab.arctan2(dy,dx)
print "points = ", points
print "distance (x) =", dx
print "distance (y) =", dy
print "distance =", pylab.sqrt( dx**2 + dy**2 )
print "Angle: theta = atan2(y/x) =", angle, "rad =", angle*180.0/pylab.pi, "deg"
print "Slope: ", dy/dx, " 1/slope =", dx/dy
print "Slope (log10 scale):", dy_log/dx
print "Slope (ln scale):", dy_ln/dx
pylab.plot([points[0][0], points[1][0]], [points[0][1], points[1][1]], '--r', lw=1.0)
pylab.plot([points[0][0], points[1][0]], [points[0][1], points[1][1]], '+r', lw=1.0)
pylab.draw()
#
elif (event.key == 'a'):
print "##############################################################"
print "Please click a point to get the position."
#from matplotlib.widgets import Cursor
#cursor = Cursor(pylab.gca(), useblit=True, color='red', linewidth=2 )
point = pylab.ginput(n=1, show_clicks=False, timeout=0)
print "point = ", point
pylab.gca().plot(point[0][0], point[0][1], '+r', lw=1.0)
pylab.draw()
def findE(self,c,f,X):
''' find E, the approximated sharon energy
Inputss
----------
- c: current bar orientation, relative to the vertical (can be double or array, if array D returns an array of D values)
- f: flanker orientation, relative to the vertical (double)
- X: flanker position, relative to the current bar (x,y)
So, as examples...
findE(0,0,[0,1]) would be two vertical bars, positioned vertically in a line.
findE(pi/2,pi/2,[1,0]) would be two horizontal bars, positioned horizontally in a line.
'''
# define x and y
x = X[0]
y = X[1]
# flanker positional angle
theta = pl.arctan2(x,y)
# find and return D
Ba = pl.arctan(tan(0.5*(-f+theta)))*2
Bb = pl.arctan(tan(0.5*(c-theta)))*2
return 4*(Ba**2 + Bb**2 - Ba*Bb)
def calc_transform():
# this transforms x -> theta, and y -> r, and gives you then new image
try:
global pngName, r_in, Im
print "calculating..."
# loading the image
Orig = pyl.asarray(Image.open(pngName))
Im = Image.fromarray(Orig, 'RGB')
# checking the dpi
try:
dpi = Im.info["dpi"]
print "dpi = ", dpi
except:
dpi = 72
print "couldn't find dpi, using default dpi = 72"
print "calculating, please wait..."
canvas.itemconfigure("text",
text="Calculating, may take several mins...")
canvas.update()
# getting the size of the original image. sy is the number of pixels in the y dimension, sx in the x direction, and sc is the number of colors (usually 3 for RGB or 4 if it also includes alpha)
(sy, sx, sc) = (Orig.shape[0], Orig.shape[1], Orig.shape[2])
# calculates the radius in pixels
r_cylinder = int(r_in * dpi)
# sets theta to be between 0 and pi (maybe user should be able to set this?)
th_range = (pyl.pi)
# sets the maximum radius (in pixels) as the radius of the cylinder plus the height of the original image. Maybe the user should be able to set this, but then we'd have to interpolate to resize the image.
max_r = r_cylinder + sy
# the final image has dimensions 2*max_r x 2*max_r, because this is the widest that a circle with radius max_r will be
fy = 2 * max_r
fx = 2 * max_r
# initialize the final image to 255s (white background)
Final = 255 * pyl.ones((fy, fx, sc), dtype=pyl.uint8)
for y in range(fy):
# x and y index into the final image
# xc and yc are centered versions of x and y, so the origin is in the middle
yc = fy / 2 - y
for x in range(fx):
xc = fx / 2 - x
# calculate r from xc and yc
r = pyl.sqrt(xc * xc + yc * yc)
# calculate theta with arctan2, which works even for angles that are bigger than 90
th = (pyl.arctan2(1. * yc, (1. * xc)))
# check if r and theta are within range
if ((th < th_range) & (th > 0)):
if ((r > r_cylinder) & (r < (max_r))):
# x_orig and y_orig are the indices you want for the original image
x_orig = int(th * sx / th_range)
y_orig = int(r - r_cylinder)
# assign the appropriate pixels of the final image based on the original image, flipping up down and left right (mirror image)
Final[y, x, :] = Orig[sy - y_orig - 1,
sx - x_orig - 1, :]
# make an image out of the array, and set the dpi
Im = Image.fromarray(Final)
Im.info["dpi"] = dpi
canvas.itemconfigure(
"text", text="calculated, view and/or save the anamorphic image")
canvas.update()
except:
canvas.itemconfigure(
"text", text="Sorry, there was an error. Please check .png")
canvas.update()
ra = float(groups[groups['target'] ==True]['ra'].unique()[0])
if ra < 0:
ra = ra * -1.
dec = float(groups[groups['target'] ==True]['dec'].unique()[0])
correctedList.append((ra,dec,0))
for i in pointing.query('go == "%s"'%name).rowNum.values:
row = rows[i]
dRA,dDec = [[p[0].get(),p[1].get()] for p in row.sourceOffset() ][row.numSample()/2]
# if dRA == 0. or dDec == 0.:
# print "no Offset positions in the Pointing Table"
# print "Is this EB a MultiSource?"
# sys.exit(1)
Pl = [pl.cos(dRA)*pl.cos(dDec), pl.sin(dRA)*pl.cos(dDec), pl.sin(dDec)]
Ps = rot(Pl, ra, dec)
correctedList.append((pl.arctan2(Ps[1], Ps[0]) % (2.*pl.pi), pl.arcsin(Ps[2]), i))
correctedAll = pd.DataFrame(correctedList, columns=['ra','dec', 'row'])
corrected = correctedAll[['ra','dec']]
corrected['series'] = 'Corrected (Pointing.bin)'
corrected = corrected.drop_duplicates()
corrected = corrected.reset_index(drop=True)
observed = field[field['target'] == True][['fieldId','ra','dec']]
observed.ra = observed.ra.astype(float)
observed.dec = observed.dec.astype(float)
observed['ra'] = observed.apply(lambda x: x['ra']*-1. if x['ra'] < 0.0 else x['ra'], axis = 1)
observed = observed.loc[observed['fieldId'].isin(fullbar) ]
observed = observed.reset_index(drop=True)
observed['series'] = 'Field.xml'
def canny(F, s, high_threshold, low_threshold):
'''Apply a canny edge detector to the image.'''
blurred = gD(F, s, 0, 0)
filt = gauss1(1.4, f1_1)
Gx = convolve1d(blurred, filt, axis=0, mode='nearest')
Gy = convolve1d(blurred, filt, axis=1, mode='nearest')
G = zeros(F.shape)
angle = zeros(F.shape, dtype='int')
after_nm = zeros(F.shape)
# Finding intensity grade
for x in xrange(len(G[0])):
for y in xrange(len(G)):
G[x][y] = sqrt(Gx[x][y] ** 2 + Gy[x][y] ** 2)
angle[x][y] = int( \
round(arctan2(Gx[x][y], Gy[x][y]) * 4 / pi + 1)) % 4
# Non-maximum suppression
for x in xrange(len(G[0])):
for y in xrange(len(G)):
if angle[x][y] == 0:
side_one = G[x+1][y] if inImage(G, x+1, y) else 0
side_two = G[x-1][y] if inImage(G, x-1, y) else 0
elif angle[x][y] == 1:
side_one = G[x-1][y-1] if inImage(G, x-1, y-1) else 0
side_two = G[x+1][y+1] if inImage(G, x+1, y+1) else 0
elif angle[x][y] == 2:
side_one = G[x][y+1] if inImage(G, x, y+1) else 0
side_two = G[x][y-1] if inImage(G, x, y-1) else 0
elif angle[x][y] == 3:
side_one = G[x-1][y+1] if inImage(G, x-1, y+1) else 0
side_two = G[x+1][y-1] if inImage(G, x+1, y-1) else 0
if not (G[x][y] > side_one and G[x][y] > side_two):
after_nm[x][y] = 0
else:
after_nm[x][y] = G[x][y]
# Hysteresis thresholding
high_threshold *= (after_nm.max() - after_nm.min()) / 255
low_threshold *= (after_nm.max() - after_nm.min()) / 255
after_threshold = zeros(after_nm.shape, dtype = int)
def follow_edge(x, y):
"""Follow an edge by recursively checking if the neighbours are in
it."""
after_threshold[x][y] = 1
for x in xrange(-1, 2):
for y in xrange(-1, 2):
if (not x or not y):
if after_nm[x][y] >= low_threshold and not after_threshold:
follow_edge(x, y)
# Make border pixels zero
for i in xrange(len(after_nm[0])):
after_nm[i][0] = 0
after_nm[i][len(after_nm) - 1] = 0
for i in xrange(len(after_nm) - 1):
after_nm[0][i] = 0
after_nm[len(after_nm[0]) - 1][i] = 0
# Follow each line
for x in xrange(len(after_nm[0])):
for y in xrange(len(after_nm)):
if after_nm[x][y] >= high_threshold and not after_threshold[x][y]:
follow_edge(x, y)
return after_threshold
def plot(self):
self.calc_psi()
r_range = P.array([1e-6, 4.0], dtype=fdtype)
z_range = P.array([-1.5, 1.5], dtype=fdtype)
r_dim = 100
z_dim = 100
# resistivity of Cu (Ohm-m)
eta_Cu = 16.8e-9
# coil resistances
R_b_coils = eta_Cu * 2 * pi * self.coil_set.r_coils / \
self.coil_set.r_widths / self.coil_set.z_widths
# minor radius
a = 1.0
# major radius
r_0 = 1.5
# radius of vacuum vessel (from r_0), plus blanket if present
blanket_width = 0.0
r_vv0 = a + blanket_width
# radius of flux conserver to Uberplate feature
a_u = 0.242
# width of Uberplate
w_u = 0.35
# width of injector
w_i = 0.28
# r location of Uberplate (injector feature is perpendicular
# to Uberplate and tangent to the FC)
r_u = r_0 + P.sqrt((2 * a - w_u) * a_u - w_u**2 / 4 + a**2)
# angle of mating point of FC and Uberplate feature
theta_u = P.arctan2(0.5 * w_u + a_u, a + a_u)
# center of upper FC to Uberplate feature arc
r_u_1 = r_u
z_u_1 = w_u / 2 + a_u
# center of lower FC to Uberplate feature arc
r_u_2 = r_u
z_u_2 = -(w_u / 2 + a_u)
# meshes for contour plots
r_arr, z_arr = P.meshgrid(P.linspace(r_range[0], r_range[1], r_dim),
P.linspace(z_range[0], z_range[1], z_dim))
def pcoils(r_coils=self.coil_set.r_coils,
r_widths=self.coil_set.r_widths,
z_coils=self.coil_set.z_coils,
z_widths=self.coil_set.z_widths,
lw=2):
# plots rectangular coils
r_sq = P.array([0.5, -0.5, -0.5, 0.5, 0.5])
z_sq = P.array([0.5, 0.5, -0.5, -0.5, 0.5])
for r_coil, r_width, z_coil, z_width in zip(
r_coils, r_widths, z_coils, z_widths):
r = r_coil + r_sq * r_width
z = z_coil + z_sq * z_width
P.plot(r, z, color='b', lw=lw)
# optional to plot coil on left side
# if plot_left_side:
# P.plot(-r, z, color='b')
# FC parameters
# inner midplane gap half-angle
delta_deg = 2
delta = delta_deg * pi / 180.0
# number of points for arcs, etc.
npts = 101
# FC arc from midplane gap to outside injector feature
theta_1 = P.linspace(pi + delta, 2 * pi - theta_u, npts)
theta_2 = P.linspace(theta_u, pi - delta, npts)
# outline of the FC, in two parts
r_FC_1 = r_0 + a * P.cos(theta_1)
z_FC_1 = a * P.sin(theta_1)
r_FC_2 = r_0 + a * P.cos(theta_2)
z_FC_2 = a * P.sin(theta_2)
# injector feature angles
theta_inj_1 = P.linspace(pi + theta_u, 1.5 * pi, npts)
theta_inj_2 = P.linspace(0.5 * pi, pi - theta_u, npts)
# outline of the injector feature, in two parts
r_inj_1 = r_u_1 + a_u * P.cos(theta_inj_1)
z_inj_1 = z_u_1 + a_u * P.sin(theta_inj_1)
r_inj_2 = r_u_2 + a_u * P.cos(theta_inj_2)
z_inj_2 = z_u_2 + a_u * P.sin(theta_inj_2)
l_inj = 0.67 - 0.5 * w_i
#smaller curve feature from Uberplate to injector
r_tran = z_u_1 - a_u - 0.5 * w_i
theta_tran_1 = P.linspace(pi, 1.5 * pi, npts)
r_tran_1 = r_u + r_tran + r_tran * P.cos(theta_tran_1)
z_tran_1 = 0.5 * w_i + r_tran + r_tran * P.sin(theta_tran_1)
# default linewidth
lw = 2
# flux conserver color
FC_col = 'r'
P.clf()
P.axes(aspect='equal')
# plot FC sections and injector transition
P.plot(r_FC_1, z_FC_1, r_FC_2, z_FC_2, color=FC_col, lw=lw)
#P.plot(-r_FC_1, z_FC_1, -r_FC_2, z_FC_2, color=FC_col, lw=lw)
#P.plot(r_inj_1, z_inj_1, -r_inj_1, z_inj_1, color=FC_col, lw=lw)
#P.plot(r_inj_2, z_inj_2, -r_inj_2, z_inj_2, color=FC_col, lw=lw)
P.plot(r_inj_1, z_inj_1, color=FC_col, lw=lw)
P.plot(r_inj_2, z_inj_2, color=FC_col, lw=lw)
# injector to Uberplate transition feature
P.plot(r_tran_1, z_tran_1, color=FC_col, lw=lw)
P.plot(r_tran_1, -z_tran_1, color=FC_col, lw=lw)
# sides of injector
#P.plot([r_u, r_u + l_inj],
# [0.5 * w_u, 0.5 * w_u], color=FC_col, lw=lw)
#P.plot([r_u, r_u + l_inj],
# [-0.5 * w_u, -0.5 * w_u], color=FC_col, lw=lw)
P.plot([r_u + r_tran, r_u + l_inj], [0.5 * w_i, 0.5 * w_i],
color=FC_col,
lw=lw)
P.plot([r_u + r_tran, r_u + l_inj], [-0.5 * w_i, -0.5 * w_i],
color=FC_col,
lw=lw)
#P.plot([-(r_0 + 0.5 * w_u), -(r_0 + 0.5 * w_u)],
# [z_u, z_u + 0.4], color=FC_col, lw=lw)
#P.plot([-(r_0 - 0.5 * w_u), -(r_0 - 0.5 * w_u)],
# [z_u, z_u + 0.4], color=FC_col, lw=lw)
# end of injector
th = P.linspace(-0.5 * pi, 0.5 * pi, npts)
#rs = r_u + l_inj + 0.5 * w_u * P.cos(th)
rs = r_u + l_inj + 0.5 * w_i * P.cos(th)
#zs = 0.5 * w_u * P.sin(th)
zs = 0.5 * w_i * P.sin(th)
P.plot(rs, zs, color=FC_col, lw=lw)
#P.plot(-rs, zs, color=FC_col, lw=lw)
delta2_deg = 0.0
delta2 = delta2_deg * pi / 180.0
r_bs = r_u + l_inj
a_bs = self.coil_set.r_coils[-3] - 0.5 * \
self.coil_set.r_widths[-3] - (r_u + l_inj)
a_b = 0.7
#r_m = 0.5 / r_bs * (a_b ** 2 - a_bs ** 2 + \
# (r_bs + r_0) ** 2 - r_0 ** 2)
#z_m = P.sqrt(a_b ** 2 - (r_m - r_0) ** 2)
#
#theta_m = P.arctan2(z_m, r_m)
#thm = P.linspace(-theta_m, theta_m, npts)
# shield for midplane coil
#delta_m = 17.4 * pi / 180.0
delta_m = -5.0 * pi / 180.0
thm = P.linspace(-0.5 * pi + delta_m, 0.5 * pi - delta_m, npts)
#P.plot(r_bs + a_bs * P.cos(thm), a_bs * P.sin(thm), color='k', lw=lw)
# angle to meet shield at midplane coil
#delta_b = 6.8 * pi / 180.0
delta_b = 11.5 * pi / 180.0
theta2 = P.linspace(-0.5 * pi, -delta_b, npts)
r_vv = r_0 + r_vv0 * P.cos(theta2)
z_vv = r_vv0 * P.sin(theta2)
#P.plot(r_vv, z_vv, color='k', lw=lw)
theta2 = P.linspace(delta_b, 0.5 * pi, npts)
r_vv = r_0 + r_vv0 * P.cos(theta2)
z_vv = r_vv0 * P.sin(theta2)
#P.plot(r_vv, z_vv, color='k', lw=lw)
#P.plot(-r_vv, z_vv, color='k', lw=lw)
#P.plot([0, r_vv[-1]], [z_vv[-1], z_vv[-1]], color='k', lw=lw)
#P.plot([0, r_vv[-1]], [-z_vv[-1], -z_vv[-1]], color='k', lw=lw)
#tripcolor(r_pr, z_pr, p_pr)
#levels = linspace(psi_mod.min(), psi_mod.max(), 30)
#levels = P.arange(-15.0, 15.0 + 0.5, 0.5) * i_fact
#delta_psi = 10.0
#psi_min = -20.0
#psi_max = 100.0
#levels = arange(psi_min, psi_max + delta_psi, delta_psi)
#P.contour(r_arr, z_arr, psi, levels=levels)
#P.prism()
#P.hsv()
#P.contour(-r_arr, z_arr, psi, levels=levels)
##P.prism()
#P.hsv()
delta_psi = 0.5
psi_min = -15.0
psi_max = 30.0
self.plasma_coil.plot_plasma()
levels = P.arange(1.0, psi_max + delta_psi, delta_psi) * self.i_fact
P.contour(r_arr, z_arr, self.psi, levels=levels, colors='w')
levels = P.arange(psi_min, 1.0 + delta_psi, delta_psi) * self.i_fact
P.matplotlib.rcParams['contour.negative_linestyle'] = 'solid'
P.contour(r_arr, z_arr, self.psi, levels=levels, colors='gray')
# need to plot black contours on midplane coil
#rsub = P.where(r_arr[0, :] >= 5.0)[0]
#levels = P.arange(1.0, psi_max + delta_psi, delta_psi) * i_fact
#P.contour(r_arr[:, rsub], z_arr[:, rsub], psi[:, rsub],
# levels=levels, colors='gray')
P.plot(self.coil_set.r_floops, self.coil_set.z_floops, 'ro')
#P.plot(-r_floops, z_floops, 'ro')
#no_text = True
no_text = False
# whether to annotate in MA or MW
show_MA = True
if not no_text:
# annotate coil powers
for ii in xrange(self.coil_set.n_coils):
if self.i_floops[ii] >= 0:
signum = '+'
else:
signum = '-'
if show_MA:
tdata = 1e-6 * self.i_floops[ii]
else:
tdata = 1e-6 * self.i_floops[ii]**2 * R_b_coils[ii]
# P.text(r_b_coils[ii], z_b_coils[ii], '%s%3.2f' %
# (signum, 1e-6 * i_floops[ii] ** 2 * R_b_coils[ii]),
# ha='center', va='center', backgroundcolor='w')
P.text(self.coil_set.r_coils[ii],
self.coil_set.z_coils[ii],
'%s%3.2f' % (signum, tdata),
ha='center',
va='center',
backgroundcolor='w')
if self.i_floops[-1] >= 0:
signum = '+'
else:
signum = '-'
P.text(r_0,
0.0,
'%s%3.2f MA' % (signum, 1e-6 * self.i_floops[-1]),
va='center',
backgroundcolor='w')
pcoils()
P.axvline()
coil_power = (self.i_floops[:self.coil_set.n_coils]**2 *
R_b_coils).sum()
coil_current = abs(self.i_floops[:-1]).sum()
if not no_text:
P.title(r'Total Coil Power = %4.2f MW, Current %4.2f MA-turns' %
(1e-6 * coil_power, 1e-6 * coil_current))
print r'Total Coil Power = %4.2f MW, Current %4.2f MA-turns' % \
(1e-6 * coil_power, 1e-6 * coil_current)
self.r_arr = r_arr
self.z_arr = z_arr
self.levels = levels
def twoDang(ang ):
if isinstance(ang,p.ndarray): ang = p.arctan2(ang[1],ang[0])
return ang
ax.fill_between(xl,
ya,
ya - s,
facecolor='red',
alpha=0.5,
interpolate=True)
# plot unsorted vertices
pl.plot(vert_uns[:, 0], vert_uns[:, 1], 'r--D', linewidth=1)
# sort vertices counter-clockwise
mid = pl.mean(vert, 0)
vert.sort(
key=lambda p: pl.arctan2(p[1] - mid[1], p[0] - mid[0]))
vert = pl.array(vert + [vert[0]])
pl.plot(vert[:, 0], vert[:, 1], 'g', linewidth=2)
# get center of gravity
a = 0
x = 0
y = 0
for i in xrange(vert.shape[0]):
v = vert[i - 1, 0] * vert[i, 1] - vert[i, 0] * vert[i - 1,
1]
a += v
x += v * (vert[i, 0] + vert[i - 1, 0])
y += v * (vert[i, 1] + vert[i - 1, 1])
a *= 0.5
def plot(self):
self.calc_psi()
r_range = P.array([1e-6, 4.0], dtype=fdtype)
z_range = P.array([-1.5, 1.5], dtype=fdtype)
r_dim = 100
z_dim = 100
# resistivity of Cu (Ohm-m)
eta_Cu = 16.8e-9
# coil resistances
R_b_coils = eta_Cu * 2 * pi * self.coil_set.r_coils / \
self.coil_set.r_widths / self.coil_set.z_widths
# minor radius
a = 1.0
# major radius
r_0 = 1.5
# radius of vacuum vessel (from r_0), plus blanket if present
blanket_width = 0.0
r_vv0 = a + blanket_width
# radius of flux conserver to Uberplate feature
a_u = 0.242
# width of Uberplate
w_u = 0.35
# width of injector
w_i = 0.28
# r location of Uberplate (injector feature is perpendicular
# to Uberplate and tangent to the FC)
r_u = r_0 + P.sqrt((2 * a - w_u) * a_u - w_u ** 2 / 4 + a ** 2)
# angle of mating point of FC and Uberplate feature
theta_u = P.arctan2(0.5 * w_u + a_u, a + a_u)
# center of upper FC to Uberplate feature arc
r_u_1 = r_u
z_u_1 = w_u / 2 + a_u
# center of lower FC to Uberplate feature arc
r_u_2 = r_u
z_u_2 = -(w_u / 2 + a_u)
# meshes for contour plots
r_arr, z_arr = P.meshgrid(P.linspace(r_range[0], r_range[1], r_dim),
P.linspace(z_range[0], z_range[1], z_dim))
def pcoils(r_coils=self.coil_set.r_coils,
r_widths=self.coil_set.r_widths,
z_coils=self.coil_set.z_coils,
z_widths=self.coil_set.z_widths, lw=2):
# plots rectangular coils
r_sq = P.array([0.5, -0.5, -0.5, 0.5, 0.5])
z_sq = P.array([0.5, 0.5, -0.5, -0.5, 0.5])
for r_coil, r_width, z_coil, z_width in zip(r_coils, r_widths,
z_coils, z_widths):
r = r_coil + r_sq * r_width
z = z_coil + z_sq * z_width
P.plot(r, z, color='b', lw=lw)
# optional to plot coil on left side
# if plot_left_side:
# P.plot(-r, z, color='b')
# FC parameters
# inner midplane gap half-angle
delta_deg = 2
delta = delta_deg * pi / 180.0
# number of points for arcs, etc.
npts = 101
# FC arc from midplane gap to outside injector feature
theta_1 = P.linspace(pi + delta, 2 * pi - theta_u, npts)
theta_2 = P.linspace(theta_u, pi - delta, npts)
# outline of the FC, in two parts
r_FC_1 = r_0 + a * P.cos(theta_1)
z_FC_1 = a * P.sin(theta_1)
r_FC_2 = r_0 + a * P.cos(theta_2)
z_FC_2 = a * P.sin(theta_2)
# injector feature angles
theta_inj_1 = P.linspace(pi + theta_u, 1.5 * pi, npts)
theta_inj_2 = P.linspace(0.5 * pi, pi - theta_u, npts)
# outline of the injector feature, in two parts
r_inj_1 = r_u_1 + a_u * P.cos(theta_inj_1)
z_inj_1 = z_u_1 + a_u * P.sin(theta_inj_1)
r_inj_2 = r_u_2 + a_u * P.cos(theta_inj_2)
z_inj_2 = z_u_2 + a_u * P.sin(theta_inj_2)
l_inj = 0.67 - 0.5 * w_i
#smaller curve feature from Uberplate to injector
r_tran = z_u_1 - a_u - 0.5 * w_i
theta_tran_1 = P.linspace(pi, 1.5 * pi, npts)
r_tran_1 = r_u + r_tran + r_tran * P.cos(theta_tran_1)
z_tran_1 = 0.5 * w_i + r_tran + r_tran * P.sin(theta_tran_1)
# default linewidth
lw = 2
# flux conserver color
FC_col = 'r'
P.clf()
P.axes(aspect='equal')
# plot FC sections and injector transition
P.plot(r_FC_1, z_FC_1, r_FC_2, z_FC_2, color=FC_col, lw=lw)
#P.plot(-r_FC_1, z_FC_1, -r_FC_2, z_FC_2, color=FC_col, lw=lw)
#P.plot(r_inj_1, z_inj_1, -r_inj_1, z_inj_1, color=FC_col, lw=lw)
#P.plot(r_inj_2, z_inj_2, -r_inj_2, z_inj_2, color=FC_col, lw=lw)
P.plot(r_inj_1, z_inj_1, color=FC_col, lw=lw)
P.plot(r_inj_2, z_inj_2, color=FC_col, lw=lw)
# injector to Uberplate transition feature
P.plot(r_tran_1, z_tran_1, color=FC_col, lw=lw)
P.plot(r_tran_1, -z_tran_1, color=FC_col, lw=lw)
# sides of injector
#P.plot([r_u, r_u + l_inj],
# [0.5 * w_u, 0.5 * w_u], color=FC_col, lw=lw)
#P.plot([r_u, r_u + l_inj],
# [-0.5 * w_u, -0.5 * w_u], color=FC_col, lw=lw)
P.plot([r_u + r_tran, r_u + l_inj],
[0.5 * w_i, 0.5 * w_i], color=FC_col, lw=lw)
P.plot([r_u + r_tran, r_u + l_inj],
[-0.5 * w_i, -0.5 * w_i], color=FC_col, lw=lw)
#P.plot([-(r_0 + 0.5 * w_u), -(r_0 + 0.5 * w_u)],
# [z_u, z_u + 0.4], color=FC_col, lw=lw)
#P.plot([-(r_0 - 0.5 * w_u), -(r_0 - 0.5 * w_u)],
# [z_u, z_u + 0.4], color=FC_col, lw=lw)
# end of injector
th = P.linspace(-0.5 * pi, 0.5 * pi, npts)
#rs = r_u + l_inj + 0.5 * w_u * P.cos(th)
rs = r_u + l_inj + 0.5 * w_i * P.cos(th)
#zs = 0.5 * w_u * P.sin(th)
zs = 0.5 * w_i * P.sin(th)
P.plot(rs, zs, color=FC_col, lw=lw)
#P.plot(-rs, zs, color=FC_col, lw=lw)
delta2_deg = 0.0
delta2 = delta2_deg * pi / 180.0
r_bs = r_u + l_inj
a_bs = self.coil_set.r_coils[-3] - 0.5 * \
self.coil_set.r_widths[-3] - (r_u + l_inj)
a_b = 0.7
#r_m = 0.5 / r_bs * (a_b ** 2 - a_bs ** 2 + \
# (r_bs + r_0) ** 2 - r_0 ** 2)
#z_m = P.sqrt(a_b ** 2 - (r_m - r_0) ** 2)
#
#theta_m = P.arctan2(z_m, r_m)
#thm = P.linspace(-theta_m, theta_m, npts)
# shield for midplane coil
#delta_m = 17.4 * pi / 180.0
delta_m = -5.0 * pi / 180.0
thm = P.linspace(-0.5 * pi + delta_m, 0.5 * pi - delta_m, npts)
#P.plot(r_bs + a_bs * P.cos(thm), a_bs * P.sin(thm), color='k', lw=lw)
# angle to meet shield at midplane coil
#delta_b = 6.8 * pi / 180.0
delta_b = 11.5 * pi / 180.0
theta2 = P.linspace(-0.5 * pi, -delta_b, npts)
r_vv = r_0 + r_vv0 * P.cos(theta2)
z_vv = r_vv0 * P.sin(theta2)
#P.plot(r_vv, z_vv, color='k', lw=lw)
theta2 = P.linspace(delta_b, 0.5 * pi, npts)
r_vv = r_0 + r_vv0 * P.cos(theta2)
z_vv = r_vv0 * P.sin(theta2)
#P.plot(r_vv, z_vv, color='k', lw=lw)
#P.plot(-r_vv, z_vv, color='k', lw=lw)
#P.plot([0, r_vv[-1]], [z_vv[-1], z_vv[-1]], color='k', lw=lw)
#P.plot([0, r_vv[-1]], [-z_vv[-1], -z_vv[-1]], color='k', lw=lw)
#tripcolor(r_pr, z_pr, p_pr)
#levels = linspace(psi_mod.min(), psi_mod.max(), 30)
#levels = P.arange(-15.0, 15.0 + 0.5, 0.5) * i_fact
#delta_psi = 10.0
#psi_min = -20.0
#psi_max = 100.0
#levels = arange(psi_min, psi_max + delta_psi, delta_psi)
#P.contour(r_arr, z_arr, psi, levels=levels)
#P.prism()
#P.hsv()
#P.contour(-r_arr, z_arr, psi, levels=levels)
##P.prism()
#P.hsv()
delta_psi = 0.5
psi_min = -15.0
psi_max = 30.0
self.plasma_coil.plot_plasma()
levels = P.arange(1.0, psi_max + delta_psi, delta_psi) * self.i_fact
P.contour(r_arr, z_arr, self.psi, levels=levels, colors='w')
levels = P.arange(psi_min, 1.0 + delta_psi, delta_psi) * self.i_fact
P.matplotlib.rcParams['contour.negative_linestyle'] = 'solid'
P.contour(r_arr, z_arr, self.psi, levels=levels, colors='gray')
# need to plot black contours on midplane coil
#rsub = P.where(r_arr[0, :] >= 5.0)[0]
#levels = P.arange(1.0, psi_max + delta_psi, delta_psi) * i_fact
#P.contour(r_arr[:, rsub], z_arr[:, rsub], psi[:, rsub],
# levels=levels, colors='gray')
P.plot(self.coil_set.r_floops, self.coil_set.z_floops, 'ro')
#P.plot(-r_floops, z_floops, 'ro')
#no_text = True
no_text = False
# whether to annotate in MA or MW
show_MA = True
if not no_text:
# annotate coil powers
for ii in xrange(self.coil_set.n_coils):
if self.i_floops[ii] >= 0:
signum = '+'
else:
signum = '-'
if show_MA:
tdata = 1e-6 * self.i_floops[ii]
else:
tdata = 1e-6 * self.i_floops[ii] ** 2 * R_b_coils[ii]
# P.text(r_b_coils[ii], z_b_coils[ii], '%s%3.2f' %
# (signum, 1e-6 * i_floops[ii] ** 2 * R_b_coils[ii]),
# ha='center', va='center', backgroundcolor='w')
P.text(self.coil_set.r_coils[ii], self.coil_set.z_coils[ii],
'%s%3.2f' % (signum, tdata),
ha='center', va='center', backgroundcolor='w')
if self.i_floops[-1] >= 0:
signum = '+'
else:
signum = '-'
P.text(r_0, 0.0, '%s%3.2f MA' % (signum, 1e-6 * self.i_floops[-1]),
va='center', backgroundcolor='w')
pcoils()
P.axvline()
coil_power = (self.i_floops[:self.coil_set.n_coils] ** 2 *
R_b_coils).sum()
coil_current = abs(self.i_floops[:-1]).sum()
if not no_text:
P.title(r'Total Coil Power = %4.2f MW, Current %4.2f MA-turns' %
(1e-6 * coil_power, 1e-6 * coil_current))
print r'Total Coil Power = %4.2f MW, Current %4.2f MA-turns' % \
(1e-6 * coil_power, 1e-6 * coil_current)
self.r_arr = r_arr
self.z_arr = z_arr
self.levels = levels
w_u = 0.35 # width of injector w_i = 0.28 # r location of Uberplate (injector feature is perpendicular # to Uberplate and tangent to the FC) r_u = r_0 + P.sqrt((2 * a - w_u) * a_u - w_u ** 2 / 4 + a ** 2) # Alternative: # Could also specific height of Uberplate, z_u, and calculate a_u as: #z_u = 1.95 #a_u = (w_u**2 / 4 + z_u**2 - a**2) / (2 * a - w_u) # angle of mating point of FC and Uberplate feature theta_u = P.arctan2(0.5 * w_u + a_u, a + a_u) # center of upper FC to Uberplate feature arc r_u_1 = r_u z_u_1 = w_u / 2 + a_u # center of lower FC to Uberplate feature arc r_u_2 = r_u z_u_2 = -(w_u / 2 + a_u) # big coil parameters b_coil_width = 0.1 b_coils_list = [] r_b_coils_list = [] z_b_coils_list = []
def do_step(self):
"""
Performs a step from the current state, using the current parameters.
The simulation results are also stored in self.[y|t]_[s|d]s_seq,
the states and times of single and double support phases.
*requires*:
self.
- params (dict): model and leg function parameters
- odd_step (bool): whether or not to trigger contact of leg2 (leg1 otherwise)
- state (6x float): the initial state
:args:
(None)
:returns:
t_ss, y_ss, t_ds, y_ds: time and simulation results for single stance and double stance
phases
:raises:
TypeError - invalid IC or parameter
SimulationError - if the simulation fails.
"""
# test initial conditions.
# test wether there is a current state and current parameters
if self.params is None:
raise TypeError("parameters not set")
if self.state is None:
raise TypeError("state (initial condition) not set")
if self.failed:
raise SimulationError("Simulation failed previously.")
#demo_p_reduced = [13100, 12900, 68.5 * pi / 180., -.05] # [k1, k2, alpha, beta]
#demo_p = { 'foot1' : [0, 0, 0],
# 'foot2' : [-1.5, 0, 0],
# 'm' : 80,
# 'g' : [0, -9.81, 0],
# 'lp1' : [13100, 1, 68.5 * pi / 180, -0.05], # leg params: stiffness, l0, alpha, beta
# 'lp2' : [12900, 1, 68.5 * pi / 180, 0.1],
# 'delta_beta' : .05
# }
p = self.params # shortcut
leadingleg = 1. if self.odd_step else 2.
pars = [p['lp1'][0],
p['lp2'][0],
p['lp1'][2],
p['lp2'][2],
p['lp1'][1],
p['lp2'][1],
p['lp1'][3],
p['lp2'][3],
p['m'],
p['g'][1],
p['foot1'][0],
p['foot1'][1],
p['foot1'][2],
p['foot2'][0],
p['foot2'][1],
p['foot2'][2],
leadingleg]
# maximal time for simulation of single stance or double stance (each)
max_T = 1.
# run single stance
self.buf[0, 1:] = array(self.state) #.copy()
N = self.odess.odeOnce(self.buf, self.t + max_T, dt=1e-3, pars = pars)
self.state = self.buf[N,1:].copy()
self.y_ss_seq.append(self.buf[:N+1, 1:].copy())
self.t_ss_seq.append(self.buf[:N+1,0].copy())
# quick sanity check: simulation time not exceeded?
if self.buf[N,0] - self.t >= max_T - 1e-2:
self.failed=True
print "N=", N
raise SimulationError("Maximal simulation time (single stance) reached!")
self.t = self.buf[N,0]
# touchdown detected:
# update foot parameters
# (1) foot2 = foot1
# (2) foot1 = [NEW]
# (3) leading_leg = ~leading_leg
# update leg positions; change trailing leg
y = self.state # shortcut
vx, vz = y[3], y[5]
a_v_com = -arctan2(vz, vx) # correct with our coordinate system
pars[13] = pars[10]
pars[15] = pars[12]
if pars[16] == 1.:
# stance leg is leg 1 -> update leg 2 params
pars[10] = y[0] + cos(pars[3]) * cos(pars[7] + a_v_com) * pars[5]
pars[12] = y[2] - cos(pars[3]) * sin(pars[7] + a_v_com) * pars[5]
#pars[13] = res[N, 1] + cos(pars[3])*cos(pars[7])*pars[5]
#pars[15] = res[N, 3] + cos(pars[3])*sin(pars[7])*pars[5]
pars[16] = 2.;
else:
pars[10] = y[0] + cos(pars[2]) * cos(pars[6] + a_v_com) * pars[4]
pars[12] = y[2] - cos(pars[2]) * sin(pars[6] + a_v_com) * pars[4]
#pars[10] = res[N, 1] + cos(pars[2])*cos(pars[6])*pars[4]
#pars[12] = res[N, 3] + cos(pars[2])*sin(pars[6])*pars[4]
pars[16] = 1.;
self.params['foot1'] = pars[10:13][:]
self.params['foot2'] = pars[13:16][:]
# run double stance
self.buf[0, 1:] = array(self.state) #.copy()
N = self.odeds.odeOnce(self.buf, self.t + max_T, dt=1e-3, pars = pars)
self.state = self.buf[N,1:].copy()
self.feet1_seq.append(self.params['foot1'])
self.feet2_seq.append(self.params['foot2'])
self.y_ds_seq.append(self.buf[:N+1, 1:].copy())
self.t_ds_seq.append(self.buf[:N+1,0].copy())
# quick sanity check: simulation time not exceeded?
if self.buf[N,0] - self.t >= max_T - 1e-2:
self.failed=True
raise SimulationError("Maximal simulation time (double stance) reached!")
self.t = self.buf[N,0]
#self.y_ds_seq.append(y2)
#self.t_ds_seq.append(t2)
self.odd_step = not self.odd_step
return self.t_ss_seq[-1], self.y_ss_seq[-1], self.t_ds_seq[-1], self.y_ds_seq[-1]
if self.odd_step:
td_pars = self.params['lp2'][1:] + [ground, ] # set touchdown parameters
td_pars_2 = self.params['lp2'] # another format of touchdown parameters (for get_touchdown)
newfoot = 'foot2' # which foot position to update?
to_evt_fun = self.legfunc1 # force generation for takeoff trigger in double support
to_evt_ds_refine = self.legfunc1 # function for refinement of DS
self.odd_step = False # next step is "even": leg "2" in single stance on ground
else:
td_pars = self.params['lp1'][1:] + [ground, ] # set touchdown parameters
td_pars_2 = self.params['lp1'] # another format of touchdown parameters (for get_touchdown)
newfoot = 'foot1' # which foot position to update?
to_evt_fun = self.legfunc2 # force generation for takeoff trigger in double support
to_evt_ds_refine = self.legfunc2 # function for refinement of DS
self.odd_step = True # next step is "odd": leg "1" in single stance on ground
# stage 1a: simulate until vy=0
self.singleStance = True
self.ode.event = self.evt_vy0
if self.state[4] <= 0:
self.failed = True
self.ErrMsg = ("initial vertical velocity < 0: single " +
"stance apex cannot be reached!")
t0 = self.t
tE = t0 + max_T
t_a, y_a = self.ode(self.state, t0, tE, dt=self.dt)
#d_pars_l2 = self.params['lp2'][1:] + [ground, ]
if self.DEBUG:
print "finished stage 1 (raw)"
if t_a[-1] >= tE:
self.failed = True
self.ErrMsg = ("max. simulation time exceeded - " +
"this often indicates simulation failure")
else:
tt1, yy1 = self.ode.refine(lambda tf, yf: yf[4])
if self.DEBUG:
print "finished stage 1 (fine)"
self.state = yy1
# compute forces
if not self.skip_forces:
forces_ss = [self.dy_Stance(xt, xy, self.params, return_force=True) for
xt, xy in zip(t_a, y_a)]
#self.forces_ss_seq.append()
t = [] # dummy, if next step is not executed
y = array([[]])
if not self.failed:
self.update_params_ss()
# stage 1b: simulate until touchdown of leading leg
# touchdown event of leading leg
self.ode.event = lambda t,states,traj,p: self.touchdown_event(t, states, traj, td_pars)
t0 = tt1
tE = t0 + max_T
t, y = self.ode(self.state, t0, tE, dt=self.dt)
if self.DEBUG:
print "finished stage 2 (raw)"
if t[-1] >= tE:
self.failed = True
self.ErrMsg = ("max. sim time exceeded in single stance - no "
+ "touchdown occurred")
else:
#d_pars_l2 = self.params['lp2'][1:] + [ground, ]
tt, yy = self.ode.refine(lambda tf, yf: self.touchdown_event_refine(tf, yf, td_pars))
if self.DEBUG:
print "finished stage 2 (fine)"
self.state = yy
forces_ss.extend([self.dy_Stance(xt, xy, self.params, return_force=True) for
xt, xy in zip(t[1:], y[1:, :])])
if not self.skip_forces:
self.forces_ss_seq.append(vstack(forces_ss))
if not self.failed:
# allow application of control law
self.t_td = tt
self.singleStance = False
self.update_params_td()
# accumulate results from stage 1a and stage 1b
if not self.failed:
t = hstack([t_a, t[1:]])
y = vstack([y_a, y[1:, :]])
# stage 2: double support
# compute leg 2 touchdown position
t2_a = []
y2_a = array([[]])
if not self.failed:
xf, yf, zf = self.get_touchdown(tt, yy, td_pars_2)
self.params[newfoot] = [xf, yf, zf]
# stage 2a: simulate until vy=0
self.ode.event = self.evt_vy0
t0 = tt
tE = t0 + max_T
t2_a, y2_a = self.ode(self.state, t0, tE, dt=self.dt)
if t2_a[-1] >= tE:
self.failed = True
self.ErrMsg = ("max. sim time exceeded - no nadir event " +
"detected in double stance")
if self.DEBUG:
print "finished stage 3 (raw)"
else:
tt2, yy2 = self.ode.refine(lambda tf, yf: yf[4])
if self.DEBUG:
print "finished stage 3 (fine)"
self.state = yy2
if not self.skip_forces:
forces_ds = [self.dy_Stance(xt, xy, self.params, return_force=True) for
xt, xy in zip(t2_a, y2_a)]
if not self.failed:
# allow application of control law
self.update_params_ds()
# stage 2b: double stance - simulate until takeoff of trailing leg
# define and solve double stance ode
#ode = integro.odeDP5(self.dy_Stance, pars=self.params)
# event is takeoff of leg 1
t2_b = []
y2_b = array([[]])
if not self.failed:
self.ode.event = lambda t,states,traj,p: self.takeoff_event(t,
states, traj, p, legfun=to_evt_fun)
t0 = tt2
tE = t0 + max_T
t2_b, y2_b = self.ode(self.state, t0, tE, dt=self.dt)
if t2_b[-1] >= tE:
self.failed = True
self.ErrMsg = ("sim. time exeeded - takeoff of trailing leg " +
"not detected")
if self.DEBUG:
print "finished stage 4 (raw)"
else:
# refinement: force reaches zero
tt, yy = self.ode.refine(lambda tf, yf: to_evt_ds_refine(tf, yf, self.params))
if self.DEBUG:
print "finished stage 4 (fine)"
self.state = yy
if not self.skip_forces:
forces_ds.extend([self.dy_Stance(xt, xy, self.params, return_force=True) for
xt, xy in zip(t2_b[1:], y2_b[1:, :])])
self.forces_ds_seq.append(vstack(forces_ds))
# allow application of control law
self.t_to = tt
self.singleStance = True
self.update_params_to()
# accumulate results from stage 1a and stage 1b
if not self.failed:
t2 = hstack([t2_a, t2_b[1:]])
y2 = vstack([y2_a, y2_b[1:, :]])
#store simulation results
if not self.failed:
self.y_ss_seq.append(y)
self.y_ds_seq.append(y2)
self.t_ss_seq.append(t)
self.t_ds_seq.append(t2)
self.feet1_seq.append(self.params['foot1'])
self.feet2_seq.append(self.params['foot2'])
if not self.failed:
if len(t2) > 0:
self.t = t2[-1]
if self.failed:
raise SimulationError(self.ErrMsg)
return t, y, t2, y2
def lmcextinct(ra, dec, **kw):
"""Use the Zaritsky & Harris (ZH) map to get A_V extinction in the LMC.
INPUT:
ra -- decimal degrees of right ascension
dec -- decimal degrees of declination
OPTIONAL INPUT:
method='griddata' /'nearest' -- interpolation method
map='both' /'hot'/'cool' -- Which ZH map to use
hot='lmc_hotav.fits' -- filename of hot map
cool='lmc_coolav.fits' -- filename of cool map
null=0.0 -- "no data" value in map
verbose=False / True -- print out some spam and eggs
EXAMPLE:
ra = hms('05:51:56.6')
dec = dms('-66:25:08.5')
lmcextinct(ra, dec)
If their map returns null, interpolate from the surroundings.
Note that these calculations are definitely _not_ optimized.
SEE ALSO:
hms, dms"""
# 2009-02-15 22:11 IJC: Created and tested.
try:
from astropy.io import fits as pyfits
except:
import pyfits
from matplotlib.mlab import griddata
from pylab import meshgrid, arange, array, sqrt, cos, sin, arctan2, arcsin
ra = array([ra]).copy().ravel()
dec = array([dec]).copy().ravel()
defaults = dict(method='griddata', map='hot', hot='lmc_hotav_clean2.fits',
cool='lmc_coolav.fits', null=0.0, verbose=False)
for key in defaults:
if (not kw.has_key(key)):
kw[key] = defaults[key]
if kw['map']=='both':
kw['map'] = 'hot'
ret1 = lmcextinct(ra, dec, **kw)
kw['map'] = 'cool'
ret2 = lmcextinct(ra, dec, **kw)
return (ret1, ret2)
else:
map = kw[kw['map']]
verbose = kw['verbose']
avmap = pyfits.getdata(map)
avhdr = pyfits.getheader(map)
nx = avhdr['naxis1']
ny = avhdr['naxis2']
cx = avhdr['crpix1']
cy = avhdr['crpix2']
x0 = avhdr['crval1']
y0 = avhdr['crval2']
dx = avhdr['cd1_1']
dy = avhdr['cd2_2']
xx,yy = meshgrid(arange(nx),arange(ny))
goodind = (avmap<>kw['null'])
# I don't know how the following WCS calculation works, but it
# does -- thank you, Calabretta & Greisen 2002!
d2r = pi/180.
phi = arctan2( xx-cx+1, yy-cy+1) + pi
theta = arctan2(1./(d2r*dy), sqrt((yy-cy)**2 + (xx-cx)**2))
mapdec = arcsin(sin(theta)*sin(y0*d2r) - cos(theta)*cos(phi)*cos(y0*d2r))/d2r
mapra = arcsin(cos(theta) * sin(phi) / cos(mapdec*d2r))/d2r + x0
if kw['method']=='griddata':
ragood = mapra[goodind]
decgood = mapdec[goodind]
avgood = avmap[goodind]
if verbose>0:
print 'ra.shape>>' + str(ra.shape)
# TBD: Vectorize this calculation; an interpolative solution
# should exist.
avlist = []
for ii in range(len(ra)):
if verbose>0:
print 'ra, dec>>' + str((ra[ii], dec[ii]))
if kw['method']=='nearest':
distmap = (mapra-ra[ii])**2 + (mapdec-dec[ii])**2
# If multiple values are equally near, average them:
val = avmap[distmap==distmap.min()].mean()
avlist.append(val)
elif kw['method']=='griddata':
avlist.append(griddata(ragood, decgood, avgood, array([ra[ii]]), array([dec[ii]])))
else:
print "Invalid method specified!"
avlist.append(-1)
return avlist
def eph2D(self,epoch):
if self.ref == None:
return ([0,0,0],[0,0,0])
dt = epoch-self.epoch
#print "dT",dt
# Step 1 - Determine mean anomaly at epoch
if self.e == 0:
M = self.M0 + dt * sqrt(self.ref.mu / self.a**3)
M %= PI2
E = M
ta = E
r3 = (self.h**2/self.ref.mu)
rv = r3 * array([cos(ta),sin(ta),0])
v3 = self.ref.mu / self.h
vv = v3 * array([-sin(ta),self.e+cos(ta),0])
return (rv,vv)
if self.e < 1:
if epoch == self.epoch:
M = self.M0
else:
M = self.M0 + dt * sqrt(self.ref.mu / self.a**3)
M %= PI2
# Step 2a - Eccentric anomaly
try:
E = so.newton(lambda x: x-self.e * sin(x) - M,M)
except RuntimeError: # Debugging a bug here, disregard
print "Eccentric anomaly failed for",self.obj.name
print "On epoch",epoch
print "Made error available at self.ERROR"
self.ERROR = [lambda x: x-self.e * sin(x) - M,M]
raise
# Step 3a - True anomaly, method 1
ta = 2 * arctan2(sqrt(1+self.e)*sin(E/2.0), sqrt(1-self.e)*cos(E/2.0))
#print "Ta:",ta
# Method b is faster
cosE = cos(E)
ta2 = arccos((cosE - self.e) / (1-self.e*cosE))
#print "e",self.e
#print "M",M
#print "E",E
#print "TA:",ta
#print "T2:",ta2
# Step 4a - distance to central body (eccentric anomaly).
r = self.a*(1-self.e * cos(E))
# Alternative (true anomaly)
r2 = (self.a*(1-self.e**2) / (1.0 + self.e * cos(ta)))
# Things get crazy
#h = sqrt(self.a*self.ref.mu*(1-self.e**2))
r3 = (self.h**2/self.ref.mu)*(1.0/(1.0+self.e*cos(ta)))
#print "R1:",r
#print "R2:",r2
#print "R3:",r3
rv = r3 * array([cos(ta),sin(ta),0])
#v1 = sqrt(self.ref.mu * (2.0/r - 1.0/self.a))
#v2 = sqrt(self.ref.mu * self.a) / r
v3 = self.ref.mu / self.h
#meanmotion = sqrt(self.ref.mu / self.a**3)
#v2 = sqrt(self.ref.mu * self.a)/r
#print "v1",v1
#print "v2",v2
#print "v3",v3
#print "mm",meanmotion
# Velocity can be calculated from the eccentric anomaly
#vv = v1 * array([-sin(E),sqrt(1-self.e**2) * cos(E),0])
# Or from the true anomaly (this one has an error..)
#vv = sqrt(self.ref.mu * self.a)/r * array([-sin(ta),self.e+cos(ta),0])
vv = v3 * array([-sin(ta),self.e+cos(ta),0])
#print "rv",rv
#print "vv",vv
#print "check",E,-sin(E),v1*-sin(E)
#print "V1:",vv
#print "V2:",vv2
return (rv,vv)
elif self.e > 1:
# Hyperbolic orbits
# Reference: Stephen Kemble: Interplanetary Mission Analysis and Design, Pg 220-221
M = self.M0 + dt * sqrt(-(self.ref.mu / self.a**3))
#print "M0",self.M0
#print "M",M
#print "M",M
#print "M0",self.M0
# Step 2b - Hyperbolic eccentric anomaly
#print "Hyperbolic mean anomaly:",M
F = so.newton(lambda x: self.e * sinh(x) - x - M,M,maxiter=1000)
#F = -F
H = M / (self.e-1)
#print "AAAA"*10
#print "F:",F
#print "H:",H
#F=H
#print "Hyperbolic eccentric anomaly:",F
# Step 3b - Hyperbolic true anomaly?
# This is wrong prooobably
#hta = arccos((cosh(F) - self.e) / (1-self.e*cosh(F)))
#hta = arccos((self.e-cosh(F)) / (self.e*cosh(F) - 1))
# TÄSSÄ ON BUGI
hta = arccos((cosh(F) - self.e) / (1 - self.e*cosh(F)))
hta2 = 2 * arctan2(sqrt(self.e+1)*sinh(F/2.0),sqrt(self.e-1)*cosh(F/2.0))
hta3 = 2 * arctan2(sqrt(1+self.e)*sinh(F/2.0),sqrt(self.e-1)*cosh(F/2.0))
hta4 = 2 * arctan2(sqrt(self.e-1)*cosh(F/2.0),sqrt(1+self.e)*sinh(F/2.0))
#print "Hyperbolic true anomaly:",degrees(hta)
# This is wrong too
#hta2 = 2 * arctan2(sqrt(1+self.e)*sin(F/2.0), sqrt(1-self.e)*cos(F/2.0))
if M == self.M0:
print "HTA1:",degrees(hta)
print "HTA2:",degrees(hta2)
print "HTA3:",degrees(hta3)
print "HTA4:",degrees(hta4)
# According to http://mmae.iit.edu/~mpeet/Classes/MMAE441/Spacecraft/441Lecture17.pdf
# this is right..
hta = hta2
#print cos(hta)
#print cosh(hta)
#####hta = arctan(sqrt((self.e-1.0)/self.e+1.0) * tanh(F/2.0)) / 2.0
#print "Mean anomaly:",M
#print "Hyperbolic eccentric anomaly:",F
#print "HTA:",hta
#raise
# Step 4b - Distance from central body?
# Can calculate it from hyperbolic true anomaly..
#p = self.a*(1-self.e**2)
#r = p / (1+self.e * cos(hta))
r3 = (self.h**2/self.ref.mu)*(1.0/(1.0+self.e*cos(hta)))
v3 = self.ref.mu / self.h
# But it's faster to calculate from eccentric anomaly
#r = self.a*(1-self.e * cos(F))
#print "Hyper r1:",r
#print "Hyper r2:",r2
rv = r3 * array([cos(hta),sin(hta),0])
#http://en.wikipedia.org/wiki/Hyperbola
#rv = array([ r * sin(hta),-self.a*self.e + r * cos(hta), 0])
#sinhta = sin(hta)
#coshta = cos(hta)
#print self.ref.mu,r,self.a,hta
#vv = sqrt(self.ref.mu *(2.0/r - 1.0/self.a)) * array([-sin(hta),self.e+cos(hta),0])
vv = v3 * array([-sin(hta),self.e+cos(hta),0])
return (rv,vv)
dec = float(source[source['target'] ==True]['dec'].unique()[0])
geo = pd.merge(antenna,station, left_on='stationId', right_on = 'stationId', how = 'inner')
geo['pos'] = geo.apply(lambda x: arrayParser(x['position'],1) , axis = 1 )
geo['lat'], geo['lon'], geo['alt'] = zip(*geo.apply(lambda x: gh.turn_xyz_into_llh(float(x.pos[0]),float(x.pos[1]),float(x.pos[2]), 'wgs84'),axis=1))
field['ra'],field['dec'] = zip(*field.apply(lambda x: arrayParser(x['referenceDir'],2)[0], axis = 1))
correctedList = list()
correctedList.append((ra,dec,0))
for i in pointing.query('go == True').rowNum.values:
row = rows[i]
dRA,dDec = [[float(str(p[0]).replace('rad','').replace(',','.')),float(str(p[1]).replace('rad','').replace(',','.'))] for p in row.sourceOffset() ][row.numSample()/2]
Pl = [pl.cos(dRA)*pl.cos(dDec), pl.sin(dRA)*pl.cos(dDec), pl.sin(dDec)]
Ps = rot(Pl, ra, dec)
correctedList.append((pl.arctan2(Ps[1], Ps[0]) % (2.*pl.pi), pl.arcsin(Ps[2]), i))
correctedAll = pd.DataFrame(correctedList, columns=['ra','dec', 'row'])
corrected = correctedAll[['ra','dec']]
corrected['series'] = 'Corrected (Pointing)'
observed = field[field['target'] == True][['fieldId','ra','dec']]
observed.ra = observed.ra.astype(float)
observed.dec = observed.dec.astype(float)
observed = observed.loc[observed['fieldId'].isin(bar) ]
observed = observed.reset_index(drop=True)
corrected = corrected.drop_duplicates()
corrected = corrected.reset_index(drop=True)
cat = SkyCoord(observed.ra.values * u.rad, observed.dec.values * u.rad, frame='icrs')
cat2 = SkyCoord(corrected.ra.values * u.rad, corrected.dec.values *u.rad, frame='icrs')
match, separ, dist = cat2.match_to_catalog_sky(cat)
data, nn = getData(ifile, minRank=minRank, maxRank=maxRank)
NPOZ = addPoz(data, minRank=minRank, maxRank=maxRank)
ans = doMds(data, atype, minRank, maxRank, vbose=vbose, NPOZ=NPOZ, ishow=True)
ofile = 'poz100_%d.csv' % minRank
ofp = open(ofile, 'w')
ofp.write('name,xpos,ypos\n')
for i, v in enumerate(ans):
name = v[0]
xpos, ypos = v[1][:]
if name=='fivetwentyone':
theta = pylab.arctan2(xpos, ypos)
for i, v in enumerate(ans):
name = v[0]
xpos, ypos = v[1][:]
if name=='Poz' and minRank<42:
xpos = -1.0
ypos = +1.0
ofp.write('%s,%.4f,%.4f\n' % (name, xpos, ypos))
ofp.close()
# doDendro(names, dissim, vbose=0,cmetric = 'euclidean')
if ishow:
pylab.show()
def deconvolve_gauss(
meas_maj, # measured major axis
meas_min, # measured minor axis
meas_pa, # measured position angle
beam_maj, # beam major axis
beam_min, # beam minor axis
beam_pa): # beam position angle
# ADAPTED FROM gaupar.for in MIRIAD via K. Sandstrom
# IO in radians
meas_theta = meas_pa
beam_theta = beam_pa
import pylab as pl
# MATH (FROM MIRIAD VIA K. SANDSTROM)
alpha = \
(meas_maj*pl.cos(meas_theta))**2 + (meas_min*pl.sin(meas_theta))**2 - \
(beam_maj*pl.cos(beam_theta))**2 - (beam_min*pl.sin(beam_theta))**2
beta = \
(meas_maj*pl.sin(meas_theta))**2 + (meas_min*pl.cos(meas_theta))**2 - \
(beam_maj*pl.sin(beam_theta))**2 - (beam_min*pl.cos(beam_theta))**2
gamma = \
2.*((meas_min**2-meas_maj**2)*pl.sin(meas_theta)*pl.cos(meas_theta) -
(beam_min**2-beam_maj**2)*pl.sin(beam_theta)*pl.cos(beam_theta))
s = alpha + beta
t = pl.sqrt((alpha - beta)**2 + gamma**2)
#FIND THE SMALLEST RESOLUTION
limit = pl.array([meas_min, meas_maj, beam_maj, beam_min]).min()
limit = 0.1 * limit * limit
src_maj = pl.nan
src_min = pl.nan
src_pa = pl.nan
if (alpha < 0 or beta < 0):
# complete failure - alpha, beta are squares:
worked = False
# ... CLOSE TO A POINT SOURCE
if (0.5 * (s - t) < limit and alpha > -1 * limit
and beta > -1 * limit):
point = True
src_maj = 0.
src_min = 0.
src_pa = pl.nan
else:
point = False
else:
src_maj = pl.sqrt(0.5 * (s + t))
# if (pl.absolute(gamma)+pl.absolute(alpha-beta) == 0):
# src_pa = 0
# else:
src_pa = 0.5 * pl.arctan2(-1 * gamma, alpha - beta)
if (s < t):
# soft failure - de_maj may still be ok, but still call it failed
# print alpha,beta,s,t
worked = False
point = False
src_min = 0.
else:
#... SUCCESS
src_min = pl.sqrt(0.5 * (s - t))
worked = True
point = False
return src_maj, src_min, src_pa, point, worked
def ellfit(x, y, wt=None):
import pylab as pl
# Calculate the best fit ellipse for an X and Y distribution, allowing
# for weighting.
# OUTPUTS:
# MAJOR - major axis in same units as x and y
# MINOR - minor axis in same units as x and y
# POSANG - the position angle CCW from the X=0 line of the coordinates
#
# Adam: The intensity weighted major and minor values are equal to the
# second moment.
# For equal weighting by pixel (of the sort that
# might be done for blob analysis) the ellipse fit to the
# half-maximum area will have semimajor axis equal to 1./1.69536 the
# second moment. For the quarter maximum surface this is 1./1.19755.
#
# i.e. if you run this with x,y down to zero intensity (like integrating
# to infinity), and wt=intensity, you get the second moments sig_major,
# sig_minor back
# if you run this with x,y down to half-intensity, and wt=None, you get
# sigx/1.6986 back (not sure why my integra differs from his slightly)
#
# but adam did not have the factor of 4 to turn eigenval into major axis
#
# translation: if we run this with intensity weight, we get
# the second moment back (a sigma). for flat weights i think he means
# the halfmax contour semimajor axis
if type(wt) == type(None):
wt = x * 0.0 + 1.0
tot_wt = wt.sum()
# WEIGHTED X AND Y CENTERS
x_ctr = (wt * x).sum() / tot_wt
y_ctr = (wt * y).sum() / tot_wt
# BUILD THE MATRIX
i11 = (wt * (x - x_ctr)**2).sum() / tot_wt
i22 = (wt * (y - y_ctr)**2).sum() / tot_wt
i12 = (wt * (x - x_ctr) * (y - y_ctr)).sum() / tot_wt
mat = [[i11, i12], [i12, i22]]
# CATCH THE CASE OF ZERO DETERMINANT
if pl.det(mat) == 0:
return pl.nan, pl.nan, pl.nan
if pl.any(pl.isnan(mat)):
return pl.nan, pl.nan, pl.nan
# WORK OUT THE EIGENVALUES
evals, evec = pl.eig(mat)
# PICK THE MAJOR AXIS
absvals = pl.absolute(evals)
major = absvals.max()
maj_ind = pl.where(absvals == major)[0][0]
major_vec = evec[maj_ind]
min_ind = 1 - maj_ind
# WORK OUT THE ORIENTATION OF THE MAJOR AXIS
posang = pl.arctan2(major_vec[1], major_vec[0])
# compared to the original idl code, this code is returning
# pi-the desired angle, so:
# posang=pl.pi-posang
# if posang<0: posang = posang+pl.pi
# MAJOR AND MINOR AXIS SIZES
# turn into real half-max major/minor axis
major = pl.sqrt(evals[maj_ind]) * 4.
minor = pl.sqrt(evals[min_ind]) * 4.
return major, minor, posang
