def set_receiver(self, _player_list):
        receiver_op_dist_list = []
        for receiver in _player_list :
            temp_receiver_op_dist_list = []
            # the sender is not a receiver candidate
            if receiver == self.sender :
                receiver_op_dist_list.append(999)
                continue

            # the distance between the robot and opponents
            for op in range(1, 5) : #[1,2,3,4]
                op_distance = helper.dist(self.cur_posture[receiver][X], self.cur_posture_op[op][X], self.cur_posture[receiver][Y], self.cur_posture_op[op][Y])
                temp_receiver_op_dist_list.append(op_distance)

            # save the shortest distance between this robot and one of opponents
            receiver_op_dist_list.append(min(temp_receiver_op_dist_list))

        receiver_ball_list = []
        for r in receiver_op_dist_list :
            # if the minimum distance between player and opponent's player is less than 0.5, this robot cannot be receiver
            if r < 0.5 or r == 999:
                receiver_ball_list.append(999)
                continue
            id = receiver_op_dist_list.index(r) + 1
            receiver_ball_distance = helper.dist(self.cur_ball[X], self.cur_posture[id][X], self.cur_ball[Y], self.cur_posture[id][Y])
            receiver_ball_list.append(receiver_ball_distance)

        if min(receiver_ball_list) < 999 :
            min_id = receiver_ball_list.index(min(receiver_ball_list)) + 1
            return min_id
        return None
Exemple #2
0
    def receive_ball(self):
        # if receiver does not exist, do nothing
        if self.receiver == None:
            return

        goal_dist = helper.dist(
            4.0, self.cur_posture[self.receiver][X], 0, self.cur_posture[self.receiver][Y])
        # if sender is in shoot chance, receiver does nothing(reset)
        if self.shoot_chance(self.sender):
            self.actions(self.receiver, None)
            return
        # if receiver is in shoot chance, receiver try to shoot
        if self.shoot_chance(self.receiver):
            if goal_dist > 0.3 * self.field[X] / 2:
                self.actions(self.receiver, 'dribble', refine=True)
                return
            else:
                self.actions(self.receiver, 'kick')
                return

        # if sender exists
        if not self.sender == None:
            s2risFace, _ = self.is_facing_target(
                self.sender, self.cur_posture[self.receiver][X], self.cur_posture[self.receiver][Y], 4)
            r2sisFace, _ = self.is_facing_target(
                self.receiver, self.cur_posture[self.sender][X], self.cur_posture[self.sender][Y], 4)
            # if sender and receiver directs each other
            if s2risFace and r2sisFace:
                if self.cur_posture[self.receiver][TH] > 0 or self.cur_posture[self.receiver][TH] < -3:
                    self.actions(self.receiver, 'follow', [
                                 self.prev_posture[self.receiver][X], self.prev_posture[self.receiver][Y] - 0.5 * self.field[Y]])
                    return
                self.actions(self.receiver, 'follow', [
                             self.prev_posture[self.receiver][X], self.prev_posture[self.receiver][Y] + 0.5 * self.field[Y]])
                return

        r_point = self.cur_ball
        # if sender exists
        if not self.sender == None:
            r_point = self.receive_position()
        receiver_ball_dist = helper.dist(
            self.cur_ball[X], self.cur_posture[self.receiver][X], self.cur_ball[Y], self.cur_posture[self.receiver][Y])
        # if ball is close to receiver
        if receiver_ball_dist > 0.3 * self.field[X] / 2:
            self.actions(self.receiver, 'follow', [
                         r_point[X], r_point[Y]], refine=True)
            return

        r2bisFace, _ = self.is_facing_target(
            self.receiver, self.cur_ball[X], self.cur_ball[Y], 4)
        if not r2bisFace:
            self.actions(self.receiver, 'follow', refine=True)
            return
        # if receiver is moving to our goal area
        if self.cur_posture[self.receiver][X] < - 0.8 * self.field[X] / 2:
            if self.cur_posture[self.receiver][X] - self.prev_posture[self.receiver][X] < 0:
                self.actions(self.receiver, 'backward')

        self.actions(self.receiver, 'dribble')
        return
