def next_position(self):
self.target_orientation = (self.target.position - self.game_state.ball_position).angle
self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
if time.time() - self.start_time >= self.rotate_time:
self.rotation_sign = self._get_direction()
if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle,
VALID_DIFF_ANGLE):
self.next_state = self.halt
return self._go_to_final_position()
elif time.time() - self.iter_time >= self.switch_time:
self.iter_time = time.time()
self._switch_rotation()
if (self.player.pose.position - self.position).norm < VALID_DISTANCE:
if self.start_time is None:
self.start_time = time.time()
self.iter_time = time.time()
self.ball_collision = True
self.speed = 1
self.offset_orientation += DIFF_ANGLE * self.rotation_sign
self.position = (self.game_state.ball_position -
Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
orientation = self.offset_orientation if time.time() - self.start_time < \
self.rotate_time else self.target_orientation
else:
orientation = self.target_orientation
return CmdBuilder().addMoveTo(Pose(self.position, orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def debug_cmd(self):
if not self.is_debug:
return
angle = None
additional_dbg = []
if self.current_state == self.go_behind_ball:
angle = 18
elif self.current_state == self.grab_ball:
angle = 25
elif self.current_state == self.kick:
angle = 45
additional_dbg = [DebugCommandFactory.circle(self.game_state.ball_position, KICK_DISTANCE, color=RED)]
if angle is not None:
angle *= np.pi/180.0
base_angle = (self.game_state.ball.position - self.target.position).angle
magnitude = 3000
ori = self.game_state.ball.position
upper = ori + Position.from_angle(base_angle + angle, magnitude)
lower = ori + Position.from_angle(base_angle - angle, magnitude)
ball_to_player = self.player.position - self.game_state.ball_position
behind_player = (ball_to_player.norm + 1000) * normalize(ball_to_player) + self.game_state.ball_position
return [DebugCommandFactory.line(ori, upper),
DebugCommandFactory.line(ori, lower),
DebugCommandFactory.line(self.game_state.ball_position, behind_player, color=CYAN)] + additional_dbg
return []
def _go_to_final_position(self):
position = self.game_state.ball_position - Position.from_angle(
self.target_orientation) * DISTANCE_FROM_BALL
return CmdBuilder().addMoveTo(
Pose(position, self.target_orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def next_position(self):
self.target_orientation = (self.target.position -
self.game_state.ball_position).angle
self.position = (
self.game_state.ball_position -
Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
if time.time() - self.start_time >= self.rotate_time:
self.rotation_sign = self._get_direction()
if compare_angle(self.target_orientation,
(self.game_state.ball_position -
self.player.position).angle,
VALID_DIFF_ANGLE):
self.next_state = self.halt
return self._go_to_final_position()
elif time.time() - self.iter_time >= self.switch_time:
self.iter_time = time.time()
self._switch_rotation()
if (self.player.pose.position - self.position).norm < VALID_DISTANCE:
if self.start_time is None:
self.start_time = time.time()
self.iter_time = time.time()
self.ball_collision = True
self.speed = 1
self.offset_orientation += DIFF_ANGLE * self.rotation_sign
self.position = (self.game_state.ball_position -
Position.from_angle(self.offset_orientation) *
DISTANCE_FROM_BALL)
if self.start_time is not None:
orientation = self.offset_orientation if time.time() - self.start_time < \
self.rotate_time else self.target_orientation
else:
orientation = self.target_orientation
return CmdBuilder().addMoveTo(
Pose(self.position, orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def _go_to_final_position(self):
position = self.game_state.ball_position - Position.from_angle(self.target_orientation) * DISTANCE_FROM_BALL
return CmdBuilder().addMoveTo(Pose(position, self.target_orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()