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)
