def faux_dataset_random_pt(eval_envs): print("Generating faux dataset") units = UnrealUnits(scale=1.0) dataset = [] for env_id in eval_envs: segment_dataset = [] config = load_env_config(env_id) template = load_template(env_id) start_pt = np.asarray(config["startPos"]) second_pt = np.asarray(config["startHeading"]) end_x = random.uniform(0, 1000) end_y = random.uniform(0, 1000) end_pt = np.asarray([end_x, end_y]) state1 = DroneState(None, start_pt) state2 = DroneState(None, second_pt) state3 = DroneState(None, end_pt) segment_dataset.append(bare_min_sample(state1, False, env_id)) segment_dataset.append(bare_min_sample(state2, False, env_id)) segment_dataset.append(bare_min_sample(state3, True, env_id)) dataset.append(segment_dataset) return dataset
def faux_dataset_random_landmark(eval_envs): print("Generating faux dataset") units = UnrealUnits(scale=1.0) dataset = [] for env_id in eval_envs: segment_dataset = [] config = load_env_config(env_id) template = load_template(env_id) path = load_path(env_id) landmark_radii = config["radius"] start_pt = np.asarray(config["startPos"]) second_pt = np.asarray(config["startHeading"]) landmark_choice_ids = list(range(len(config["landmarkName"]))) choice_id = random.choice(landmark_choice_ids) target_x = config["xPos"][choice_id] target_y = config["zPos"][choice_id] target_lm_pos = np.asarray([target_x, target_y]) landmark_dir = target_lm_pos - start_pt method = template["side"] theta = math.atan2(landmark_dir[1], landmark_dir[0]) + math.pi if method == "infront": theta = theta elif method == "random": theta = random.random() * 2 * math.pi elif method == "behind": theta = theta + math.pi elif method == "left": theta = theta - math.pi / 2 elif method == "right": theta = theta + math.pi / 2 x, z = target_lm_pos[0], target_lm_pos[1] landmark_radius = landmark_radii[choice_id] sample_point = np.array([ x + math.cos(theta) * landmark_radius, z + math.sin(theta) * landmark_radius ]) state1 = DroneState(None, start_pt) state2 = DroneState(None, second_pt) state3 = DroneState(None, sample_point) segment_dataset.append(bare_min_sample(state1, False, env_id)) segment_dataset.append(bare_min_sample(state2, False, env_id)) segment_dataset.append(bare_min_sample(state3, True, env_id)) dataset.append(segment_dataset) return dataset
def reset(self, seg_idx=0, landmark_pos=None, random_yaw=0): self.rate.reset() self.drone.reset_environment() start_pos, start_angle = self.get_start_pos(seg_idx, landmark_pos) if self.cv_mode: start_rpy = start_angle self.drone.teleport_3d(start_pos, start_rpy, pos_in_airsim=False) else: start_yaw = start_angle if self.params["randomize_init_pos"]: yaw_offset = float( np.random.normal(0, self.params["init_yaw_variance"], 1)) pos_offset = np.random.normal(0, self.params["init_pos_variance"], 2) print("offset:", pos_offset, yaw_offset) start_pos = np.asarray(start_pos) + pos_offset start_yaw = start_yaw + yaw_offset self.drone.teleport_to(start_pos, start_yaw) self.rate.sleep(quiet=True) time.sleep(0.2) drone_state, image = self.drone.get_state() self.done = False return DroneState(image, drone_state)
def reset(self, seg_idx=0, landmark_pos=None, random_yaw=0): try: self.rate.reset() self.drone.reset_environment() self.stepcount = 0 start_pos, start_angle = self.get_start_pos(seg_idx, landmark_pos) if self.cv_mode: start_rpy = start_angle self.drone.teleport_3d(start_pos, start_rpy, pos_in_airsim=False) else: start_yaw = start_angle if self.params["randomize_init_pos"]: np.random.seed() yaw_offset = float(np.random.normal(0, self.params["init_yaw_variance"], 1)) pos_offset = np.random.normal(0, self.params["init_pos_variance"], 2) #print("offset:", pos_offset, yaw_offset) start_pos = np.asarray(start_pos) + pos_offset start_yaw = start_yaw + yaw_offset self.drone.teleport_to(start_pos, start_yaw) if self.params.get("voice"): cmd = self.get_current_nl_command() say(cmd) self.drone.rollout_begin(self.get_current_nl_command()) self.rate.sleep(quiet=True) #time.sleep(0.2) drone_state, image = self.drone.get_state() return DroneState(image, drone_state) except RolloutException as e: raise PomdpInterface.EnvException("Retry rollout")
def step(self, action): """ Takes an action, executes it in the simulation and returns the state, reward and done indicator :param action: array of length 4: [forward velocity, left velocity, yaw rate, stop probability] :return: DroneState object, reward (float), done (bool) """ #Action drone_action = action[0:3] drone_stop = action[3] #print("Action: ", action) if drone_stop > 0.99: drone_action = np.array([0, 0, 0]) self.drone.send_local_velocity_command( unnormalize_action(drone_action)) self.rate.sleep(quiet=True) drone_state, image = self.drone.get_state() state = DroneState(image, drone_state) reward, done = self._get_reward(state, action) return state, reward, done
def observe(self, prev_action): try: drone_state, image = self.drone.get_state() state = DroneState(image, drone_state) out_of_bounds = self._is_out_of_bounds(state) reward = self._get_reward(state, prev_action, out_of_bounds) self.stepcount += 1 expired = self.stepcount > self.max_horizon drone_stop = prev_action[3] done = out_of_bounds or drone_stop > 0.5 or expired # Actually stop the drone (execute stop-action) if done: self.drone.send_local_velocity_command([0, 0, 0, 1]) self.drone.rollout_end() # TODO: Respect the done return state, reward, done, expired, out_of_bounds except RolloutException as e: raise PomdpInterface.EnvException("Retry rollout")