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()
