示例#1
0
def optimal_value_strategy(s, action_list):

    actions_consequences = []
    rotations = []
    for action in action_list:

        if action.name is "none":
            rotation = 0
        else:
            # optimize action
            rotation, _ = heinrich_test(s, action, False, iterations=20)

        rotations += [rotation]

        # apply optimized rotation
        s.pose.rotate(rotation)

        actions_consequences.append(
            Sim.simulateAction(action, s, num_particles=30))

        # restore previous rotation
        s.pose.rotate(-rotation)

    # Decide best action
    selected_action_idx = Sim.decide_minimal(actions_consequences, s)

    return selected_action_idx, rotations[selected_action_idx]
def main():
    state = State()
    """
    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 780, 150, 8.454482265522328, 6.992268841997358)
    sidekick_left = a.Action("sidekick_left", 750, 150, 86.170795364136380, 10.669170653645670)
    sidekick_right = a.Action("sidekick_right", 750, 150, -89.657943335302260, 10.553726275058064)
    """

    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    while True:
        actions_consequences = []
        # Simulate Consequences
        for action in action_list:
            single_consequence = a.ActionResults([])
            actions_consequences.append(
                Sim.simulate_consequences(action, single_consequence, state,
                                          30))

        # actions_consequences is now a list of ActionResults

        # Decide best action
        best_action = Sim.decide_smart(actions_consequences, state)

        draw_actions(actions_consequences, state,
                     action_list[best_action].name)
示例#3
0
def main():
    state = State(1000, 0)

    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 1080, 150, 0, 7)
    sidekick_left = a.Action("sidekick_left", 750, 150, 90, 10)
    sidekick_right = a.Action("sidekick_right", 750, 150, -90, 10)

    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    while plt.get_fignums():
        actions_consequences = []
        # Simulate Consequences
        for action in action_list:
            single_consequence = a.ActionResults([])
            actions_consequences.append(
                Sim.simulate_consequences(action, single_consequence, state,
                                          40))

        # actions_consequences is now a list of ActionResults

        # Decide best action
        best_action = Sim.decide_smart(actions_consequences, state)

        draw_actions(actions_consequences, state,
                     action_list[best_action].name)
示例#4
0
def direct_kick_strategy_cool(s, action_list, take_best=False):

    actions_consequences = []
    rotations = []

    fastest_action_dir = None
    fastest_action_idx = 0

    for idx, action in enumerate(action_list):

        if action.name is "none":
            rotation = 0
        else:
            # optimize action
            a0 = minimal_rotation(s, action, 1)
            a1 = minimal_rotation(s, action, -1)
            if np.abs(a0) < np.abs(a1):
                rotation = a0
            else:
                rotation = a1

            if np.abs(rotation) > 3:
                print(
                    "WARNING: in direct_kick_strategy_cool no kick found after rotation {0}."
                    .format(rotation))

        rotations += [rotation]

        # apply optimized rotation
        s.pose.rotate(rotation)

        actions_consequences.append(
            Sim.simulateAction(action, s, num_particles=30))

        # restore previous rotation
        s.pose.rotate(-rotation)

        if action.name is not "none":
            if fastest_action_dir is None or np.abs(rotation) < np.abs(
                    fastest_action_dir):
                fastest_action_dir = rotation
                fastest_action_idx = idx

    # Decide best action
    if take_best:
        selected_action_idx = Sim.decide_minimal(actions_consequences, s)
        return selected_action_idx, rotations[selected_action_idx]
    else:
        # print fastest_action_idx, selected_action_idx
        return fastest_action_idx, fastest_action_dir
示例#5
0
def main(num_samples, num_reps, x_step, y_step, rotation_step):
    state = State()

    file_idx = 0

    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 1080, 150, 0, 7)
    sidekick_left = a.Action("sidekick_left", 750, 150, 90, 10)
    sidekick_right = a.Action("sidekick_right", 750, 150, -90, 10)

    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    whole_decisions = []

    # use this to iterate over the whole green
    # field_x_range = range(int(-field.x_field_length*0.5), int(field.x_field_length*0.5) + x_step, x_step)
    # field_y_range = range(int(-field.y_field_length*0.5), int(field.y_field_length*0.5) + y_step, y_step)

    # use this to just iterate over the playing field
    x_range = range(int(-field.x_length * 0.5), int(field.x_length * 0.5) + x_step, x_step)
    y_range = range(int(-field.y_length * 0.5), int(field.y_length * 0.5) + x_step, y_step)

    for rot in range(0, 360, rotation_step):
        print("Rotation: " + str(rot))
        for x in x_range:
            for y in y_range:
                state.update_pos(m2d.Vector2(x, y), rotation=rot)
                # Do this multiple times and write the decisions as a histogram
                decision_histogramm = [0, 0, 0, 0]  # ordinal scale -> define own metric in evaluation script
                for num_simulations in range(0, num_reps):
                    actions_consequences = []
                    # Simulate Consequences
                    for action in action_list:
                        single_consequence = a.ActionResults([])
                        actions_consequences.append(Sim.simulate_consequences(action, single_consequence, state, num_samples))

                    # Decide best action
                    best_action = Sim.decide_smart(actions_consequences, state)
                    decision_histogramm[best_action] += 1

                # write the position and decision in on list
                whole_decisions.append([x, y, rot, decision_histogramm])

    # make sure not to overwrite anything
    while os.path.exists('{}{:d}.pickle'.format('data/simulate_every_pos-' + str(num_samples) + '-' + str(num_reps) + '-v', file_idx)):
        file_idx += 1

    pickle.dump(whole_decisions, open(
                'data/simulate_every_pos-' + str(num_samples) + '-' + str(num_reps) + '-v' + str(file_idx) + '.pickle', "wb"))
示例#6
0
def update(samples, likelihoods, s, action, m_min, m_max):
    # likelihoods = np.zeros(samples.shape)
    test_action = copy.deepcopy(action)
    simulation_consequences = []

    simulation_num_particles = 1
    for idx in range(0, len(samples)):
        # modify the action with the sample
        test_action.angle = action.angle + samples[idx]

        test_action_consequence = a.ActionResults([])
        simulation_consequences.append(
            Sim.simulate_consequences(test_action, test_action_consequence, s, simulation_num_particles))

        if test_action_consequence.category("INFIELD") > 0:
            potential = -pf.evaluate_action2(test_action_consequence, s)
            likelihoods[idx] = potential
            m_min = min(m_min, potential)
            m_max = max(m_max, potential)
        elif test_action_consequence.category("OPPGOAL") > 0:
            likelihoods[idx] = m_max
        else:
            likelihoods[idx] = m_min

    return likelihoods, simulation_consequences, m_min, m_max
示例#7
0
    def step(self):

        # reset stuff
        self.turn_around_ball = 0
        self.rotate = 0
        self.walk_dist = 0

        # ---------------
        # approach ball
        # ---------------
        self.rotate = self.state.ball_position.angle()
        self.walk_dist = self.state.ball_position.abs()

        # HACK: execute motion request
        self.state.pose.rotate(self.rotate)
        self.state.pose.translate(self.walk_dist, 0)
        self.state.ball_position = m2d.Vector2(100.0, 0.0)

        # -----------------
        #  make a decision
        # -----------------

        self.selected_action_idx, self.turn_around_ball = self.strategy(
            self.state, self.action_list)
        selected_action = self.action_list[self.selected_action_idx]

        # --------------------
        #  execute the action
        # --------------------
        if selected_action.name == "none":
            # print("INFO: NONE action while ball outside of the field.")
            if self.state_category == a.Category.INFIELD:
                print("WARNING: action is NONE, what should we do here? " +
                      str(self.selected_action_idx))
                print("STATE: robot = {0}".format(self.state.pose))
        else:
            # rotate around ball if necessary
            self.state.pose.rotate(self.turn_around_ball)

            # hack: perfect world without noise
            perfect_world = False
            if perfect_world:
                real_action = a.Action("real_action", selected_action.speed, 0,
                                       selected_action.angle, 0)
            else:
                real_action = selected_action

            # expected_ball_pos should be in local coordinates for rotation calculations
            action_results = Sim.simulateAction(real_action,
                                                self.state,
                                                num_particles=1)

            # store the state and update the state
            new_ball_position = action_results.positions()[0]
            self.state_category = new_ball_position.cat()
            self.state.ball_position = new_ball_position.pos()