Exemple #3
0
    def receive_position(self):
        step = 5
        ball_receiver_dist = helper.dist(self.cur_ball[X],
                                         self.cur_posture[self.receiver][X],
                                         self.cur_ball[Y],
                                         self.cur_posture[self.receiver][Y])
        prev_ball_receiver_dist = helper.dist(
            self.prev_ball[X], self.prev_posture[self.receiver][X],
            self.prev_ball[Y], self.prev_posture[self.receiver][Y])

        diff_dist = prev_ball_receiver_dist - ball_receiver_dist
        if diff_dist > 0:
            step = ball_receiver_dist  # diff_dist

        step = min(step, 15)

        predict_pass_point = self.predict_ball_location(step)

        ball_goal_dist = helper.dist(self.cur_ball[X], self.field[X] / 2,
                                     self.cur_ball[Y], 0)
        prev_ball_goal_dist = helper.dist(self.prev_ball[X], self.field[X] / 2,
                                          self.prev_ball[Y], 0)
        if ball_goal_dist > prev_ball_goal_dist:
            predict_pass_point[X] = predict_pass_point[X] - 0.15

        return predict_pass_point
Exemple #4
0
 def set_pos_parameters(self, id, target_pts, params, mult=1.2):
     prev_dist = helper.dist(
         self.prev_posture[id][X], target_pts[X], self.prev_posture[id][Y], target_pts[Y])
     cur_dist = helper.dist(
         self.cur_posture[id][X], target_pts[X], self.cur_posture[id][Y], target_pts[Y])
     if cur_dist > prev_dist - 0.02:
         params = [params[0] * mult, params[1]
                   * mult, params[2] * mult, params[3]]
     return params
Exemple #5
0
    def send_ball(self):
        if self.sender == None:
            return

        goal_dist = helper.dist(4.0, self.cur_posture[self.sender][X], 0,
                                self.cur_posture[self.sender][Y])
        # if the sender has a shoot chance, it tries to shoot
        if self.shoot_chance(self.sender):
            if goal_dist > 0.3 * self.field[X] / 2:
                self.actions(self.sender, 'dribble', refine=True)
                return
            else:
                self.actions(self.sender, 'kick')
                return

        # if the receiver exists, get the distance between the sender and the receiver
        sender_receiver_dist = None
        if not self.receiver == None:
            sender_receiver_dist = helper.dist(
                self.cur_posture[self.sender][X],
                self.cur_posture[self.receiver][X],
                self.cur_posture[self.sender][Y],
                self.cur_posture[self.receiver][Y])

        # if the sender is close to the receiver, the sender kicks the ball
        if not sender_receiver_dist == None:
            if sender_receiver_dist < 0.3 and not self.cur_posture[
                    self.receiver][TOUCH]:
                self.actions(self.sender, 'kick')
                return

        ift, theta_diff = self.is_facing_target(self.sender, self.cur_ball[X],
                                                self.cur_ball[Y])
        if not ift:
            # after the sender kicks, it stops
            if theta_diff > math.pi * 3 / 4:
                self.actions(self.sender, None)
                return
            else:
                self.actions(self.sender, 'follow', refine=True)
                return

        # if the ball is in front of the sender and sender is moving backward
        if self.cur_posture[self.sender][X] < -0.8 * self.field[X] / 2:
            if self.cur_posture[self.sender][X] - self.prev_posture[
                    self.sender][X] < 0:
                self.actions(self.sender, 'backward')

        self.actions(self.sender, 'dribble', refine=True)
        return
Exemple #6
0
 def set_sender_condition(self):
     for i in range(1, 5):
         # if this robot is near the ball, it will be a sender candidate
         dist = helper.dist(self.cur_posture[i][X], self.cur_ball[X],
                            self.cur_posture[i][Y], self.cur_ball[Y])
         if dist < 0.5 and self.cur_posture[i][ACTIVE]: return True
     return False
    def denoise(self):
        """
        return a new vehicle with outlier removed
        """
        #        if len(self.vlist) < 3:
        #            return self
        #        new_vlist = [self.vlist[0]]
        #        for i in xrange(1, len(self.vlist) - 1):
        #            if dist(self.get_location(i), self.get_location(i-1)) > outlier_dist_thres \
        #            and dist(self.get_location(i), self.get_location(i+1)) > outlier_dist_thres:
        #                print ("remove outlier in %s" % self.get_no()), self.get_location(i-1), \
        #                self.get_location(i), self.get_location(i+1)
        #                continue
        #            new_vlist.append(self.vlist[i])
        #        new_vlist.append(self.vlist[-1])

        # Assume self.vlist[0] is not outlier and time is continuous.
        if not self.vlist:
            return self
        new_vlist = [self.vlist[0]]
        for i in xrange(1, len(self.vlist)):
            if dist(self.get_location(i),
                    (new_vlist[-1][2], new_vlist[-1][1])) < outlier_dist_thres:
                new_vlist.append(self.vlist[i])
        return Vehicle(new_vlist, self.get_no())
Exemple #8
0
 def length(self):
     """
     total distance between adjacent sites,
     use lines to link adjacent sites.
     """
     if self.total_len == None:
         self.total_len = sum(dist(self.get_location(i), self.get_location(i - 1))
                              for i in xrange(1, len(self)))
     return self.total_len
Exemple #9
0
 def length(self):
     """
     total distance between adjacent sites,
     use lines to link adjacent sites.
     """
     if self.total_len == None:
         self.total_len = sum(
             dist(self.get_location(i), self.get_location(i - 1))
             for i in xrange(1, len(self)))
     return self.total_len
Exemple #10
0
    def find_closest_robot(self):
        # find the closest defender
        min_idx = 0
        min_dist = 9999.99
        def_dist = 9999.99

        all_dist = []

        for i in [1, 2]:
            measured_dist = helper.dist(self.cur_ball[X],
                                        self.cur_posture[i][X],
                                        self.cur_ball[Y],
                                        self.cur_posture[i][Y])
            all_dist.append(measured_dist)
            if (measured_dist < min_dist):
                min_dist = measured_dist
                def_dist = min_dist
                min_idx = i

        self.def_idx = min_idx

        # find the closest forward
        min_idx = 0
        min_dist = 9999.99
        atk_dist = 9999.99

        for i in [3, 4]:
            measured_dist = helper.dist(self.cur_ball[X],
                                        self.cur_posture[i][X],
                                        self.cur_ball[Y],
                                        self.cur_posture[i][Y])
            all_dist.append(measured_dist)
            if (measured_dist < min_dist):
                min_dist = measured_dist
                atk_dist = min_dist
                min_idx = i

        self.atk_idx = min_idx

        # record the robot closer to the ball between the two too
        self.closest_order = np.argsort(all_dist) + 1
    def set_sender(self, _player_list):
        distance_list = []
        for sender in _player_list :
            predict_ball = self.predict_ball_location(3)
            ball_distance = helper.dist(predict_ball[X], self.cur_posture[sender][X], predict_ball[Y], self.cur_posture[sender][Y])
            distance_list.append(ball_distance)

        # if the distance between ball and sender is less than 1, choose the closest robot as the sender
        if min(distance_list) < 1.0 :
            return distance_list.index(min(distance_list)) + 1

        # otherwise, there is no sender
        return None
def subset_match_with_dist(vehicle, start, end, route, grids):
    """
    return whether vehicle[start..end] match route.
    unlike subset_match, subset_match_with_dist 
    considers match_dist, is slower but more accurate.
    """
    matched_sites_no = set()
    route_no = route.get_no()
    for i in xrange(start, end + 1):
        loc = vehicle.get_location(i)
        id_x, id_y = grid_index(loc)
        if grids[id_x][id_y].has_key(route_no):
            sites_no_set = set(
                site_no for site_no in grids[id_x][id_y][route_no]
                if dist(route.get_location(site_no), loc) < match_dist)
            matched_sites_no |= sites_no_set
    return True if len(matched_sites_no) / len(
        route) > match_thres_one else False
def split_vehicle_by_route(vehicle, route):
    """
    split vehicle according to route
    """
    split_ids = []
    last_dist = float('inf')
    last_id = -split_pts_int - 100
    for i in xrange(len(vehicle)):
        # choose best split points for current route
        # from continuous split points
        cur_dist = dist(vehicle.get_location(i), route.get_dest_loc())
        if cur_dist < match_dist:
            if i - last_id >= split_pts_int:
                split_ids.append([i, route.get_no()])
                last_dist = cur_dist
            elif cur_dist <= last_dist:
                split_ids[-1][0] = i
                last_dist = cur_dist
            last_id = i
    return split_ids
Exemple #14
0
def vehicle_site_time(vehicle, start, end, route, grids):
    """
    include start, end
    return [(site_no, time), (site_no, time)]
    """
    times = [None for _ in xrange(route.site_count())]
    dists = [100000 for _ in xrange(route.site_count())]
    for i in xrange(start, end + 1):
        loc = vehicle.get_location(i)
        idx, idy = grid_index(loc)
        sites = grids[idx][idy].get(route.get_no(), set())
        for site in sites:
            d = dist(route.site_location(site), vehicle.get_location(i))
            if d <= dists[site]:
                dists[site] = d
                times[site] = vehicle.get_GpsTime(i)
    ret_times = []
    for site, time in enumerate(times):
        if time != None:
            ret_times.append((site, time))
    return ret_times
Exemple #15
0
def vehicle_site_time(vehicle, start, end, route, grids):
    """
    include start, end
    return [(site_no, time), (site_no, time)]
    """
    times = [None for _ in xrange(route.site_count())]
    dists = [100000 for _ in xrange(route.site_count())]
    for i in xrange(start, end + 1):
        loc = vehicle.get_location(i)
        idx, idy = grid_index(loc)
        sites = grids[idx][idy].get(route.get_no(), set())
        for site in sites:
            d = dist(route.site_location(site), vehicle.get_location(i))
            if d <= dists[site]:
                dists[site] = d
                times[site] = vehicle.get_GpsTime(i)
    ret_times = []
    for site, time in enumerate(times):
        if time != None:
            ret_times.append((site, time))
    return ret_times
            
            
Exemple #16
0
    def denoise(self):
        """
        return a new vehicle with outlier removed
        """
#        if len(self.vlist) < 3:
#            return self
#        new_vlist = [self.vlist[0]]
#        for i in xrange(1, len(self.vlist) - 1):
#            if dist(self.get_location(i), self.get_location(i-1)) > outlier_dist_thres \
#            and dist(self.get_location(i), self.get_location(i+1)) > outlier_dist_thres:
#                print ("remove outlier in %s" % self.get_no()), self.get_location(i-1), \
#                self.get_location(i), self.get_location(i+1)
#                continue
#            new_vlist.append(self.vlist[i])
#        new_vlist.append(self.vlist[-1])
        
        # Assume self.vlist[0] is not outlier and time is continuous.
        if not self.vlist:
            return self
        new_vlist = [self.vlist[0]]
        for i in xrange(1, len(self.vlist)):
            if dist(self.get_location(i), (new_vlist[-1][2], new_vlist[-1][1])) < outlier_dist_thres:
                new_vlist.append(self.vlist[i])
        return Vehicle(new_vlist, self.get_no())
Exemple #17
0
	def dock_collide(self, object):
		"""Is the given object in range of self's dock?"""
		return helper.dist(self.pos,object.pos) < self.dock_radius + object.radius
