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