示例#8
0
def direct_kick_strategy(s, action_list):

    turn_speed = math.radians(5.0)
    action_dir = 0
    turn_direction = 0

    while True:
        # turn towards the direction of the action
        s.pose.rotate(action_dir)

        # Simulate Consequences
        actions_consequences = [
            Sim.simulateAction(action, s, num_particles=30)
            for action in action_list
        ]

        # Decide best action
        selected_action_idx = Sim.decide_minimal(actions_consequences, s)

        # restore the previous orientation
        s.pose.rotate(-action_dir)

        if selected_action_idx != 0:
            break
        elif np.abs(action_dir) > math.pi:
            print(
                "WARNING: in direct_kick_strategy no kick found after rotation {0}."
                .format(action_dir))
            break
        else:
            # decide on rotation direction once
            if turn_direction == 0:
                attack_direction = attack_dir.get_attack_direction(s)
                turn_direction = np.sign(
                    attack_direction.angle())  # "> 0" => left, "< 0" => right

            # set motion request
            action_dir += turn_direction * turn_speed

    return selected_action_idx, action_dir
示例#9
0
def minimal_rotation(s, action, turn_direction):

    turn_speed = math.radians(5.0)
    action_dir = 0

    none = a.Action("none", 0, 0, 0, 0)
    none_actions_consequences = Sim.simulateAction(none, s, num_particles=30)

    while True:
        # turn towards the direction of the action
        s.pose.rotate(action_dir)

        # Simulate Consequences
        actions_consequences = Sim.simulateAction(action, s, num_particles=30)

        # Decide best action
        selected_action_idx = Sim.decide_minimal(
            [none_actions_consequences, actions_consequences], s)

        # restore the previous orientation
        s.pose.rotate(-action_dir)

        if selected_action_idx != 0:
            break
        elif np.abs(action_dir) > math.pi:
            # print("WARNING: in minimal_rotation no kick found after rotation {0}.".format(action_dir))
            break
        else:
            # decide on rotation direction once
            if turn_direction == 0:
                attack_direction = attack_dir.get_attack_direction(s)
                turn_direction = np.sign(
                    attack_direction.angle())  # "> 0" => left, "< 0" => right

            # set motion request
            action_dir += turn_direction * turn_speed

    return action_dir
示例#10
0
def update(samples, likelihoods, state, action, m_min, m_max):
    # likelihoods = np.zeros(samples.shape)
    test_action = copy.deepcopy(action)
    simulation_consequences = []

    simulation_num_particles = 1

    stats = np.zeros((len(samples), 3))
    number = 0.0

    for i in range(0, len(samples)):
        # modify the action with the sample
        test_action.angle = action.angle + samples[i]

        results = a.ActionResults([])
        simulation_consequences.append(
            Sim.simulate_consequences(test_action, results, state,
                                      simulation_num_particles))

        stats[i, 0] = results.likelihood(Category.OPPGOAL)
        stats[i, 1] = results.likelihood(Category.INFIELD)
        number = results.likelihood(Category.NUMBER)

        if stats[i, 0] + stats[i, 1] > 0:
            potential = -pf.evaluate_action(results, state)
            stats[i, 2] = potential

    max_goal = np.max(stats[:, 0])
    min_value = np.min(stats[:, 2])
    max_value = np.max(stats[:, 2])
    diff = max_value - min_value
    if diff == 0:
        diff = 1.0

    for i in range(0, len(samples)):
        if stats[i, 1] + stats[i, 0] >= max(0.0,
                                            Sim.good_threshold_percentage):
            likelihoods[i] = (stats[i, 1] + stats[i, 0]) * (
                (stats[i, 2] - min_value) / diff) + stats[i, 0]
        else:
            likelihoods[i] = 0

    return likelihoods, simulation_consequences, m_min, m_max
示例#11
0
def main(x, y, s, rotation_step, num_iter):
    enable_drawing = False

    start_x = x
    start_y = y
    s.update_pos(m2d.Vector2(start_x, start_y), rotation=0)

    # Only simulate kick_short -> robot_rotation == kick direction and we want to simulate the kick direction - how a kick in this direction is
    # executed doesnt matter.
    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 1080, 0, 0,
                          0)  # Maybe set std again to 150mm

    action_list = [no_action, kick_short]

    repetitions = num_iter
    best_times = []
    best_rotations = []

    for reps in range(repetitions):
        best_rotation = 0

        # Container for all times and corresponding rotations for one position
        times_single_position = np.array([])
        single_position_rot = np.array([])

        for rot in range(0, 360, rotation_step):
            # print("Start Rotation: " + str(rot))
            s.update_pos(m2d.Vector2(x, y), rotation=rot)
            num_kicks = 0
            num_turn_degrees = 0
            goal_scored = False
            choosen_rotation = 'none'  # This is used for deciding the rotation direction once per none decision
            total_time = 0

            while not goal_scored:

                actions_consequences = []
                # Simulate Consequences
                for action in action_list:
                    single_consequence = a.ActionResults([])
                    # a.num_particles can be 1 since there is no noise at all
                    actions_consequences.append(
                        Sim.simulate_consequences(action, single_consequence,
                                                  s, 1))

                # Decide best action
                best_action = Sim.decide_smart(actions_consequences, s)

                # expected_ball_pos should be in local coordinates for rotation calculations
                expected_ball_pos = actions_consequences[
                    best_action].expected_ball_pos_mean

                # Check if expected_ball_pos inside opponent goal
                opp_goal_back_right = m2d.Vector2(
                    field.opponent_goalpost_right.x + field.goal_depth,
                    field.opponent_goalpost_right.y)
                opp_goal_box = m2d.Rect2d(opp_goal_back_right,
                                          field.opponent_goalpost_left)

                goal_scored = opp_goal_box.inside(s.pose * expected_ball_pos)
                inside_field = field.field_rect.inside(s.pose *
                                                       expected_ball_pos)
                if goal_scored:
                    # print("Goal " + str(total_time) + " " + str(math.degrees(s.pose.rotation)))

                    # Apparently when using this weird things happen
                    # rotation = np.arctan2(expected_ball_pos.y, expected_ball_pos.x)
                    # rotation_time = np.abs(math.degrees(rotation) / s.rotation_vel)
                    # distance = np.hypot(expected_ball_pos.x, expected_ball_pos.y)
                    # distance_time = distance / s.walking_vel
                    # total_time += distance_time  #  + rotation_time
                    break

                elif not inside_field and not goal_scored:
                    # Asserts that expected_ball_pos is inside field or inside opp goal
                    # print("Error: This position doesn't manage a goal")
                    total_time = float('nan')
                    break

                elif not action_list[best_action].name == "none":
                    if enable_drawing is True:
                        draw_robot_walk(s, s.pose * expected_ball_pos,
                                        action_list[best_action].name)
                    # calculate the time needed
                    rotation = np.arctan2(expected_ball_pos.y,
                                          expected_ball_pos.x)
                    rotation_time = np.abs(
                        math.degrees(rotation) / s.rotation_vel)
                    distance = np.hypot(expected_ball_pos.x,
                                        expected_ball_pos.y)
                    distance_time = distance / s.walking_vel
                    total_time += distance_time + rotation_time

                    # reset the rotation direction
                    choosen_rotation = 'none'

                    # update the robots position
                    s.update_pos(s.pose * expected_ball_pos,
                                 math.degrees(s.pose.rotation + rotation))

                    num_kicks += 1

                elif action_list[best_action].name == "none":
                    if enable_drawing is True:
                        draw_robot_walk(s, s.pose * expected_ball_pos,
                                        action_list[best_action].name)
                    # Calculate rotation time
                    total_time += np.abs(5 / s.rotation_vel)

                    attack_direction = attack_dir.get_attack_direction(s)
                    attack_direction = math.degrees((attack_direction.angle()))

                    if (attack_direction > 0 and choosen_rotation is 'none'
                        ) or choosen_rotation is 'left':
                        s.update_pos(s.pose.translation,
                                     math.degrees(s.pose.rotation) +
                                     5)  # Should turn right
                        choosen_rotation = 'left'
                    elif (attack_direction <= 0 and choosen_rotation is 'none'
                          ) or choosen_rotation is 'right':
                        s.update_pos(s.pose.translation,
                                     math.degrees(s.pose.rotation) -
                                     5)  # Should turn left
                        choosen_rotation = 'right'
                    else:
                        print("Error at: " + str(s.pose.translation.x) +
                              " - " + str(s.pose.translation.y) + " - " +
                              str(math.degrees(s.pose.rotation)))
                        break

                    num_turn_degrees += 1

                else:
                    sys.exit("There should not be other actions")

            if not np.isnan(total_time):  # Maybe this already works
                times_single_position = np.append(times_single_position,
                                                  total_time)
                single_position_rot = np.append(single_position_rot, [rot])

        # end while not goal scored
        # TODO FIX nan issue!!!
        if len(times_single_position
               ) is 0:  # This can happen for positions on the field borders
            print("Every rotation would shoot out")
            best_times.append(float('nan'))
            best_rotations.append(best_rotation)
        else:
            min_val = times_single_position.min()
            min_idx = np.where(times_single_position == min_val)
            # best_rotation = np.mean(single_position_rot[min_idx])

            best_rotation = np.arctan2(
                np.sum(np.sin(np.deg2rad(single_position_rot[min_idx]))),
                np.sum(np.cos(np.deg2rad(single_position_rot[min_idx]))))
            best_rotation = np.rad2deg(best_rotation)
            best_times.append(np.min(times_single_position))
            best_rotations.append(best_rotation)
    # end all rotations
    print("Shortest Time: " + str(np.nanmean(best_times)) +
          " with global Rotation of robot: " + str(np.mean(best_rotations)) +
          " StartX " + str(start_x) + " Y: " + str(start_y))

    return np.mean(best_times), np.mean(best_rotations)
示例#12
0
def main(x, y, rot, s, num_iter):
    s.update_pos(m2d.Vector2(x, y), rotation=rot)

    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 1280, 0, 0, 0)
    sidekick_left = a.Action("sidekick_left", 750, 0, 90, 0)
    sidekick_right = a.Action("sidekick_right", 750, 0, -90, 0)

    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    # Do several decision cycles not just one to get rid of accidental decisions
    timings = []
    n_kicks = []
    n_turns = []
    for idx in range(num_iter):
        num_kicks = 0
        num_turn_degrees = 0
        goal_scored = False
        total_time = 0
        choosen_rotation = 'none'  # This is used for deciding the rotation direction once per none decision
        s.update_pos(m2d.Vector2(x, y), rotation=rot)
        while not goal_scored:
            actions_consequences = []
            # Simulate Consequences
            for action in action_list:
                single_consequence = a.ActionResults([])
                actions_consequences.append(
                    Sim.simulate_consequences(action, single_consequence, s,
                                              a.num_particles))

            # Decide best action
            best_action = Sim.decide_smart(actions_consequences, s)

            # expected_ball_pos should be in local coordinates for rotation calculations
            expected_ball_pos = actions_consequences[
                best_action].expected_ball_pos_mean

            # Check if expected_ball_pos inside opponent goal
            opp_goal_back_right = m2d.Vector2(
                field.opponent_goalpost_right.x + field.goal_depth,
                field.opponent_goalpost_right.y)
            opp_goal_box = m2d.Rect2d(opp_goal_back_right,
                                      field.opponent_goalpost_left)

            goal_scored = opp_goal_box.inside(s.pose * expected_ball_pos)
            inside_field = field.field_rect.inside(s.pose * expected_ball_pos)
            if goal_scored:
                num_kicks += 1
                # print("Goal " + str(total_time) + " " + str(math.degrees(s.pose.rotation)))
                break

            elif not inside_field and not goal_scored:
                # Asserts that expected_ball_pos is inside field or inside opp goal
                # print("Error: This position doesn't manage a goal")
                total_time = float('nan')
                break

            elif not action_list[best_action].name == "none":

                # draw_robot_walk(actions_consequences, state, state.pose * expected_ball_pos, action_list[best_action].name)

                # calculate the time needed
                rotation = np.arctan2(expected_ball_pos.y, expected_ball_pos.x)
                rotation_time = np.abs(m.degrees(rotation) / s.rotation_vel)
                distance = np.hypot(expected_ball_pos.x, expected_ball_pos.y)
                distance_time = distance / s.walking_vel
                total_time += distance_time + rotation_time

                # update total turns
                num_turn_degrees += np.abs(m.degrees(rotation))

                # reset the rotation direction
                choosen_rotation = 'none'

                # update the robots position
                s.update_pos(s.pose * expected_ball_pos,
                             m.degrees(s.pose.rotation + rotation))
                num_kicks += 1

            elif action_list[best_action].name == "none":
                # print(str(state.pose * expected_ball_pos) + " Decision: " + str(action_list[best_action].name))
                # draw_robot_walk(actions_consequences, state, state.pose * expected_ball_pos, action_list[best_action].name)
                turn_rotation_step = 5
                # Calculate rotation time
                total_time += np.abs(turn_rotation_step / s.rotation_vel)

                attack_direction = attack_dir.get_attack_direction(s)

                if (attack_direction > 0 and choosen_rotation
                    ) is 'none' or choosen_rotation is 'left':
                    s.update_pos(s.pose.translation,
                                 m.degrees(s.pose.rotation) +
                                 turn_rotation_step)  # Should turn right
                    choosen_rotation = 'left'
                elif (attack_direction <= 0 and choosen_rotation is 'none'
                      ) or choosen_rotation is 'right':
                    s.update_pos(s.pose.translation,
                                 m.degrees(s.pose.rotation) -
                                 turn_rotation_step)  # Should turn left
                    choosen_rotation = 'right'

                num_turn_degrees += turn_rotation_step
            else:
                sys.exit("There should not be other actions")
        # print("Total time to goal: " + str(total_time))
        # print("Num Kicks: " + str(num_kicks))
        # print("Num Turns: " + str(num_turn_degrees))

        timings.append(total_time)
        n_kicks.append(num_kicks)
        n_turns.append(num_turn_degrees)

    return np.nanmean(timings), np.nanmean(n_kicks), np.nanmean(n_turns)
def simulate_goal_cycle(variation=False):
    state = State()
    sim_data = [copy.deepcopy(state)]

    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 780, 150, 8.454482265522328, 6.992268841997358)
    sidekick_left = a.Action("sidekick_left", 750, 150, 86.170795364136380, 10.669170653645670)
    sidekick_right = a.Action("sidekick_right", 750, 150, -89.657943335302260, 10.553726275058064)

    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    # Todo: Maybe do several decision cycles not just one to get rid of accidental decisions
    num_kicks = 0
    num_turn_degrees = 0
    goal_scored = False

    while not goal_scored:

        actions_consequences = []
        # Simulate Consequences
        for action in action_list:
            single_consequence = a.ActionResults([])
            actions_consequences.append(Sim.simulate_consequences(action, single_consequence, state, a.num_particles))

        # Decide best action
        best_action = Sim.decide_smart(actions_consequences, state)

        # state.next_action = best_action #not next but last action
        sim_data[len(sim_data)-1].next_action = best_action

        # expected_ball_pos should be in local coordinates for rotation calculations
        expected_ball_pos = actions_consequences[best_action].expected_ball_pos_mean

        if variation is True:
            expected_ball_pos = var_kick(best_action, expected_ball_pos)

        # Check if expected_ball_pos inside opponent goal
        opp_goal_back_right = m2d.Vector2(field.opponent_goalpost_right.x + field.goal_depth,
                                          field.opponent_goalpost_right.y)
        opp_goal_box = m2d.Rect2d(opp_goal_back_right, field.opponent_goalpost_left)

        goal_scored = opp_goal_box.inside(state.pose * expected_ball_pos)
        inside_field = field.field_rect.inside(state.pose * expected_ball_pos)

        # Assert that expected_ball_pos is inside field or inside opp goal
        if not inside_field and not goal_scored:
            sim_data.append(copy.deepcopy(state))
            print("Error")
            # For histogram -> note the this position doesnt manage a goal
            break

        if not action_list[best_action].name == "none":

            print(str(state.pose * expected_ball_pos) + " Decision: " + str(action_list[best_action].name))

            # update the robots position
            rotation = np.arctan2(expected_ball_pos.y, expected_ball_pos.x)
            print(math.degrees(rotation))
            state.update_pos(state.pose * expected_ball_pos, state.pose.rotation + rotation)
            sim_data.append(copy.deepcopy(state))

            num_kicks += 1

        elif action_list[best_action].name == "none":
            print(str(state.pose * expected_ball_pos) + " Decision: " + str(action_list[best_action].name))

            attack_direction = attack_dir.get_attack_direction(state)
            # Todo: can run in a deadlock for some reason
            if attack_direction > 0:
                state.update_pos(state.pose.translation, state.pose.rotation + math.radians(10))  # Should be turn right
                # state.pose.rotation += math.radians(10)  # Should be turn right
                print("Robot turns right - global rotation turns left")

            else:
                state.update_pos(state.pose.translation, state.pose.rotation - math.radians(10))  # Should be turn left
                # state.pose.rotation -= math.radians(10)  # Should be turn left
                print("Robot turns left - global rotation turns right")

            sim_data.append(copy.deepcopy(state))

            num_turn_degrees += 1

    # print("Num Kicks: " + str(num_kicks))
    # print("Num Turns: " + str(num_turn_degrees))

    return sim_data
