def footPos(b, g): """ a - shoulder angle is always 0 b - femur angle (-90, 90) g - tibia angle (-180, 0) + /| / | A / | +---+ B """ Lc = 0.026 # mm Lf = 0.042 Lt = 0.063 b = d2r(b) g = d2r(g) # i don't use this right now A = Lf*sin(b) B = Lf*cos(b) pos = [ Lc + B, 0.0, -Lt + A ] return pos
def ik_to(self, x0, y0, z0):
"""
Calculates each joint angle to get the leg to coordinates x0,y0,z0
:return: the correct angles.
"""
# math adapted from http://arduin0.blogspot.fi/2012/01/inverse-kinematics-ik-implementation.html
# after I thought I had problems with my own
dx = x0 - self.position[0]
dy = y0 - self.position[1]
dz = z0 - self.position[2]
COXA_LENGTH = 20
FEMUR_LENGTH = robotData.femurLength
TIBIA_LENGTH = robotData.tibiaLength
x, y, z = dy * self.ydirection, -dz, -dx * self.ydirection
tibiaAngle = acos(
((sqrt(((sqrt(x**2 + z**2)) - COXA_LENGTH)**2 + y**2))**2 -
TIBIA_LENGTH**2 - FEMUR_LENGTH**2) /
(-2 * FEMUR_LENGTH * TIBIA_LENGTH)) * 180 / pi
coxaAngle = atan2(z, x) * 180 / pi
femurAngle = (((atan(((sqrt(x**2 + z**2)) - COXA_LENGTH) / y)) + (acos(
(TIBIA_LENGTH**2 - FEMUR_LENGTH**2 -
(sqrt(((sqrt(x**2 + z**2)) - COXA_LENGTH)**2 + y**2))**2) /
(-2 * FEMUR_LENGTH *
(sqrt(((sqrt(x**2 + z**2)) - COXA_LENGTH)**2 + y**2)))))) * 180 /
pi) - 90
return d2r(coxaAngle), d2r(femurAngle), d2r(tibiaAngle - 90)
def level2_valarray(fileobject,moment="REF",scan=0,rhiaz=False):
omadused=fileobject.scan_info()[scan]
try:
momentindex=omadused["moments"].index(moment)
except:
return None
varavatehulk=omadused["ngates"][momentindex]
asimuudid=list(fileobject.get_azimuth_angles([scan]))
andmed=fileobject.get_data(moment,varavatehulk,[scan])
steps=fileobject.get_range(scan,moment)
samm=(steps[2]-steps[1])/1000.0 #Derive xrange resolution in kms
mindistance=(steps[0]/1000.0-samm/2.0)
dataarray=[]
vanapiir=(asimuudid[-1]+asimuudid[0])/2.0 #Computing azimuth boundaries for gates
for i in xrange(len(andmed)):
jargmine = i+1 if i < len(andmed)-1 else 0
praeguneaz=asimuudid[i]
jargmineaz=asimuudid[jargmine]
if jargmineaz < praeguneaz:
jargmineaz+=360
uuspiir=(praeguneaz+jargmineaz)/2.0
d_az=uuspiir-vanapiir
if not rhiaz:
dataarray.append([d2r(vanapiir),d2r(d_az),andmed[i].tolist(None),mindistance])
else:
if (praeguneaz <= rhiaz and jargmineaz > rhiaz) or (jargmineaz > 360 and rhiaz < d_az):
return andmed[i].tolist(None)
vanapiir=uuspiir
return dataarray
def forward(self, t1, t2, t3, t4, degrees=True):
"""
Forward kinematics of the leg, note, default angles are all degrees.
The input angles are referenced to the DH frame arrangement.
"""
l1 = self.coxaLength
l2 = self.femurLength
l3 = self.tibiaLength
l4 = self.tarsusLength
if degrees:
t1 = d2r(t1)
t2 = d2r(t2)
t3 = d2r(t3)
t4 = d2r(t4)
x = (l1 + l2 * cos(t2) + l3 * cos(t2 + t3) +
l4 * cos(t2 + t3 + t4)) * cos(t1)
y = (l1 + l2 * cos(t2) + l3 * cos(t2 + t3) +
l4 * cos(t2 + t3 + t4)) * sin(t1)
z = l2 * sin(t2) + l3 * sin(t2 + t3) + l4 * sin(t2 + t3 + t4)
return (
x,
y,
z,
)
def geocoords(azrange,rlat,rlon,zoomlevel):
R=6371.0
az=d2r(azrange[0]) #Azimuth in radians
r=azrange[1]
rlat=d2r(rlat) #To radians
lat=asin(sin(rlat)*cos(r/R)+cos(rlat)*sin(r/R)*cos(az))
lon=rlon+r2d(atan2(sin(az)*sin(r/R)*cos(rlat),cos(r/R)-sin(rlat)*sin(lat)))
return round(r2d(lat),5),round(lon,5)
def keep_body_horizontal(self):
"""
Attempts to move feet in order to keep the body horizontal
"""
self.robot.read_imu()
roll = self.robot.orientation[1]
pitch = self.robot.orientation[0]
sin = math.sin(math.radians(pitch))
self.robot.move_leg_to_point('front_left', *rotate([0, 75, -50], 'y', d2r(roll)))
self.robot.move_leg_to_point('front_right', *rotate([0, 75, -50], 'y', d2r(roll)))
def keep_body_horizontal(self):
"""
Attempts to move feet in order to keep the body horizontal
"""
self.robot.read_imu()
roll = self.robot.orientation[1]
pitch = self.robot.orientation[0]
sin = math.sin(math.radians(pitch))
self.robot.move_leg_to_point('front_left',
*rotate([0, 75, -50], 'y', d2r(roll)))
self.robot.move_leg_to_point('front_right',
*rotate([0, 75, -50], 'y', d2r(roll)))
def keep_feet_horizontal(self):
"""
attempts to move feet to horizontal position, using the IMU inputs.
"""
self.robot.read_imu()
# print "orientation:", self.orientation
roll = self.robot.orientation[1]
pitch = self.robot.orientation[0]
sin = math.sin(math.radians(pitch))
# self.move_leg_to_point('front_left', 80, 75, -50 + sin*30)
# self.move_leg_to_point('front_right', -(80), 75, -50 - sin*30)
self.robot.move_leg_to_point('front_left', *rotate([60,75,-50],'y',-d2r(roll)))
self.robot.move_leg_to_point('front_right', *rotate([-60,75,-50],'y',-d2r(roll)))
def fk(self, a, b, g): """ angle are all degrees """ Lc = self.coxaLength Lf = self.femurLength Lt = self.tibiaLength if 0: phi = a theta2 = b theta3 = g params = [ # a_ij alpha_ij S_j theta_j [Lc, d2r(90.0), 0.0, d2r(theta2)], # frame 12 [Lf, d2r(0.0), 0.0, d2r(theta3)] # frame 23 ] r = T(params, d2r(phi)) foot = r.dot(np.array([Lt, 0.0, 0.0, 1.0])) return foot[0:-1] # DH return vector size 4, only grab (x,y,z) which are the 1st 3 elements else: a = d2r(a) b = d2r(b) g = d2r(g) foot = [ Lc*cos(a) + Lf*cos(a)*cos(b) + Lt*(-sin(b)*sin(g)*cos(a) + cos(a)*cos(b)*cos(g)), Lc*sin(a) + Lf*sin(a)*cos(b) + Lt*(-sin(a)*sin(b)*sin(g) + sin(a)*cos(b)*cos(g)), Lf*sin(b) + Lt*(sin(b)*cos(g) + sin(g)*cos(b)) ] return np.array(foot)
def keep_feet_horizontal(self):
"""
attempts to move feet to horizontal position, using the IMU inputs.
"""
self.robot.read_imu()
# print "orientation:", self.orientation
roll = self.robot.orientation[1]
pitch = self.robot.orientation[0]
sin = math.sin(math.radians(pitch))
# self.move_leg_to_point('front_left', 80, 75, -50 + sin*30)
# self.move_leg_to_point('front_right', -(80), 75, -50 - sin*30)
self.robot.move_leg_to_point('front_left',
*rotate([60, 75, -50], 'y', -d2r(roll)))
self.robot.move_leg_to_point('front_right',
*rotate([-60, 75, -50], 'y', -d2r(roll)))
def setUp(self):
TestCase.setUp(self)
self.outputdir = tempfile.mkdtemp()
self.workdir = tempfile.mkdtemp()
ops = Options('test')
ops.beam = PencilBeam(5e3)
ops.geometry.body.material = \
Material({6: 0.4, 13: 0.6}, absorption_energy_eV={ELECTRON: 234.0})
ops.detectors['xray'] = \
PhotonIntensityDetector.annular(d2r(40.0), d2r(5.0))
ops.limits.add(ShowersLimit(1))
self.ops = Converter().convert(ops)[0]
self.worker = Worker(program)
def axis_angle(vec, axis, theta):
"""
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation
Quaternion (axis/angle) to rotation matrix
in:
vec - point to rotate
axis - axis of rotation
theta - angle of rotation (degrees)
out: rotated vector/point
"""
theta = d2r(theta)
axis = np.array([0, 0, 1])
# axis = axis / sqrt(np.dot(axis, axis)) # normalizing axis
a = cos(theta / 2.0)
b, c, d = axis * sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
rot = np.array([
[aa+bb-cc-dd, 2.0*(bc-ad), 2.0*(bd+ac)],
[2.0*(bc+ad), aa-bb+cc-dd, 2.0*(cd-ab)],
[2.0*(bd-ac), 2.0*(cd+ab), aa-bb-cc+dd]
])
# print('rotateAroundCenter', np.dot(rot, vec))
return np.dot(rot, vec)
def setUp(self):
TestCase.setUp(self)
self.outputdir = tempfile.mkdtemp()
self.workdir = tempfile.mkdtemp()
ops = Options('test')
ops.beam = PencilBeam(5e3)
ops.geometry.body.material = \
Material({6: 0.4, 13: 0.6}, absorption_energy_eV={ELECTRON: 234.0})
ops.detectors['xray'] = \
PhotonIntensityDetector.annular(d2r(40.0), d2r(5.0))
ops.limits.add(ShowersLimit(1))
self.ops = Converter().convert(ops)[0]
self.worker = Worker(program)
def setUp(self):
unittest.TestCase.setUp(self)
self.ops = Options("op1")
self.ops.detectors['det1a'] = \
PhotonIntensityDetector((d2r(35.0), d2r(45.0)), (0.0, d2r(360.0)))
self.ops.detectors['det1b'] = \
PhotonIntensityDetector((d2r(30.0), d2r(40.0)), (0.0, d2r(360.0)))
self.ops.detectors['det2a'] = \
PhotonDepthDetector((d2r(30.0), d2r(40.0)), (0.0, d2r(360.0)), 100)
self.ops.detectors['det3'] = ShowersStatisticsDetector()
self.expander = OptionsExpanderSingleDetectorSameOpening([])
def fk(self, a, b, g): """ Forward kinematics of the leg, note, angle are all degrees """ Lc = self.coxaLength Lf = self.femurLength Lt = self.tibiaLength a = d2r(a) b = d2r(b) g = d2r(g) foot = [ (Lc + Lf*cos(b) + Lt*cos(b + g))*cos(a), (Lc + Lf*cos(b) + Lt*cos(b + g))*sin(a), Lf*sin(b) + Lt*sin(b + g) ] return np.array(foot)
def valarray(stream,min_val=-32,increment=0.5,product=94): #Values array for NEXRAD Level 3
'''Array of reflectivity values (data stream, minimum dBz value, dBZ increment, amount of radials)'''
dataarray=[]
radials_count=int(halfw(stream[0:2])) #Get amount of radials
p=2 #pointer
for i in xrange(radials_count):
amt=int(halfw(stream[p:p+2]))
az=int(halfw(stream[p+2:p+4]))/10.0
d_az=int(halfw(stream[p+4:p+6]))/10.0
mindistance=0
datarow=[]
p+=6
if product == 159 or product == 161 or product == 163:
#If Dual pol products with scale and offset
#scale=increment
#offset=min_val
for j in xrange(p,p+amt):
val=getbyte(stream[j])
if val > 1:
datarow.append((val-min_val)/increment)
else:
datarow.append(None)
elif product == 165: #If HCA
#Override usual decoding system and plot them as numbers
for j in xrange(p,p+amt):
val=getbyte(stream[j])
if val == 0: datarow.append(None)
elif val > 0 and val < 140: datarow.append(val/10-1)
elif val >= 140: datarow.append(val/10-4)
else:
for j in xrange(p,p+amt):
val=getbyte(stream[j])
if val > 1:
datarow.append(min_val+increment*(val-2.0))
else:
datarow.append(None)
dataarray.append([d2r(az),d2r(d_az),datarow,mindistance])
p+=amt
return dataarray
def Transform(lines, func):
for line in lines:
m = re.match(r'p f(\S+) w(\S+) h(\S+) v(\S+) (.*)', line)
if m:
f, w, h, v, post = m.groups()
w = int(w)
if w & 31:
w = (w + 31) & ~31
print 'Rounding size up to %d' % w
print 'Possible tile sizes: %d , %d , %d , etc.' % (w / 32, w / 16,
w / 8)
post = re.sub(' S[0-9,]+ ', ' ', post) # reset crop
yield 'p f0 w%d h%d v90 %s' % (w, w, post)
continue
m = re.match(r'i (.*) r(\S+) p(\S+) y(\S+) (.*)', line)
if m:
pre, r, p, y, post = m.groups()
r, p, y = func(d2r(float(r)), d2r(float(p)), d2r(float(y)))
yield 'i %s r%r p%r y%r %s' % (pre, r2d(r), r2d(p), r2d(y), post)
continue
yield line
def hdf5_valarray(fail,scan=None,rhiaz=None):
andmefail=HDF5Fail(fail,"r")
version=andmefail["what"].attrs.get("version")
if scan==None:
if version == "H5rad 1.2":
scan="scan1"
else:
scan="dataset1/data1"
angle=0
dataraw=andmefail["/"+scan+"/data"]
d_angle=d2r(360.0/len(dataraw))
if rhiaz != None: #If was collecting single gates for a whole RHI
dataraw=[dataraw[int(rhiaz*len(dataraw)/360.0)]]
whatattrs=andmefail["/"+scan+"/what"].attrs
quantity=whatattrs.get(u"quantity")
gain=whatattrs.get(u"gain")
offset=whatattrs.get(u"offset")
nodata=whatattrs.get(u"nodata")
undetect=whatattrs.get(u"undetect")
#print "------------"
#print "Quantity:",quantity
#print "Gain:",gain
#print "Offset:",offset
#print "Nodata:",nodata
#print "Undetect",undetect
if nodata == 0:
maxval=offset+gain*255
else:
maxval=offset+gain*nodata
#print "Seega maksimaalne võimalik väärtus:",maxval
maxradials=[]
dataarray=[]
for i in dataraw:
maxradials.append(i.min())
rida=i*1.0
rida[rida == undetect]=-999
rida[rida == nodata]=-999
rida[rida != -999]*=gain
rida[rida != -999]+=offset
rida=rida.tolist()
if quantity == "HCLASS":
rida=map(IrisMETEO,rida)
if rhiaz != None:
return map(tonone,rida)
else:
dataarray.append([angle,d_angle,map(tonone,rida),0])
angle+=d_angle
andmefail.close()
return dataarray
def leiasuund(rad,rad2,y,paised,zoom=1,center=[1000,1000],samm=1):
'''makepath(algasimuut, (asimuudi) samm, kaugus radarist, suurendusaste, renderduse keskpunkti asukoht), kauguse samm'''
koosinus=1 if not isinstance(paised[0],int) else cos(d2r(float(paised[17])))#Kui on nexradi produktid, arvesta nurga muutusega!
r=y*koosinus
r_new=r+samm*koosinus
coords1=getcoords((r_new,rad),zoom,center)
coords2=getcoords((r_new,rad+rad2),zoom,center)
coords3=getcoords((r,rad),zoom,center)
coords4=getcoords((r,rad+rad2),zoom,center)
startx1,starty1=coords3
startx2,starty2=coords4
endx1,endy1=coords1
endx2,endy2=coords2
dx1=endx1-startx1
dx2=endx2-startx2
dy1=endy1-starty1
dy2=endy2-starty2
return startx1,startx2,starty1,starty2,dx1,dx2,dy1,dy2
def beamheight(GR,alpha):
r=6371.0 #Earth radius
a=d2r(alpha) #Antenna angle in radians
R=GR/cos(a)
return R*sin(a)+(R**2)/(2*1.21*r) #WSR-88D height
from cmath import rect
from math import radians as d2r
from_polar = lambda r, d: rect(r, d2r(d))
with open('input.txt') as f:
instructions = [(line[0], int(line[1:-1])) for line in f]
position = 0 + 0j
orientation = 1 + 0j
for inst, n in instructions:
if inst == 'F': position += n * orientation
elif inst == 'N': position += n * +1j
elif inst == 'W': position += n * -1
elif inst == 'E': position += n * +1
elif inst == 'S': position += n * -1j
else:
sign = 1 if inst == 'L' else -1
rotation = from_polar(1, sign * n)
orientation *= rotation
print(round(abs(position.real) + abs(position.imag)))
position = 0 + 0j
waypoint = 10 + 1j
for inst, n in instructions:
if inst == 'F': position += n * waypoint
elif inst == 'N': waypoint += n * +1j
elif inst == 'W': waypoint += n * -1
elif inst == 'E': waypoint += n * +1
elif inst == 'S': waypoint += n * -1j
else:
