def _reset(self): obs = HuskyNavigateEnv._reset(self) self.cube_image = copy(obs["rgb_filled"]) self.depth = copy(obs["depth"]).squeeze() attempts = 0 #print("Generating target and spawn locations") while attempts < 100: self.target_x = np.random.uniform(self.min_target_x, self.max_target_x) self.target_y = np.random.uniform(self.min_target_y, self.max_target_y) spawn_x = np.random.uniform(self.min_spawn_x, self.max_spawn_x) spawn_y = np.random.uniform(self.min_spawn_y, self.max_spawn_y) distance = np.linalg.norm(np.array([spawn_x, spawn_y]) - np.array([self.target_x, self.target_y])) if distance < self.max_target_distance and distance > self.min_target_distance: break attempts += 1 if attempts == 100: raise Exception("Agent could not be spawned. Try expanding the min and max target range.") self.config["initial_pos"] = [spawn_x, spawn_y, self.default_z] obs["rgb_filled"] = self._add_target_into_image(obs, [self.target_x, self.target_y, self.default_z], color=self.target_color) #print("Generating obstacles") self.reset_cube_locations() x, y = self.config["initial_pos"][0:2] for location in self.cube_locations: obs["rgb_filled"] = self._add_obstacle_into_image(obs, location, color=self.obstacle_color) return obs
def _reset(self): if self.start_locations_file is not None and (not self.fixed_endpoints): new_start_location = self.valid_locations[np.random.randint(self.n_points), :] self.config["initial_pos"] = [new_start_location[0], new_start_location[1], self.default_z] obs = HuskyNavigateEnv._reset(self) self.occupancy_map = np.zeros((self.x_vals.shape[0], self.y_vals.shape[0])) yaw = self.robot.get_rpy()[2] obs["map"] = self.render_map(rotate=True, rotate_angle=yaw, translate=True) return obs
def _reset(self): self.start_location = self.select_agent_location() self.config["initial_pos"] = [self.start_location[0], self.start_location[1], self.default_z] self.target_location = self.select_target() obs = HuskyNavigateEnv._reset(self) obs["target"] = self.calculate_target_observation() if self.render_map: self.map_renderer.clear_nonstatic_layers() self.map_renderer.update_target(*self.target_location) self.map_renderer.update_spawn(*self.start_location) self.map_renderer.update_agent(*self.start_location) obs["map"] = self.map_renderer.render() else: obs["map"] = np.zeros((self.resolution, self.resolution, 3)) return obs
def _reset(self): self.config["initial_pos"] = self.select_new_agent_location() self.target_x, self.target_y = self.select_new_target() print("new agent pos", self.config["initial_pos"]) print("new target", self.target_x, self.target_y) obs = HuskyNavigateEnv._reset(self) self.cube_image = copy(obs["rgb_filled"]) self.depth = copy(obs["depth"]).squeeze() obs["rgb_filled"] = self._add_cube_into_image(obs, [self.target_x, self.target_y, self.default_z]) if self.render_map: self.map_renderer.clear_nonstatic_layers() self.map_renderer.update_target(self.target_x, self.target_y, radius=0.125) self.map_renderer.update_spawn(*self.config["initial_pos"][0:2], radius=0.125) self.map_renderer.update_agent(*self.config["initial_pos"][0:2]) obs["map"] = self.map_renderer.render() else: obs["map"] = np.zeros((self.resolution, self.resolution, 3)) return obs