Exemple #18
0
def match_card_from_vehicles(card, mapper, connector, routes, grids, vehicles,
                             cvmapper):
    """
    find the best match for the card
    """
    route_nos = mapper.get_lineid_from_statid(card.get_no()[1])

    cand_vehicles_nos = set()
    for route_no in route_nos:
        cand_vehicles_nos |= connector.get_cands(route_no)

    cand_vehicles = [
        vehicles.get_vehicle(vehicle_no) for vehicle_no in cand_vehicles_nos
    ]

    card = card.get_simplified_card(between_card_time)
    #plt.close('all')
    match_results = []
    for vehicle in cand_vehicles:
        match_num_1 = 0
        best_offset = 0
        for offset in offsets:
            match_num = 0
            time_sequences = map(lambda x: x + offset,
                                 card.get_time_sequence())
            locations = vehicle.get_locations_at_timestamps(time_sequences)
            #            x, y = zip(*filter(None, locations))
            #            plt.figure()
            #            vehicle.plot(0, len(vehicle))
            #            plt.plot(x, y, 'gs')
            #            for route_no in list(route_nos)[:1]:
            #                route = routes.get_route(route_no)
            #                route.plot(0, len(route), 'ro')
            #
            #            plt.show()

            last_location = (0, 0)
            for location in locations:
                if location == None:
                    continue
                gid = grid_index(location)
                for route_no in route_nos:
                    tmp_set = grids[gid[0]][gid[1]].get(route_no, set())
                    if not tmp_set or dist(location,
                                           last_location) <= dist_card_time:
                        continue
                    route = routes.get_route(route_no)
                    min_dist = min(
                        dist(location, route.get_location(i)) for i in tmp_set)
                    if min_dist < dist_stop:
                        match_num += 1
                        break
                last_location = location
            if match_num > match_num_1:
                match_num_1 = match_num
                best_offset = offset

        print vehicle.get_no(), match_num_1, '/', len(card), best_offset
        match_results.append((match_num_1, vehicle, best_offset))

    match_results.sort(key=lambda x: x[0], reverse=True)
    if not match_results:
        cvmapper.add_map(card.get_no(), None)
        return ("No Match", )

    cvmapper.add_map(card.get_no(),
                     (match_results[0][1].get_no(), match_results[0][2]))
    if (float(match_results[0][0]) / len(card)) >= min_ratio:
        if (len(match_results) == 1 or match_results[0][0] >
            (1 + thres_ratio) * match_results[1][0]):
            return "Accurate Match", match_results[0][0], '/', len(
                card), match_results[0][1].get_no(), match_results[0][2]
        else:
            return "Inaccurate Match", match_results[0][0], '/', len(
                card), match_results[0][1].get_no(), match_results[0][2]
    else:
        return "Low Match", match_results[0][0], '/', len(
            card), match_results[0][1].get_no(), match_results[0][2]
Exemple #19
0
	def in_jump_range(self, pilot):
		return helper.dist(self.pos,pilot.vessel.pos) <= 10 + pilot.vessel.radius
		
		
		
		
Exemple #20
0
    'W_conv2': W_conv2,
    'b_conv2': b_conv2
}
init_new_vars_op = tf.initialize_variables(
    [W_conv1, b_conv1, W_conv2, b_conv2])

x1 = tf.placeholder(tf.float32, [None, 784], name='x1')
z1 = tf.placeholder(tf.float32, [None, z_dim], name='z1')

gz1 = helper.Generator(z1)

x1_real_conv = helper.random_discriminator(x1, rr, D1)

gz1_real_conv = helper.random_discriminator(gz1, rr, D1)

loss_1 = helper.dist(x1_real_conv, gz1_real_conv, b_size)
loss_2 = helper.dist(gz1_real_conv, x1_real_conv, b_size)
g_loss = tf.cond(tf.less(loss_1, loss_2), lambda: loss_2, lambda: loss_1)

gen_params = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES, "Generator")

step = tf.Variable(0, trainable=False)
rate = tf.train.exponential_decay(step_size, step, 3000, 0.96)

optimizer = tf.train.AdamOptimizer(learning_rate=rate,
                                   beta1=0.5,
                                   beta2=0.9,
                                   name='Adam1')
