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_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): 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: