def group_textboxes(self, laparams, boxes):
if len(boxes) > 100:
# Grouping this many boxes would take too long and it doesn't make much sense to do so
# considering the type of grouping (nesting 2-sized subgroups) that is done here.
logging.info("Too many boxes (%d) to group, skipping.", len(boxes))
return boxes
dists = []
for obj1, obj2 in zip(boxes[0:], boxes[1:]):
dists.append((0, dist(obj1, obj2), obj1, obj2))
dists.sort()
plane = Plane(boxes)
while dists:
(c, d, obj1, obj2) = dists.pop(0)
if c == 0 and isany(obj1, obj2, plane):
dists.append((1, d, obj1, obj2))
continue
if (isinstance(obj1, (LTTextBoxVertical, LTTextGroupTBRL)) or
isinstance(obj2, (LTTextBoxVertical, LTTextGroupTBRL))):
group = LTTextGroupTBRL([obj1, obj2])
else:
group = LTTextGroupLRTB([obj1, obj2])
plane.remove(obj1)
plane.remove(obj2)
dists = [n for n in dists if not n[2] in (obj1, obj2) and not n[3] in (obj1, obj2)]
for other in plane:
dists.append((0, dist(group, other), group, other))
dists.sort()
plane.add(group)
assert len(plane) == 1
return list(plane)
def pm6_error(params):
#run Gaussian jobs with new parameters
for i,example in enumerate(examples):
running_jobs = [ job('%s-%d-%d' % (name,counter[0],i), 'PM6=(Input,Print) Opt=Loose', example.atoms, extra_section=param_string%tuple(params), queue=queue, force=True) ]
#wait for all jobs to finish
for j in running_jobs:
j.wait()
#get forces and energies resulting from new parameters
geom_error = 0.0
force_error = 0.0
for i,example in enumerate(examples):
try:
new_energy, new_atoms = parse_atoms('%s-%d-%d'%(name,counter[0],i), check_convergence=False)
except:
print '%s-%d-%d'%(name,counter[0],i), 'has no data'
exit()
if parse_atoms('%s-%d-%d'%(name,counter[0],i)) is None:
print '%s-%d-%d'%(name,counter[0],i), 'did not converge fully'
#geom_error += 10.0 #discourage failure
#compare results
for b in example.bonds:
d1 = utils.dist( example.atoms[b[0]], example.atoms[b[1]] )
d2 = utils.dist( new_atoms[b[0]], new_atoms[b[1]] )
geom_error += (d1-d2)**2
error = geom_error/n_bonds
print error**0.5, params
counter[0]+=1
return error
def group_textboxes(self, laparams, boxes):
dists = []
for (obj1, obj2) in zip(boxes[0:], boxes[1:]):
dists.append((0, dist(obj1, obj2), obj1, obj2))
#for i in xrange(len(boxes)):
# obj1 = boxes[i]
# for j in xrange(i + 1, len(boxes)):
# obj2 = boxes[j]
# dists.append((0, dist(obj1, obj2), obj1, obj2))
#dists.sort()
plane = Plane(boxes)
while dists:
(c, d, obj1, obj2) = dists.pop(0)
if c == 0 and isany(obj1, obj2, plane):
dists.append((1, d, obj1, obj2))
continue
if (isinstance(obj1, LTTextBoxVertical) or
isinstance(obj1, LTTextGroupTBRL) or
isinstance(obj2, LTTextBoxVertical) or
isinstance(obj2, LTTextGroupTBRL)):
group = LTTextGroupTBRL([obj1, obj2])
else:
group = LTTextGroupLRTB([obj1, obj2])
plane.remove(obj1)
plane.remove(obj2)
dists = [n for n in dists if not n[2] in (obj1, obj2) and not n[3] in (obj1, obj2)]
for other in plane:
dists.append((0, dist(group, other), group, other))
#dists.sort()
plane.add(group)
assert len(plane) == 1
return list(plane)
def create(self, focus1, focus2, k_point):
self.tielines = c.create_line(0, 0, 0, 0, **self.tieline_specs)
self.parents = [focus1, focus2, k_point]
self.k = dist(focus1.coord, k_point.coord) + dist(focus2.coord, k_point.coord)
self.become_child()
c.lower(self.handle)
c.lower(self.tielines)
c.lower(1) # the grid
def compare_distances_only():
utils.Molecule.set_params('oplsaa4.prm')
for f in ['propanenitrile', 'butanenitrile', 'pentanenitrile', 'hexanenitrile', 'aminopropane2', 'aminobutane2', 'aminopentane2', 'aminohexane2', 'hexane2', 'methane', 'hcn2', 'cyanoacetylene2', 'acrylonitrile2', 'cyanoallene2', 'acetonitrile2', 'hc5n2']:
atoms0 = utils.Molecule(f+'.arc').atoms
atoms1 = gaussian.parse_atoms('gaussian/'+f+'_1.log')[1]
for i in range(len(atoms0)):
atoms0[i].index = i+1
atoms1[i].index = i+1
#atoms0[i].x = atoms1[i].xp
#atoms0[i].y = atoms1[i].y
#atoms0[i].z = atoms1[i].z
bonds0 = filetypes.get_bonds(atoms0)
bonds1 = filetypes.get_bonds(atoms1)
#filetypes.write_arc(f[:-1]+'3', atoms0)
#continue
angles0, dihedrals0 = filetypes.get_angles_and_dihedrals(atoms0, bonds0)
angles1, dihedrals1 = filetypes.get_angles_and_dihedrals(atoms1, bonds1)
bond_error = 0.0
for i in range(len(bonds0)):
bond_error += abs(bonds0[i].d - bonds1[i].d)
angles0.sort(key=lambda a: tuple([n.index for n in a.atoms]) )
angles1.sort(key=lambda a: tuple([n.index for n in a.atoms]) )
angle_error = 0.0
for i in range(len(angles0)):
d0 = utils.dist(angles0[i].atoms[0], angles0[i].atoms[2])
d1 = utils.dist(angles1[i].atoms[0], angles1[i].atoms[2])
angle_error += abs(d0-d1)
dihedrals0.sort(key=lambda a: tuple([n.index for n in a.atoms]) )
dihedrals1.sort(key=lambda a: tuple([n.index for n in a.atoms]) )
if dihedrals0:
dihedral_error = 0.0
for i in range(len(dihedrals0)):
#if dihedral is single-bonded free rotor, ignore comparison
if (dihedrals0[i].atoms[1].type.index,dihedrals0[i].atoms[1].type.index) == (78,78):
continue
d0 = utils.dist(dihedrals0[i].atoms[0], dihedrals0[i].atoms[3])
d1 = utils.dist(dihedrals1[i].atoms[0], dihedrals1[i].atoms[3])
dihedral_error += abs(d0-d1)
print f, bond_error/len(bonds0), angle_error/len(angles0), 180/math.pi*dihedral_error/len(dihedrals0)
else:
print f, bond_error/len(bonds0), angle_error/len(angles0), 0.0
def update(self):
focus1, focus2, k_point = self.parents
k = dist(focus1.coord, k_point.coord) - dist(focus2.coord, k_point.coord)
# Make the tielines.
c.coords(self.tielines,
focus1.coord[0], focus1.coord[1],
k_point.coord[0], k_point.coord[1],
focus2.coord[0], focus2.coord[1])
def circle_points((cx, cy), radius):
return (
(cx + radius, cy), (cx - radius, cy),
(cx, cy + radius), (cx, cy - radius))
def find_closest(f1, f2):
f1x, f1y = f1
f2x, f2y = f2
# The circle points k from f1.
f1c = ((f1x + k, f1y), (f1x, f1y + k), (f1x - k, f1y), (f1x, f1y - k))
# The circle points k from f2.
f2c = ((f2x + k, f2y), (f2x, f2y + k), (f2x - k, f2y), (f2x, f2y - k))
# Find the two points x such that f1x == f2x.
near_points = set(filter(
lambda x: dist(f1, x) == dist(f2, x),
f1c + f2c))
return near_points
def angle(source, ray):
sx, sy = source
rx, ry = ray
radius = dist(source, ray)
xshort = radius - abs(rx - sx)
arc_len = 2 * xshort
circumference = 8 * radius
if circumference:
angle = 360 * float(arc_len) / circumference
if rx > sx and ry > sy: # quadrant 1
return angle
elif rx < sx and ry > sy: # quadrant 2
return 180 - angle
elif rx < sx and ry < sy: # quadrant 3
return 180 + angle
elif rx > sx and ry < sy: # quadrant 4
return 360 - angle
elif rx == sx and ry > sy:
return 90
elif rx == sx and ry < sy:
return 270
elif rx > sx and ry == sy:
return 0
elif rx < sx and ry == sy:
return 180
def waitForFish(cap, loc, timeout = 15):
start_time = time.time()
dist_thresh = 10
while cap.isOpened():
frame = getFrame(cap)
frame = cv2.bitwise_and(frame, circle_mask)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray_circle_mask = cv2.cvtColor(circle_mask, cv2.COLOR_BGR2GRAY)
img_mean, img_std_dev = cv2.meanStdDev(gray, mask=gray_circle_mask)
circles = cv2.HoughCircles(gray,cv2.cv.CV_HOUGH_GRADIENT,1, minDist=50, param1=50,param2=30,minRadius=20,maxRadius=40)
if circles is not None:
circles = np.uint16(np.around(circles))
for circle_num, i in enumerate(circles[0,:]):
center = (i[0], i[1])
radius = i[2]
fish_mask = np.zeros(frame.shape, np.uint8)
cv2.circle(fish_mask, center, radius, (255, 255, 255),-1)
if dist(loc, center) > dist_thresh:
return True
elapsed_time = time.time() - start_time
if elapsed_time > timeout:
return False
def aggressive_preprocess(world):
'''
inplace, because why not?
'''
#return
data = world.data
rx, ry = rxy = world.robot_coords
num_lambdas = 0
sectors = [Counter() for _ in range(4)]
for i in range(len(data)):
xy = world.index_to_coords(i)
if dist(rxy, xy) > 7:
if data[i] in '\\*':
x, y = xy
idx = 0
if x-rx > y-ry:
idx += 1
if x-rx > ry-y:
idx += 2
sectors[idx][data[i]] += 1
data[i] = '?'
for sector in sectors:
data.extend(sector.elements())
def simulate_vehicles(self, Veh, N):
self.N = N
for v in Veh:
if int(v[7]) == 0:
coor_n_act = self.Nodes[v[2][0]][2]
coor_n_next = self.Nodes[v[2][1]][2]
a_t = self.clock
f_t = utils.dist(coor_n_act, coor_n_next) / self.Connectors[v[1]][3]
over = (a_t - v[0]) / (f_t)
v[5] = over
if over >= 1:
v = self.simulate_percolation(v)
(v[3], self.N[v[2][1]][0]) = self.transvase(v)
self.N[v[2][1]][1] = 18
self.N[v[2][1]][1] = self.clock
v[0] = self.clock
if v[2][1] == self.Connectors[v[1]][1][-1]:
v[4] = False
elif v[2][1] == self.Connectors[v[1]][1][0]:
v[4] = True
if v[4]:
v[2][0] = v[2][1]
v[2][1] = utils.next_node(self.Connectors[v[1]], v[2][0])
elif not v[4]:
v[2][0] = v[2][1]
v[2][1] = utils.previous_node(self.Connectors[v[1]], v[2][0])
else:
v[7] -= self.sec
v[0] = self.clock
self.clock += self.sec
time.sleep(0.03)
return [self.N, Veh]
def calibrateBoard(cap):
global robot_pts
calib_dist = 7.875
# cap = cv2.VideoCapture(1)
cv2.namedWindow("Frame")
cv2.setMouseCallback("Frame", getRobotPixelLoc)
while cap.isOpened():
frame = getFrame(cap)
for pt in robot_pts:
cv2.circle(frame, pt, 2, (255, 0, 255), 2)
cv2.imshow("Frame", frame)
if len(robot_pts) >= 2:
cv2.setMouseCallback("Frame", nullCallback)
break
if cv2.waitKey(1) & 0xFF == ord('q'):
break
pt_dist = dist(robot_pts[1], robot_pts[0])
pixel_len = calib_dist/pt_dist
return pixel_len, robot_pts
def make_arms(ax,ay):
step = .1
test_points = ((ax + step, ay), (ax, ay + step), (ax - step, ay), (ax, ay - step))
free_points = filter(
lambda x:
dist(p1, x) > dist(p1, (ax, ay)) and \
dist(p2, x) > dist(p2, (ax, ay)),
test_points)
if len(free_points) == 1:
bx, by = free_points[0]
if bx < ax:
cx = 0
elif bx > ax:
cx = 2000
elif bx == ax:
cx = ax
if by < ay:
cy = 0
elif by > ay:
cy = 2000
elif by == ay:
cy = ay
elif len(free_points) == 2:
b1x, b1y = free_points[0]
b2x, b2y = free_points[1]
if min(b1x,b2x) < ax:
cx = 0
elif max(b1x,b2x) > ax:
cx = 2000
elif b1x == ax and b2x == ax:
cx = ax
if min(b1y,b2y) < ay:
cy = 0
elif max(b1y,b2y) > ay:
cy = 2000
elif b1y == ay and b2y == ay:
cy = ay
return cx, cy
def salesman_lower_bound_naive(world, need_exit=True):
if world.collected_lambdas == world.total_lambdas:
# go straight to exit
return dist(world.robot_coords, world.lift_coords)
max_dist = 0
for xy in world.enumerate_lambdas():
d1 = dist(world.robot_coords, xy)
if need_exit:
d2 = dist(xy, world.lift_coords)
else:
d2 = 0
#print d1, d2
max_dist = max(max_dist,
d1+d2)
return max_dist
def find_closest(f1, f2):
f1x, f1y = f1
f2x, f2y = f2
fdist = dist(f1, f2)
a = (fdist - k) * .5
b = fdist - a
# The circle points b from f1.
f1c = ((f1x + b, f1y), (f1x, f1y + b), (f1x - b, f1y), (f1x, f1y - b))
# The circle points a from f2.
f2c = ((f2x + a, f2y), (f2x, f2y + a), (f2x - a, f2y), (f2x, f2y - a))
# Find the two points x such that f1x + f2x = f1f2
near_points = set(filter(
lambda x: dist(f1, x) + dist(f2, x) == dist(f1, f2),
f1c + f2c))
return near_points
def makeCircleMask(frame, pts): circle_mask = np.zeros(frame.shape, np.uint8) center = midpoint(pts[0], pts[1]) center_pixel = tuple(map(int, center)) radius = int(dist(pts[0], pts[1])/2) cv2.circle(circle_mask, center_pixel, radius, (255, 255, 255),-1) #circle_mask = cv2.cvtColor(circle_mask, cv2.COLOR_BGR2GRAY) return circle_mask, center, radius
def update(self):
cx, cy = self.parents[0].coord
radius = self.radius = dist(self.parents[0].coord, self.parents[1].coord)
c.coords(self.handle,
cx - radius, cy,
cx, cy - radius,
cx + radius, cy,
cx, cy + radius,
cx - radius, cy)
def update(self):
p1, p2 = self.parents
midp = (p1.coord[0] + p2.coord[0]) * .5, (p1.coord[1] + p2.coord[1]) * .5
k = dist(p1.coord, midp)
def circle_points((cx,cy), radius):
return (
(cx + radius, cy), (cx - radius, cy),
(cx, cy + radius), (cx, cy - radius))
def HotMagma(x, y):
a = atan2(x, y)
r = dist(x, y, 0.0, 0.0)
if r == 0:
return (0, 0)
u = 0.5 * a / pi
v = sin(2.0 * r)
return (u, v)
def FancyEye(x, y):
a = atan2(x, y)
r = dist(x, y, 0.0, 0.0)
if r == 0:
return (0, 0)
u = 0.04 * y + 0.06 * cos(a * 3.0) / r
v = 0.04 * x + 0.06 * sin(a * 3.0) / r
return (u, v)
def Anamorphosis(x, y):
a = atan2(x, y)
r = dist(x, y, 0.0, 0.0)
if r == 0:
return (0, 0)
u = cos(a) / (3.0 * r)
v = sin(a) / (3.0 * r)
return (u, v)
def Neon2(i, j):
x = lerp(-1.0, 1.0, float(i) / width)
y = lerp(-1.0, 1.0, float(j) / height)
a = atan2(x, y)
r = dist(x, y, 0.0, 0.0) + 0.2 * sin(7 * a)
if r == 0:
return 0
return int(16 / r)
def Neon1(i, j):
x = lerp(-1.5, 1.5, float(i) / width)
y = lerp(-2.0, -0.5, float(j) / height)
# a = atan2(x, y)
r = dist(x, y, 0.0, 0.0)
if r == 0:
return 0
return int(512 / r)
def get_goal(self, world, robot):
'''
Selects a goal for robot
'''
if robot.penalty:
return None
elif self.current_task == 'game' and world.game_state is not None:
utils.defender_rotation_to_defend_point(robot, world.ball, world.our_goal.vector, defender.GOAL_RADIUS, world.our_side)
if robot.has_ball(world.ball):
info("Defender goal choice: Pass the ball")
return defender.Pass(world, robot)
elif utils.dist(world.our_goal, world.ball) < defender.GOAL_RADIUS:
return None
elif not utils.ball_heading_to_our_goal(world) and utils.defender_should_grab_ball(world):
info("Defender goal choice: Retrieve the ball")
return defender.GetBall(world, robot)
else:
info("Defender goal choice: Do the wiggle dance!")
return defender.Defend(world, robot)
elif self.current_task == 'oldgame' and world.game_state is not None:
if robot.has_ball(world.ball):
info("Defender goal choice: kick the ball")
return defender.Pass(world, robot)
elif utils.ball_heading_to_our_goal(world) and world.in_our_half(world.ball):
info("Defender goal choice: Intercept")
return defender.ReactiveGrabGoal(world, robot)
elif utils.ball_is_static(world) and world.in_our_half(world.ball):
ourdist = math.hypot(world.ball.x - robot.x, world.ball.y - robot.y)
oppdists = [math.hypot(world.ball.x - r.x, world.ball.y - r.y)
for r in world.their_robots if not r.is_missing()]
if (len(oppdists) == 0 or ourdist < min(oppdists)) and world.game_state == 'normal-play':
info("Defender goal choice: Retrieve ball")
return defender.GetBall(world, robot)
else:
info("Defender goal choice: Block")
return defender.Block(world, robot)
else:
info("Defender goal choice: Return to defence area")
return defender.ReturnToDefenceArea(world, robot)
elif self.current_task == 'move-grab':
return defender.GetBall(world, robot)
elif self.current_task == 'defend':
return defender.Defend(world, robot)
elif self.current_task == 'reactive-grab':
return defender.ReactiveGrabGoal(world, robot)
elif self.current_task == 'm1':
return defender.ReceivingPass(world, robot)
elif self.current_task == 'm2':
return defender.ReceiveAndPass(world, robot)
elif self.current_task == 'm31':
return defender.InterceptPass(world, robot)
elif self.current_task == 'm32':
return defender.InterceptGoal(world, robot)
def Neon3(i, j):
x = lerp(-1.0, 1.0, float(i) / width)
y = lerp(-0.8, 1.2, float(j) / height)
a = atan2(x, y)
r = dist(x, y, 0.0, 0.0) + 0.2 * saw(5.0 * a /
(2.0 * pi)) + 0.1 * abs(sin(5.0 * a))
#r = dist(x, y, 0.0, 0.0) + 0.2 * abs(sin(5.0 * a))
if r == 0:
return 0
return int(r * 64)
def move(self, world, dest):
"Move along the shortest path to the destination"
if dist(self.rect.center, dest) <= self.speed:
self.coord = dest
self.rect.center = dest
else:
if dest == self.rect.center:
return
a = dest[0] - self.coord[0]
b = dest[1] - self.coord[1]
d = math.sqrt(a ** 2 + b ** 2) / self.speed
self.coord = (self.coord[0] + a / d, self.coord[1] + b / d)
self.rect.center = self.coord
def main():
calibrateVision()
cap = cv2.VideoCapture(1)
frame = getFrame(cap)
height, width = frame.shape[:2]
video = cv2.VideoWriter('detection_demo.avi', cv2.cv.CV_FOURCC(*'XVID'),30,(width,height))
circle_mask = findCircleMask(frame)
while cap.isOpened():
frame = getFrame(cap)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray = cv2.bitwise_and(gray, circle_mask)
cv2.imshow("Gray", gray)
for pt in ramp_pts:
cv2.circle(frame, pt, 2, (255, 255, 0), 2)
fish_contours = findFish(frame, circle_mask)
filtered_fish_contours = filterFish(fish_contours)
for ii in range(0, len(filtered_fish_contours)):
filtered_fish_contours[ii] = cv2.convexHull(filtered_fish_contours[ii])
(x,y),radius = cv2.minEnclosingCircle(filtered_fish_contours[ii])
center = (int(x),int(y))
radius = int(radius)
cv2.circle(frame,center,radius,(0,255,0),2)
for ramp_pt in ramp_pts:
if dist(ramp_pt, center) < radius:
cv2.circle(frame,center,radius,(0,255,0),2)
break
else:
cv2.circle(frame,center,radius,(0,0,255),2)
# for ramp_pt in ramp_pts:
# if inRect(ramp_pt, (x,y), (x+w,y+h)):
# cv2.rectangle(frame,(x,y),(x+w,y+h),(0,255,0),2)
# break
# else:
# cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),2)
video.write(frame)
cv2.imshow("Frame", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def update(self):
f1x, f1y = f1 = self.parents[0].coord
f2x, f2y = f2 = self.parents[1].coord
k = self.k = dist(f1, self.parents[2].coord) + dist(f2, self.parents[2].coord)
kx, ky = self.parents[2].coord
distance_between = dist((f1x,f1y), (f2x,f2y))
r = (k - distance_between) * .5
if f1x <= f2x:
x1 = f1x
x2 = f2x
else:
x1 = f2x
x2 = f1x
if f1y <= f2y:
y1 = f2y
y2 = f1y
else:
y1 = f1y
y2 = f2y
c.coords(self.handle,
x1 - r, y1,
x1 - r, y2,
x1, y2 - r,
x2, y2 - r,
x2 + r, y2,
x2 + r, y1,
x2, y1 + r,
x1, y1 + r,
x1 - r, y1)
c.coords(self.tielines,
f1x, f1y, kx, ky, f2x, f2y)
def aggressive_preprocess(world):
'''
inplace, because why not?
'''
#return
data = world.data
rxy = world.robot_coords
num_lambdas = 0
for i in range(len(data)):
xy = world.index_to_coords(i)
if dist(rxy, xy) > 7:
if data[i] == '\\':
num_lambdas += 1
data[i] = '!'
data.extend(['\\']*num_lambdas)
def is_node_ok(self, node):
if len(node[1][0]) < 1 or len(node[1][1]) < 1:
return "Node size not correct"
if node[0] == '':
return "Fill the name field"
for n in self.Nodes:
if n[0] == node[0]:
return "Name already used"
if node[2] != -1:
M = Map(self.Nodes, self.Connectors)
if utils.dist(n[2], node[2]) < M.hitbox:
return "Node too close to a peer"
del(M)
else:
return "Choose a position for the node"
return True
def A_star(grid, wall, goal, h, frontier, inner, g_score, f_score, come_from): curr = next(iter(frontier)) # choose as current the node with the higher F score, where F(n) = G(n) + H(n) for node in frontier: if f_score[node] < f_score[curr]: curr = node # check if it is the goal if curr == goal: path = reconstruct_path(curr, come_from) return None, None, None, path # switch the selected node from frontier to inner set frontier.discard(curr) inner.add(curr) # get all the nodes adjacent to the current one neighborhood = get_neighborhood(curr, grid, wall) for neighbour in neighborhood: # if a node is reachable for the first time, initialize its G score to Inf if neighbour not in g_score.keys(): g_score[neighbour] = Inf # skip the inner nodes if neighbour in inner: continue # compute the new G score new_gScore = g_score[curr] + dist(curr, neighbour) # if a shorter path to reach the neighbour has been found --> update its F score if new_gScore < g_score[neighbour]: come_from[neighbour] = curr g_score[neighbour] = new_gScore f_score[neighbour] = new_gScore + h[neighbour] if neighbour not in frontier: frontier.add(neighbour) return frontier, inner, g_score, come_from
def forward(self, x, switch):
d = dist(x, c)
d_min = d.min(dim=1, keepdim=True)[0]
s = self.trans(d_min)
mean = self.mean(x)
if switch:
a = self.alph(x) * 2
b = self.bet(x) / 2
gamma_dist = D.Gamma(a + 1e-4, b + 1e-4)
if self.training:
samples_var = gamma_dist.rsample(torch.Size([50]))
x_var = (1.0 / (samples_var + 1e-4))
else:
samples_var = gamma_dist.rsample(torch.Size([2000]))
x_var = (1.0 / (samples_var + 1e-4))
var = (1 - s) * x_var + s * (
(20.0**2) * torch.ones_like(x_var)) # HYPERPARAMETER
else:
var = torch.tensor([15.0])
return mean, var
def forward(self, x, switch):
d = dist(x, c)
d_min = d.min(dim=1, keepdim=True)[0]
s = self.trans(d_min)
mean = self.mean(x)
if switch:
a = self.alph(x)
b = self.bet(x)
gamma_dist = D.Gamma(a+1e-8, 1.0/(b+1e-8))
if self.training:
samples_var = gamma_dist.rsample(torch.Size([num_draws_train]))
x_var = (1.0/(samples_var+1e-8))
else:
samples_var = gamma_dist.rsample(torch.Size([1000]))
x_var = (1.0/(samples_var+1e-8))
# var = (1-s) * x_var + s*torch.tensor([y_std**2], device=x.device) # HYPERPARAMETER
var = (1-s) * x_var + s * y_std ** 2
else:
var = 0.05*torch.ones_like(mean)
return mean, var
def forward(self, x, switch):
d = dist(x, c)
d_min = d.min(dim=1, keepdim=True)[0]
s = self.trans(d_min)
mean = self.mean(x)
if switch:
a = self.alph(x)
b = self.bet(x)
gamma_dist = D.Gamma(a + 1e-8, 1.0 / (b + 1e-8))
if self.training:
samples_var = gamma_dist.rsample(torch.Size([50]))
x_var = (1.0 / (samples_var + 1e-8))
else:
samples_var = gamma_dist.rsample(torch.Size([2000]))
x_var = (1.0 / (samples_var + 1e-8))
var = (1 - s) * x_var + s * torch.tensor([3.5**2
]) # HYPERPARAMETER
else:
var = torch.tensor([0.05])
return mean, var
def costwithcurrents(node1, node2, AUVspeed, alpha):
### BEGIN SOLUTION
S = AUVspeed
#Use the current at the first node (could also use the average between node1 and node2)"
V_current = node1.current
#Unit vector from node1 to node2: use only x,y components
s = np.array(node2.position)-np.array(node1.position)
s = s[0:2].flatten()
ns = np.linalg.norm(s)
if ns < 1e-10:
return (0.0, np.array((0.0,0.0)))
s = s/ns
c = (V_current[0]*s[1]-V_current[1]*s[0])
# Compute V_AUV - the required AUV velocity direction to get us from node1 to node2, accounting for the current
# uses quadratic formula, so have to check if we want the negative or positive solution
V_AUVy = (np.sqrt((s[0]**2)*(s[1]**2)*(S**2)+ (s[1]**4)*(S**2) -(s[1]**2)*c**2) -s[0]*c)
V_AUVpp = np.array((np.sqrt(S**2 - V_AUVy**2),V_AUVy))
V_AUVpn = np.array((np.sqrt(S**2 - V_AUVy**2),-V_AUVy))
V_AUVnp = np.array((-np.sqrt(S**2 - V_AUVy**2),V_AUVy))
V_AUVnn = np.array((-np.sqrt(S**2 - V_AUVy**2),-V_AUVy))
list = [V_AUVpp,V_AUVpn, V_AUVnp,V_AUVnn]
V_AUV = list[np.argmax([np.dot(V+V_current,s) for V in list])]
# compute the speed we are travelling in the target direction, i.e. from node1 to node2
SpeedInTargetDirection = np.dot(V_AUV,s) + np.dot(V_current,s)
# compute the time it will take to travel from node1 to node2 - handle edge case that 0 speed=inf time
time = dist(node1,node2)/SpeedInTargetDirection if SpeedInTargetDirection != 0 else 0
timeToDestination = (time if time > 0 else np.inf)
riskfactor = (1/(1-alpha*node2.risk + 1e-10))
return float(riskfactor*timeToDestination), V_AUV
def fit(self, k: int, data = None):
"""
Treina o clusterizador com os dados passados em 'data'
Parametros
-------------
`k`: número de clusters a serem formados nos dados.
`data`: conjunto de dados utilizado no treinamento modelo.
"""
if data is None:
raise ValueError('Não foi passado um conjunto de dados para o treinamento')
if not k >= 2:
raise ValueError(f'O valor de "k" deve ser maior ou igual a 2. Valor passado: {k}')
# está iniciando um novo 'fit'
if self._centroids != None:
self._centroids = None
self.__set_initial_centroids(k = k, data = data)
# inicializa os clusters (inicialmente todos estão vazios)
cluster_objects = {c: [] for c in range(k)}
for _ in range(self._max_iter):
for obj in data:
#calcula a distancia do objeto para todos os centroides
distancias = np.array([dist(obj, c) for c in self._centroids])
# adiciona o objeto ao centroide cuja a distancia é a menor entre todos os centroides
cluster_objects[ distancias.argmin() ].append(obj)
# atualiza os centroides calculando a média de cada atributos que está em cada cluster
self.__update_centroids(cluster_objects)
return self._centroids
def plot_distributions(signals,
ax=None,
quantiles=1000,
title='',
logscale=False):
if not isinstance(signals, dict): signals = {'signal': signals}
if ax is None:
fig, ax = plt.subplots()
if isinstance(ax, str) and ax == 'interactive':
ax = InteractiveFigure().get_axes()
if title: title += '\n'
if len(signals) == 1:
title += f'({len(list(signals.values())[0]):d} samples)'
else:
lengths = ', '.join([f'{len(signals[sig]):d}' for sig in signals])
title += f'({lengths:s} samples respectively)'
for i, sig in enumerate(signals):
signal = np.log10(1+np.abs(signals[sig])) * np.sign(signals[sig]) \
if logscale else signals[sig]
dist = utils.dist(signal, 100 * np.arange(quantiles + 1) / quantiles)
ax.plot((0, 100),
2 * [dist[1]],
color=utils.DEF_COLORS[i],
linestyle=':')
ax.plot(100 * np.arange(quantiles + 1) / quantiles,
dist[2:],
color=utils.DEF_COLORS[i],
linestyle='-',
label=sig)
ax.set_xlim((0, 100))
ax.grid()
ax.set_xlabel('Quantile [%]', fontsize=12)
ax.set_ylabel('Log-signal [base 10]' if logscale else 'Signal',
fontsize=12)
ax.set_title(title, fontsize=14)
if len(signals) > 1:
ax.legend()
utils.draw()
def __init__(self, level, pos, *, centerpos=True, source_sprite, target):
super().__init__(level,
pos,
centerpos=centerpos,
norm_vector=utils.Vector(0, 0))
self.source_sprite = source_sprite # It always comes back!
self.target = target
self.dist = utils.dist(self.pos, self.target)
self.surface = self.base_image
self.length = self.base_length
self.curve_points = None
self.curve = self.new_curve()
self.time_trans = lambda t: utils.translate_to_zero_to_one_bounds(
t, (0, self.length))
self.time = 0
self.rotation = 0
self.coming_back = False
self.coming_back_curve = False
self.hit = set()
self.act_damage = self.max_damage
self.last_source_sprite_pos = self.source_sprite.rect.topleft
self.last_adist = self.dist
def onRingLeftDrag(self, event):
cz = self.curZoom
#print "drag:", event.x, event.y
# calculate new radius
center = self.pixLoc
cur = (event.x, event.y)
# calculate dist form center to cur
d = utils.dist(center, cur) / cz
if d <= 10:
self.remove()
return
self.r = d - self.ringSpacing[0]
self.z_r = cz * self.r
self.reDraw()
self.parentSheet.updateNodeEdges(self)
def update(self, full_obstacle_list, goal_pos):
self.steps += 1
if self.prev_full_obstacle_list_size != len(full_obstacle_list):
print("Environment has been changed. Reset master_policy")
self.prev_full_obstacle_list_size = len(full_obstacle_list)
master_policy.clear()
self.obstacles_in_view = [] #delete all the old obstacles in view
for obs in full_obstacle_list:
if utils.dist(self.pos, obs) < constants.SENSOR_MAX_R:
self.obstacles_in_view.append(obs)
#re-calculate direction to goal
self.goal_brg = utils.brg_in_deg(self.pos, goal_pos)
#re-estimate sensor output by weighted sum method
co1, need_turn = self.s_array.update(self.pos, self.goal_brg,
self.obstacles_in_view, "w_sum",
full_obstacle_list, master_policy,
I_was_here, goal_pos)
obstacles_in_3x3_grid = utils.check_obstacle_3x3(
self.pos, full_obstacle_list)
if len(obstacles_in_3x3_grid) == 0:
self.co = utils.brg_in_deg(self.pos, goal_pos)
print("path clear. ignoring recommendation")
elif need_turn: #do we need to turn
self.co = co1
print("path not clear. following recommendation")
else: # path is not fully clear, but there are no immediate obstacles
pass
#the robot by one step...
self.move(1)
print(f"master_policy.keys()={master_policy.keys()}")
return self.has_hit_obstacle(
full_obstacle_list), self.has_reached_goal(goal_pos)
def create_cmd(ctx, number, dimension_x, dimension_y, par_a, par_b):
image = np.zeros((dimension_x, dimension_y, 3), 'uint8')
mode = 'RGB'
mid_x = int(dimension_x / 2)
mid_y = int(dimension_y / 2)
if number == 0:
for j in range(-int(par_a / 2), int((par_a + 1) / 2)):
for i in range(-int(par_a / 2), int((par_a + 1) / 2)):
image[mid_x + i, mid_y + j, ] = 255
if number == 1:
freq = par_a
phase = par_b
for j in range(dimension_y):
for i in range(dimension_x):
image[i, j, ] = 127 * math.sin(
utils.dist(mid_x, mid_y, i, j) * 2 * math.pi * freq +
phase) + 127
# image[i,j,] = 127 * math.sin(utils.dist(0, 0, i, j)*freq + phase) + 127
ctx.obj['image'] = image
ctx.obj['img_mode'] = mode
def check_nodes(self):
for node in self.openset.union(self.closedset):
if node.parent and (not self.ros_node.hasLimitedVision or dist(node, self.ros_node.pos) < self.ros_node.visionRadius):
# If the node is in vision radius
if self.ros_node.cast_ray(node.pos, node.parent.pos, radius=UAV_THICKNESS)[0]:
self.clearSubtree(node)
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
plt_goal_angle_arr = np.array([])
goal_x = -0.5
goal_y = -0.5
goal_coor = np.array([goal_x, goal_y])
goal_angle = np.arctan2(goal_y, goal_x)
goal_angle %= 2 * np.pi
print("goal angle = %f" % np.rad2deg(goal_angle))
base_speed = 100
# timeout = abs(17 * (goal_angle / np.pi)) + 1
goal_r = 0.05
angle = self.odometry.theta
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
while abs(dist_to_goal) > goal_r:
goal_angle = np.arctan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
goal_angle %= 2 * np.pi
print("(%f, %f), dist to goal = %f\n" %
(self.odometry.x, self.odometry.y, dist_to_goal))
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
angle = self.odometry.theta
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
output_dist = self.pid_controller_dist.update(
0, dist_to_goal, self.time.time())
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
# self.create.drive_direct(
# int((dist_to_goal * base_speed) + output),
# int((dist_to_goal * base_speed) - output)
# )
# self.create.drive_direct(
# int(output_dist + output),
# int(output_dist - output)
# )
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
np.savetxt("goalAngleOutput.csv", plt_goal_angle_arr, delimiter=",")
def __get_lines(self):
def union_x(l1, l2):
if l1[2] < l1[0]:
return union_x([l1[2], l1[3], l1[0], l1[1]], l2)
if l2[2] < l2[0]:
return union_x(l1, [l2[2], l2[3], l2[0], l2[1]])
if l2[0] < l1[0]:
return union_x(l2, l1)
if l1[2] < l2[2]:
return [l1[0], l1[1], l2[2], l2[3]]
return l1
def union_y(l1, l2):
if l1[3] < l1[1]:
return union_y([l1[2], l1[3], l1[0], l1[1]], l2)
if l2[3] < l2[1]:
return union_y(l1, [l2[2], l2[3], l2[0], l2[1]])
if l2[1] < l1[1]:
return union_y(l2, l1)
if l1[3] < l2[3]:
return [l1[0], l1[1], l2[2], l2[3]]
return l1
def union(l1, l2):
lx = union_x(l1, l2)
ly = union_y(l1, l2)
vx = np.array([lx[2] - lx[0], lx[3] - lx[1]])
vy = np.array([ly[2] - ly[0], ly[3] - ly[1]])
if norm(vx) < norm(vy):
return ly
return lx
def verify_and_delete(i1, i2, d_lines):
if i1 in d_lines and i2 in d_lines:
l1 = d_lines[i1]
l2 = d_lines[i2]
v1 = [l1[2] - l1[0], l1[3] - l1[1]]
v2 = [l2[2] - l2[0], l2[3] - l2[1]]
cos_a = np.dot(v1, v2) / (norm(v1) * norm(v2))
if abs(cos_a) > 0.9:
if dist_to_line(l1, [l2[0], l2[1]]) < 15 and \
dist_to_line(l1, [l2[2], l2[3]]) < 15:
d_lines[i1] = union(l1, l2)
del d_lines[i2]
edged = self.img_edged
minLineLength = 60
maxLineGap = 10
lines = cv2.HoughLinesP(edged.copy(),
1,
np.pi / 180,
threshold=50,
minLineLength=minLineLength,
maxLineGap=maxLineGap)
lines = list(map(lambda line: line[0], lines))
lines_orig = deepcopy(lines)
d_lines = {i: line for i, line in enumerate(lines)}
combs = combinations(range(len(lines)), 2)
for comb in combs:
verify_and_delete(comb[0], comb[1], d_lines)
lines = [val for val in d_lines.values()]
lines = sorted(
lines,
key=(lambda line: dist([line[0], line[1]], [line[2], line[3]])),
reverse=True)
return lines_orig, lines[:9]
def dictExtractor(self, state, action):
if action is None:
return [('trapped', 1.)]
if action.norm() == 1:
agent = state.snakes[self.id]
# pretend agent moves with action
authorized_move = agent.authorizedMove(
action) # check before moving
last_tail = agent.position.pop()
agent.position.appendleft(
utils.add(agent.head(), action.direction()))
elif action.norm() > 1:
agent = deepcopy(state.snakes[self.id])
authorized_move = agent.authorizedMove(
action) # check before moving
agent.move(action)
else:
agent = state.snakes[self.id]
authorized_move = True
head = agent.head()
dir_ = agent.orientation()
def relPos(p):
return self.relativePos(head, p, dir_)
features = [(('candy', v, relPos(c)), 1.)
for c, v in state.candies.iteritems()
if utils.dist(head, c) < self.radius]
features += [
(('adv-head', relPos(s.head())), 1.)
for k, s in state.snakes.iteritems()
if k != self.id and utils.dist(head, s.head()) < self.radius
]
features += [
(('adv-tail', relPos(s.position[i])), 1.)
for k, s in state.snakes.iteritems()
for i in xrange(1, len(s.position))
if k != self.id and utils.dist(head, s.position[i]) < self.radius
]
features += [
(('my-tail', relPos(state.snakes[self.id].position[i])), 1.)
for i in xrange(1, len(state.snakes[self.id].position)) if
utils.dist(head, state.snakes[self.id].position[i]) < self.radius
]
# features += [
# (('x', min(head[0], state.grid_size - 1 - head[0])), 1.),
# (('y', min(head[1], state.grid_size - 1 - head[1])), 1.)
# ]
wall_features = []
if dir_ == (0, 1):
wall_features += [(('wall-xl', head[0]), 1.),
(('wall-xr', state.grid_size - 1 - head[0]), 1.),
(('wall-yt', state.grid_size - 1 - head[1]), 1.),
(('wall-yb', head[1]), 1.)]
elif dir_ == (0, -1):
wall_features += [(('wall-xr', head[0]), 1.),
(('wall-xl', state.grid_size - 1 - head[0]), 1.),
(('wall-yb', state.grid_size - 1 - head[1]), 1.),
(('wall-yt', head[1]), 1.)]
elif dir_ == (1, 0):
wall_features += [(('wall-yb', head[0]), 1.),
(('wall-yt', state.grid_size - 1 - head[0]), 1.),
(('wall-xl', state.grid_size - 1 - head[1]), 1.),
(('wall-xr', head[1]), 1.)]
elif dir_ == (-1, 0):
wall_features += [(('wall-yt', head[0]), 1.),
(('wall-yb', state.grid_size - 1 - head[0]), 1.),
(('wall-xr', state.grid_size - 1 - head[1]), 1.),
(('wall-xl', head[1]), 1.)]
features += [(f, v) for f, v in wall_features
if abs(f[1]) < self.radius]
if not authorized_move:
features += [("non-auth", 1.)]
# revert changes
if action.norm() == 1:
agent.position.popleft()
agent.position.append(last_tail)
return features
def world_gen(database_name='world'):
client = MongoClient()
db = client[database_name]
db_clear(db)
t = time.clock()
metrics = {
'collisions': {
'simplebot': (t, 0),
'wigglebot': (t, 0)
},
'pickups': {
'simplebot': (t, 0),
'wigglebot': (t, 0)
},
'dropoffs': {
'simplebot': (t, 0),
'wigglebot': (t, 0)
},
}
# sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# sock.connect((HOST, PORT))
# settings
N = SETTINGS['N']
MAX_X = SETTINGS['MAX_X']
MAX_Y = SETTINGS['MAX_Y']
db_update_dict(db, 'settings', SETTINGS)
# metrics
# t = time.clock()
# metrics = {'collisions':[(t,0)],
# 'pickups':[(t,0)],
# 'dropoffs':[(t,0)]}
# db_update_dict(db, 'metrics', metrics)
# update_metrics(db, 'collisions',0)
# db_insert_metrics(db, 'pickups',0)
# db_insert_metrics(db, 'dropoffs',0)
#%%
df = pd.DataFrame(index=range(N),
data={
'x': rnd_vec(N, MAX_X),
'y': rnd_vec(N, MAX_Y)
})
# alex
df['private_key'] = 'empty' # bigchaindb private key for each unique robot
df['public_key'] = 'empty' # bigchaindb public key for each unique robot
df['state'] = 'empty' # intial state of the robot. this state is later stored in metadata tag in blockchaindb
for ix, row in df.iterrows():
current_keypair = generate_keypair()
private_key = current_keypair.private_key
public_key = current_keypair.public_key
df.loc[ix, 'private_key'] = private_key
df.loc[ix, 'public_key'] = public_key
# alex end
# blender specific columns
df['blender_name'] = ''
df['blender_type'] = 'object'
df.loc[0, 'blender_name'] = 'car.001'
#df.loc[:N/2,'machine_type'] = 'simplebot'
df['machine_type'] = 'simplebot' #'simplebot'#'wigglebot'#
#MORTEN BEGIN
df.loc[
N - 1,
'machine_type'] = 'roguebot' #overwriting 1 so one rogue node availlable
#MORTEN END
df['radius'] = 0.9
df['collision'] = False
df['cost'] = 0.0
#df['u']=0.0
#df['v']=0.0
df['x_trg'] = rnd_vec(N, MAX_X)
df['y_trg'] = rnd_vec(N, MAX_Y)
db_update_df(db, df)
while True:
# read machines information
# get rid of nans
df = db_read_df(db)
settings = db_read_dict(db, 'settings')
tokens = {}
#alex
#tokens['app_id'] = '4a33bc96'
#tokens['app_key'] = '5e9699fbe0c5bca83d35e3a7e63ba1c1'
#jur
#tokens['app_id'] = 'dbd40a9c'
#tokens['app_key'] = 'a8062ad9546eba03f4f61ad7f6d4afac'
#from bigchaindb_driver.crypto import generate_keypair
#master_keys = generate_keypair()
for machine_module in machine_modules:
machine_module.set_df(df, settings, metrics, db)
#update settings for failing bigchaindb
db_update_dict(db, 'settings',
{'USE_BIGCHAINDB': settings['USE_BIGCHAINDB']})
# velocity has been set by machines
if 'u' in df.columns and 'v' in df.columns:
selection = ~df.u.isna() & ~df.v.isna()
df.loc[selection,
'x_new'] = df.loc[selection, 'x'] + df.loc[selection, 'u']
df.loc[selection,
'y_new'] = df.loc[selection, 'y'] + df.loc[selection, 'v']
for ix, row in df.iterrows():
fr = df[df.index != ix]
df.loc[ix, 'collision'] = any(
dist(row.x_new, row.y_new, fr.x_new, fr.y_new) <= (
fr.radius + row.radius))
collisions = len(df[df.collision])
if collisions > 0:
print('collisions', collisions)
df.loc[~df.collision, 'x'] = df.loc[~df.collision, 'x_new']
df.loc[~df.collision, 'y'] = df.loc[~df.collision, 'y_new']
db_update_df(db, df.loc[selection])
#xi, yi, zi=fn_cost(df, method=INTERPOLATION)
# only if we have a target
xi, yi, zi = cost_function(df, selection, 0, settings)
db_update_grid(db, 'world', xi, yi, zi)
grid = {'world': {'x': xi, 'y': yi, 'z': zi}}
#metrics = db_read_metrics(db)
yield df, grid, metrics
#!/usr/bin/env python2
from PIL import Image
from utils import constrain, lerp, dist
if __name__ == "__main__":
size = (128, 128)
data = []
for i in range(size[0]):
for j in range(size[1]):
x = lerp(-1.5, 1.5, float(i) / size[0])
y = lerp(-1.5, 1.5, float(j) / size[1])
pixel = 1.0 - dist(0.0, 0.0, x, y)
data.append(int(constrain(pixel, 0.0, 1.0) * 255))
light = Image.new('L', size)
light.putdata(data)
light.save("light.png", "PNG")
def is_capture(self, oppo): if oppo not in self.state_oppo_neigh: return False return dist(self.state.x, self.state_oppo_neigh[oppo].x) < self.r
def heuristic_no_current_no_risk(node1, goal_node, AUV_speed):
### BEGIN SOLUTION
timetodestination = dist(node1,goal_node)/AUV_speed
return timetodestination
def level(self, x): return dist(np.array([self.x0, self.y0]), x) - self.size
def heuristicwithrisk(node1, node2, AUVspeed):
### BEGIN SOLUTION
timetodestination = dist(node1,node2)/AUVspeed
return timetodestination
### END SOLUTION
def main(self):
"""
dims : graph's dimensions (numpy array of tuples)
obstacles : obstacles to be placed in the graph (list of shapely.Polygons)
home : home location (tuple)
init_state : start location (tuple)
goal_state : goal location (tuple)
delta : distance between nodes (int)
k : shrinking ball facotr (int)
n : number of waypoints to push (int)
path : list of waypoints (list of tuples)
case : case number to set behavior (int)
Units: meters
"""
#rospy.init_node('RRTnode')
#rospy.wait_for_service('/control/waypoints')
wp_push = rospy.ServiceProxy('/control/waypoints', WaypointPush)
#rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_cb)
#rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.global_heading)
#rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_position)
rate = rospy.Rate(1) # send msgs at 1 Hz
start_time = rospy.get_time()
rate.sleep()
# while loop for gps_check:
while self.gps_status == -1 or self.gps_status is None:
rospy.loginfo("BAD GPS")
rate.sleep()
while not self.home_set:
rospy.loginfo("Home not set!")
rate.sleep()
# E-x N-y D-z
dims = np.array([(-500, 2500), (-100, 2900), (-50, -50)])
obstacle = [(Point(1300, 1300).buffer(500))]
init = self.pos
goal = (2300.0, 2600.0, -50.0)
delta = 100
k = 2
graph = Graph(dims, obstacle)
path = None
trail = []
position = []
start_index = 0
rospy.loginfo("Calculating Trajectory...")
#print('Calculating Trajectory...\n')
while not rospy.is_shutdown():
if graph.num_nodes() == 0:
#t0 = time.time()
rrt = RRT(graph, init, goal, delta, k, path, self.heading)
if graph.num_nodes() <= 250:
path, leaves = rrt.search()
else:
init = path[path.index(rrt.brute_force(self.pos, path)) + 1]
trail.append(path)
position.append(self.pos)
rate.sleep()
graph.clear()
pathNED = np.asarray(path)
wp_msg = []
pathLLA = self.frame.ConvNED2LLA(pathNED.T)
for i in range(len(pathNED)):
wp_point = Waypoint()
wp_point.frame = 3
wp_point.x_lat = pathLLA[0][i]
wp_point.y_long = pathLLA[1][i]
wp_point.z_alt = pathLLA[2][i]
wp_msg += [wp_point]
print(wp_msg, '\n')
resp = wp_push(start_index, wp_msg)
print(resp, '\n')
start_index += path.index(rrt.brute_force(self.pos, path)) + 1
#print('Search Time: {} secs\n'.format(time.time() - t0))
rate.sleep()
if path is not None:
if dist(self.pos, goal) <= delta * 3 or goal in path:
rospy.loginfo("Goal reached")
#print('Goal Reached.\n')
print('trail : ', trail, '\n')
print('position: ', position, '\n')
break
print('EOL')
def fgw_barycenters(N,
Ys,
Cs,
ps,
lambdas,
alpha,
fixed_structure=False,
fixed_features=False,
p=None,
loss_fun='square_loss',
max_iter=100,
tol=1e-9,
verbose=False,
log=True,
init_C=None,
init_X=None):
"""
Compute the fgw barycenter as presented eq (5) in [3].
----------
N : integer
Desired number of samples of the target barycenter
Ys: list of ndarray, each element has shape (ns,d)
Features of all samples
Cs : list of ndarray, each element has shape (ns,ns)
Structure matrices of all samples
ps : list of ndarray, each element has shape (ns,)
masses of all samples
lambdas : list of float
list of the S spaces' weights
alpha : float
Alpha parameter for the fgw distance
fixed_structure : bool
Wether to fix the structure of the barycenter during the updates
fixed_features : bool
Wether to fix the feature of the barycenter during the updates
init_C : ndarray, shape (N,N), optional
initialization for the barycenters' structure matrix. If not set random init
init_X : ndarray, shape (N,d), optional
initialization for the barycenters' features. If not set random init
Returns
----------
X : ndarray, shape (N,d)
Barycenters' features
C : ndarray, shape (N,N)
Barycenters' structure matrix
log_:
T : list of (N,ns) transport matrices
Ms : all distance matrices between the feature of the barycenter and the other features dist(X,Ys) shape (N,ns)
References
----------
.. [3] Vayer Titouan, Chapel Laetitia, Flamary R{\'e}mi, Tavenard Romain
and Courty Nicolas
"Optimal Transport for structured data with application on graphs"
International Conference on Machine Learning (ICML). 2019.
"""
np.random.seed(2)
S = len(Cs)
d = reshaper(Ys[0]).shape[1] #dimension on the node features
if p is None:
p = np.ones(N) / N
Cs = [np.asarray(Cs[s], dtype=np.float64) for s in range(S)]
Ys = [np.asarray(Ys[s], dtype=np.float64) for s in range(S)]
lambdas = np.asarray(lambdas, dtype=np.float64)
if fixed_structure:
if init_C is None:
C = Cs[0]
else:
C = init_C
else:
if init_C is None:
xalea = np.random.randn(N, 2)
C = dist(xalea, xalea)
C /= C.max()
else:
C = init_C
if fixed_features:
if init_X is None:
X = Ys[0]
else:
X = init_X
else:
if init_X is None:
X = np.zeros((N, d))
else:
X = init_X
#T=[np.outer(p,q) for q in ps]
T = [random_gamma_init(p, q) for q in ps]
# X is N,d
# Ys is ns,d
Ms = update_Ms(X, Ys)
# Ms is N,ns
cpt = 0
err_feature = 1
err_structure = 1
if log:
log_ = {}
log_['err_feature'] = []
log_['err_structure'] = []
log_['Ts_iter'] = []
while ((err_feature > tol or err_structure > tol) and cpt < max_iter):
Cprev = C
Xprev = X
if not fixed_features:
Ys_temp = [reshaper(y).T for y in Ys]
X = update_feature_matrix(lambdas, Ys_temp, T, p)
# X must be N,d
# Ys must be ns,d
Ms = update_Ms(X.T, Ys)
if not fixed_structure:
if loss_fun == 'square_loss':
# T must be ns,N
# Cs must be ns,ns
# p must be N,1
T_temp = [t.T for t in T]
C = update_square_loss(p, lambdas, T_temp, Cs)
# Ys must be d,ns
# Ts must be N,ns
# p must be N,1
# Ms is N,ns
# C is N,N
# Cs is ns,ns
# p is N,1
# ps is ns,1
T = [
fgw_lp((1 - alpha) * Ms[s],
C,
Cs[s],
p,
ps[s],
loss_fun,
alpha,
numItermax=max_iter,
stopThr=1e-5,
verbose=verbose) for s in range(S)
]
# T is N,ns
log_['Ts_iter'].append(T)
err_feature = np.linalg.norm(X - Xprev.reshape(d, N))
err_structure = np.linalg.norm(C - Cprev)
if log:
log_['err_feature'].append(err_feature)
log_['err_structure'].append(err_structure)
if verbose:
if cpt % 200 == 0:
print('{:5s}|{:12s}'.format('It.', 'Err') + '\n' + '-' * 19)
print('{:5d}|{:8e}|'.format(cpt, err_structure))
print('{:5d}|{:8e}|'.format(cpt, err_feature))
cpt += 1
log_[
'T'] = T # ce sont les matrices du barycentre de la target vers les Ys
log_['p'] = p
log_['Ms'] = Ms #Ms sont de tailles N,ns
return X.T, C, log_
def calc_gamma(l1, l2):
inter = find_intersection(l1, l2)
if inter is None:
return None
return (utils.dist(l1[0], inter) + utils.dist(l1[1], inter) + utils.dist(
l2[0], inter) + utils.dist(l2[1], inter)) * np.sin(angle(l1, l2)) / 4
def robot_dist(x):
return dist(self.getPos(), entCenter(x))
def step(self, u):
# check move_base is working
if not self.ros.check_movebase:
self.reset()
done = False
info = {}
if u.shape != self.action_space.shape:
raise ValueError("action size ERROR.")
# planner's direction reward
_p_x = self.ros.planner_path[0].pose.position.x
_p_y = self.ros.planner_path[0].pose.position.y
if any([_p_x, _p_y]) and any([u[0], u[1]]):
r = np.cos(utils.angle_between([_p_x, _p_y],
[u[0], u[1]])) * 0.3 - 0.4
else:
# print("################## OMG ######################")
r = -0.01
# if utils.dist((u[0], u[1]), (0, 0)) < 0.05:
# print("V too small")
# else:
# pass
self.ros.action_to_vel(u)
time.sleep(0.1)
self.ros.stop_robots()
# moving cost
# r = -0.3
# planner's length reward
_path_len = len(self.ros.planner_path)
r += (np.sign(_path_len - self._planner_benchmark) * -0.3) - 0.4
self._planner_benchmark = _path_len
# ARRIVED GOAL
_r_x = self.ros.odom[0]
_r_y = self.ros.odom[1]
_r_yaw = self.ros.odom[2]
_g_x = self.goals[self.current_robot_num][0]
_g_y = self.goals[self.current_robot_num][1]
_g_yaw = self.goals[self.current_robot_num][2]
# if utils.dist([_r_x, _r_y], [_g_x, _g_y]) <= ARRIVED_RANGE_XY and abs(_r_yaw - _g_yaw) <= ARRIVED_RANGE_YAW:
if utils.dist([_r_x, _r_y], [_g_x, _g_y]) <= ARRIVED_RANGE_XY:
done = True
r = 50
# robot's direction reward
# r += np.cos(abs(_g_yaw - _r_yaw)) * 0.1 - 0.1
## The robot which is in collision will get punishment
if self.ros.collision_check:
r = -20.0
done = True
info = {"I got collision..."}
o = self.ros.get_observation
if o is None:
self.reset()
_o = self.normalize_observation(o)
return _o, r, done, info
def heuristicwithcurrents(node1, node2, AUVspeed, max_strength=1):
### BEGIN SOLUTION
timetodestination = dist(node1,node2)/(AUVspeed + max_strength)
return timetodestination
def drawDebugRects(self, image):
if utils.isGray(image):
faceColor = 255
leftEyeColor = 255
rightEyeColor = 255
noseColor = 255
mouthColor = 255
else:
faceColor = (255, 255, 255)
leftEyeColor = (0, 0, 255)
rightEyeColor = (0, 255, 255)
noseColor = (0, 255, 0)
mouthColor = (255, 0, 0)
white = (255, 255, 255)
for face in self.faces:
rects.outlineRect(image, face.faceRect, faceColor)
rects.outlineRect(image, face.leftEyeRect, leftEyeColor)
rects.outlineRect(image, face.rightEyeRect, rightEyeColor)
rects.outlineRect(image, face.noseRect, noseColor)
rects.outlineRect(image, face.mouthRect, mouthColor)
k = 5
leftEye = None
rightEye = None
mouth = None
nose = None
if face.leftEyeRect is not None:
leftEye = (face.leftEyeRect[0] + int(face.leftEyeRect[2] / 2),
face.leftEyeRect[1] + int(face.leftEyeRect[3] / 2))
rect = (leftEye[0] - k, leftEye[1] - k, 2 * k, 2 * k)
rects.outlineRect(image, rect, white)
if face.rightEyeRect is not None:
rightEye = (face.rightEyeRect[0] +
int(face.rightEyeRect[2] / 2),
face.rightEyeRect[1] +
int(face.rightEyeRect[3] / 2))
rect = (rightEye[0] - k, rightEye[1] - k, 2 * k, 2 * k)
rects.outlineRect(image, rect, white)
if face.mouthRect is not None:
mouth = (face.mouthRect[0] + int(face.mouthRect[2] / 2),
face.mouthRect[1] + int(face.mouthRect[3] / 2))
rect = (mouth[0] - k, mouth[1] - k, 2 * k, 2 * k)
rects.outlineRect(image, rect, white)
if face.noseRect is not None:
nose = (face.noseRect[0] + int(face.noseRect[2] / 2),
face.noseRect[1] + int(face.noseRect[3] / 2))
rect = (nose[0] - k, nose[1] - k, 2 * k, 2 * k)
rects.outlineRect(image, rect, white)
if leftEye and rightEye and nose and mouth is not None:
cv2.line(image, leftEye, rightEye, white, 1)
cv2.line(image, leftEye, mouth, white, 1)
cv2.line(image, mouth, rightEye, white, 1)
cv2.line(image, nose, rightEye, white, 1)
cv2.line(image, nose, leftEye, white, 1)
cv2.line(image, nose, mouth, white, 1)
# Descritores
distEyes = utils.dist(leftEye, rightEye)
distLeftEyeToNose = utils.dist(leftEye, nose)
distRightEyeToNose = utils.dist(rightEye, nose)
distLeftEyeToMouth = utils.dist(leftEye, mouth)
distRightEyeToMouth = utils.dist(rightEye, mouth)
distNoseToMouth = utils.dist(nose, mouth)
#print(distEyes, distLeftEyeToNose, distRightEyeToNose, distLeftEyeToMouth, distRightEyeToMouth, distNoseToMouth)
print(distEyes/distLeftEyeToNose, distEyes/distRightEyeToNose, distEyes/distLeftEyeToMouth, distEyes/distRightEyeToMouth, \
distLeftEyeToNose/distLeftEyeToMouth, distRightEyeToNose/distRightEyeToMouth)
# Gerando caracterizadores faciais
#print(">> Face: ",face.faceRect)
#print(">> Left eye: ",face.leftEyeRect)
#print(">> Right eye: ",face.rightEyeRect)
#print(">> Nose: ", face.noseRect)
#print(">> Mouth: ",face.mouthRect)
def sub_callback(msg):
state = mcap_msg_to_player_state(msg)
if dist(state.x, self.state.x) <= R and state.z > 0.23:
pset.update({p: state})
else:
pset.pop(p, '')
def cost_no_current_no_risk(node1, node2, AUV_speed):
### BEGIN SOLUTION
timetodestination = dist(node1,node2)/AUV_speed
s = computeunitvector(node1,node2)
V_AUV = s*AUV_speed
return timetodestination, V_AUV
