def get_start_pos(self, seg_idx=0, landmark_pos=None):
# If we are not in CV mode, then we have a path to follow and track reward for following it closely
if not self.cv_mode:
if self.instruction_set:
start_pos = self.instruction_set[seg_idx]["start_pos"]
start_angle = self.instruction_set[seg_idx]["start_yaw"]
else:
start_pos = [0, 0, 0]
start_angle = 0
# If we are in CV mode, there is no path to be followed. Instead we are collecting images of landmarks.
# Initialize the drone to face the position provided. TODO: Generalize this to other CV uses
else:
drone_angle = random.uniform(0, 2 * math.pi)
drone_dist_mult = random.uniform(0, 1)
drone_dist = 60 + drone_dist_mult * 300
drone_pos_dir = np.asarray(
[math.cos(drone_angle),
math.sin(drone_angle)])
start_pos = landmark_pos + drone_pos_dir * drone_dist
start_height = random.uniform(-1.5, -2.5)
start_pos = [start_pos[0], start_pos[1], start_height]
drone_dir = -drone_pos_dir
start_yaw = vec_to_yaw(drone_dir)
start_roll = 0
start_pitch = 0
start_angle = [start_roll, start_pitch, start_yaw]
return start_pos, start_angle
def get_affine_matrix(self, path, start_idx, origin, world_scaling_factor):
if start_idx > len(path) - 2:
return None, None
start_pt = path[start_idx]
next_pt = path[start_idx + 1]
dir_vec = next_pt - start_pt
dir_yaw = vec_to_yaw(dir_vec)
if self.yaw_rand_range > 0:
dir_yaw_offset = random.uniform(-self.yaw_rand_range, self.yaw_rand_range)
dir_yaw += dir_yaw_offset
if self.pos_rand_image > 0:
pos_offset = random.uniform(0, self.pos_rand_range)
angle = random.uniform(-np.pi, np.pi)
offset_vec = pos_offset * np.array([np.cos(angle), np.sin(angle)])
start_pt += offset_vec
affine_s = get_affine_scale_2d([world_scaling_factor, world_scaling_factor])
affine_t = get_affine_trans_2d(-start_pt)
affine_rot = get_affine_rot_2d(-dir_yaw)
affine_t2 = get_affine_trans_2d(origin)
#return affine_t
affine_total = np.dot(affine_t2, np.dot(affine_s, np.dot(affine_rot, affine_t)))
out_crop_size = tuple(np.asarray(origin) * 2)
return affine_total, out_crop_size
def make_annotations(end_i):
P.initialize_experiment()
annotations = {"train": [], "test": [], "dev": []}
train_range, dev_range, test_range = get_split_ranges(end_i)
assert (
train_range[1] - train_range[0]
) % NEW_CONFIG_EVERY_N == 0, "training set size must be a multiple of NEW_CONFIG_EVERY_N"
for config_id in range(end_i):
config_path = paths.get_env_config_path(config_id)
path_path = paths.get_curve_path(config_id)
instruction_path = paths.get_instructions_path(config_id)
with open(config_path) as fp:
config = json.load(fp)
with open(path_path) as fp:
curve = json.load(fp)
with open(instruction_path) as fp:
instruction = fp.readline()
token_list = clean_instruction(instruction)
curve_np = np.asarray(list(zip(curve["x_array"], curve["z_array"])))
split = "train" if train_range[0] <= config_id < train_range[1] else \
"dev" if dev_range[0] <= config_id < dev_range[1] else \
"test" if test_range[0] <= config_id < test_range[1] else None
#start_dir = np.asarray(config["startHeading"]) - np.asarray(config["startPos"])
start_dir = curve_np[1] - curve_np[0]
start_yaw = vec_to_yaw(start_dir)
start_yaw_cfg = np.rad2deg(-start_yaw + np.pi / 2)
dataset = {
"id": str(config_id),
"start_z": [curve["z_array"][0]],
"start_x": [curve["x_array"][0]],
"end_z": [curve["z_array"][-1]],
"end_x": [curve["x_array"][-1]],
"start_rot": [start_yaw_cfg],
"config_file": "configs/random_config_%d.json" % config_id,
"instructions_file":
"instructions/instructions_%d.txt" % config_id,
"path_file": "paths/random_curve_%d.json" % config_id,
"moves": [],
"valid": True,
"num_tokens": [len(token_list)],
"instructions": [instruction]
}
annotations[split].append(dataset)
print("Added annotations for env: " + str(config_id))
with open(paths.get_instruction_annotations_path(), "w") as fp:
json.dump(annotations, fp)
def is_goal_visible(self, instr_seg):
end = np.asarray(instr_seg["end_pos"])
start = np.asarray(instr_seg["start_pos"])
vec_start_to_end = end - start
endp_yaw = vec_to_yaw(vec_start_to_end)
start_yaw = instr_seg["start_yaw"]
yaw_diff = endp_yaw - start_yaw
yaw_diff_abs = math.fabs(clip_angle(yaw_diff))
goal_visible = 2 * yaw_diff_abs < math.radians(self.hfov)
return goal_visible
def visualize_twist_as_pose(self, cmd_vel_twist):
last_pos = np.asarray(self.last_drn_pos)
vel_vec = np.asarray([cmd_vel_twist.linear.x, cmd_vel_twist.linear.y, 0])
next_pos = last_pos + vel_vec
yaw = vec_to_yaw(next_pos - last_pos)
yaw_rot = yaw + 0.5* cmd_vel_twist.angular.z
quat = quaternion_from_euler(0, 0, yaw)
quat_rot = quaternion_from_euler(0, 0, yaw_rot)
self.rviz.add_pose_and_publish_array("cmd_history_lin", last_pos, quat)
self.rviz.add_pose_and_publish_array("cmd_history_ang", last_pos, quat_rot)
def get_action_go_to_carrot(self, curr_pos, curr_yaw, carrot_pos):
carrot_dir = carrot_pos - curr_pos
carrot_yaw = vec_to_yaw(carrot_dir)
delta_yaw = carrot_yaw - curr_yaw
delta_yaw = clip_angle(delta_yaw)
vel_x = self.params["vel_x"] / (1 + 2 * math.fabs(delta_yaw) / math.pi)
#vel_x = self.params["VEL_X"]
vel_yaw = self.params["k_yaw"] * delta_yaw
action = np.asarray([vel_x, 0, vel_yaw])
return action
def make_annotations(start_i, end_i):
P.initialize_experiment()
annotations = {
"train": [],
"test": [],
"dev": []
}
for config_id in range(start_i, end_i):
config_path = paths.get_env_config_path(config_id)
path_path = paths.get_curve_path(config_id)
instruction_path = paths.get_instructions_path(config_id)
with open(config_path) as fp:
config = json.load(fp)
with open(path_path) as fp:
curve = json.load(fp)
with open(instruction_path) as fp:
instruction = fp.readline()
token_list = clean_instruction(instruction)
split = get_split((config_id % 100) / 100.0)
start_dir = np.asarray(config["startHeading"]) - np.asarray(config["startPos"])
start_yaw = vec_to_yaw(start_dir)
start_yaw_cfg = np.rad2deg(-start_yaw + np.pi/2)
dataset = {
"id": str(config_id),
"start_z": [curve["z_array"][0]],
"start_x": [curve["x_array"][0]],
"end_z": [curve["z_array"][-1]],
"end_x": [curve["x_array"][-1]],
"start_rot": [start_yaw_cfg],
"config_file": "configs/random_config_%d.json" % config_id,
"instructions_file": "instructions/instructions_%d.txt" % config_id,
"path_file": "paths/random_curve_%d.json" % config_id,
"moves": [],
"valid": True,
"num_tokens": [len(token_list)],
"instructions": [instruction]
}
annotations[split].append(dataset)
print ("Added annotations for env: " + str(config_id))
with open(paths.get_instruction_annotations_path(), "w") as fp:
json.dump(annotations, fp)
def get_action_go_to_carrot(self, curr_pos, curr_yaw, carrot_pos):
#print(f"BCP: curr_pos: {curr_pos} curr_yaw: {curr_yaw} carrot_pos: {carrot_pos}")
carrot_dir = carrot_pos - curr_pos
carrot_yaw = vec_to_yaw(carrot_dir)
delta_yaw = carrot_yaw - curr_yaw
delta_yaw = clip_angle(delta_yaw)
#print(f"BCP: Carrot pos: {carrot_pos}, current pos: {curr_pos}")
#print(f"BCP: Carrot yaw: {carrot_yaw}, current yaw: {curr_yaw}")
vel_x = self.params["vel_x"] / (1 + 2 * math.fabs(delta_yaw) / math.pi)
#vel_x = self.params["VEL_X"]
vel_yaw = self.params["k_yaw"] * delta_yaw
action = np.asarray([vel_x, 0, vel_yaw])
return action
def get_action_go_to_position(self, target_pos, current_pos, current_yaw):
target_pos_drone = pos_to_drone(current_pos, current_yaw, target_pos)
target_heading = target_pos - current_pos
target_yaw = vec_to_yaw(target_heading)
diff_yaw_raw = target_yaw - current_yaw
diff_yaw = clip_angle(diff_yaw_raw)
diff_x = target_pos_drone[0]
vel_x = self.params["vel_x"]
vel_yaw = diff_yaw * self.params["k_yaw"]
action = np.asarray([vel_x, 0, vel_yaw])
action = self.clip_vel(action)
return action
def get_action_go_to_position(self, target_pos, current_pos, current_yaw):
target_pos_drone = pos_to_drone(current_pos, current_yaw, target_pos)
target_heading = target_pos - current_pos
target_yaw = vec_to_yaw(target_heading)
diff_yaw = clip_angle(target_yaw - current_yaw)
diff_x = target_pos_drone[0]
diff_y = target_pos_drone[1]
vel_x = diff_x * self.params["K_X"]
vel_y = diff_y * self.params["K_Y"]
vel_yaw = diff_yaw * self.params["K_Yaw"]
action = np.asarray([vel_x, vel_y, vel_yaw])
action = self.clip_vel(action)
return action
def reduce_speed_for_bendy_paths(self, action, current_pos, current_yaw,
lookahead_pos):
lookahead_dir = lookahead_pos - current_pos
lookahead_dir = lookahead_dir / np.linalg.norm(lookahead_dir)
lookahead_yaw = vec_to_yaw(lookahead_dir)
diff = math.fabs(current_yaw - lookahead_yaw)
diff = clip_angle(diff)
diff = math.fabs(diff)
diff /= 3.14159
diff *= self.params["K_X_Lookahead_Reduction"]
multiplier = 1 - diff
if multiplier < 0.3:
multiplier = 0.3
if multiplier > 1:
multiplier = 1.0
#print("diff:", diff, "m:", multiplier)
action[0] *= multiplier
return action
def get_start_pt_and_yaw(path, start_idx, map_w, map_h, yaw_rand_range):
path_img = cf_to_img(path, np.array([map_w, map_h]))
if start_idx > len(path) - 2:
return None, None
start_pt = path_img[start_idx]
# Due to the way the data is collected, turns result in multiple subsequent points at the same location,
# which messes up the start orientation. That's why we search for the next point that isn't the same as current pt
next_idx = start_idx + 1
next_pt = path_img[next_idx]
while (next_pt == start_pt).all() and next_idx < len(path) - 1:
next_idx += 1
next_pt = path_img[next_idx]
dir_vec = next_pt - start_pt
dir_yaw = vec_to_yaw(dir_vec) - np.pi / 2
if yaw_rand_range > 0:
dir_yaw_offset = random.gauss(0, yaw_rand_range)
#dir_yaw_offset = random.uniform(-yaw_rand_range, yaw_rand_range)
dir_yaw = dir_yaw + dir_yaw_offset
return start_pt, dir_yaw
def reduce_speed_for_turns(self, action, curr_pos, curr_yaw, carrot_pos):
carrot_yaw = vec_to_yaw(carrot_pos - curr_pos)
yaw_diff = clip_angle(carrot_yaw - curr_yaw)
scaling = math.exp(-math.fabs(yaw_diff))
action[0] = action[0] * scaling
return action
