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