コード例 #1
0
ファイル: layout.py プロジェクト: mcs07/pdfminer
 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)
コード例 #2
0
ファイル: g09.py プロジェクト: hherbol/clancelot
	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
コード例 #3
0
ファイル: layout.py プロジェクト: brechin/pdfminer2
 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)
コード例 #4
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
 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
コード例 #5
0
ファイル: dft.py プロジェクト: jminuse/azotosome-paper
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
コード例 #6
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
    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))
コード例 #7
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
            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
コード例 #8
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
        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
コード例 #9
0
ファイル: control.py プロジェクト: sethmccammon/bestinverse
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
コード例 #10
0
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())
コード例 #11
0
 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]
コード例 #12
0
ファイル: vision.py プロジェクト: sethmccammon/bestinverse
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
コード例 #13
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
            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
コード例 #14
0
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
コード例 #15
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
        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
コード例 #16
0
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
コード例 #17
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
    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)
コード例 #18
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
    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))
コード例 #19
0
ファイル: uvmap.py プロジェクト: cahirwpz/demoscene
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)
コード例 #20
0
ファイル: uvmap.py プロジェクト: cahirwpz/demoscene
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)
コード例 #21
0
ファイル: uvmap.py プロジェクト: cahirwpz/demoscene
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)
コード例 #22
0
ファイル: neons-2.py プロジェクト: cahirwpz/demoscene
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)
コード例 #23
0
ファイル: neons-2.py プロジェクト: cahirwpz/demoscene
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)
コード例 #24
0
ファイル: planner.py プロジェクト: modulo-/SDP-2016-Team-F
 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)
コード例 #25
0
ファイル: neons-2.py プロジェクト: cahirwpz/demoscene
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)
コード例 #26
0
ファイル: units.py プロジェクト: alexhenning/Tug-of-War
 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
コード例 #27
0
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
コード例 #28
0
ファイル: graphics.py プロジェクト: exupero/taxicabland
    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)
コード例 #29
0
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)
コード例 #30
0
ファイル: main.py プロジェクト: alaxa27/NetworkSimulator
 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
コード例 #31
0
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
コード例 #32
0
        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
コード例 #33
0
        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
コード例 #34
0
        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
コード例 #35
0
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
コード例 #36
0
    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
コード例 #37
0
ファイル: data.py プロジェクト: ido90/Earthquakes
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()
コード例 #38
0
 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
コード例 #39
0
ファイル: Thought.py プロジェクト: tannerbohn/MindMap
    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)
コード例 #40
0
    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)
コード例 #41
0
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
コード例 #42
0
ファイル: incr_phi_star.py プロジェクト: rhidra/autopilot
 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)
コード例 #43
0
ファイル: lab5_move.py プロジェクト: raisaifquat/USC-CSCI445
    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=",")
コード例 #44
0
ファイル: cube.py プロジェクト: DaniilSNikulin/DepthMap
    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]
コード例 #45
0
    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
コード例 #46
0
ファイル: world.py プロジェクト: jurryt/ob_sim
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
コード例 #47
0
#!/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")
コード例 #48
0
	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
コード例 #49
0
def heuristic_no_current_no_risk(node1, goal_node, AUV_speed):
    ### BEGIN SOLUTION
    timetodestination = dist(node1,goal_node)/AUV_speed
    return timetodestination
コード例 #50
0
	def level(self, x):
		return dist(np.array([self.x0, self.y0]), x) - self.size
コード例 #51
0
def heuristicwithrisk(node1, node2, AUVspeed):
    ### BEGIN SOLUTION
    timetodestination = dist(node1,node2)/AUVspeed
    return timetodestination 
    ### END SOLUTION
コード例 #52
0
    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')
コード例 #53
0
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_
コード例 #54
0
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
コード例 #55
0
 def robot_dist(x):
     return dist(self.getPos(), entCenter(x))
コード例 #56
0
    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
コード例 #57
0
def heuristicwithcurrents(node1, node2, AUVspeed, max_strength=1):
    ### BEGIN SOLUTION
    timetodestination = dist(node1,node2)/(AUVspeed + max_strength)
    return timetodestination 
コード例 #58
0
    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)
コード例 #59
0
		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, '')
コード例 #60
0
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