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_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
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) field_rect = m2d.Rect2d(m2d.Vector2(-x_length*0.5, -y_length*0.5), m2d.Vector2(x_length*0.5, y_length*0.5))
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)
x_own_groundline = -x_opponent_groundline
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)
field_rect = m2d.Rect2d(m2d.Vector2(-x_length * 0.5, -y_length * 0.5),
m2d.Vector2(x_length * 0.5, y_length * 0.5))
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)
