def optimal_value_strategy(s, action_list):
actions_consequences = []
rotations = []
for action in action_list:
if action.name is "none":
rotation = 0
else:
# optimize action
rotation, _ = heinrich_test(s, action, False, iterations=20)
rotations += [rotation]
# apply optimized rotation
s.pose.rotate(rotation)
actions_consequences.append(
Sim.simulateAction(action, s, num_particles=30))
# restore previous rotation
s.pose.rotate(-rotation)
# Decide best action
selected_action_idx = Sim.decide_minimal(actions_consequences, s)
return selected_action_idx, rotations[selected_action_idx]
def main():
state = State()
"""
no_action = a.Action("none", 0, 0, 0, 0)
kick_short = a.Action("kick_short", 780, 150, 8.454482265522328, 6.992268841997358)
sidekick_left = a.Action("sidekick_left", 750, 150, 86.170795364136380, 10.669170653645670)
sidekick_right = a.Action("sidekick_right", 750, 150, -89.657943335302260, 10.553726275058064)
"""
action_list = [no_action, kick_short, sidekick_left, sidekick_right]
while True:
actions_consequences = []
# Simulate Consequences
for action in action_list:
single_consequence = a.ActionResults([])
actions_consequences.append(
Sim.simulate_consequences(action, single_consequence, state,
30))
# actions_consequences is now a list of ActionResults
# Decide best action
best_action = Sim.decide_smart(actions_consequences, state)
draw_actions(actions_consequences, state,
action_list[best_action].name)
def main():
state = State(1000, 0)
no_action = a.Action("none", 0, 0, 0, 0)
kick_short = a.Action("kick_short", 1080, 150, 0, 7)
sidekick_left = a.Action("sidekick_left", 750, 150, 90, 10)
sidekick_right = a.Action("sidekick_right", 750, 150, -90, 10)
action_list = [no_action, kick_short, sidekick_left, sidekick_right]
while plt.get_fignums():
actions_consequences = []
# Simulate Consequences
for action in action_list:
single_consequence = a.ActionResults([])
actions_consequences.append(
Sim.simulate_consequences(action, single_consequence, state,
40))
# actions_consequences is now a list of ActionResults
# Decide best action
best_action = Sim.decide_smart(actions_consequences, state)
draw_actions(actions_consequences, state,
action_list[best_action].name)
def direct_kick_strategy_cool(s, action_list, take_best=False):
actions_consequences = []
rotations = []
fastest_action_dir = None
fastest_action_idx = 0
for idx, action in enumerate(action_list):
if action.name is "none":
rotation = 0
else:
# optimize action
a0 = minimal_rotation(s, action, 1)
a1 = minimal_rotation(s, action, -1)
if np.abs(a0) < np.abs(a1):
rotation = a0
else:
rotation = a1
if np.abs(rotation) > 3:
print(
"WARNING: in direct_kick_strategy_cool no kick found after rotation {0}."
.format(rotation))
rotations += [rotation]
# apply optimized rotation
s.pose.rotate(rotation)
actions_consequences.append(
Sim.simulateAction(action, s, num_particles=30))
# restore previous rotation
s.pose.rotate(-rotation)
if action.name is not "none":
if fastest_action_dir is None or np.abs(rotation) < np.abs(
fastest_action_dir):
fastest_action_dir = rotation
fastest_action_idx = idx
# Decide best action
if take_best:
selected_action_idx = Sim.decide_minimal(actions_consequences, s)
return selected_action_idx, rotations[selected_action_idx]
else:
# print fastest_action_idx, selected_action_idx
return fastest_action_idx, fastest_action_dir
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 update(samples, likelihoods, s, action, m_min, m_max):
# likelihoods = np.zeros(samples.shape)
test_action = copy.deepcopy(action)
simulation_consequences = []
simulation_num_particles = 1
for idx in range(0, len(samples)):
# modify the action with the sample
test_action.angle = action.angle + samples[idx]
test_action_consequence = a.ActionResults([])
simulation_consequences.append(
Sim.simulate_consequences(test_action, test_action_consequence, s, simulation_num_particles))
if test_action_consequence.category("INFIELD") > 0:
potential = -pf.evaluate_action2(test_action_consequence, s)
likelihoods[idx] = potential
m_min = min(m_min, potential)
m_max = max(m_max, potential)
elif test_action_consequence.category("OPPGOAL") > 0:
likelihoods[idx] = m_max
else:
likelihoods[idx] = m_min
return likelihoods, simulation_consequences, m_min, m_max
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 direct_kick_strategy(s, action_list):
turn_speed = math.radians(5.0)
action_dir = 0
turn_direction = 0
while True:
# turn towards the direction of the action
s.pose.rotate(action_dir)
# Simulate Consequences
actions_consequences = [
Sim.simulateAction(action, s, num_particles=30)
for action in action_list
]
# Decide best action
selected_action_idx = Sim.decide_minimal(actions_consequences, s)
# restore the previous orientation
s.pose.rotate(-action_dir)
if selected_action_idx != 0:
break
elif np.abs(action_dir) > math.pi:
print(
"WARNING: in direct_kick_strategy no kick found after rotation {0}."
.format(action_dir))
break
else:
# decide on rotation direction once
if turn_direction == 0:
attack_direction = attack_dir.get_attack_direction(s)
turn_direction = np.sign(
attack_direction.angle()) # "> 0" => left, "< 0" => right
# set motion request
action_dir += turn_direction * turn_speed
return selected_action_idx, action_dir
def minimal_rotation(s, action, turn_direction):
turn_speed = math.radians(5.0)
action_dir = 0
none = a.Action("none", 0, 0, 0, 0)
none_actions_consequences = Sim.simulateAction(none, s, num_particles=30)
while True:
# turn towards the direction of the action
s.pose.rotate(action_dir)
# Simulate Consequences
actions_consequences = Sim.simulateAction(action, s, num_particles=30)
# Decide best action
selected_action_idx = Sim.decide_minimal(
[none_actions_consequences, actions_consequences], s)
# restore the previous orientation
s.pose.rotate(-action_dir)
if selected_action_idx != 0:
break
elif np.abs(action_dir) > math.pi:
# print("WARNING: in minimal_rotation no kick found after rotation {0}.".format(action_dir))
break
else:
# decide on rotation direction once
if turn_direction == 0:
attack_direction = attack_dir.get_attack_direction(s)
turn_direction = np.sign(
attack_direction.angle()) # "> 0" => left, "< 0" => right
# set motion request
action_dir += turn_direction * turn_speed
return action_dir
def update(samples, likelihoods, state, action, m_min, m_max):
# likelihoods = np.zeros(samples.shape)
test_action = copy.deepcopy(action)
simulation_consequences = []
simulation_num_particles = 1
stats = np.zeros((len(samples), 3))
number = 0.0
for i in range(0, len(samples)):
# modify the action with the sample
test_action.angle = action.angle + samples[i]
results = a.ActionResults([])
simulation_consequences.append(
Sim.simulate_consequences(test_action, results, state,
simulation_num_particles))
stats[i, 0] = results.likelihood(Category.OPPGOAL)
stats[i, 1] = results.likelihood(Category.INFIELD)
number = results.likelihood(Category.NUMBER)
if stats[i, 0] + stats[i, 1] > 0:
potential = -pf.evaluate_action(results, state)
stats[i, 2] = potential
max_goal = np.max(stats[:, 0])
min_value = np.min(stats[:, 2])
max_value = np.max(stats[:, 2])
diff = max_value - min_value
if diff == 0:
diff = 1.0
for i in range(0, len(samples)):
if stats[i, 1] + stats[i, 0] >= max(0.0,
Sim.good_threshold_percentage):
likelihoods[i] = (stats[i, 1] + stats[i, 0]) * (
(stats[i, 2] - min_value) / diff) + stats[i, 0]
else:
likelihoods[i] = 0
return likelihoods, simulation_consequences, m_min, m_max
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_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, rot, s, num_iter):
no_action = a.Action("none", 0, 0, 0, 0)
kick_short = a.Action("kick_short", 1280, 0, 0, 0)
sidekick_left = a.Action("sidekick_left", 750, 0, 90, 0)
sidekick_right = a.Action("sidekick_right", 750, 0, -90, 0)
action_list = [no_action, kick_short, sidekick_left, sidekick_right]
s.update_pos(m2d.Vector2(x, y), rotation=rot) # rot is in degrees
# Do several decision cycles not just one to get rid of accidental decisions
timings = []
n_kicks = []
n_turns = []
for idx in range(num_iter):
num_kicks = 0
num_turn_degrees = 0
goal_scored = False
total_time = 0
s.update_pos(m2d.Vector2(x, y), rotation=rot)
while not goal_scored:
# Change Angle of all actions according to the particle filter
# best_dir is the global rotation for that kick
best_dir = 360
best_action = 0
for ix, action in enumerate(action_list):
if action.name is "none":
continue
tmp, _ = calculate_best_direction(s, action_list[ix], False, iterations=20)
# print("Best dir: " + str(math.degrees(tmp)) + " for action: " + action_list[idx].name)
if np.abs(tmp) < np.abs(best_dir):
best_dir = tmp
best_action = ix
# print("Best dir: " + str(math.degrees(best_dir)) + " for action: " + action_list[best_action].name)
# Rotate the robot so that the shooting angle == best_dir
s.pose.rotation = s.pose.rotation + best_dir
# only model turning when it's significant
if np.abs(best_dir) > 5:
total_time += np.abs(math.degrees(best_dir) / s.rotation_vel)
num_turn_degrees += np.abs(math.degrees(best_dir))
# after turning evaluate the best action again to calculate the expected ball position
actions_consequences = []
single_consequence = a.ActionResults([])
actions_consequences.append(Sim.simulate_consequences(action_list[best_action], single_consequence, s, a.num_particles))
# expected_ball_pos should be in local coordinates for rotation calculations
expected_ball_pos = actions_consequences[0].expected_ball_pos_mean
# Check if expected_ball_pos inside opponent goal
opp_goal_back_right = m2d.Vector2(field.opponent_goalpost_right.x + field.goal_depth,
field.opponent_goalpost_right.y)
opp_goal_box = m2d.Rect2d(opp_goal_back_right, field.opponent_goalpost_left)
goal_scored = opp_goal_box.inside(s.pose * expected_ball_pos)
inside_field = field.field_rect.inside(s.pose * expected_ball_pos)
# Assert that expected_ball_pos is inside field or inside opp goal
if not inside_field and not goal_scored:
# print("Error: This position doesn't manage a goal")
# total_time = float('nan')
# Maybe still treat it as goal since it's some weird issue with particle not inside the goal which screws up the mean
break
# calculate the time needed
rotation = np.arctan2(expected_ball_pos.y, expected_ball_pos.x)
rotation_time = np.abs(math.degrees(rotation) / s.rotation_vel)
distance = np.hypot(expected_ball_pos.x, expected_ball_pos.y)
distance_time = distance / s.walking_vel
total_time += distance_time + rotation_time
# update the robots position
s.update_pos(s.pose * expected_ball_pos, math.degrees(s.pose.rotation + rotation))
num_turn_degrees += np.abs(math.degrees(rotation))
num_kicks += 1
timings.append(total_time)
n_kicks.append(num_kicks)
n_turns.append(num_turn_degrees)
return np.nanmin(timings), np.nanmean(n_kicks), np.nanmean(n_turns)
