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")