def findMiddle(token, check, player):
if not check:
x, y = token.getCoordinates()
# store all the necessary moves needed to win except the middle at the current token
upLeft = Coordinates(x - 1, y - 1)
upRight = Coordinates(x - 1, y + 1)
downLeft = Coordinates(x + 1, y - 1)
downRight = Coordinates(x + 1, y + 1)
neededTokenToWin = [upLeft, upRight, downLeft, downRight]
visitedList = []
p1Visited = player.getVisitedCoordinates()
for i in range(len(p1Visited)):
visitedList.append(p1Visited[i])
counter = 0
for i in visitedList:
for j in neededTokenToWin:
if i == j:
counter += 1
continue
if counter == 4: # It should make sure that the visitedList has no duplicated coordinates since logically it should not happen
# print('Middle found')
return True
else:
# print('Middle not found')
return False
else:
# print('Middle not found')
return False
def requires(self):
if Config.get().sampleBorders():
return (MakeRegions(), Coordinates.CreateSampleCoordinates(),
DenoiserCode())
else:
return (MakeRegions(), Coordinates.CreateFullCoordinates(),
DenoiserCode())
def requires(self):
return (PreReqs.LabelNames(),
Coordinates.CreateFullCoordinates(),
Coordinates.CreateSampleCoordinates(),
BorderGeoJSONWriterCode(),
BorderFactoryCode(),
MakeRegions(),
Denoise())
def createflightpathgraph(self):
x = [Coordinates.xy((el["phi"], el["lambda"]))[0] for el in self.flightdata]
y = [Coordinates.xy((el["phi"], el["lambda"]))[1] for el in self.flightdata]
assert (len(x) == len(y))
self.flightpathgraph = ROOT.TGraph(len(x), array.array('f', x), array.array('f', y))
self.flightpathgraph.SetMarkerStyle(20)
self.flightpathgraph.SetMarkerSize(0.3)
self.flightpathgraph.SetMarkerColor(ROOT.kRed)
def requires(self):
config = Config.get()
if config.sampleBorders():
return (Coordinates.CreateSampleCoordinates(),
cartograph.PreReqs.SampleCreator(
config.get("ExternalFiles", "vecs_with_id")),
ContourCode(), CreateContinents(), MakeRegions())
else:
return (Coordinates.CreateFullCoordinates(),
cartograph.PreReqs.LabelNames(), ContourCode(),
CreateContinents(), MakeRegions())
def find_goal_and_start_coordinates(self):
find_goal = False
find_start = False
for i in xrange(len(self.map)):
for j in xrange(len(self.map[i][0])):
if self.map[i][0][j] == 'E':
self.goal_coordinates = Coordinates.Coordinates(j,i)
find_goal = True
if self.map[i][0][j] == 'T':
self.start_coordinates = Coordinates.Coordinates(j,i)
find_start = True
if find_goal and find_start:
return
def test_get_current_coordinates(self) -> None:
finder10 = DistanceToDestinationFinder("")
self.assertEqual(Coordinates(0, 0), finder10.get_current_coordinates())
finder10.apply_turn_move_instruction("R1")
self.assertEqual(Coordinates(0, 1), finder10.get_current_coordinates())
finder10.apply_turn_move_instruction("R1")
self.assertEqual(Coordinates(-1, 1),
finder10.get_current_coordinates())
finder10.apply_turn_move_instruction("R1")
self.assertEqual(Coordinates(-1, 0),
finder10.get_current_coordinates())
finder10.apply_turn_move_instruction("R1")
self.assertEqual(Coordinates(0, 0), finder10.get_current_coordinates())
def __init__(self,hexagons=None,addresses=None,start=0,end=7,steps=[1],level=None,values=[]):
frame = inspect.currentframe()
_,_,_,values = inspect.getargvalues(frame)
nones = values.values().count(None)
inputs = 3
if inputs-nones > 1:
raise Exception, "Must specify one of either hexagons/addresses/level"
converter = coords.addressToUVW(4)
self.hexagon_array = []
if hexagons != None:
self.hexagon_array = hexagons
elif addresses != None:
self.addresses = addresses
elif level != None:
for i in range(7**level):
self.hexagon_array.append(Hexagon(spiral=Spiral(i),converter=converter))
else:
i = start
while i < end:
for n in steps:
self.hexagon_array.append(Hexagon(spiral=Spiral(i),converter=converter))
i += n
def __init__(self,
hexagons=None,
addresses=None,
start=0,
end=7,
steps=[1],
level=None,
values=[]):
frame = inspect.currentframe()
_, _, _, values = inspect.getargvalues(frame)
nones = values.values().count(None)
inputs = 3
if inputs - nones > 1:
raise Exception, "Must specify one of either hexagons/addresses/level"
converter = coords.addressToUVW(4)
self.hexagon_array = []
if hexagons != None:
self.hexagon_array = hexagons
elif addresses != None:
self.addresses = addresses
elif level != None:
for i in range(7**level):
self.hexagon_array.append(
Hexagon(spiral=Spiral(i), converter=converter))
else:
i = start
while i < end:
for n in steps:
self.hexagon_array.append(
Hexagon(spiral=Spiral(i), converter=converter))
i += n
def requires(self):
config = Config.get()
return (Coordinates.CreateSampleCoordinates(),
Popularity.SampleCreator(
config.get("ExternalFiles",
"vecs_with_id")), ContourCode(),
CreateContinents(), MakeRegions())
def __init__(self, raw_instructions: str) -> None:
"""
Constructor.
- Parses raw_instructions into a List
- Adds starting position to coordinates_history list
"""
self.instructions: List = raw_instructions.split(", ")
self.coordinates_history.append(Coordinates(0, 0))
def Get_Delta_Vector(self, a, b):
# Return the delta vector
del_vec = C.Coordinates("Delta_" + a.name + b.name)
del_vec.Set_Cartesian((b.x - a.x) / self.freq_n,
(b.y - a.y) / self.freq_n,
(b.z - a.z) / self.freq_n)
return del_vec
def GetCoordinates(address):
currCoord = geo.get(address)
if currCoord is None:
geo[address] = currCoord = geolocator.geocode(address)
if currCoord is None:
print("Address " + address + " is not recognized.")
return Coordinates.Coordinates(latitude=0, longitude=0)
return currCoord
def convert_to_robot_coord(coordinates):
T = np.array([[0.0, 0.0, 1.0, -0.087], #generated by quaternion_to_rot_matrix.py
[-1.0, 0.0, 0.0, 0.013],
[0.0, -1.0, 0.0, 0.287],
[0, 0, 0, 1]])
coord = np.append(coordinates, [1])
new_coord = np.matmul(T, coord)
return Coordinates.Coordinates(new_coord[0], new_coord[1], new_coord[2])
def intersection(self,A,B,C,D):
inter = Coordinates()
# soit le segment [AB] d'équation yS = aS*xS + bS
aS = None #coefficient directeur du segment [AB]
bS = None # ordonée à l'origine du segment [AB]
# soit le segment [CD] d'équation yF = aF*xF + bF
aF = None #coefficient directeur du segment [CD]
bF = None # ordonée à l'origine du segment [CD]
# si aucun des deux segments n'est vertical
if ((A.x!=B.x)and(C.x!=D.x)) :
aS = (B.y-A.y)/(B.x-A.x)
bS = (A.y*B.x - B.y*A.x)/(B.x-A.x)
aF = (D.y-C.y)/(D.x-C.x)
bF = (C.y*D.x - D.y*C.x)/(D.x-C.x)
# le point d'intersection a alors pour coordonnées le couple solution du système y = aS*x+bS
# y = aF*x+bF
det = aF-aS
# on a donc l'inverse de la matrice | aS -1 | égale à : __1__ t| -1 -aF |
# | aF -1 | aF-aS | 1 aS |
# qui nous permet de trouver x et y
inter.x = (1/det)*(bS-bF)
inter.y = (1/det)*(aF*bS-aS*bF)
# si un des 2 segments est vertical
else :
if (A.x==B.x):
aF = (D.y-C.y)/(D.x-C.x)
bF = (C.y*D.x - D.y*C.x)/(D.x-C.x)
inter.x = A.x
inter.y = aF*inter.x + bF
elif (C.x==D.x):
aS = (B.y-A.y)/(B.x-A.x)
bS = (A.y*B.x - B.y*A.x)/(B.x-A.x)
inter.x = C.x
inter.y = aS*inter.x + bS
return inter
def add_doors(self):
doors = []
for i in range(0, len(self.walls.coords) - 1):
x1, y1 = self.walls.coords[i]
x2, y2 = self.walls.coords[i + 1]
orientation = Coord.orientation((x1, y1), (x2, y2))
if orientation == "horizontal":
if Coord.min_space(x1, x2):
point = (Coord.place(x1, x2), y1)
rotate = 90
if orientation == "vertical":
if Coord.min_space(y1, y2):
point = (x1, Coord.place(y1, y2))
rotate = 0
if random.randint(0, 1):
doors.append({'point': point, 'rotate': rotate})
i += 1
return doors
def requires(self):
config = Config.get()
return (
RegionCode(),
Coordinates.CreateSampleCoordinates(),
cartograph.PreReqs.SampleCreator(
config.get("ExternalFiles", "best_vecs_with_id")),
cartograph.EnsureDirectoriesExist(),
)
def get_current_coordinates(self) -> Coordinates:
"""
Calculates and returns the current position relative to the
starting point.
"""
north, south, east, west = self.get_nsew_values()
latitude = north - south
longitude = east - west
return Coordinates(latitude, longitude)
def getCoords(direccs, numbers):
coords = Coordinates.Coordinates(direccs)
for x in range(len(coords)):
AllCoords.append(coords[x])
AllAddress.append(direccs[x])
AllNums.append(numbers[x])
ACSV.a_csv(
[x.split("\n")[2] if "Check" not in x else x for x in AllCoords],
AllNums, output)
def requires(self):
config = Config.get()
if config.sampleBorders():
return (MakeSampleRegions(), WikiBrainNumbering(),
cartograph.EnsureDirectoriesExist(),
Coordinates.CreateSampleAnnoyIndex())
else:
return (
WikiBrainNumbering(),
cartograph.EnsureDirectoriesExist(),
)
def toRealCoordinates(temp):
if (bool(re.search('[a-zA-Z]', temp[1:]))):
raise Exception(
"illegal input, you have to enter the alphabetic part (A to L) and numerical part (1 to 10) in order"
)
if 65 <= ord(temp[0]) <= 76 and 1 <= int(temp[1:]) <= 10:
y = ord(temp[0]) - 65 # starting from 0
x = int(temp[1:]) - 1 # starting from 0
reversedX = 9 - x
print('x: ' + str(reversedX) + ' ' + 'y: ' + str(y))
return Coordinates(reversedX, y)
raise Exception(
"illegal input, you have to enter the alphabetic part (A to L) and numerical part (1 to 10) in order"
)
def add_doors(self):
doors = []
for room in self.rooms:
for i in range(0, len(room['shape'].exterior.coords) - 1):
x1, y1 = room['shape'].exterior.coords[i]
x2, y2 = room['shape'].exterior.coords[i + 1]
orientation = Coord.orientation((x1, y1), (x2, y2))
if orientation == "horizontal":
if Coord.min_space(x1, x2):
point = (Coord.place(x1, x2), y1)
rotate = 90
if orientation == "vertical":
if Coord.min_space(y1, y2):
point = (x1, Coord.place(y1, y2))
rotate = 0
door_space = Polygon([
point, (point[0] + 10, point[1]),
(point[0] + 10, point[1] + 10), (point[0] + 10, point[1])
])
if not door_space.intersects(self.exterior):
doors.append({'point': point, 'rotate': rotate})
break
i += 1
return doors
def add_room_items(self):
items = []
choices = []
#for room in self.rooms:
# type = room['type'].lower()
# choices = Files.Icons.read_items(type)
# for _ in range(4):
# print random.choice(choices)
for room in self.rooms:
for i in range(0, len(room['shape'].exterior.coords) - 1):
x1, y1 = room['shape'].exterior.coords[i]
x2, y2 = room['shape'].exterior.coords[i + 1]
orientation = Coord.orientation((x1, y1), (x2, y2))
if orientation == "horizontal":
if Coord.min_space(x1, x2):
point = (Coord.place(x1, x2), y1 - 5)
rotate = 0
if not Point(point).within(room['shape']):
point = (Coord.place(x1, x2), y1 + 5)
rotate = 180
if orientation == "vertical":
if Coord.min_space(y1, y2):
point = (x1 + 5, Coord.place(y1, y2))
rotate = 90
if not Point(point).within(room['shape']):
point = (x1 - 5, Coord.place(y1, y2))
rotate = 270
item_space = Polygon([
point, (point[0] + 10, point[1]),
(point[0] + 10, point[1] + 10), (point[0] + 10, point[1])
])
if self.check_unique_items('gfx/study/book1.png', items):
for thing in items:
if Coord.obj_clear(thing['location'], point):
items.append({
'file':
'gfx/study/books1.png',
'size':
Files.Icons('gfx/study/books1.png').size(),
'rotate':
rotate,
'location':
point
})
return items
def __init__(self, number=2):
self.noPlayers = number
self.players = []
self.names = []
self.pieces = []
#create Player instnacers
for i in range(number):
self.players.append(
Player.Player(input("\n\nPlayer {0} name: ".format(i + 1)),
input("Player {0} color: ".format(i + 1)),
i + 1))
#fill Names list
for i in self.players:
self.names.append(i.retName())
#initialize dictionary of piece positions
self.coords = Coordinates.Coordinates(self.players)
def __init__(self, *arg, **kw): cfg = dict() JoyApp.__init__(self, cfg=cfg, *arg, **kw) self.arm = Arm(); self.f = gcf() self.f.clf() self.ax = self.f.gca(projection='3d') motors = [] motors.append(self.robot.at.Nx02) motors.append(self.robot.at.Nx08) motors.append(self.robot.at.Nx04) motors.append(self.robot.at.Nx06) self.motors = Motors(motors) self.orientation = PaperOrientation.HORIZONTAL self.angles1 = None self.angles2 = None self.coordinates = Coordinates() self.calibrateIdx = -1 self.calibrationPoints = [] self.pos3d = None self.timeForPlot = self.onceEvery(1.0/3.0) self.moveToPointPlan = MoveToPointPlan(self) self.drawLinePlan = DrawLinePlan(self) self.drawSquarePlan = DrawSquarePlan(self) self.multipleLinePlan = MultipleLinePlan(self) self.moveToPointPlan.setPaperOrientation(self.orientation) self.startPoints = [[5, -10], [10, -15]] self.endPoints = [[18, -18], [1, -2]] self.lines = [ [(18,10), (10,22)], [(18,35), (27,24)], [(19,10), (27,25)], [(18,35), (10,22)], [(29,37), (30,11)], [(42,36), (42,12)], [(29,12), (41,36)], [(14,46), (13,70)], [(31,45), (20,58)], [(22,58), (32,70)], [(45,46), (33,54)], [(38,70), (49,61)], [(34,55), (49,62)] ]
def add_windows(self):
windows = []
for i in range(0, len(self.walls.coords) - 1):
x1, y1 = self.walls.coords[i]
x2, y2 = self.walls.coords[i + 1]
orientation = Coord.orientation((x1, y1), (x2, y2))
if orientation == "horizontal":
if Coord.min_space(x1, x2):
point = (Coord.place(x1, x2), y1)
rotate = 90
if orientation == "vertical":
if Coord.min_space(y1, y2):
point = (x1, Coord.place(y1, y2))
rotate = 0
if Coord.obj_clear(self.doors, point) and Coord.obj_clear(
windows, point):
windows.append({'point': point, 'rotate': rotate})
i += 1
return windows
def __init__(self,
canon=None,
standard=None,
spiral=None,
side_size=1 / sqrt(3),
orientation='side',
colour='Orange',
opacity=1.0,
line_width=0.0,
converter=None,
value=0):
frame = inspect.currentframe()
_, _, _, values = inspect.getargvalues(frame)
numberOfNones = values.values().count(None)
if 3 - numberOfNones != 1:
raise Exception, "Number of address inputs = {}. Must specify just one input to build hex".format(
3 - numberOfNones)
self.side_size = side_size
self.orientation = orientation
self.colour = colour
self.opacity = opacity
self.line_width = line_width
self.vertices = []
self.value = value
if standard != None:
self.standard = standard
elif canon != None:
self.standard = sa.standardFromCanon(canon)
elif spiral != None:
self.standard = sa.standardFromSpiral(spiral)
else:
raise Exception, "No address in input"
#Centre
self.centre_uvw = converter(self.standard)
self.centre_xy = coords.UVWToXY(self.centre_uvw)
def generate_simulated_tod(filename, maps, include_noise=False, output_dir=''):
"""
Generates a simulated C-BASS TOD stream
filename - name of reloaded fits file
maps - dictionary containing model maps (I/Q/U) in celestial frame
"""
hdu = fits.open(filename)
fstrp = filename.split('/')[-1]
el = hdu[1].data['EL']
if include_noise:
rms = Tsys / np.sqrt(bw / sr) # From QU_Simulator_Functions
else:
rms = None
# Calculate parallactic angle
pa = Coordinates.pa(np.mod(hdu[1].data['RA'], 2 * np.pi),
hdu[1].data['DEC'],
hdu[1].data['MJD'],
antenna_observatory['OVRO'][0] * np.pi / 180.,
antenna_observatory['OVRO'][1] * np.pi / 180.,
degrees=False)
# Interpolate from map:
theta, phi = (np.pi / 2. - hdu[1].data['DEC']), np.mod(
hdu[1].data['RA'], 2 * np.pi)
tods = {pol: hp.get_interp_val(m, theta, phi) for pol, m in maps.items()}
# Clockwise rotation matrix: +PA for telescope to sky, -PA for sky to telescope
tods['Q'], tods['U'] = rotateQU(tods['Q'], tods['U'], -pa)
todvec = np.array([tods['I'], tods['Q'], tods['U']])
populate_hdu(hdu, tods, todvec, rms=rms)
hdu.writeto('{}/{}'.format(output_dir, fstrp), overwrite=True)
hdu.close()
return todvec, pa, theta, phi, el
def Get_Edges_Equal_Distance(self):
# Based on equal division of the face into triangles - This leads to different sized strut lengths
# If it was based on the equal division of the angle at the same radius then all strut lengths the same!!
# What does this do to the structural integrity of the dome??
# Return all the points/vertices of the face divided into equilateral triangles based on the frequency
ret_list = list()
# Coordinates of the starting point
x0 = self.x1.x
y0 = self.x1.y
z0 = self.x1.z
delta_x1x2 = self.Get_Delta_Vector( self.x1, self.x2 )
delta_x1x3 = self.Get_Delta_Vector( self.x1, self.x3 )
for i in range(0,self.freq_n+1):
for j in range( 1, self.freq_n - i + 1):
# All marked as points?? Doesnt matter, vertices already stored!!
# Can remove any vertices from the list if we want, although all the same
a = C.Coordinates("pointA")
a.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
b = C.Coordinates("pointB")
b.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
c = C.Coordinates("pointC")
c.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
# Calculate the new coordinates based on the offset from the initial point (A)
a.Set_Cartesian( x0 + i*delta_x1x2.x + j*delta_x1x3.x, y0 + i*delta_x1x2.y + j*delta_x1x3.y, z0 + i*delta_x1x2.z + j*delta_x1x3.z)
b.Set_Cartesian( x0 + i*delta_x1x2.x + (j-1)*delta_x1x3.x, y0 + i*delta_x1x2.y + (j-1)*delta_x1x3.y, z0 + i*delta_x1x2.z + (j-1)*delta_x1x3.z)
c.Set_Cartesian( x0 + (i+1)*delta_x1x2.x + (j-1)*delta_x1x3.x, y0 + (i+1)*delta_x1x2.y + (j-1)*delta_x1x3.y, z0 + (i+1)*delta_x1x2.z + (j-1)*delta_x1x3.z)
e1 = E.Edge("edge" + str(CF.nEdge) )
e1.Set_Edge_Number( CF.nEdge )
e1.Set_Points( a, b )
CF.nEdge += 1
e2 = E.Edge("edge" + str(CF.nEdge) )
e2.Set_Edge_Number( CF.nEdge )
e2.Set_Points( b, c )
CF.nEdge += 1
e3 = E.Edge("edge" + str(CF.nEdge) )
e3.Set_Edge_Number( CF.nEdge )
e3.Set_Points( c, a )
CF.nEdge += 1
ret_list.append(e1)
ret_list.append(e2)
ret_list.append(e3)
return ret_list
def requires(self):
return (MakeRegions(), CalculateZoomsCode(),
Coordinates.CreateFullCoordinates(),
Popularity.PopularityIdentifier())
angles, triangles = Opt.optimize(Data.getAngles(), 0.05 , 100, 10) print(angles) print(triangles) print(Opt.getPoints(angles)) triError, pointError = Opt.getError(triangles, Opt.getPoints(angles)) print(triError) print(pointError) triError, pointError = Opt.getError(Opt.getTriangles(Data.getAngles()), Opt.getPoints(Data.getAngles())) print(triError) print(pointError) """ angles = Data.getAngles() print(Opt.getErrors(angles)) #angles, triangles = Opt.optimize(angles, 0.05 , 100, 10) print(Opt.getErrors(angles)) angles = LF.makeConcatonatedAngles(angles,10) lengths = LF.findAllLengths(angles, "GJK", 330.0025) print(lengths) #print(angles) azi = Co.findAllAzimuths(angles, "FH", 257.78) #print(azi) coords = Co.findCoordinates(azi, lengths, "F") print(coords) Draw.drawPoints(coords) Draw.drawSides(azi ,coords)
def coordprecess(self,coords,epoch_now,epoch):
Coordinates.coordprecess(self,coords,epoch_now,epoch)
def inputcoordSorter(self,ra,dec,epoch):
Coordinates.inputcoordSorter(self,ra,dec,epoch)
def Get_Edges_Equal_Angles(self):
# Calculate the edge coordinates based on the division of the angle between the points
ret_list = list()
# Coordinates of the starting point
x0 = self.x1
y0 = self.x2
z0 = self.x3
delta_theta_x1x2 = (y0.theta - x0.theta)/self.freq_n
delta_phi_x1x2 = (y0.phi - x0.phi)/self.freq_n
delta_theta_x1x3 = (z0.theta - x0.theta)/self.freq_n
delta_phi_x1x3 = (z0.phi - x0.phi)/self.freq_n
if (delta_theta_x1x2 > -CF.TINY) and (delta_theta_x1x2 < CF.TINY):
delta_theta_x1x2 = 0
if (delta_theta_x1x3 > -CF.TINY) and (delta_theta_x1x3 < CF.TINY):
delta_theta_x1x3 = 0
for i in range(0,self.freq_n+1):
for j in range( 1, self.freq_n - i +1 ):
# All marked as points?? Doesnt matter, vertices already stored!!
# Can remove any vertices from the list if we want, although all the same
a = C.Coordinates("pointA")
a.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
b = C.Coordinates("pointB")
b.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
c = C.Coordinates("pointC")
c.Set_Point_Number(CF.nPoint)
CF.nPoint += 1
# r, theta, phi
# Calculate the new coordinates based on the offset from the initial point (A)
a.Set_Polar( x0.r, x0.theta + j*delta_theta_x1x2 + i*delta_theta_x1x3, x0.phi + j*delta_phi_x1x2 + i*delta_phi_x1x3)
#a.Set_Polar( x0 + i*delta_x1x2.x + j*delta_x1x3.x, y0 + i*delta_x1x2.y + j*delta_x1x3.y, z0 + i*delta_x1x2.z + j*delta_x1x3.z)
b.Set_Polar( x0.r, x0.theta + (j-1)*delta_theta_x1x2 + i*delta_theta_x1x3, x0.phi + (j-1)*delta_phi_x1x2 + i*delta_phi_x1x3)
#b.Set_Polar( x0 + i*delta_x1x2.x + (j-1)*delta_x1x3.x, y0 + i*delta_x1x2.y + (j-1)*delta_x1x3.y, z0 + i*delta_x1x2.z + (j-1)*delta_x1x3.z)
c.Set_Polar( x0.r, x0.theta + (j-1)*delta_theta_x1x2 + (i+1)*delta_theta_x1x3, x0.phi + (j-1)*delta_phi_x1x2 + (i+1)*delta_phi_x1x3)
#c.Set_Polar( x0 + (i+1)*delta_x1x2.x + (j-1)*delta_x1x3.x, y0 + (i+1)*delta_x1x2.y + (j-1)*delta_x1x3.y, z0 + (i+1)*delta_x1x2.z + (j-1)*delta_x1x3.z)
e1 = E.Edge("edge" + str(CF.nEdge) )
e1.Set_Edge_Number( CF.nEdge )
e1.Set_Points( a, b )
CF.nEdge += 1
e2 = E.Edge("edge" + str(CF.nEdge) )
e2.Set_Edge_Number( CF.nEdge )
e2.Set_Points( b, c )
CF.nEdge += 1
e3 = E.Edge("edge" + str(CF.nEdge) )
e3.Set_Edge_Number( CF.nEdge )
e3.Set_Points( c, a )
CF.nEdge += 1
ret_list.append(e1)
ret_list.append(e2)
ret_list.append(e3)
return ret_list
#t2 = C.Coordinates("t2")
#t2.Set_Cartesian( 500, -500, 500 )
#t2.Set_Point_Number( CF.nPoint )
#CF.nPoint += 1
#gs.Add_Vertex(t2)
#t3 = C.Coordinates("t3")
#t3.Set_Cartesian( 0, 500, 500 )
#t3.Set_Point_Number( CF.nPoint )
#CF.nPoint += 1
#gs.Add_Vertex(t3)
# Icosahedron vertice coordinates
a = C.Coordinates("a")
a.Set_Cartesian( 0, 0, D.Decimal(Z1_mm) )
a.Set_Point_Number( CF.nPoint )
CF.nPoint += 1
gs.Add_Vertex(a)
b = C.Coordinates("b")
b.Set_Cartesian( 0, CF.R_mm, Z2_mm )
b.Set_Point_Number( CF.nPoint )
CF.nPoint += 1
gs.Add_Vertex(b)
c = C.Coordinates("c")
c.Set_Cartesian( Cx_mm, Cy_mm, Z2_mm)
c.Set_Point_Number( CF.nPoint )
CF.nPoint += 1
class MainApp(JoyApp):
def __init__(self, *arg, **kw):
cfg = dict()
JoyApp.__init__(self, cfg=cfg, *arg, **kw)
self.arm = Arm();
self.f = gcf()
self.f.clf()
self.ax = self.f.gca(projection='3d')
motors = []
motors.append(self.robot.at.Nx02)
motors.append(self.robot.at.Nx08)
motors.append(self.robot.at.Nx04)
motors.append(self.robot.at.Nx06)
self.motors = Motors(motors)
self.orientation = PaperOrientation.HORIZONTAL
self.angles1 = None
self.angles2 = None
self.coordinates = Coordinates()
self.calibrateIdx = -1
self.calibrationPoints = []
self.pos3d = None
self.timeForPlot = self.onceEvery(1.0/3.0)
self.moveToPointPlan = MoveToPointPlan(self)
self.drawLinePlan = DrawLinePlan(self)
self.drawSquarePlan = DrawSquarePlan(self)
self.multipleLinePlan = MultipleLinePlan(self)
self.moveToPointPlan.setPaperOrientation(self.orientation)
self.startPoints = [[5, -10], [10, -15]]
self.endPoints = [[18, -18], [1, -2]]
self.lines = [
[(18,10), (10,22)], [(18,35), (27,24)], [(19,10), (27,25)], [(18,35), (10,22)],
[(29,37), (30,11)], [(42,36), (42,12)], [(29,12), (41,36)], [(14,46), (13,70)],
[(31,45), (20,58)], [(22,58), (32,70)], [(45,46), (33,54)], [(38,70), (49,61)],
[(34,55), (49,62)] ]
def onStart(self):
pass
def moveToPos(self, pos):
self.moveToPointPlan.setPoint(pos)
self.moveToPointPlan.start()
# angles = self.arm.inverseKinematics(pos, self.motors.get_angles())
# if (angles == None):
# print("Can't reach location")
# return
# self.motors.set_angles(angles)
# print("Target Angles: " + str(array(angles) * 180/pi))
def onEvent(self, evt):
if self.timeForPlot():
motorAngles = array(self.motors.get_angles())
modelAngles = self.arm.convertMotorToModelAngles(motorAngles)
self.arm.plotReal3D(modelAngles, self.ax)
self.ax.set(xlim=[-50,50],ylim=[-50,50],zlim=[-50,50])
draw()
pause(0.001)
if evt.type == TIMEREVENT:
return
if evt.type != KEYDOWN:
return
if evt.key == K_q:
self.angles1 = self.motors.get_angles()
print("Set Angles1")
elif evt.key == K_w:
self.angles2 = self.motors.get_angles()
print("Set Angles2")
elif evt.key == K_e:
if (self.angles1 == None):
print("Angles 1 not set, press q to set")
return
self.moveToAnglesPlan.setAngles(self.angles1)
self.moveToAnglesPlan.run()
# self.motors.set_angles(self.angles1)
elif evt.key == K_r:
if (self.angles1 == None):
print("Angles 2 not set, press w to set")
return
# self.motors.set_angles(self.angles2)
self.moveToAnglesPlan.setAngles(self.angles2)
self.moveToAnglesPlan.run()
elif evt.key == K_t:
if (self.orientation == PaperOrientation.HORIZONTAL):
self.orientation = PaperOrientation.VERTICAL
print("VERTICAL")
else:
self.orientation = PaperOrientation.HORIZONTAL
print("Horizontal")
elif evt.key == K_l:
startPoints = []
endPoints = []
for line in self.lines:
startPoint = list(line[0])
endPoint = list(line[1])
temp = startPoint[0]
startPoint[0] = startPoint[1]
startPoint[1] = -temp
temp = endPoint[0]
endPoint[0] = endPoint[1]
endPoint[1] = -temp
startPoints.append(startPoint)
endPoints.append(endPoint)
self.multipleLinePlan.setPoints(startPoints, endPoints, self.orientation)
self.multipleLinePlan.start()
elif evt.key == K_z:
self.motors.go_slack()
elif evt.key == K_p:
angles = self.motors.get_angles()
print("Motor Angles: " + str(array(angles) * 180 / pi))
modelAngles = self.arm.convertMotorToModelAngles(angles)
print("Model Angles: " + str(array(modelAngles) * 180/pi))
pos3d = self.arm.getTool(modelAngles)[0:3]
pos3d[2] -= effectorHeight
print("Pos: " + str(pos3d))
elif evt.key == K_s:
if (not self.coordinates.isCalibrated()):
print("Not calibrated")
return
self.drawSquarePlan.setOrientation(self.orientation)
self.drawSquarePlan.start()
# self.drawLinePlan.setPoints([10, 0], [10, 10], self.orientation)
# self.drawLinePlan.start()
elif evt.key == K_c:
modelAngles = self.arm.convertMotorToModelAngles(self.motors.get_angles())
temp = array(self.arm.getTool(modelAngles))[0:3]
if (self.orientation == PaperOrientation.HORIZONTAL):
temp[2] -= effectorHeight
elif (self.orientation == PaperOrientation.VERTICAL):
temp[0] += effectorHeight
if (self.calibrateIdx == -1):
print("Started Calibration. Move arm to lower left of paper, then press \'c\'")
elif (self.calibrateIdx == 0):
print("Move arm to lower right of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 1):
print("Move arm to upper right of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 2):
print("Move arm to upper left of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 3):
print ("Finished Calibration")
self.calibrationPoints.append(temp)
self.coordinates.calibrate(array(self.calibrationPoints), self.orientation)
self.arm.setPaperPoints(array(self.calibrationPoints))
self.calibrationPoints = []
self.calibrateIdx += 1
if (self.calibrateIdx >= 4):
self.calibrateIdx = -1
elif evt.key == K_a:
if (self.coordinates.isCalibrated()):
self.pos3d = self.coordinates.transformPaperToReal([paperWidth/2, paperLength/2, 1])
else:
self.pos3d = [30, 0, 20]
self.moveToPos(self.pos3d)
elif evt.key == K_UP:
self.pos3d[0] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_DOWN:
self.pos3d[0] -= 2
self.moveToPos(self.pos3d)
elif evt.key == K_LEFT:
self.pos3d[1] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_RIGHT:
self.pos3d[1] -= 2
self.moveToPos(self.pos3d)
elif evt.key == K_i:
self.pos3d[2] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_k:
self.pos3d[2] -= 2
self.moveToPos(self.pos3d)