def draw_robot_walk(s, expected_ball_pos, best_action): plt.clf() axes = plt.gca() origin = s.pose.translation arrow_head = m2d.Vector2(500, 0).rotate(s.pose.rotation) tools.draw_field() axes.add_artist( Circle(xy=(s.pose.translation.x, s.pose.translation.y), radius=100, fill=False, edgecolor='white')) axes.add_artist( Circle(xy=(expected_ball_pos.x, expected_ball_pos.y), radius=120, fill=True, edgecolor='blue')) axes.arrow(origin.x, origin.y, arrow_head.x, arrow_head.y, head_width=100, head_length=100, fc='k', ec='k') axes.text(0, 0, best_action, fontsize=12) plt.pause(0.1)
def __init__(self): self.pose = m2d.Pose2D() self.pose.translation = m2d.Vector2(-2900, -2200) self.pose.rotation = math.radians(55) self.ball_position = m2d.Vector2(100.0, 0.0) self.obstacle_list = ([]) # is in global coordinates self.potential_field_function = "influence_01" self.own_robots = [m2d.Vector2(-2000, -1000)] self.opp_robots = [] # m2d.Vector2(-1800, -1000) self.actions = [no_action, kick_short, sidekick_left, sidekick_right ] # possible actions the robot can perform
def predict(self, ball, noise): gforce = 9.80620 * 1e3 # mm/s^2 if self.speed == 0: # means action is none return ball if noise: if self.speed_std > 0: speed = np.random.normal(self.speed, self.speed_std) else: speed = self.speed if self.angle_std > 0: angle = np.random.normal(math.radians(self.angle), math.radians(self.angle_std)) else: angle = math.radians(self.angle) distance = speed * speed / friction / gforce / 2.0 # friction*mass*gforce*distance = 1/2*mass*speed*speed else: distance = self.speed * self.speed / friction / gforce / 2.0 angle = math.radians(self.angle) noisy_action = m2d.Vector2(distance, 0.0) noisy_action = noisy_action.rotate(angle) return ball + noisy_action
def test_rotate(self): a = m2d.Vector2(1, 0) a = a.rotate(math.radians(90)) a.x = math.floor(a.x) a.y = math.floor(a.y) self.assertEqual(a.x, 0.0) self.assertEqual(a.y, 1.0)
def draw_robot_walk_lines(line): plt.clf() axes = plt.gca() tools.draw_field(axes) count = 0 action_name_list = ["none", "short", "left", "right"] for state in line: origin = state.pose.translation ball_pos = state.pose * state.ball_position arrow_head = m2d.Vector2(500, 0).rotate(state.pose.rotation) axes.add_artist(Circle(xy=(origin.x, origin.y), radius=100, fill=False, edgecolor='white')) axes.add_artist(Circle(xy=(ball_pos.x, ball_pos.y), radius=120, fill=True, edgecolor='blue')) axes.arrow(origin.x, origin.y, arrow_head.x, arrow_head.y, head_width=100, head_length=100, fc='k', ec='k') axes.text(origin.x, origin.y - 350, count, fontsize=8) axes.text(origin.x, 3500 - (count % 4)*250, action_name_list[state.next_action] + " " + str(count), fontsize=8) count += 1 # -- Add counter for moves # -- Add executed action # -- prove Ball position ist correct # -- Haeufungspunkte (z.B. rotieren um Ball) Zahlen verbessern - mehr Uebersichtlichkeit plt.show()
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()
def calculate_potential_field(point, target_point): # we are attracted to the target point field_f = global_exponential_attractor(target_point, point) # we are repelled by the opponents player_f = m2d.Vector2(0, 0) player_f.abs() # NOT Implemented yet # my self - NOTE this differs from the cpp Implementation in order to factor in the robots own position player_f -= compact_exponential_repeller(m2d.Vector2(0, 0), point) # field_f is of type Vector2 ff = field_f.abs() * 0.8 if player_f.abs() > ff: player_f = player_f.normalize_length(ff) return field_f + player_f
def evaluate_action_with_robots(results, state): pf_normal_value = evaluate_action(results, state) sum_potential = 0.0 number_of_actions = 0.0 for p in results.positions(): if p.cat() == Category.INFIELD or p.cat() == Category.OPPGOAL: sum_potential += evaluate_single_pos_with_robots( state.pose * p.pos(), state.opp_robots, state.own_robots) number_of_actions += 1 assert number_of_actions > 0 sum_potential /= number_of_actions squared_difference = np.abs( pf_normal_value - sum_potential ) # decide whether considering other robots is appropriate if squared_difference < 0.05: # upper bound - how to choose? return pf_normal_value # ab hier experimental # new Ball pos with passing new_ball_pos = results.expected_ball_pos_mean # update state new_state = copy.copy(state) new_state.ball_position = m2d.Vector2(0.0, 0.0) # Ball = Robot new_state.translation = new_ball_pos # new_state.rotation = pass # maybe rotation as needed for direkt / shortest path to the ball new_state.potential_field_function = "normal" # Simulation as in Simulation.py (decide_smart) actions_consequences = [] # Simulate Consequences for action in new_state.actions: # new_state.actions includes the possible kicks single_consequence = ActionResults([]) actions_consequences.append( Sim.simulate_consequences(action, single_consequence, new_state, 30)) best_action_with_team = Sim.decide_smart(actions_consequences, state) # compare new best action # TODO: Add simulation of a second kick without other robots to have better comparison # needed: action which would have been done without other robots # what should get returned?? to compare with other actions """ if best_value > pf_normal_value: return sum_potential """ return sum_potential
def compact_exponential_repeller(target_point, point): player_repeller_strenth = 1500 player_repeller_radius = 2000 a = player_repeller_strenth d = player_repeller_radius v = target_point - point t = v.abs() # should be double precision if t >= d-100: return m2d.Vector2(0, 0) return v.normalize() * math.exp(a / d - a / (d - t))
def projectEdgel(x, y, cMatrix): v = m3.Vector3() v.x = getFocalLength() v.y = 320 - x v.z = 240 - y v = cMatrix.rotation * v result = m2.Vector2() result.x = v.x result.y = v.y result = result * (cMatrix.translation.z / (-v.z)) result.x = result.x + cMatrix.translation.x result.y = result.y + cMatrix.translation.y return (result.x, result.y)
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"))
def draw_actions(actions_consequences, likelihoods, state, best_action, normal_angle, better_angle, angle_list): plt.clf() tools.draw_field(plt.gca()) axes = plt.gca() axes.add_artist( Circle(xy=(state.pose.translation.x, state.pose.translation.y), radius=100, fill=False, edgecolor='white')) axes.text(-4500, 3150, str(best_action) + " with rotation: " + str(better_angle), fontsize=12) origin = state.pose.translation likelihoods = normalize(likelihoods) for angle in angle_list: arrow_head = m2d.Vector2(500, 0).rotate(state.pose.rotation + math.radians(normal_angle + angle)) axes.arrow(origin.x, origin.y, arrow_head.x, arrow_head.y, head_width=100, head_length=100, fc='k', ec='k') # arrow_head = m2d.Vector2(500, 0).rotate(state.pose.rotation + math.radians(normal_angle + better_angle)) # axes.arrow(origin.x, origin.y, arrow_head.x, arrow_head.y, head_width=100, head_length=100, fc='k', ec='k') x = np.array([]) y = np.array([]) for consequence in actions_consequences: for particle in consequence.positions(): ball_pos = state.pose * particle.ball_pos # transform in global coordinates x = np.append(x, [ball_pos.x]) y = np.append(y, [ball_pos.y]) plt.scatter(x, y, c='r', alpha=0.5) plt.pause(0.0001)
def main(): num_random_pos = 20 random_x = [randint(int(-field.x_length / 2 + 25), int(field.x_length / 2 - 25)) for p in range(num_random_pos)] random_y = [randint(int(-field.y_length / 2 + 25), int(field.y_length / 2 - 25)) for p in range(num_random_pos)] random_r = np.random.randint(360, size=num_random_pos) random_r = np.radians(random_r) # record the experiment header experiment = { 'kind': 'random', 'actions': actions, 'frames': [] } positions = [m2d.Pose2D(m2d.Vector2(x, y), r) for (x, y, r) in zip(random_x, random_y, random_r)] counter = mp.Value('i', 0) pool = mp.Pool(initializer=init, initargs=(counter, ), processes=4) runner = pool.map_async(make_run, positions) #wait until done # NOTE: this has to be done this way, so the programm can be interrupted by keyboard # http://xcodest.me/interrupt-the-python-multiprocessing-pool-in-graceful-way.html while not runner.ready(): try: experiment['frames'] = runner.get(1) # a very long timeout except mp.TimeoutError as ex: pass pool.close() # make sure not to overwrite anything file_idx = 0 while True: output_file = 'data/simulation_{0}.pickle'.format(file_idx) if os.path.exists(output_file): file_idx += 1 else: pickle.dump(experiment, open(output_file, "wb")) break
def draw_robot_walk(actions_consequences, s, expected_ball_pos, best_action): plt.clf() axes = plt.gca() origin = s.pose.translation arrow_head = m2d.Vector2(500, 0).rotate(s.pose.rotation) x = np.array([]) y = np.array([]) tools.draw_field(axes) axes.add_artist( Circle(xy=(s.pose.translation.x, s.pose.translation.y), radius=100, fill=False, edgecolor='white')) axes.add_artist( Circle(xy=(expected_ball_pos.x, expected_ball_pos.y), radius=120, fill=True, edgecolor='blue')) axes.arrow(origin.x, origin.y, arrow_head.x, arrow_head.y, head_width=100, head_length=100, fc='k', ec='k') axes.text(-4500, 3150, best_action, fontsize=12) for consequence in actions_consequences: for particle in consequence.positions(): ball_pos = s.pose * particle.ball_pos # transform in global coordinates x = np.append(x, [ball_pos.x]) y = np.append(y, [ball_pos.y]) plt.scatter(x, y, c='r', alpha=0.5) plt.pause(0.5)
import math import numpy as np from matplotlib import pyplot as plt from tools import field_info as field from naoth import math2d as m2d from tools import potential_field as pf from tools import tools from state import State if __name__ == "__main__": state = State() state.opp_robots.append( m2d.Pose2D(m2d.Vector2(2000, 1000), math.radians(0))) state.own_robots.append( m2d.Pose2D(m2d.Vector2(-2000, 1000), math.radians(0))) plt.clf() x_val = np.arange(-field.x_field_length / 2, field.x_field_length / 2, 10) y_val = np.arange(-field.y_field_length / 2, field.y_field_length / 2, 10) potentials = np.zeros((len(y_val), len(x_val))) # There is probably a better implementation for this step_x = 0 step_y = 0 for x in x_val: for y in y_val: # potentials[step_y][step_x] = pf.evaluate_single_pos(m2d.Vector2(x, y)) potentials[step_y][step_x] = pf.evaluate_single_pos_with_robots( m2d.Vector2(x, y), state.opp_robots, state.own_robots) step_y += 1
def test_add(self): a = m2d.Vector2(100, 100) b = m2d.Vector2(1000, 10) c = a + b self.assertEqual(c.x, 1100) self.assertEqual(c.y, 110)
def test_divison(self): a = m2d.Vector2(100, 100) b = a / 2 self.assertEqual(b.x, 50.0) self.assertEqual(b.y, 50.0)
def __init__(self, categorized_ball_position_list): self.ball_positions = categorized_ball_position_list # type is list of CategorizedBallPosition self.cat_histogram = {n : 0.0 for n in Category} #self.cat_histogram = [0]*Category.NUMBER_OF_BallPositionCategory.value #len(Categories) # type is list self.expected_ball_pos_mean = m2d.Vector2() # mean - should be in local coordinates self.expected_ball_pos_median = m2d.Vector2() # median - should be in local coordinates
def test_abs(self): a = m2d.Vector2(100, 0) b = a.abs() self.assertEqual(b, 100)
def main(): state = State() file_idx = 0 cell_width = 50 iteration = 1 rotation_step = 5 dummy_container = [] show_image = False axes = plt.gca() tools.draw_field(axes) x_range = range( int(-field.x_length * 0.5) + 4 * cell_width, int(field.x_length * 0.5), 4 * cell_width) y_range = range( int(-field.y_length * 0.5) + 4 * cell_width, int(field.y_length * 0.5), 4 * cell_width) # run for the whole field for x in x_range: for y in y_range: time, angle = simulate_best_angle(x, y, state, rotation_step, iteration) if not np.isinf(time): v = m2d.Vector2(100.0, 0.0).rotate(math.radians(angle)) axes.arrow(x, y, v.x, v.y, head_width=100, head_length=100, fc='k', ec='k') else: print("WARNING: Time is Nan") v = m2d.Vector2(100.0, 0.0).rotate(math.radians(angle)) axes.arrow(x, y, v.x, v.y, head_width=100, head_length=100, fc='r', ec='r') dummy_container.append([x, y, time, angle]) while (os.path.exists('{}{:d}.png'.format( '../data/potential_field_generation/potential_field_gen_own', file_idx)) or os.path.exists('{}{:d}.pickle'.format( '../data/potential_field_generation/potential_field_gen_own', file_idx))): file_idx += 1 plt.savefig('{}{:d}.png'.format( '../data/potential_field_generation/potential_field_gen_own', file_idx)) pickle.dump( dummy_container, open( '../data/potential_field_generation/potential_field_gen_own' + str(file_idx) + '.pickle', "wb")) # make sure not to overwrite anything if show_image: plt.show()
ny[y] = y nxi = np.array(sorted(nx.keys())) nyi = np.array(sorted(ny.keys())) for i, v in enumerate(nxi): nx[v] = i for i, v in enumerate(nyi): ny[v] = i f = np.zeros((len(ny), len(nx))) g = np.zeros((len(ny), len(nx))) # create opponent goal line as line segment left_goal_post = m2d.Vector2(field.x_opponent_groundline, field.y_left_goalpost) right_goal_post = m2d.Vector2(field.x_opponent_groundline, field.y_right_goalpost) goal_line = m2d.LineSegment(left_goal_post, right_goal_post) plt.clf() tools.draw_field(plt.gca()) ax = plt.gca() # create the scalar fields for x in range(int(-field.x_length * 0.5), int(field.x_length * 0.5), x_step): for y in range(int(-field.y_length * 0.5), int(field.y_length * 0.5), y_step): a = np.array([x + x_step / 2, y + y_step / 2]) # b = np.array([4500, 0]) projection = goal_line.projection(m2d.Vector2(x, y))
if __name__ == "__main__": # Constants for robot velRot = 60 # grad pro second velWalk = 200 # mm pro second size_x = 4500 size_y = 3000 x = np.arange(-size_x, size_x, 50) y = np.arange(-size_y, size_y, 50) xm, ym = np.meshgrid(x, y) x_dim = x.size y_dim = y.size zm = np.zeros((y_dim, x_dim)) own_robots = [m2d.Vector2(0, 0)] # TODO where should the friendly robot be according to the graphic in my thesis # own_robots = [] opp_robots = [] for i in range(int(y_dim)): for j in range(int(x_dim)): zm[i, j] = pfield.evaluate_action_with_robots( m2d.Vector2(xm[i][j], ym[i][j]), opp_robots, own_robots) # plot fig = plt.figure(frameon=False) ax = fig.gca() ax.set_aspect("equal") ax.set_xlabel("x [mm]") ax.set_ylabel("y [mm]")
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
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)
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_consequences(action, categorized_ball_positions, state, num_particles): categorized_ball_positions.reset() # calculate the own goal line own_goal_dir = (field.own_goalpost_right - field.own_goalpost_left).normalize() own_left_endpoint = field.own_goalpost_left + own_goal_dir * ( field.goalpost_radius + field.ball_radius) own_right_endpoint = field.own_goalpost_right - own_goal_dir * ( field.goalpost_radius + field.ball_radius) own_goal_line_global = m2d.LineSegment(own_left_endpoint, own_right_endpoint) # calculate opponent goal lines and box opp_goal_back_left = m2d.Vector2( field.opponent_goalpost_left.x + field.goal_depth, field.opponent_goalpost_left.y) opp_goal_back_right = m2d.Vector2( field.opponent_goalpost_right.x + field.goal_depth, field.opponent_goalpost_right.y) # Maybe add list of goal backsides here goal_backsides = ([]) goal_backsides.append( m2d.LineSegment(opp_goal_back_left, opp_goal_back_right)) goal_backsides.append( m2d.LineSegment(field.opponent_goalpost_left, opp_goal_back_left)) goal_backsides.append( m2d.LineSegment(field.opponent_goalpost_right, opp_goal_back_right)) opp_goal_box = m2d.Rect2d(opp_goal_back_right, field.opponent_goalpost_left) # current ball position global_ball_start_position = state.pose * state.ball_position # virtual ultrasound obstacle line obstacle_line = m2d.LineSegment(state.pose * m2d.Vector2(400, 200), state.pose * m2d.Vector2(400, -200)) mean_test_list_x = [] mean_test_list_y = [] # now generate predictions and categorize for i in range(0, num_particles): # predict and calculate shoot line global_ball_end_position = state.pose * action.predict( state.ball_position, True) shootline = m2d.LineSegment(global_ball_start_position, global_ball_end_position) # check if collision detection with goal has to be performed # if the ball start and end positions are inside of the field, you don't need to check collision_with_goal = False t_min = 0 # dummy value if not field.field_rect.inside( global_ball_end_position) or not field.field_rect.inside( global_ball_start_position): t_min = shootline.length for side in goal_backsides: t = shootline.line_intersection(side) if 0 <= t < t_min and side.intersect(shootline): t_min = t collision_with_goal = True # if there are collisions with the back goal lines, calculate where the ball will stop if collision_with_goal: global_ball_end_position = shootline.point(t_min - field.ball_radius) shootline = m2d.LineSegment(global_ball_start_position, global_ball_end_position) # Obstacle Detection obstacle_collision = False # TODO: fix obstacle collision somehow ''' for obstacle in state.obstacle_list: dist = math.sqrt((state.pose.translation.x-obstacle.x)**2 + (state.pose.translation.y-obstacle.y)**2) # check for distance and rotation # Todo it's wrong: Now if obstacle is near, then obstacle is in front of the robot if dist < 400 and shootline.intersect(obstacle_line): obstacle_collision = True ''' if opp_goal_box.inside(global_ball_end_position): category = Category.OPPGOAL elif obstacle_collision and obstacle_line.intersect( shootline) and shootline.intersect(obstacle_line): category = Category.COLLISION elif (field.field_rect.inside(global_ball_end_position) or (global_ball_end_position.x <= field.opponent_goalpost_right.x and field.opponent_goalpost_left.y > global_ball_end_position.y > field.opponent_goalpost_right.y)): category = Category.INFIELD elif shootline.intersect( own_goal_line_global) and own_goal_line_global.intersect( shootline): category = Category.OWNGOAL elif global_ball_end_position.x > field.x_opponent_groundline: category = Category.OPPOUT elif global_ball_end_position.x < field.x_opponent_groundline: category = Category.OWNOUT elif global_ball_end_position.y > field.y_left_sideline: category = Category.LEFTOUT elif global_ball_end_position.y < field.y_right_sideline: category = Category.RIGHTOUT else: category = Category.INFIELD local_test_pos = state.pose / global_ball_end_position mean_test_list_x.append(local_test_pos.x) mean_test_list_y.append(local_test_pos.y) categorized_ball_positions.add(state.pose / global_ball_end_position, category) # calculate the most likely ball position in a separate simulation run categorized_ball_positions.expected_ball_pos_mean = m2d.Vector2( np.mean(mean_test_list_x), np.mean(mean_test_list_y)) categorized_ball_positions.expected_ball_pos_median = m2d.Vector2( np.median(mean_test_list_x), np.median(mean_test_list_y)) return categorized_ball_positions
if __name__ == "__main__": single_run = False if single_run: # run a single direction calculate_best_direction(1000, 1000, 50, True) else: # run for the whole field x_pos = range(-5200, 5300, 250) y_pos = range(-3700, 3800, 250) xx, yy = np.meshgrid(x_pos, y_pos) vx = np.zeros(xx.shape) vy = np.zeros(xx.shape) f = np.zeros(xx.shape) print(xx.shape, len(x_pos), len(y_pos)) for ix in range(0, len(x_pos)): for iy in range(0, len(y_pos)): direction, direction_std = calculate_best_direction(float(x_pos[ix]), float(y_pos[iy]), 10, False) v = m2d.Vector2(100.0, 0.0).rotate(direction) vx[iy, ix] = v.x vy[iy, ix] = v.y f[iy, ix] = direction_std plt.figure() tools.draw_field() Q = plt.quiver(xx, yy, vx, vy, np.degrees(f)) plt.show()
def test_sub(self): a = m2d.Vector2(100, 100) b = m2d.Vector2(1000, 10) c = b - a self.assertEqual(c.x, 900) self.assertEqual(c.y, -90)
def test_neg(self): a = m2d.Vector2(100, 100) b = -a self.assertEqual(b.x, -100) self.assertEqual(b.y, -100)
x_opponent_goal = x_opponent_groundline x_own_goal = -x_opponent_goal x_opponent_penalty_area = x_opponent_groundline - x_penalty_area_length x_own_penalty_area = -x_opponent_penalty_area y_left_goalpost = goal_width / 2.0 y_right_goalpost = -y_left_goalpost y_left_penalty_area = y_penalty_area_length / 2.0 y_right_penalty_area = -y_left_penalty_area y_left_sideline = y_length / 2.0 y_right_sideline = -y_left_sideline opponent_goalpost_left = m2d.Vector2(x_opponent_goal+25, y_left_goalpost) opponent_goalpost_right = m2d.Vector2(x_opponent_goal+25, y_right_goalpost) own_goalpost_left = m2d.Vector2(x_own_goal-25, y_left_goalpost) own_goalpost_right = m2d.Vector2(x_own_goal-25, y_right_goalpost) # From Simulation.cpp opp_goal_back_left = m2d.Vector2(opponent_goalpost_left.x + goal_depth, opponent_goalpost_left.y) opp_goal_back_right = m2d.Vector2(opponent_goalpost_right.x + goal_depth, opponent_goalpost_right.y) opp_goal_box = m2d.Rect2d(opp_goal_back_right, opponent_goalpost_left) own_goal_back_left = m2d.Vector2(own_goalpost_left.x - goal_depth, own_goalpost_left.y) own_goal_back_right = m2d.Vector2(own_goalpost_right.x - goal_depth, own_goalpost_right.y) own_goal_box = m2d.Rect2d(own_goal_back_right, own_goalpost_left)