示例#14
0
def main(x, y, rot, s, num_iter):
    no_action = a.Action("none", 0, 0, 0, 0)
    kick_short = a.Action("kick_short", 1280, 0, 0, 0)
    sidekick_left = a.Action("sidekick_left", 750, 0, 90, 0)
    sidekick_right = a.Action("sidekick_right", 750, 0, -90, 0)
    action_list = [no_action, kick_short, sidekick_left, sidekick_right]

    s.update_pos(m2d.Vector2(x, y), rotation=rot)  # rot is in degrees

    # Do several decision cycles not just one to get rid of accidental decisions
    timings = []
    n_kicks = []
    n_turns = []
    for idx in range(num_iter):
        num_kicks = 0
        num_turn_degrees = 0
        goal_scored = False
        total_time = 0
        s.update_pos(m2d.Vector2(x, y), rotation=rot)
        while not goal_scored:
            # Change Angle of all actions according to the particle filter
            # best_dir is the global rotation for that kick
            best_dir = 360
            best_action = 0
            for ix, action in enumerate(action_list):
                if action.name is "none":
                    continue
                tmp, _ = calculate_best_direction(s, action_list[ix], False, iterations=20)
                # print("Best dir: " + str(math.degrees(tmp)) + " for action: " + action_list[idx].name)
                if np.abs(tmp) < np.abs(best_dir):
                    best_dir = tmp
                    best_action = ix
            # print("Best dir: " + str(math.degrees(best_dir)) + " for action: " + action_list[best_action].name)

            # Rotate the robot so that the shooting angle == best_dir
            s.pose.rotation = s.pose.rotation + best_dir
            # only model turning when it's significant
            if np.abs(best_dir) > 5:
                total_time += np.abs(math.degrees(best_dir) / s.rotation_vel)
                num_turn_degrees += np.abs(math.degrees(best_dir))

            # after turning evaluate the best action again to calculate the expected ball position
            actions_consequences = []
            single_consequence = a.ActionResults([])
            actions_consequences.append(Sim.simulate_consequences(action_list[best_action], single_consequence, s, a.num_particles))

            # expected_ball_pos should be in local coordinates for rotation calculations
            expected_ball_pos = actions_consequences[0].expected_ball_pos_mean

            # Check if expected_ball_pos inside opponent goal
            opp_goal_back_right = m2d.Vector2(field.opponent_goalpost_right.x + field.goal_depth,
                                              field.opponent_goalpost_right.y)
            opp_goal_box = m2d.Rect2d(opp_goal_back_right, field.opponent_goalpost_left)

            goal_scored = opp_goal_box.inside(s.pose * expected_ball_pos)
            inside_field = field.field_rect.inside(s.pose * expected_ball_pos)

            # Assert that expected_ball_pos is inside field or inside opp goal
            if not inside_field and not goal_scored:
                # print("Error: This position doesn't manage a goal")
                # total_time = float('nan')
                # Maybe still treat it as goal since it's some weird issue with particle not inside the goal which screws up the mean
                break

            # calculate the time needed
            rotation = np.arctan2(expected_ball_pos.y, expected_ball_pos.x)
            rotation_time = np.abs(math.degrees(rotation) / s.rotation_vel)
            distance = np.hypot(expected_ball_pos.x, expected_ball_pos.y)
            distance_time = distance / s.walking_vel

            total_time += distance_time + rotation_time

            # update the robots position
            s.update_pos(s.pose * expected_ball_pos, math.degrees(s.pose.rotation + rotation))
            num_turn_degrees += np.abs(math.degrees(rotation))
            num_kicks += 1

        timings.append(total_time)
        n_kicks.append(num_kicks)
        n_turns.append(num_turn_degrees)

    return np.nanmin(timings), np.nanmean(n_kicks), np.nanmean(n_turns)