def calculateCollision(lines, start, end):
motionLine = m2d.LineSegment(start, end)
t_min = motionLine.length
collision = False
for segment in lines:
t = motionLine.line_intersection(segment)
if 0 <= t < t_min and segment.intersect(motionLine):
t_min = t
collision = True
# if there are collisions with the back goal lines, calculate where the ball will stop
if collision:
return motionLine.point(t_min - field.ball_radius), True
else:
return end, False
def simulateAction(action, state, num_particles):
result = ActionResults([])
# current ball position
global_ball_start = state.pose * state.ball_position
opp_goal_backsides = [
m2d.LineSegment(field.opp_goal_back_left, field.opp_goal_back_right),
m2d.LineSegment(field.opponent_goalpost_left,
field.opp_goal_back_left),
m2d.LineSegment(field.opponent_goalpost_right,
field.opp_goal_back_right)
]
own_goal_backsides = [
m2d.LineSegment(field.own_goal_back_left, field.own_goal_back_right),
m2d.LineSegment(field.own_goalpost_left, field.own_goal_back_left),
m2d.LineSegment(field.own_goalpost_right, field.own_goal_back_right)
]
# now generate predictions and categorize
for i in range(0, num_particles):
# predict and calculate shoot line
global_ball_end = state.pose * action.predict(state.ball_position,
True)
# ball is crossing a field line
if not field.field_rect.inside(
global_ball_end) or not field.field_rect.inside(
global_ball_start):
global_ball_end, _ = calculateCollision(opp_goal_backsides,
global_ball_start,
global_ball_end)
global_ball_end, _ = calculateCollision(own_goal_backsides,
global_ball_start,
global_ball_end)
category = classifyBallPosition(global_ball_end)
result.add(state.pose / global_ball_end, category)
# calculate the most likely ball position in a separate simulation run
ball_pos_array = [[p.pos().x, p.pos().y] for p in result.positions()]
mean = np.mean(ball_pos_array, axis=0)
median = np.median(ball_pos_array, axis=0)
result.expected_ball_pos_mean = m2d.Vector2(mean[0], mean[1])
result.expected_ball_pos_median = m2d.Vector2(median[0], median[1])
return result
def get_goal_target(state, point):
goal_post_offset = 200.0
# normalized vector from left post to the right
left_to_right = (state.pose / field.opponent_goalpost_right -
state.pose / field.opponent_goalpost_left).normalize()
# a normal vector pointing from the goal towards the field
goal_normal = left_to_right
goal_normal.rotate_right()
# the endpoints of our line are a shortened version of the goal line
left_endpoint = (state.pose / field.opponent_goalpost_left
) + left_to_right * goal_post_offset
right_endpoint = (state.pose / field.opponent_goalpost_right
) - left_to_right * goal_post_offset
# this is the goal line we are shooting for
goal_line = m2d.LineSegment(left_endpoint, right_endpoint)
# project the point on the goal line
target = goal_line.projection(point)
# this is the cos of the angle between the vectors (leftEndpoint-point) and (rightEndpoint-point)
# simple linear algebra: <l-p,r-p>/(||l-p||*||r-p||)
goal_angle_cos = (
state.pose / field.opponent_goalpost_left - point).normalize() * (
state.pose / field.opponent_goalpost_right - point).normalize()
goal_line_offset_front = 100.0
goal_line_offset_back = 100.0
# asymetric quadratic scale
# goalAngleCos = -1 => t = -goalLineOffsetBack
# goalAngleCos = 1 => t = goalLineOffsetFront;
c = (goal_line_offset_front + goal_line_offset_back) * 0.5
v = (goal_line_offset_front - goal_line_offset_back) * 0.5
t = goal_angle_cos * (goal_angle_cos * c + v)
# move the target depending on the goal opening angle
target = target + goal_normal.normalize_length(t)
return target
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
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))
b = np.array([projection.x, projection.y])
f[ny[y], nx[x]] = np.sqrt(np.sum((a - b)**2))