grads_and_vars = optimizer.compute_gradients(g_loss, var_list=gen_params)
vars = [x[1] for x in grads_and_vars]
gradients = [x[0] for x in grads_and_vars]
Exemple #21
0
    player_input = pygame.key.get_pressed()
    if player_input[pygame.K_UP] or player_input[pygame.K_w]:
        player_y -= player_speed
    if player_input[pygame.K_RIGHT] or player_input[pygame.K_d]:
        player_x += player_speed
    if player_input[pygame.K_DOWN] or player_input[pygame.K_s]:
        player_y += player_speed
    if player_input[pygame.K_LEFT] or player_input[pygame.K_a]:
        player_x -= player_speed

    ## game logic ##
    # place aim reticule
    mouse_x, mouse_y = pygame.mouse.get_pos()
    aim_x = mouse_x - player_x
    aim_y = mouse_y - player_y
    mouse_dist = dist(0, 0, aim_x, aim_y)
    aim_x = int(aim_x / mouse_dist * 30)
    aim_y = int(aim_y / mouse_dist * 30)

    # add a new bullet to the list
    if fire == True:
        bullets.append([player_x, player_y, aim_x // 2, aim_y // 2])
        fire = False

    # move the bullets, and delete them if they leave the screen
    alive_bullets = []
    for bullet in bullets:
        bullet[0] += bullet[2]
        bullet[1] += bullet[3]
        if -50 <= bullet[0] <= WIDE + 50 and -50 <= bullet[1] <= HIGH + 50:
            alive_bullets.append(bullet)
Exemple #22
0
 def in_position(self, id, xTarget, yTarget, tol):
     return (helper.dist(self.xDesired, self.cur_posture[id][X],
                         self.yDesired, self.cur_posture[id][Y]) < tol)
Exemple #23
0
    def move_to_circles(self, id, x, y, radius=0.5, speed=1.0, goal_speed=0.0, goal_angle=None, max_velocity = False, kick=True, debug = False):
        if max_velocity:
            speed = self.max_linear_velocity[id]
        
        max_turn_d_th = 480*DEGTORAD
        min_turn_d_th = 90*DEGTORAD

        d_t = self.received_frame.time - self.previous_frame.time
        if d_t == 0.0:
            d_t = 0.2
        
        my_x = self.cur_posture[id][X]
        my_y = self.cur_posture[id][Y]

        last_x = self.prev_posture[id][X]
        last_y = self.prev_posture[id][Y]

        dist = helper.dist(my_x, x, my_y, y)
        last_dist = helper.dist(last_x, x, last_y, y)
        d_dist = (dist - last_dist)/d_t

        angle_to_target = self.direction_angle(id, x, y)
        my_angle = self.cur_posture[id][TH]
        last_angle = self.prev_posture[id][TH]

        diff_theta = helper.clamp(my_angle - angle_to_target) # right-facing angle to turn to target
        d_theta = (helper.clamp(my_angle - last_angle)) / d_t

        direction = 1.0
        if abs(diff_theta) > math.pi/2:  # drive backwards if easier
            direction = -1.0
            diff_theta = helper.clamp(diff_theta-math.pi)

        if goal_angle != None:
            assert type(goal_angle) == type(1.0) or type(goal_angle) == type(1)
            # move to the target position at a given orientation (Two turn-circles)

            dir_x = math.cos(goal_angle)
            dir_y = math.sin(goal_angle)

            bot_dir_x = math.cos(my_angle)
            bot_dir_y = math.sin(my_angle)

            max_corridor_dist = self.axle_length[id]/2 + math.sin(3*DEGTORAD)*dist # Cone shaped approach corridor

            tar_to_bot_x = my_x - x
            tar_to_bot_y = my_y - y

            intersect_dist_tar = (bot_dir_x*tar_to_bot_x + bot_dir_y*tar_to_bot_y) / (bot_dir_x*dir_x + bot_dir_y*dir_y)
            intersect_dist_bot = -(dir_x*tar_to_bot_x + dir_y*tar_to_bot_y) / (bot_dir_x*dir_x + bot_dir_y*dir_y)

            rel_angle = goal_angle - my_angle
            turn_radius = intersect_dist_tar * math.tan(rel_angle/2.0)

            if debug:
                print(f"int_d_ball: {intersect_dist_tar}")
                print(f"int_d_bot : {intersect_dist_bot}")
                print(f"rel_angle : {rel_angle}")
                print(f"turn_rad  : {turn_radius}")

            corridor_dist = dir_x*tar_to_bot_y - dir_y*tar_to_bot_x
            correct_direction = (dir_x*tar_to_bot_x + dir_y*tar_to_bot_y) >= 0.0

            if abs(corridor_dist) > max_corridor_dist or not correct_direction:
                # If outside the corridor
                x_int_goal = x + dir_x*radius
                y_int_goal = y + dir_y*radius

                int_dist = helper.dist(my_x, x_int_goal, my_y, y_int_goal)

                if int_dist > dist:
                    x_temp_goal = tar_to_bot_y * corridor_dist
                    y_temp_goal = -tar_to_bot_x * corridor_dist

                    l_temp = math.sqrt(x_temp_goal ** 2 + y_temp_goal ** 2)
                    x_temp_goal /= l_temp
                    y_temp_goal /= l_temp

                    x_temp_goal = x + x_temp_goal
                    y_temp_goal = y + y_temp_goal

                    self.move_to_circles(id, x_temp_goal, y_temp_goal, radius, speed, goal_speed=speed*0.99, max_velocity=max_velocity, debug=debug)
                    return
                else:
                    self.move_to_circles(id, x_int_goal, y_int_goal, radius, speed, goal_speed=speed*0.99, max_velocity=max_velocity, debug=debug)
                    return

                
        if ((abs(diff_theta)*RADTODEG < 2 and dist < radius) or (self.player_state[id] == 'kick')) and d_dist < 0.0 and goal_speed > speed:
            # If inside the corridor and close to the target
            # Kick
            #print("kicking")

            self.player_state[id] = 'kick'
            targ_speed = dist/radius * speed + (1-dist/radius) * goal_speed

            self.set_wheel_velocity(id, targ_speed*direction, targ_speed*direction)
            return
        else:
            self.player_state[id] = None

        # just move to the target position. Orientation is not important (Only one turn-circle)

        f_dist = Dist(dist)

        # Speed
        speed_far =    speed
        speed_small =  (speed + goal_speed)/2
        speed_zero =   goal_speed

        # Defuzzy Speed
        speed = f_dist['zero'] * speed_zero + f_dist['small'] * speed_small + (f_dist['med'] + f_dist['big'] + f_dist['large']) * speed_far

        # Fuzzy diff_theta
        # The fuzzy domain Ang is in ° for convenience

        f_d_theta = Ang(diff_theta*RADTODEG)

        # Use p-d control for the angular velocity
        if id == 0:
            p = 1
            d = 0.12
        elif id == 1 or id == 2:
            p = 1
            d = 0.12
        else:
            p = 1
            d = 0.12

        p_d = p*diff_theta + d*d_theta

        # In the turn-circle (meaning while diff_theta is not small)
        max_circ_radius = dist/4.0*math.cos(diff_theta)
        circ_rad = min(max_circ_radius, radius)
        #circ_rad = max_circ_radius
        if p_d < 0.0:
            circ_rad *= -1.0

        circ_l, circ_r = self.speed_turn_circle(id, circ_rad*direction, speed*direction)

        # On the straight (when diff_theta is small)
        straight_rad = 1/(3*math.sin(p_d))
        if p_d < 0.0:
            straight_rad *= -1.0

        stra_l, stra_r = self.speed_turn_circle(id, straight_rad*direction, speed*direction)

        # When angle is zero
        zero_l, zero_r = speed * direction, speed * direction

        # Defuzzy straight and circle radius
        used_l  = f_d_theta['zero'] * zero_l + f_d_theta['small'] * stra_l + (f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large']) * circ_l
        used_r  = f_d_theta['zero'] * zero_r + f_d_theta['small'] * stra_r + (f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large']) * circ_r

        used_l, used_r = self.limit_speed_to_ang_vel(id, used_l, used_r, min(max_turn_d_th, max(abs(8*diff_theta + 0.05*d_theta), min_turn_d_th)))

        used_d_th = (used_l - used_r) / self.axle_length[id]

        if debug:
            print("__________________________________________")
            print(f"id:{id}")
            print(f"diff_theta:{diff_theta*RADTODEG}")
            print(f"d_theta:{d_theta*RADTODEG}")
            print(f"th_zero:{f_d_theta['zero']}, th_small:{f_d_theta['small']}, th_big:{(f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large'])}")
            print(f"dist:{dist}, d_dist:{d_dist}")
            print(f"d_zero:{f_dist['zero']}, d_small:{f_dist['small']}, d_big:{(f_dist['med'] + f_dist['big'] + f_dist['large'])}")
            print(f"speed:{speed}")
            print(f"used_l:{used_l}, used_r:{used_r}")
            print(f"used_speed:{(used_l + used_r)/2.0}")
            print(f"used_d_th:{used_d_th*RADTODEG}")
            print(f"direction:{direction}")

        self.set_wheel_velocity(id, used_l, used_r)
def match_card_from_vehicles(card, mapper, connector, routes, grids, vehicles, cvmapper):
    """
    find the best match for the card
    """
    route_nos = mapper.get_lineid_from_statid(card.get_no()[1])
    
    cand_vehicles_nos = set()
    for route_no in route_nos:    
        cand_vehicles_nos |= connector.get_cands(route_no)

    cand_vehicles = [vehicles.get_vehicle(vehicle_no)
                     for vehicle_no in cand_vehicles_nos]
                
    card = card.get_simplified_card(between_card_time)
    #plt.close('all')
    match_results = []
    for vehicle in cand_vehicles:
        match_num_1 = 0
        best_offset = 0
        for offset in offsets:
            match_num = 0
            time_sequences = map(lambda x: x + offset, card.get_time_sequence())
            locations = vehicle.get_locations_at_timestamps(time_sequences)
#            x, y = zip(*filter(None, locations))
#            plt.figure()
#            vehicle.plot(0, len(vehicle))
#            plt.plot(x, y, 'gs')
#            for route_no in list(route_nos)[:1]:
#                route = routes.get_route(route_no)
#                route.plot(0, len(route), 'ro')
#                
#            plt.show()

            last_location = (0, 0)
            for location in locations:
                if location == None:
                    continue
                gid = grid_index(location)
                for route_no in route_nos:
                    tmp_set = grids[gid[0]][gid[1]].get(route_no, set())
                    if not tmp_set or dist(location, last_location) <= dist_card_time:
                        continue
                    route = routes.get_route(route_no)
                    min_dist = min(dist(location, route.get_location(i)) for i in tmp_set)
                    if min_dist < dist_stop:
                        match_num += 1
                        break
                last_location = location
            if match_num > match_num_1:
                match_num_1 = match_num
                best_offset = offset
            
        print vehicle.get_no(), match_num_1, '/', len(card), best_offset
        match_results.append((match_num_1, vehicle, best_offset))
    
    match_results.sort(key = lambda x: x[0], reverse = True)
    if not match_results:
        cvmapper.add_map(card.get_no(), None)
        return ("No Match",)
    
    cvmapper.add_map(card.get_no(), (match_results[0][1].get_no(), match_results[0][2]))
    if (float(match_results[0][0]) / len(card)) >= min_ratio:
        if (len(match_results) == 1 or match_results[0][0] > (1+thres_ratio) * match_results[1][0]):
            return "Accurate Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
        else:
            return "Inaccurate Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
    else:
        return "Low Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
Exemple #25
0
	def dock_range(self, vessel):
		"""Are this and that within each other's dock range?"""
		return helper.dist(self.pos,vessel.pos) < self.dock_radius + vessel.dock_radius
Exemple #26
0
	def collide(self, object):
		"""did self collide with the given space obj?"""
		return helper.dist(self.pos, object.pos) < self.radius + object.radius
Exemple #27
0
 def pass_received(self, idReceiver):
     return (helper.dist(self.cur_posture[idReceiver][X], self.cur_ball[X],
                         self.cur_posture[idReceiver][Y], self.cur_ball[Y])
             < 0.2)