def _find_path_internal(self) -> None: model: BasicLSTMModule = self._services.resources.model_dir.load(self._load_name) model.init_running_algorithm(self._get_grid()) history_frequency: Dict[Point, int] = {} last_agent_pos: Point = self._get_grid().agent.position stuck_threshold = 5 it = 0 while it < self._max_it: # goal reached if radius intersects if self._get_grid().is_agent_in_goal_radius(): self.move_agent(self._get_grid().goal.position) break next_move: Point = Point.from_tensor(model.forward_features(self._get_grid())) self.move_agent(self._get_grid().apply_move(next_move, self._get_grid().agent.position)) last_agent_pos = self._get_grid().agent.position new_freq: int = history_frequency.get(last_agent_pos, 0) + 1 history_frequency[last_agent_pos] = new_freq # fail safe if new_freq >= stuck_threshold: break it += 1 self.key_frame()
def _get_new_vertex(self, q_near: Vertex, q_sample: Point, max_dist) -> Vertex: dir = q_sample.to_tensor() - q_near.position.to_tensor() if torch.norm(dir) <= max_dist: return Vertex(q_sample) dir_normalized = dir / torch.norm(dir) q_new = Point.from_tensor(q_near.position.to_tensor() + max_dist * dir_normalized) return Vertex(q_new)
def render(self, *discarded) -> None: self._services.graphics.renderer.push_line_thickness(50) features = MapProcessing.extract_features(self._map, [ "distance_to_goal_normalized", "raycast_8_normalized", "direction_to_goal_normalized", "agent_goal_angle" ]) rc = self.ray_colour() for (ray, dir) in zip(features["raycast_8_normalized"], self._map.ALL_POINTS_MOVE_VECTOR): ray = 50.0 * ray - self._map.get_movement_cost(Point(0, 0), dir) dir = dir.to_tensor() / torch.norm(dir.to_tensor()) vec = Point.from_tensor(self._map.agent.position.to_tensor() + ray * dir) self.__render_line(self._map.agent.position, vec, rc) bc = self.bearing_colour() self.__render_line(self._map.agent.position, self._map.goal.position, bc) self.__render_arc(self._map.agent.position, self._map.goal.position, bc) self._services.graphics.renderer.pop_line_thickness()