def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: initial_pose = client.simGetVehiclePose() initial_state = client.getMultirotorState() if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) if initial_state.landed_state == airsim.LandedState.Landed: print("[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() # else: # client.hoverAsync().join() # airsim.LandedState.Flying path = [airsim.Vector3r(*position) for position, _orientation in args.viewpoints] future = client.moveOnPathAsync( path, velocity=2, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=-1.5), # FIXME ) _take_pictures_loop(client) future.join() client.reset() print("[ff] Drone reset")
def fly(client: airsim.MultirotorClient, args) -> None: if args.verbose: print(f"[ff] HomeGeoPoint:\n{client.getHomeGeoPoint()}\n") print(f"[ff] VehiclePose:\n{client.simGetVehiclePose()}\n") #print(f"[ff] MultirotorState:\n{client.getMultirotorState()}\n") home_UE4 = Vec3.from_GeoPoint(client.getHomeGeoPoint()) ground_offset_NED = Vec3.from_Vector3r(client.simGetVehiclePose().position) #assert ground_offset_NED.x == ground_offset_NED.y == 0 # assumes the drone is at PlayerStart #player_start = Vec3.from_Vector3r( # client.simGetObjectPose(client.simListSceneObjects("PlayerStart.*")[0]).position #) print(f"[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() fligh_path = BLOCKS_PATH # TODO add option to change on argparse print(f"[ff] Moving on path...", flush=True) client.moveOnPathAsync(path=[ airsim.Vector3r(coord.x, coord.y, coord.z) for coord in fligh_path ], velocity=5).join() print(f"[ff] Landing", flush=True) client.landAsync().join() # print(f"[ff] Going home", flush=True) # client.goHomeAsync().join() print(f"[ff] Done")
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: if args.verbose: print( f"[ff] HomeGeoPoint: {Vec3.from_GeoPoint(client.getHomeGeoPoint())}\n" ) print( f"[ff] VehiclePose.position: {Vec3.from_Vector3r(client.simGetVehiclePose().position)}\n" ) initial_state = client.getMultirotorState() if initial_state.landed_state == airsim.LandedState.Landed: print(f"[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() else: client.hoverAsync().join() # airsim.LandedState.Flying #__move_on_path(client, args.flight_path, args.flight_velocity) #__move_on_box(client, z=-20, side=20, velocity=args.flight_velocity) if not args.use_viewpoints: future = client.moveOnPathAsync([ airsim.Vector3r(coord.x, coord.y, coord.z) for coord in args.flight_path ], args.flight_velocity) else: import viewpoints future = client.moveOnPathAsync( [airsim.Vector3r(*position) for position in viewpoints.Positions], args.flight_velocity) print(f"[ff] Press [space] to take pictures") ch, img_count, img_responses = msvcrt.getch(), 0, [] while ch == b' ': img_responses.extend( client.simGetImages([ airsim.ImageRequest( ff.CameraName.bottom_center, airsim.ImageType.Scene, False, True # compressed PNG image ) ])) img_count += 1 print(f" {img_count} pictures taken", end="\r") ch = msvcrt.getch() print() print(f"[ff] Waiting for drone to finish path...", end=" ", flush=True) future.join() print(f"done.") for i, response in enumerate(img_responses): airsim.write_file(os.path.join(args.output_folder, f"out_{i}.png"), response.image_data_uint8) time.sleep(4) print(f"[ff] Drone reset") client.reset()
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: initial_pose = client.simGetVehiclePose() initial_state = client.getMultirotorState() if args.verbose: ff.print_pose(initial_pose, to_eularian_angles=airsim.to_eularian_angles) if initial_state.landed_state == airsim.LandedState.Landed: print("[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() else: client.hoverAsync().join() # airsim.LandedState.Flying print("[ff] Flying viewpoints...") print("[ff] Press [space] to take pictures (or any other key to stop)" ) # TODO future = _move_by_path(client, args) img_count = 0 while True: if msvcrt.kbhit(): if msvcrt.getch() != b" ": break # https://stackoverflow.com/a/13207813 response, *_ = client.simGetImages([ airsim.ImageRequest( ff.CameraName.front_center, airsim.ImageType.Scene, False, True, # compressed PNG image ) ]) img_count += 1 print(f" {img_count} pictures taken", end="\r") airsim.write_file( os.path.join(args.output_folder, f"out_{img_count}.png"), response.image_data_uint8, ) # TODO save poses to .log file print("camera_position:", response.camera_position) # Vector3r print("camera_orientation:", response.camera_orientation) # Quaternionr print() print(f"[ff] Waiting for drone to finish path...", end=" ", flush=True) future.join() print("done") time.sleep(4) client.reset() print("[ff] Drone reset")
class MyClient(MultirotorClient): def __init__(self, **kwargs): super(MyClient, self).__init__(**kwargs) self.client = MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True) self.client.armDisarm(True) #self.client.reset() # 실행 시 뒤에 코드가 무시될 수 있어 필요할 때만 주석 해제 self.client.takeoffAsync().join() self.roll = PIDDouble() self.pitch = PIDDouble() # Set PID Gain self.set_pid_gain() #Save Flight Data self.flight_data = pd.DataFrame() self.flight_data['altitude'] = 0 self.now = None # Drone Value self.angle = {'pitch': None, 'roll': None, 'yaw': None} self.rate = {'pitch': None, 'roll': None, 'yaw': None} self.altitude = None self.longitude = None self.latitude = None self.DT = 0.001 self.ccr = {1: None, 2: None, 3: None, 4: None, 'duration': 1} self.motor = {1: 0.60001, 2: 0.60001, 3: 0.6, 4: 0.6, 'duration': 1} def __del__(self): self.now = time.localtime() self.now = time.strftime('%d%H%M%S', self.now) # self.flight_data.to_csv(f'data/simulation_data_{self.now}') def run(self): while 1: start = time.time() # Read Sensor Data # IMU self.read_angle() self.read_rate() # Barometer self.read_altitude() # GPS self.read_gps() # Save Data self.flight_data['altitude'] = self.altitude # PID Control self.Double_Roll_Pitch_PID_Calculation(self.pitch, 0, self.angle['pitch'], self.rate['pitch'], self.DT) self.Double_Roll_Pitch_PID_Calculation(self.roll, 0, self.angle['roll'], self.rate['roll'], self.DT) # PWM self.ccr[ 1] = 84000 + 600 * 84 - self.pitch.IN.pid_result + self.roll.IN.pid_result self.ccr[ 2] = 84000 + 600 * 84 + self.pitch.IN.pid_result + self.roll.IN.pid_result self.ccr[ 3] = 84000 + 600 * 84 + self.pitch.IN.pid_result - self.roll.IN.pid_result self.ccr[ 4] = 84000 + 600 * 84 - self.pitch.IN.pid_result - self.roll.IN.pid_result for i in range(4): if self.ccr[i + 1] < 84000: self.ccr[i + 1] = 84000 elif self.ccr[i + 1] > 168000: self.ccr[i + 1] = 168000 self.ccr[i + 1] = (self.ccr[i + 1] - 84000) / 84000 # Write Motors self.client.moveByMotorPWMsAsync(self.ccr[4], self.ccr[2],\ self.ccr[1], self.ccr[3], self.ccr['duration']) # print('roll : ',self.angle['roll'],' pitch : ', self.angle['pitch'], ' yaw : ', self.angle['yaw']) self.DT = time.time() - start if keyboard.read_key() == 'q': break del self.client def read_angle(self): self.angle['pitch'], self.angle['roll'], self.angle['yaw'] = \ airsim.to_eularian_angles(self.client.getImuData('Imu', 'Drone1').orientation) self.angle['pitch'] *= -1 def read_rate(self): self.rate['roll'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.x_val self.rate['pitch'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.y_val self.rate['yaw'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.z_val self.rate['pitch'] *= -1 def read_altitude(self): self.altitude = self.client.getBarometerData( barometer_name='Barometer', vehicle_name='Drone1').altitude def read_gps(self): self.longitude = self.client.getGpsData( 'Gps', 'Drone1').gnss.geo_point.longitude self.latitude = self.client.getGpsData( 'Gps', 'Drone1').gnss.geo_point.latitude def set_pid_gain(self): self.pitch.IN.kp = 104 self.pitch.IN.ki = 0 self.pitch.IN.kd = 7 self.pitch.OUT.kp = 14 self.pitch.OUT.ki = 0 self.pitch.OUT.kd = 3 self.roll.IN.kp = 104 self.roll.IN.ki = 0 self.roll.IN.kd = 7 self.roll.OUT.kp = 14 self.roll.OUT.ki = 0 self.roll.OUT.kd = 3 def Double_Roll_Pitch_PID_Calculation( self, axis: PIDDouble, set_point_angle: float, angle: float, # BNO080 Rotation Angle rate: float, # ICM-20602 Angular Rate DT: float # loop time ): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# # P calculation axis.OUT.reference = set_point_angle axis.OUT.meas_value = angle axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # I calculation, DT need to be given in every loop axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 2000 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D calculation OUTER_DERIV_FILT_ENABLE = True axis.OUT.error_deriv = -rate # Define derivative of outer loop (rate = ICM-20602 Angular Rate) if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = rate # ICM-20602 angular rate # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop def Single_Yaw_Heading_PID_Calculation( axis: PIDSingle, set_point_angle: float, angle: float, # BNO080 Rotation Angle rate: float, # ICM-20602 Angular Rate DT: float # loop time ): # *********** Single PID Begin (Yaw Angular Position) *************# axis.reference = set_point_angle # Set point of yaw heading @ yaw stick is center. axis.meas_value = angle # Current BNO080_Yaw angle @ yaw stick is center. # P Calculation axis.error = axis.reference - axis.meas_value # Define error of yaw angle control if (axis.error > 180): axis.error -= 360 elif (axis.error < -180): axis.error += 360 axis.p_result = axis.error * axis.kp # Calculate P result of yaw angle control # I Calculation axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw angle control axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw angle control # D Calculation axis.error_deriv = -rate # Define differentiation of yaw angle control axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw angle control axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw angle contro def Single_Yaw_Rate_PID_Calculation( axis: PIDSingle, set_point_rate: float, rate: float, # ICM-20602 Angular Rate DT: float): # *********** Single PID Begin (Yaw Angular Rate Control) *************# axis.reference = set_point_rate # Set point of yaw heading @ yaw stick is not center. axis.meas_value = rate # Current ICM20602.gyro_z @ yaw stick is not center. # P calculation axis.error = axis.reference - axis.meas_value # Define error of yaw rate control axis.p_result = axis.error * axis.kp # Calculate P result of yaw rate control # I calculation axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw rate control axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw rate control # D calculation axis.error_deriv = -( axis.meas_value - axis.meas_value_prev ) / DT # Define differentiation of yaw rate control axis.meas_value_prev = axis.meas_value # Refresh value_prev to the latest value axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw rate control axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw control # *******************************************************************# def Double_Altitude_PID_Calculation(axis: PIDDouble, set_point_altitude: float, current_altitude: float, DT: float): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# axis.OUT.reference = set_point_altitude # Set point of outer PID control axis.OUT.meas_value = current_altitude # Actual Altitude from Fusion # P Calculation axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop # I Calculation axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 500 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D Calculation axis.OUT.error_deriv = -(axis.OUT.meas_value - axis.OUT.meas_value_prev) / DT axis.OUT.meas_value_prev = axis.OUT.meas_value OUTER_DERIV_FILT_ENABLE = True if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = -axis.OUT.error_deriv_filt # ICM-20602 angular rate # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop if axis.IN.pid_result < -2100: axis.IN.pid_result = -2100 if axis.IN.pid_result > 16800: axis.IN.pid_result = 16800 def Double_GPS_PID_Calculation(axis: PIDDouble, set_point_gps: float, gps: float, DT: float): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# axis.OUT.reference = set_point_gps # Set point of outer PID control axis.OUT.meas_value = gps # P calculation axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop # I calculation axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 500 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D calculation axis.OUT.error_deriv = -(axis.OUT.meas_value - axis.OUT.meas_value_prev ) / DT # Define derivative of outer loop axis.OUT.meas_value_prev = axis.OUT.meas_value OUTER_DERIV_FILT_ENABLE = True if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = -axis.OUT.error_deriv # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: if args.z is not None and args.z > 0: print("Warning: AirSim uses NED coordinates, meaning +z is down") print(" (to fly upwards use negative z values)\n") state = client.getMultirotorState() start_pos = { 'NED': get_NED_coords(client, state), 'UE4': get_UE4_coords(client, state) } # obs.: UE4's PlayerStart coordinates correspond to ~(0, 0, 0) in AirSim's NED system coords_format = "({:.2f} {:.2f} {:.2f})" print("Starting position (NED): {}".format(coords_format).format( *start_pos['NED'])) if args.verbose: print("Starting position (UE4): {}".format(coords_format).format( *start_pos['UE4'])) if args.relative: if args.z is None: args.z = 0 xyz = tuple( sum(axis) for axis in zip(start_pos['NED'], (args.x, args.y, args.z))) else: if args.z is None: args.z = start_pos['NED'][2] # keep the same altitude xyz = (args.x, args.y, args.z) if state.landed_state == airsim.LandedState.Landed: print(f"\nlanded_state = {state.landed_state} (Landed)") print("[ff] Taking off... ", end="", flush=True) client.takeoffAsync().join() print("done.") else: # NOTE .exe environments seem to always return Landed print(f"\nlanded_state = {state.landed_state} (Flying)") client.hoverAsync().join() if args.teleport: print("[ff] Teleporting to {}... ".format(coords_format).format(*xyz), end="", flush=True) pose = airsim.Pose() pose.position = airsim.Vector3r(*xyz) client.simSetVehiclePose(pose, ignore_collision=True) time.sleep(4) # wait a few seconds after teleporting else: print("[ff] Moving to {}... ".format(coords_format).format(*xyz), end="", flush=True) client.moveToPositionAsync(*xyz, args.velocity).join() print("done.") print(f"[ff] Hovering for {args.wait_sec} seconds... ", end="", flush=True) client.hoverAsync().join() time.sleep(args.wait_sec) print("done.\n") state = client.getMultirotorState() print("[ff] Ending position (NED): {}".format(coords_format).format( *get_NED_coords(client, state))) if args.verbose: print("[ff] Ending position (UE4): {}".format(coords_format).format( *get_UE4_coords(client, state)))