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