def get_drone_frame_vector_from_world_frame_vector(self, airsim_quat, vec): import numpy as np q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64) n = np.dot(q, q) if n < np.finfo(float).eps: return airsim.Vector3r(0.0, 1.0, 0.0) q *= np.sqrt(2.0 / n) q = np.outer(q, q) rotation_matrix = np.array([[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0]], [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0]], [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]]]) rotation_matrix = np.linalg.inv(rotation_matrix) world_vec = np.array([vec.x_val, vec.y_val, vec.z_val]) drone_vec = np.matmul(rotation_matrix, world_vec) return airsim.Vector3r(drone_vec[0], drone_vec[1], drone_vec[2])
def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale = 1.0): import numpy as np # convert gate quaternion to rotation matrix # ref: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion; https://www.lfd.uci.edu/~gohlke/code/transformations.py.html q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64) n = np.dot(q, q) if n < np.finfo(float).eps: return airsim.Vector3r(0.0, 1.0, 0.0) q *= np.sqrt(2.0 / n) q = np.outer(q, q) rotation_matrix = np.array([[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0]], [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0]], [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]]]) gate_facing_vector = rotation_matrix[:,1] return airsim.Vector3r(scale * gate_facing_vector[0], scale * gate_facing_vector[1], scale * gate_facing_vector[2])
def takeoff_with_moveOnSpline(self, takeoff_height=2.0): if self.level_name == "ZhangJiaJie_Medium": takeoff_height = 1.0 # time.sleep(1) start_position = self.airsim_client.simGetVehiclePose( vehicle_name=self.drone_name).position start_position.x_val += randf(0, 1) start_position.y_val += randf(-1, 0) # start_position.z_val += randf(0, 1) # start_position.x_val += -2 # start_position.y_val += 2 # start_position.z_val += -1 takeoff_waypoint = airsim.Vector3r( start_position.x_val, start_position.y_val, start_position.z_val - takeoff_height) if (self.plot_transform): self.airsim_client.plot_transform( [airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveToPositionAsync(takeoff_waypoint.x_val, takeoff_waypoint.y_val, takeoff_waypoint.z_val, 1).join()
def takeoff_with_moveOnSpline(self, takeoff_height=0.1): if self.level_name == "ZhangJiaJie_Medium": takeoff_height = -3 # time.sleep(1) start_position = self.airsim_client.simGetVehiclePose( vehicle_name=self.drone_name).position takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, -takeoff_height) if (self.plot_transform): self.airsim_client.plot_transform( [airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync( [takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join()
def get_world_frame_vel_from_drone_frame_vel(self, airsim_quat, velocity): import numpy as np # convert gate quaternion to rotation matrix. # ref: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion; https://www.lfd.uci.edu/~gohlke/code/transformations.py.html q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64) n = np.dot(q, q) if n < np.finfo(float).eps: return airsim.Vector3r(0.0, 1.0, 0.0) q *= np.sqrt(2.0 / n) q = np.outer(q, q) rotation_matrix = np.array([[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0]], [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0]], [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]]]) drone_frame_vel_array = np.array([velocity.x_val, velocity.y_val, velocity.z_val]) world_vel = np.matmul(rotation_matrix, drone_frame_vel_array) return airsim.Vector3r(world_vel[0], world_vel[1], world_vel[2])
def takeoff_with_moveOnSpline(self, takeoff_height = 1.0): start_position = self.airsim_client.simGetVehiclePose(vehicle_name=self.drone_name).position takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val-takeoff_height) self.pass_position_vec = takeoff_waypoint self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name).join()
def takeoff_with_moveOnSpline(self, takeoff_height = 0.1): if self.level_name == "ZhangJiaJie_Medium": takeoff_height = 0.3 start_position = self.airsim_client.simGetVehiclePose(vehicle_name=self.drone_name).position takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, -takeoff_height) self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name).join()
def takeoff_with_moveOnSpline(self, takeoff_height = 0.1): if self.level_name == "ZhangJiaJie_Medium": takeoff_height = 0.3 takeoff_waypoint = airsim.Vector3r(0, 0, -takeoff_height) if(self.plot_transform): self.airsim_client.plot_transform([airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join()
def convex_combination(vec1, vec2, eta): ''' 0 <= eta <= 1, indicating how close it is to the vec2 e.g. eta = 1, vec_result = vec2 eta = 0, vec_result = vec1 ''' vec_result = airsim.Vector3r(0, 0, 0) vec_result.x_val = (1 - eta) * vec1.x_val + eta * vec2.x_val vec_result.y_val = (1 - eta) * vec1.y_val + eta * vec2.y_val vec_result.z_val = (1 - eta) * vec1.z_val + eta * vec2.z_val return vec_result
def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale = 1.0): import numpy as np q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64, copy=True) n = np.dot(q, q) if n < np.finfo(float).eps: return np.identity(4) q *= np.sqrt(2.0 / n) q = np.outer(q, q) rotation_matrix = np.array([ [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], [ 0.0, 0.0, 0.0, 1.0]]) gate_facing_vector = rotation_matrix[:-1,1] return airsim.Vector3r(scale * gate_facing_vector[0], scale * gate_facing_vector[1], scale * gate_facing_vector[2])
def __init__(self, drone_name, drone_params, use_vel_constraints=False, race_tier=1, viz_traj=True): super().__init__(drone_name=drone_name) self.drone_name = drone_name self.drone_params = drone_params self.use_vel_constraints = use_vel_constraints self.viz_traj = viz_traj self.gates_complete = -1 self.logPath = "C:/Users/shubh/Documents/AirSim/AirSimExe/Saved/Logs/RaceLogs" self.race_tier = race_tier self.action_high = 1 self.action_low = -1 self.max_vel = drone_params["v_max"] self.observation_space = [33, None] self.action_space = [4, None] self.reward = 0 self.prev_gate = -1 drone_state = self.airsim_client.getMultirotorState(self.drone_name) self.time = drone_state.timestamp self.prev_time = drone_state.timestamp self.last_gate_change_time = drone_state.timestamp start_position = drone_state.kinematics_estimated.position self.cur_waypt = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val - 1.0) self.disq = False self.scene_ls = self.airsim_client.simListSceneObjects() self.scene_poses = [] for obj in self.scene_ls: self.scene_poses.append( self.airsim_client.simGetObjectPose( obj).position.to_numpy_array()) self.scene_poses = np.array(self.scene_poses)
def reset_race(self): self.airsim_client.simResetRace() time.sleep(1) self.gates_complete = -1 self.reward = 0 self.disq = False self.prev_gate = -1 drone_state = self.airsim_client.getMultirotorState(self.drone_name) self.time = drone_state.timestamp self.prev_time = drone_state.timestamp self.last_gate_change_time = drone_state.timestamp start_position = drone_state.kinematics_estimated.position self.cur_waypt = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val - 1.0) self.start_race(self.race_tier) self.takeoff_with_moveOnSpline() time.sleep(1) return self.state_to_feature()
def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py baseline_racer = BaselineRacer(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) if args.level_name == "Final_Tier_1": args.race_tier = 1 if args.level_name == "Final_Tier_2": args.race_tier = 2 if args.level_name == "Final_Tier_3": args.race_tier = 3 baseline_racer.start_race(args.race_tier) baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() baseline_racer.get_ground_truth_gate_poses() baseline_racer.start_image_callback_thread() baseline_racer.start_odometry_callback_thread() time.sleep(1) start_drone_heading_vec = baseline_racer.get_world_frame_vel_from_drone_frame_vel( baseline_racer.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(1, 0, 0)) baseline_racer.airsim_client.moveByVelocityAsync( start_drone_heading_vec.x_val, start_drone_heading_vec.y_val, start_drone_heading_vec.z_val, duration=0.5).join() baseline_racer.start_control_thread() if (baseline_racer.next_gate_idx == 21 and baseline_racer.isPassGate()): print('TERMINATE PROGRAM') baseline_racer.stop_image_callback_thread() baseline_racer.stop_control_thread() baseline_racer.reset_race()
kp_vel_z=0.4, kd_vel_z=0.0, kp_yaw=3.0, kd_yaw=0.1) client.setTrajectoryTrackerGains(traj_tracker_gains, vehicle_name=drone_name) time.sleep(0.2) # Start race client.simStartRace(tier=tier) # Take off # client.takeoffAsync().join() # client.reset() start_position = client.simGetVehiclePose(vehicle_name=drone_name).position # # print(start_position) takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val - takeoff_height) client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=viz_traj, viz_traj_color_rgba=viz_traj_color_rgba, vehicle_name=drone_name).join() print(client.simGetLastGatePassed(drone_name)) # Gates gate_names_sorted_bad = sorted(client.simListSceneObjects("Gate.*")) gate_indices_bad = [
def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py pygame.init() done = False pygame.joystick.init() baseline_racer = BaselineRacer(drone_name="drone_1", viz_traj=args.viz_traj, viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) baseline_racer.start_race(args.race_tier) baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() baseline_racer.get_ground_truth_gate_poses() baseline_racer.start_image_callback_thread() # baseline_racer.start_odometry_callback_thread() # baseline_racer.airsim_client.moveByVelocityAsync(0, 1, 0, duration = 10).join() duration = 0.05 gain = 5 yaw_gain = 50 while not done: pygame.event.get() joystick = pygame.joystick.Joystick(0) joystick.init() # -1 ~ 1 value # -1 : # x_val = -joystick.get_axis(1) # y_val = joystick.get_axis(0) # z_val = joystick.get_axis(4) # yaw_val = joystick.get_axis(3) # x_val = -1.2*joystick.get_axis(4) # y_val = joystick.get_axis(3) # z_val = 1.3*joystick.get_axis(1) # yaw_val = 2*joystick.get_axis(0) x_val = -1.2*joystick.get_axis(1) y_val = joystick.get_axis(3) z_val = 1.3*joystick.get_axis(4) yaw_val = 2*joystick.get_axis(0) reset_button = joystick.get_button(5) quit_button = joystick.get_button(4) # print(x_val, y_val, z_val, yaw_val) drone_vel_vec = airsim.Vector3r(x_val, y_val, z_val) world_vel_vec = baseline_racer.get_world_frame_vel_from_drone_frame_vel(baseline_racer.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").orientation, drone_vel_vec) # print(world_vel_vec.x_val, world_vel_vec.y_val, world_vel_vec.z_val) drone_pos = baseline_racer.airsim_client.simGetVehiclePose("drone_1") print(drone_pos) baseline_racer.airsim_client.moveByVelocityAsync(world_vel_vec.x_val*gain, world_vel_vec.y_val*gain, world_vel_vec.z_val*gain, duration = duration, yaw_mode=airsim.YawMode(True,yaw_val*yaw_gain)).join() # baseline_racer.airsim_client.moveByAngleRatesThrottleAsync(0, 0, yaw_val*gain, 0, duration = duration).join() if(quit_button): done = True if(reset_button): baseline_racer.reset_race() baseline_racer.start_race() baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() baseline_racer.get_ground_truth_gate_poses() pygame.quit() pygame.init() pygame.joystick.init() baseline_racer.stop_image_callback_thread() # baseline_racer.stop_odometry_callback_thread() baseline_racer.reset_race() pygame.quit()
def to_airsim_vector(np_arr): assert np.size(np_arr) == 3 return airsim.Vector3r(np.float(np_arr[0]), np.float(np_arr[1]), np.float(np_arr[2]))
def control_callback(self): Dist = 3.2 if (self.next_gate_idx < 3): velocity_x_o = 6 Dist = -2 elif (self.next_gate_idx == 3): velocity_x_o = 6 Dist = 1 elif(self.next_gate_idx == 4): # the small gate velocity_x_o = 5.4 Dist = 1 elif(self.next_gate_idx == 5): velocity_x_o = 5.5 Dist = -1 elif(self.next_gate_idx == 6): # the wall velocity_x_o = 8 Dist = -2 elif(self.next_gate_idx == 7 or self.next_gate_idx == 8): # behind the tree velocity_x_o = 7 Dist = -3 elif(self.next_gate_idx == 9): # before the long one velocity_x_o = 7 Dist = 1 # elif(self.next_gate_idx == 10): # velocity_x_o = 7 # Dist = 1 elif(self.next_gate_idx == 12): # the small gate before the big gate velocity_x_o = 6 Dist = 1 elif(self.next_gate_idx == 13): # the big gate velocity_x_o = 12 Dist = -3 elif(self.next_gate_idx == 18): velocity_x_o = 3.5 elif (self.next_gate_idx == 19): Dist = 3.5 velocity_x_o = 3 elif(self.next_gate_idx == 20): Dist = -2 velocity_x_o = 6 else: velocity_x_o = 6 # 3.8 duration = 0.05 last_gate_idx = self.airsim_client.simGetLastGatePassed("drone_1") if(last_gate_idx < 30): if(last_gate_idx == self.next_gate_idx): self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position self.next_gate_idx = last_gate_idx + 1 print(str(self.next_gate_idx) + " PASS") elif(self.next_gate_idx == 13 and self.isPassGate()): print("using isPassGate()") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position print(str(self.next_gate_idx) + " PASS") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(13, 0, 0)) # self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val - 3, duration = 1).join() elif(self.next_gate_idx == 17 and self.isPassGate()): print("using isPassGate() gate 17: Go ahead") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position print(str(self.next_gate_idx) + " PASS") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(6, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 1).join() elif(self.next_gate_idx == 20 and self.isPassGate()): print("using isPassGate()") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position print(str(self.next_gate_idx) + " PASS") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(2, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 2).join() elif(self.next_gate_idx == 19 and self.isPassGate()): print("using isPassGate()") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position print(str(self.next_gate_idx) + " PASS") elif(self.next_gate_idx == 5 and self.isPassGate()): print("using isPassGate()") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position print(str(self.next_gate_idx) + " PASS") elif(self.next_gate_idx == 7 and self.gate6_go_ahead == False): print("gate 6: go ahead") self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(12, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val + 1.2, duration = 2).join() self.gate6_go_ahead = True print(str(self.next_gate_idx) + " PASS") if self.detect_flag : with self.img_mutex: # velocity_x_o = ((1-self.vision_lam) * velocity_x_o + self.vision_lam * self.prev_vel) #print("detection control") ####################model###################### ## box_width : real_gate_width = u : distance_y ## distance_y = 1.5 * u / box_width self.curr_time = time.time() param_A = 13.401 param_B = -1.976 self.estimate_depth = param_A * np.exp(param_B * self.W) #############Control Gain param################ velocity_x = velocity_x_o + (self.estimate_depth - Dist)*0.187 - abs(self.distance_y_curr) * 0.2 #- abs(self.distance_z_curr) * 0.1 Vely_P_Gain = velocity_x*0.62 # 0.47 is good when vel 3.8 #0.9 is good when vel = 3 #0.43 Vely_D_Gain = velocity_x*0.17 Velz_P_Gain = velocity_x*0.62 # 0.49 is good when vel 3.8 #0.9 is good when vel = 3 #0.43 Velz_D_Gain = velocity_x*0.09 Yaw_P_Gain = velocity_x*0.7 #1.5 is good when vel = 3 Yaw_D_Gain = 0.07 ##################Get Error#################### Dead_zone = 0.025 if(self.next_gate_idx == 20): self.distance_y_curr = (1.5 * (self.Mx - 0.50) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.40) / self.H) if abs((self.Mx - 0.50)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.42) ) < Dead_zone: self.distance_z_curr = 0 elif(self.next_gate_idx == 19): self.distance_y_curr = (1.5 * (self.Mx - 0.80) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.42) / self.H) if abs((self.Mx - 0.80)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.42) ) < Dead_zone: self.distance_z_curr = 0 elif(self.next_gate_idx == 4): # the small gate self.distance_y_curr = (1.5 * (self.Mx - 0.55) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.50) / self.H) if abs((self.Mx - 0.55)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.50) ) < Dead_zone: self.distance_z_curr = 0 elif(self.next_gate_idx == 13): # the big gate self.distance_y_curr = (1.5 * (self.Mx - 0.50) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.7) / self.H) if abs((self.Mx - 0.50)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.7) ) < Dead_zone: self.distance_z_curr = 0 elif(self.next_gate_idx == 15): # the big curve after the big gate self.distance_y_curr = (1.5 * (self.Mx - 0.55) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.50) / self.H) if abs((self.Mx - 0.55)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.50) ) < Dead_zone: self.distance_z_curr = 0 else: self.distance_y_curr = (1.5 * (self.Mx - 0.5) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.48) / self.H) if abs((self.Mx - 0.50)) < Dead_zone: self.distance_y_curr = 0 if abs((self.My - 0.48) ) < Dead_zone: self.distance_z_curr = 0 self.desired_yaw_curr = math.atan2(self.distance_y_curr, self.estimate_depth) *57.2859 # print(self.desired_yaw_curr) if self.chk_first_flag: cmd_vel_x = velocity_x#velocity_x*(1+0.1*(self.estimate_depth-6)) cmd_vel_y = self.distance_y_curr*Vely_P_Gain cmd_vel_z = self.distance_z_curr*Velz_P_Gain cmd_yaw = self.desired_yaw_curr*Yaw_P_Gain self.chk_first_flag = False else: dt = self.curr_time - self.prev_time df_distance_y = (self.distance_y_curr - self.distance_y_prev)/dt df_distance_z = (self.distance_z_curr - self.distance_z_prev)/dt df_yaw = (self.desired_yaw_curr - self.desired_yaw_prev)/dt #print("[df_y, df] | ", df_distance_y, dt) cmd_vel_x = velocity_x#velocity_x*(1+0.1*(self.estimate_depth-6)) cmd_vel_y = self.distance_y_curr*Vely_P_Gain + df_distance_y*Vely_D_Gain cmd_vel_z = self.distance_z_curr*Velz_P_Gain + df_distance_z*Velz_D_Gain cmd_yaw = self.desired_yaw_curr*Yaw_P_Gain + df_yaw*Yaw_D_Gain velocity_mag = math.sqrt(cmd_vel_x**2 + cmd_vel_y**2 + cmd_vel_z**2) velocity_gain = ((1-self.vision_lam) * velocity_mag + self.vision_lam * self.prev_vel) / velocity_mag cmd_vel_x *= velocity_gain cmd_vel_y *= velocity_gain cmd_vel_z *= velocity_gain velocity_vector_drone = airsim.Vector3r(cmd_vel_x, cmd_vel_y, cmd_vel_z) v_pose = self.airsim_client.simGetVehiclePose(vehicle_name="drone_1") velocity_vector_world = self.get_world_frame_vel_from_drone_frame_vel(v_pose.orientation, velocity_vector_drone) ####################Do Control################# # print(" ", cmd_yaw) self.airsim_client.moveByVelocityAsync(velocity_vector_world.x_val, velocity_vector_world.y_val, velocity_vector_world.z_val, duration = duration, yaw_mode=airsim.YawMode(True,cmd_yaw)).join() #print("[E_Depth Y_vel Z_vel Yaw velocity_x] : ", self.estimate_depth, " | ", cmd_vel_y, " | ", cmd_vel_z, " | ", desired_yaw_curr, " | ", velocity_x) # if(self.next_gate_idx > 18): # print("[Depth disY desiredYaw] : ", self.estimate_depth, " | ", self.distance_y_curr, " | ", self.desired_yaw_curr) # #print("[E_Depth] : ", self.estimate_depth) ################update variables############### self.prev_time = self.curr_time self.distance_y_prev = self.distance_y_curr self.distance_z_prev = self.distance_z_curr self.desired_yaw_prev = self.desired_yaw_curr self.position_control_on = False self.prev_vel = math.sqrt(velocity_vector_world.x_val ** 2 + velocity_vector_world.y_val ** 2 + velocity_vector_world.z_val ** 2) elif(self.next_gate_idx < len(self.gate_poses_ground_truth)): ################################################ # get approximate yaw vector ############################################## gate_pose = self.gate_poses_ground_truth[self.next_gate_idx].position gate_ori = self.gate_poses_ground_truth[self.next_gate_idx].orientation # next_gate_pose = self.gate_poses_ground_truth[self.next_gate_idx + 1].position # next_gate_ori = self.gate_poses_ground_truth[self.next_gate_idx +1].orientation drone_pose = self.airsim_client.simGetVehiclePose("drone_1").position drone_ori = self.airsim_client.simGetVehiclePose("drone_1").orientation error_pose_vec = gate_pose - drone_pose # print(error_pose_vec) error_pose_vec_in_drone = self.get_drone_frame_vector_from_world_frame_vector(drone_ori, error_pose_vec) mag_error_pose_vec = math.sqrt(error_pose_vec_in_drone.x_val ** 2 + error_pose_vec_in_drone.y_val ** 2) angle = math.acos(error_pose_vec_in_drone.x_val / mag_error_pose_vec)*57.2859 if (error_pose_vec_in_drone.y_val < 0): angle = -angle ################################################ # get approximate yaw vector ############################################## gate_pose_array = [gate_pose.x_val, gate_pose.y_val, gate_pose.z_val] drone_pose_array = [drone_pose.x_val, drone_pose.y_val, drone_pose.z_val] # next_gate_pose_array = [next_gate_pose.x_val, next_gate_pose.y_val, next_gate_pose.z_val] target_lambda_pose = self.get_lam_point(gate_pose_array, drone_pose_array, self.lam) target_lambda_z_pose = self.get_lam_point(gate_pose_array, drone_pose_array, self.lam_z) ## ground z val = 3.5 if(target_lambda_z_pose[2] > -1): target_lambda_z_pose[2] = -1 target_pose = gate_pose_array if(target_pose[2] > -1): target_pose[2] = -1 # target_lambda_pose = gate_pose_array # next_target_pose = next_gate_pose_array # vel_error = self.pass_position_vec - airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2]) vel_error = airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2]) - drone_pose vel_mag = math.sqrt(vel_error.x_val ** 2 + vel_error.y_val ** 2 + vel_error.z_val ** 2) vel_fun = 0.80 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 ######################################################### drone_vel_for_start = airsim.Vector3r(6,0,0) world_vel_for_start = self.get_world_frame_vel_from_drone_frame_vel(drone_ori, drone_vel_for_start) ######################################################### # print(drone_pose.z_val) if(self.next_gate_idx == 7): # the gate behind the tree vel_fun = 0.75 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) # elif(self.next_gate_idx == 10): # vel_fun = 0.3 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 # self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 0): # start gate vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], self.pass_position_vec.z_val - 1, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 4): vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1.5, target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 8 or self.next_gate_idx == 9): vel_fun = 0.8 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2] + 1.5, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 13): vel_fun = 0.7 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 14): vel_fun = 0.8 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1, target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 15): vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1, target_pose[2] + 5, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) elif(self.next_gate_idx == 20): vel_fun = 0.5 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543 self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2] + 3, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) else: self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle)) self.prev_vel = vel_fun
# 'z_val': 1.6281375885009766}, # 'linear_velocity': <Vector3r> { 'x_val': -6.9823994636535645, # 'y_val': 4.4047698974609375, # 'z_val': -0.12542438507080078}, # 'orientation': <Quaternionr> { 'w_val': 0.9962534308433533, # 'x_val': -0.01565200462937355, # 'y_val': -0.020513707771897316, # 'z_val': -0.08254285156726837}, # 'position': <Vector3r> { 'x_val': -17.122417449951172, # 'y_val': 45.57592010498047, # 'z_val': -47.230995178222656}} planner = planning.RapidPlanner(0,19.72,6.28,0.05,0.05) state = asim.KinematicsState() state.position = asim.Vector3r(77.43971252, -96.87151337, -5.48000002) # state.linear_velocity = asim.Vector3r(7.5843505859375, 0.7305274605751038, 6.088780403137207) # state.linear_acceleration = asim.Vector3r(-2.003018379211426, -0.30256304144859314, -5.39830207824707) # state.orientation = asim.Quaternionr(-0.012809118255972862, 0.05772315710783005, -0.03406976908445358,0.9976689219474792) # state.position.x_val = 6.373129367828369 # state.position.y_val = 81.43741607666016 # state.position.z_val = -42.87995529174805 # path = planner.getShortestPath(state, numpy.array( [ 6.373129367828369 , 81.43741607666016 , -43.87995529174805])) path = planner.getShortestPath(state, numpy.array([-111.93122864, 120.21295929 , -46.08000031 , -0.64279248 , 0.76604036,0.])) # path2 = planner.getExtendedPath(numpy.array([ 0, -1 , -20])) # raw_path = numpy.concatenate((path, path2)) # path2 = planner.getExtendedPath(numpy.array([ 10.388415, 80.77406, -43.579998])) # path3 = planner.getExtendedPath(numpy.array([ 18.110466 ,76.26078, -43.579998]))
def control_callback(self): if (self.next_gate_idx < 10): velocity_x_o = 6 elif (self.next_gate_idx == 8): velocity_x_o = 5.5 elif (self.next_gate_idx == 20): velocity_x_o = 7 elif (self.next_gate_idx < 12): velocity_x_o = 7 elif (self.next_gate_idx == 21): velocity_x_o = 10 elif (self.next_gate_idx == 18): velocity_x_o = 7 else: velocity_x_o = 9 duration = 0.05 velocity_x_o -= 1 if (self.isPassGate()): self.go_ahead_flag = False ########### TaeYeon ############ if (self.next_gate_idx == 9): print("pass gate next gate idx 9: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(8, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=1).join() if (self.next_gate_idx == 10): print("pass gate next gate idx 10: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(4, 2, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=1).join() if (self.next_gate_idx == 14): print("pass gate next gate idx 14: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(8, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=1).join() if (self.next_gate_idx == 16): print("pass gate next gate idx 16: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(3, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=0.5).join() if (self.next_gate_idx == 17): print("pass gate next gate idx 17: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(8, 8, 2)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=3).join() if (self.next_gate_idx == 18): print("pass gate next gate idx 18: go ahead") drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(4, -3, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=1).join() if (self.next_gate_idx == 19): drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(6, 0, 0)) self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=1).join() # if(self.next_gate_idx == 20): # drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(10, -4, -10)) # self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 2.5).join() if (self.detect_flag and (self.prev_vel_mag < 10 or self.next_gate_idx == 9 or self.next_gate_idx == 10)): with self.img_mutex: # velocity_x_o = ((1-self.vision_lam) * velocity_x_o + self.vision_lam * self.prev_vel) #print("detection control") ####################model###################### ## box_width : real_gate_width = u : distance_y ## distance_y = 1.5 * u / box_width self.curr_time = time.time() param_A = 6.834360049 param_B = -2.999596062 self.estimate_depth = param_A * np.exp(param_B * self.Area) #############Control Gain param################ Dist = 10 velocity_x = velocity_x_o + ( self.estimate_depth - Dist) * 0.18 - abs( self.distance_y_curr) * 0.3 - abs( self.distance_z_curr) * 0.3 #0.35 Vely_P_Gain = velocity_x * 0.55 #0.7 Vely_D_Gain = velocity_x * 0.2 Velz_P_Gain = velocity_x * 0.62 Velz_D_Gain = velocity_x * 0.09 Yaw_P_Gain = velocity_x * 0.55 #1.5 is good when vel = 3 Yaw_D_Gain = 0.07 ##################Get Error#################### self.distance_y_curr = (1.5 * (self.Mx - 0.5) / self.W) self.distance_z_curr = (1.5 * (self.My - 0.48) / self.H) self.desired_yaw_curr = math.atan2( self.distance_y_curr, self.estimate_depth) * 57.2859 if self.chk_first_flag: cmd_vel_x = velocity_x #velocity_x*(1+0.1*(self.estimate_depth-6)) cmd_vel_y = self.distance_y_curr * Vely_P_Gain cmd_vel_z = self.distance_z_curr * Velz_P_Gain cmd_yaw = self.desired_yaw_curr * Yaw_P_Gain self.chk_first_flag = False else: dt = self.curr_time - self.prev_time df_distance_y = (self.distance_y_curr - self.distance_y_prev) / dt df_distance_z = (self.distance_z_curr - self.distance_z_prev) / dt df_yaw = (self.desired_yaw_curr - self.desired_yaw_prev) / dt #print("[df_y, df] | ", df_distance_y, dt) cmd_vel_x = velocity_x #velocity_x*(1+0.1*(self.estimate_depth-6)) cmd_vel_y = self.distance_y_curr * Vely_P_Gain + df_distance_y * Vely_D_Gain cmd_vel_z = self.distance_z_curr * Velz_P_Gain + df_distance_z * Velz_D_Gain cmd_yaw = self.desired_yaw_curr * Yaw_P_Gain + df_yaw * Yaw_D_Gain velocity_mag = math.sqrt(cmd_vel_x**2 + cmd_vel_y**2 + cmd_vel_z**2) velocity_gain = ( (1 - self.vision_lam) * velocity_mag + self.vision_lam * self.prev_vel) / velocity_mag cmd_vel_x *= velocity_gain cmd_vel_y *= velocity_gain cmd_vel_z *= velocity_gain if (self.next_gate_idx == 21): print("last gate detect control") velocity_vector_drone = airsim.Vector3r( cmd_vel_z, cmd_vel_y, cmd_vel_x) else: velocity_vector_drone = airsim.Vector3r( cmd_vel_x, cmd_vel_y, cmd_vel_z) v_pose = self.airsim_client.simGetVehiclePose( vehicle_name="drone_1") velocity_vector_world = self.get_world_frame_vel_from_drone_frame_vel( v_pose.orientation, velocity_vector_drone) ####################Do Control################# self.airsim_client.moveByVelocityAsync( velocity_vector_world.x_val, velocity_vector_world.y_val, velocity_vector_world.z_val, duration=duration, yaw_mode=airsim.YawMode(True, cmd_yaw)).join() #print("[E_Depth Y_vel Z_vel Yaw velocity_x] : ", self.estimate_depth, " | ", cmd_vel_y, " | ", cmd_vel_z, " | ", desired_yaw_curr, " | ", velocity_x) #print("[E_Depth] : ", self.estimate_depth) ################update variables############### self.prev_time = self.curr_time self.distance_y_prev = self.distance_y_curr self.distance_z_prev = self.distance_z_curr self.desired_yaw_prev = self.desired_yaw_curr self.position_control_on = False self.prev_vel = math.sqrt(velocity_vector_world.x_val**2 + velocity_vector_world.y_val**2 + velocity_vector_world.z_val**2) elif (self.next_gate_idx < len(self.gate_poses_ground_truth)): ################################################ # get approximate yaw vector ############################################## gate_pose = self.gate_poses_ground_truth[ self.next_gate_idx].position gate_ori = self.gate_poses_ground_truth[ self.next_gate_idx].orientation drone_pose = self.airsim_client.simGetVehiclePose( "drone_1").position drone_ori = self.airsim_client.simGetVehiclePose( "drone_1").orientation error_pose_vec = gate_pose - drone_pose error_pose_vec_in_drone = self.get_drone_frame_vector_from_world_frame_vector( drone_ori, error_pose_vec) mag_error_pose_vec = math.sqrt(error_pose_vec_in_drone.x_val**2 + error_pose_vec_in_drone.y_val**2) angle = math.acos( error_pose_vec_in_drone.x_val / mag_error_pose_vec) * 57.2859 if (error_pose_vec_in_drone.y_val < 0): angle = -angle ################################################ # get approximate yaw vector ############################################## gate_pose_array = [ gate_pose.x_val, gate_pose.y_val, gate_pose.z_val ] drone_pose_array = [ drone_pose.x_val, drone_pose.y_val, drone_pose.z_val ] if (self.next_gate_idx > 17): target_lambda_pose = self.get_lam_point( gate_pose_array, drone_pose_array, 0.7) elif (self.next_gate_idx > 11): target_lambda_pose = self.get_lam_point( gate_pose_array, drone_pose_array, 0.56) elif (self.next_gate_idx == 20): target_lambda_pose = self.get_lam_point( gate_pose_array, drone_pose_array, 0.25) else: target_lambda_pose = self.get_lam_point( gate_pose_array, drone_pose_array, self.lam) target_lambda_z_pose = self.get_lam_point(gate_pose_array, drone_pose_array, self.lam_z) # else: # target_lambda_pose = self.get_lam_point(gate_pose_array, [self.pass_position_vec.x_val, self.pass_position_vec.y_val, self.pass_position_vec.z_val], self.lam) # target_lambda_z_pose = self.get_lam_point(gate_pose_array, [self.pass_position_vec.x_val, self.pass_position_vec.y_val, self.pass_position_vec.z_val], self.lam_z) # target_lambda_z_pose = [0 ,0 ,gate_pose_array[2]] # if(self.next_gate_idx < 11): # vel_error = self.pass_position_vec - airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2]) # vel_mag = math.sqrt(vel_error.x_val ** 2 + vel_error.y_val ** 2 + vel_error.z_val ** 2) # else: vel_error = airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2]) - drone_pose vel_mag = math.sqrt(vel_error.x_val**2 + vel_error.y_val**2 + vel_error.z_val**2) # vel_fun = 0.54 * vel_mag + (-0.084) * (abs(angle) / vel_mag) #0.543 vel_fun = 0.5 * vel_mag + (-0.084) * (abs(angle) / vel_mag) if (vel_mag < 2): self.go_ahead_flag = True if (self.next_gate_idx == 0): drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(3, 0, 0)) self.airsim_client.moveByVelocityAsync( drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration=self.duration_move_ahead).join() self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity=vel_fun, yaw_mode=airsim.YawMode( True, angle)) elif (self.next_gate_idx == 5): self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], gate_pose_array[2] + 10, velocity=vel_fun) elif (2 < self.next_gate_idx < 7): self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], gate_pose_array[2] + 10, velocity=vel_fun, yaw_mode=airsim.YawMode( True, angle)) elif (self.next_gate_idx < 5): self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity=vel_fun, yaw_mode=airsim.YawMode( True, angle)) elif (self.next_gate_idx == 9): pass elif (self.next_gate_idx == 10): start_drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(6, 0, 0)) self.airsim_client.moveByVelocityAsync( start_drone_heading_vec.x_val, start_drone_heading_vec.y_val, start_drone_heading_vec.z_val, duration=0.4).join() elif (self.next_gate_idx == 21): vel_error = airsim.Vector3r( target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2] + 20) - drone_pose vel_mag = math.sqrt(vel_error.x_val**2 + vel_error.y_val**2 + vel_error.z_val**2) # vel_fun = 0.3 * vel_mag + (-0.084) * (abs(angle) / vel_mag) #0.543 vel_fun = 8 self.airsim_client.moveToPositionAsync( target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2] + 10, velocity=vel_fun, yaw_mode=airsim.YawMode(True, angle)) else: if (self.go_ahead_flag == False): if (self.next_gate_idx > 10): self.airsim_client.moveToPositionAsync( target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity=vel_fun * 1.2, yaw_mode=airsim.YawMode(True, angle)) elif (self.next_gate_idx == 20): self.airsim_client.moveToPositionAsync( target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2] - 10, velocity=vel_fun * 1.3, yaw_mode=airsim.YawMode(True, angle)) else: self.airsim_client.moveToPositionAsync( target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity=vel_fun, yaw_mode=airsim.YawMode(True, angle)) else: start_drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel( self.airsim_client.simGetVehiclePose( "drone_1").orientation, airsim.Vector3r(3, 0, 0)) self.airsim_client.moveByVelocityAsync( start_drone_heading_vec.x_val, start_drone_heading_vec.y_val, start_drone_heading_vec.z_val, duration=0.5).join() self.prev_vel = vel_fun self.prev_vel_mag = vel_mag
def rotate_vector(q, v): v_quat = v.to_Quaternionr() v_rotated_ = q * v_quat * q.inverse() return airsim.Vector3r(x_val=v_rotated_.x_val, y_val=v_rotated_.y_val, z_val=v_rotated_.z_val)
from scipy.interpolate import CubicSpline, CubicHermiteSpline import airsimneurips as airsim import cvxpy as cp import numpy as np import time gate_dimensions = [1.6, 1.6] gate_facing_vector = airsim.Vector3r(x_val=0, y_val=1, z_val=0) def rotate_vector(q, v): v_quat = v.to_Quaternionr() v_rotated_ = q * v_quat * q.inverse() return airsim.Vector3r(x_val=v_rotated_.x_val, y_val=v_rotated_.y_val, z_val=v_rotated_.z_val) class SplinedTrack: """This class represents a Track defined by Gates. A spline is fitted through the Gates with tangential constraints. This spline is then sampled at 2048 points. """ def __init__(self, gate_poses): self.gates = gate_poses self.n_gates = np.size(gate_poses, 0) positions = np.array( [pose.position.to_numpy_array() for pose in gate_poses]) dists = np.linalg.norm(positions[1:, :] - positions[:-1, :], axis=1) self.arc_length = np.zeros(shape=self.n_gates)
def run(self): self.get_ground_truth_gate_poses( ) # Tier 2 gives noisy prior of gate poses mu_1_airsimvector = self.gate_poses_ground_truth[0].position mu_1 = np.reshape( np.array([ mu_1_airsimvector.x_val, mu_1_airsimvector.y_val, mu_1_airsimvector.z_val ]), (3, 1)) sigma_1 = 50 * np.identity(3) sigma_2 = 50 * np.identity(3) while self.airsim_client.isApiControlEnabled( vehicle_name=self.drone_name): # Take image response = self.airsim_client.simGetImages([ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ]) img1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) image_rgb = img1d.reshape(response[0].height, response[0].width, 3) # Get quad state when the image was taken state = self.airsim_client.simGetVehiclePose() rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame( state) # Get gate mask mask1 = cv2.inRange(image_rgb, self.lower_green1, self.upper_green1) mask2 = cv2.inRange(image_rgb, self.lower_green2, self.upper_green2) mask3 = cv2.inRange(image_rgb, self.lower_green3, self.upper_green3) mask4 = cv2.inRange(image_rgb, self.lower_green4, self.upper_green4) mask = mask1 + mask2 + mask3 + mask4 dilated_gate = cv2.dilate(mask, self.kernel, iterations=8) gate_contours = None gate_contours, ____ = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cv2.imshow("mask", mask) cv2.imshow("dilated_gate", dilated_gate) cv2.waitKey(1) gate_corners = None smallest_measurement_error = 100 # find four gate corners that give the smallest error between the gate center and mean # check each contour for contour in gate_contours: gate_contour_epsilon = 0.01 * cv2.arcLength(contour, True) gate_contour_approx = cv2.approxPolyDP(contour, gate_contour_epsilon, True) gate_corners = cv2.convexHull(gate_contour_approx) gate_corner_combinations = [None] gate_corner_combinations = list(combinations(gate_corners, 4)) # check for each combination of four gate corner points within each contour for comb in gate_corner_combinations: four_gate_corners = np.empty((0, 2)) for i in comb: i = np.reshape(i, (1, 2)) four_gate_corners = np.append(four_gate_corners, i, axis=0) measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot = self.find_measurement_error( four_gate_corners, mu_1, state, rot_matrix_quad2global) aspect_ratio_too_large = self.find_aspect_ratio( four_gate_corners) if measurement_error < smallest_measurement_error and aspect_ratio_too_large == False: waypoint_1_best = copy.deepcopy(waypoint_glob_1) waypoint_2_best = copy.deepcopy(waypoint_glob_2) smallest_measurement_error = copy.deepcopy( measurement_error) gate_center_pixel_best = copy.deepcopy( gate_center_pixel) gate_corners_plot_best = copy.deepcopy( gate_corners_plot) next_gate_position_dist = abs( np.linalg.norm( np.reshape( np.array([ state.position.x_val, state.position.y_val, state.position.z_val ]), (3, 1)) - mu_1)) self.moveonspline_count += 1 # If the smallest error if large, then continue toward the best estimate of the gate location if smallest_measurement_error > 5.0: self.no_gate_count += 1 print("Gate NOT detected") if next_gate_position_dist > 30.0: vmax = 10.0 amax = 3.0 no_gate_count_max = 15 else: vmax = 5.0 amax = 3.0 no_gate_count_max = 3 if self.no_gate_count == no_gate_count_max and next_gate_position_dist >= self.largest_next_gate_position_dist: self.no_gate_count = 0 self.too_close_count = 0 print("Move toward best estimate", next_gate_position_dist) self.airsim_client.moveOnSplineAsync( [airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2, 0])], vel_max=vmax, acc_max=amax, add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=True, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=False, replan_lookahead_sec=1.0) # else, input the measurement to the Kalman Filter and proceed to the updated estimate else: self.no_gate_count = 0 self.measurement_count += 1 if self.measurement_count == 1: mu_2 = np.reshape(waypoint_2_best, (3, 1)) #initialize mu_2 # filter both waypoints mu_1, sigma_1 = self.kalman_filter( mu_1, sigma_1, np.reshape(waypoint_1_best, (3, 1))) mu_2, sigma_2 = self.kalman_filter( mu_2, sigma_2, np.reshape(waypoint_2_best, (3, 1))) waypoint_1 = airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2, 0]) waypoint_2 = airsim.Vector3r(mu_2[0, 0], mu_2[1, 0], mu_2[2, 0]) if next_gate_position_dist > self.largest_next_gate_position_dist: print("Gate detected, Measurement Taken") print(self.measurement_count) self.too_close_count = 0 #cv2.circle(image_rgb, (int(gate_center_pixel_best[0]), int(gate_center_pixel_best[1])), 10, (255, 0, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[0][0]), int(gate_corners_plot_best[0][1])), 10, (255, 100, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[1][0]), int(gate_corners_plot_best[1][1])), 10, (255, 0, 100), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[2][0]), int(gate_corners_plot_best[2][1])), 10, (255, 200, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[3][0]), int(gate_corners_plot_best[3][1])), 10, (255, 0, 200), -1) cv2.imshow("image_rgb", image_rgb) cv2.waitKey(1) if self.moveonspline_count >= 5: if self.next_gate_idx >= len( self.gate_poses_ground_truth) - 1: self.next_gate_idx = -1 self.airsim_client.moveOnSplineAsync( [ waypoint_1, waypoint_2, self.gate_poses_ground_truth[self.next_gate_idx + 1].position ], vel_max=7.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=True, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=False, replan_lookahead_sec=1.0) self.moveonspline_count = 0 # If too close to the gate don't collect measurements, continue to waypoint, reset filter with new mean and covariance if next_gate_position_dist < self.largest_next_gate_position_dist: print("Gate too close") time.sleep(2) self.no_gate_count = 0 self.too_close_count += 1 if self.too_close_count == 1: print("reset gate too close") self.next_gate_idx += 1 if self.next_gate_idx >= len(self.gate_poses_ground_truth): self.next_gate_idx = 0 print("race end") mu_1_airsimvector = self.gate_poses_ground_truth[ self.next_gate_idx].position mu_1 = np.reshape( np.array([ mu_1_airsimvector.x_val, mu_1_airsimvector.y_val, mu_1_airsimvector.z_val ]), (3, 1)) sigma_1 = 50 * np.identity(3) sigma_2 = 50 * np.identity(3) self.measurement_count = 0