def draw_actions(actions_consequences, state, best_action, normal_angle, better_angle, angle_list): plt.clf() tools.draw_field() axes = plt.gca() axes.add_artist(Circle(xy=(state.pose.translation.x, state.pose.translation.y), radius=100, fill=False, edgecolor='white')) axes.text(0, 0, best_action, fontsize=12) origin = state.pose.translation # arrow_head = m2d.Vector2(500, 0).rotate(math.radians(normal_angle + better_angle)) 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 draw_actions(actions_consequences, state, best_action): plt.clf() tools.draw_field() axes = plt.gca() # axes.text(0, 0, best_action, fontsize=12) axes.add_artist(Circle(xy=(state.pose.translation.x, state.pose.translation.y), radius=100, fill=False, edgecolor='white')) axes.text(0, 0, best_action, fontsize=12) # Add the other robots for own in state.own_robots: axes.add_artist(Circle(xy=(own.x, own.y), radius=100, fill=True, edgecolor='blue')) for opp in state.opp_robots: axes.add_artist(Circle(xy=(opp.x, opp.y), radius=100, fill=True, edgecolor='red')) 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 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 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 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)
def draw_actions(actions_consequences, state, best_action): 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, best_action, fontsize=12) x = np.array([]) y = np.array([]) for consequence in actions_consequences: expected_ball_pos_mean = state.pose * consequence.expected_ball_pos_mean expected_ball_pos_median = state.pose * consequence.expected_ball_pos_median axes.add_artist( Circle(xy=(expected_ball_pos_mean.x, expected_ball_pos_mean.y), radius=100, fill=False, edgecolor='yellow')) axes.add_artist( Circle(xy=(expected_ball_pos_median.x, expected_ball_pos_median.y), radius=100, fill=False, edgecolor='blue')) 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, zorder=100) plt.pause(0.0001)
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()
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 step_y = 0 step_x += 1 CS1 = plt.contourf(x_val, y_val, potentials, 10, alpha=1, cmap="coolwarm", frameon=False) CS = plt.contour(CS1, levels=CS1.levels, zorder=81) plt.clabel(CS, inline=True, fontsize=10, colors="black") tools.draw_field(plt.gca()) plt.show()
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()
plt.plot(h2[:, 0], h2[:, 1], '-ok') plt.plot(h3[:, 0], h3[:, 1], '-or') plt.plot(h4[:, 0], h4[:, 1], '-oy') plt.pause(2) ''' state = copy.deepcopy(origin) print("run direct_kick_strategy") history3 = run_experiment(state, direct_kick_strategy_cool, all_actions) print([np.degrees(h.turn_around_ball) for h in history3[0:-1]]) print([h.selected_action_idx for h in history3[0:-1]]) h3 = np.array( [[h.state.pose.translation.x, h.state.pose.translation.y] for h in history3]) v3 = np.array( [[np.cos(h.state.pose.rotation), np.sin(h.state.pose.rotation)] for h in history3]) * 200 plt.clf() axes = plt.gca() tools.draw_field(axes) plt.plot(h3[:, 0], h3[:, 1], '-or') plt.quiver(h3[:, 0], h3[:, 1], v3[:, 0], v3[:, 1], units='width') plt.show()
reads data/strategy_times.pickle Pickle File format: x, y, rot, time_current_impl, time_particle_filter reads data/strategy_actions.pickle Pickle File format: x, y, rot, c_kicks, c_turns, p_kicks, p_turns """ version = 4 strategies = pickle.load( open("../data/strategy_times" + str(version) + ".pickle", "rb")) actions = pickle.load( open("../data/strategy_actions" + str(version) + ".pickle", "rb")) ax = plt.gca() tools.draw_field(ax) nx = {} ny = {} for pos in strategies: x, y, angle, time_old, time_particle = pos nx[x] = x 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):
def draw_field(): # draw the blank field plt.clf() tools.draw_field(plt.gca()) axes = plt.gca()