def push_ball(self): # self.debug.add_log(1, "Grab ball called") # self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm)) if (self.last_ball_position - self.player.pose.position).norm < 40: self.next_state = self.halt self.last_time = time.time() elif self._is_player_opposing_ball_and_target(-0.9): self.next_state = self.push_ball else: self.next_state = self.get_behind_ball # self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle)) target = self.target.position player = self.player.pose.position player_to_target = target - player player_to_target = 0.5 * player_to_target / np.linalg.norm(player_to_target) speed_pose = Pose(Position.from_array(player_to_target)) return MoveTo(speed_pose)
def push_ball(self): # self.debug.add_log(1, "Grab ball called") # self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm)) if (self.last_ball_position - self.player.pose.position).norm < 40: self.next_state = self.halt self.last_time = time.time() elif self._is_player_opposing_ball_and_target(-0.9): self.next_state = self.push_ball else: self.next_state = self.get_behind_ball # self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle)) target = self.target.position player = self.player.pose.position player_to_target = target - player player_to_target = 0.5 * player_to_target / np.linalg.norm( player_to_target) speed_pose = Pose(Position.from_array(player_to_target)) return MoveTo(speed_pose)
def test_givenNumpyArray_whenFromArray_thenPositionIsCopy(self): pos = Position.from_array(A_ARRAY) self.assertIsNot(pos, A_ARRAY)
def test_givenNumpyArray_whenFromArray_thenReturnNewPosition(self): pos = Position.from_array(A_ARRAY) self.assertEqual(pos.x, A_ARRAY[0]) self.assertEqual(pos.y, A_ARRAY[1])
def from_array(cls, start: ndarray, target: ndarray) -> 'Path': if start.size != 2 or start.size != 2: raise ValueError('Cannot create a path with less then two points') return cls(Position.from_array(start), Position.from_array(target))