def run_simulation(kalman_arguments, parameters, energy_parameters, active_parameters):
errors = {}
file_manager = FileManager(parameters)
USE_TRACKBAR = parameters["USE_TRACKBAR"]
simulation_mode = parameters["SIMULATION_MODE"]
length_of_simulation = parameters["LENGTH_OF_SIMULATION"]
loop_mode = parameters["LOOP_MODE"]
#connect to the AirSim simulator
if simulation_mode == "use_airsim":
airsim_client = airsim.MultirotorClient(length_of_simulation)
airsim_client.confirmConnection()
#if loop_mode == "normal":
# airsim_client.enableApiControl(True)
# airsim_client.armDisarm(True)
print('Taking off')
airsim_client.initInitialDronePos()
airsim_client.changeAnimation(ANIM_TO_UNREAL[file_manager.anim_num])
airsim_client.changeCalibrationMode(True)
#if loop_mode == "normal":
# airsim_client.takeoffAsync(timeout_sec = 20).join()
airsim_client.simSetCameraOrientation(str(0), airsim.to_quaternion(CAMERA_PITCH_OFFSET, 0, 0))
time.sleep(2)
elif simulation_mode == "saved_simulation":
airsim_client = DroneFlightClient(length_of_simulation, file_manager.anim_num, file_manager.non_simulation_files)
#file_manager.label_list = airsim_client.label_list
#pause airsim until we set stuff up
airsim_client.simPause(True)
cropping_tool = Crop(loop_mode = loop_mode)
pose_client = PoseEstimationClient(energy_parameters, cropping_tool, animation=file_manager.anim_num, intrinsics_focal=airsim_client.focal_length, intrinsics_px=airsim_client.px, intrinsics_py=airsim_client.py)
current_state = State(use_single_joint=pose_client.USE_SINGLE_JOINT, model_settings=pose_client.model_settings())
potential_states_fetcher = PotentialStatesFetcher(airsim_client, pose_client, active_parameters)
file_manager.save_initial_drone_pos(airsim_client)
#shoulder_vector = initial_positions[R_SHOULDER_IND, :] - initial_positions[L_SHOULDER_IND, :] #find initial human orientation!
#INITIAL_HUMAN_ORIENTATION = np.arctan2(-shoulder_vector[0], shoulder_vector[1]) #in unreal coordinates
if USE_TRACKBAR:
create_trackbars(current_state.radius, active_parameters["Z_POS"])
determine_calibration_mode(airsim_client.linecount, pose_client)
################
if loop_mode == "normal":
#normal_simulation_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager)
teleport_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, loop_mode, parameters)
elif loop_mode == "openpose":
openpose_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager)
elif loop_mode == "teleport":
teleport_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, loop_mode, parameters)
elif loop_mode == "create_dataset":
create_test_set(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager)
################
#calculate errors
airsim_client.simPause(True)
if len(pose_client.error_3d) != 0:
errors["ave_3d_err"] = sum(pose_client.error_3d)/len(pose_client.error_3d)
errors["middle_3d_err"] = sum(pose_client.middle_pose_error)/len(pose_client.middle_pose_error)
else:
errors["ave_3d_err"] = None
errors["middle_3d_err"] = None
simple_plot(pose_client.processing_time, file_manager.estimate_folder_name, "processing_time", plot_title="Processing Time", x_label="Frames", y_label="Time")
if (pose_client.modes["mode_3d"] == 3):
simple_plot(pose_client.error_2d, file_manager.estimate_folder_name, "2D error", plot_title="error_2d", x_label="Frames", y_label="Error")
#simple_plot(pose_client.error_3d[:pose_client.CALIBRATION_LENGTH], file_manager.estimate_folder_name, "3D error", plot_title="calib_error_3d", x_label="Frames", y_label="Error")
#simple_plot(pose_client.error_3d[pose_client.CALIBRATION_LENGTH:], estimate_folder_name, "3D error", plot_title="online_error_3d", x_label="Frames", y_label="Error")
print('End it!')
airsim_client.reset()
pose_client.reset(file_manager.plot_loc)
file_manager.close_files()
return errors
def run_simulation(parameters, energy_parameters, active_parameters):
"""
Description:
Main function that runs simulation
Inputs:
parameters: dict of general parameters
energy_parameters: dict of pose estimation parameters
active_parameters: dict of active mode parameters
Returns:
errors: dict of errors
"""
bone_len_file_name = get_bone_len_file_name(energy_parameters["MODES"])
file_manager = FileManager(parameters, bone_len_file_name)
date_time_name = time.strftime("%Y-%m-%d-%H-%M")
print("experiment began at:", date_time_name)
length_of_simulation = parameters["LENGTH_OF_SIMULATION"]
loop_mode = parameters["LOOP_MODE"]
calibration_mode = parameters["CALIBRATION_MODE"]
port = parameters["PORT"]
simulation_mode = parameters["SIMULATION_MODE"]
experiment_ind = parameters["EXPERIMENT_NUMBER"]
#set random seeds
my_rng = rng_object(parameters["SEED"], file_manager.saved_vals_loc, active_parameters["DRONE_POS_JITTER_NOISE_STD"], energy_parameters["NOISE_3D_INIT_STD"])
#connect to the AirSim simulator
if simulation_mode == "use_airsim":
airsim_client = airsim.MultirotorClient(length_of_simulation, port=port)
airsim_client.confirmConnection()
airsim_client.reset()
if loop_mode == "flight_mode" or loop_mode == "try_controller_control":
airsim_client.enableApiControl(True)
airsim_client.armDisarm(True)
camera_offset_x = 45/100
else:
camera_offset_x = 0
airsim_client.initInitialDronePos()
airsim_client.changeAnimation(ANIM_TO_UNREAL[file_manager.anim_num])
if loop_mode == "flight_mode" or loop_mode == "try_controller_control":
airsim_client.takeoffAsync(timeout_sec = 15).join()
airsim_client.simSetCameraOrientation(str(0), airsim.to_quaternion(0, 0, 0))
elif simulation_mode == "saved_simulation":
camera_offset_x = 0
if file_manager.anim_num == "drone_flight":
airsim_client = DroneFlightClient(length_of_simulation, file_manager.anim_num, file_manager.non_simulation_files)
elif file_manager.anim_num == "mpi_inf_3dhp":
airsim_client = MPI_Dataset_Client(length_of_simulation, file_manager.test_sets_loc, experiment_ind)
else:
airsim_client = Synth_Dataset_Client(length_of_simulation, file_manager.test_sets_loc, file_manager.anim_num, experiment_ind)
file_manager.init_photo_loc_dir(airsim_client.image_main_dir)
#file_manager.label_list = airsim_client.label_list
#pause airsim until we set stuff up
airsim_client.simPause(True, loop_mode)
pose_client = PoseEstimationClient(param=energy_parameters, general_param=parameters,
intrinsics=airsim_client.intrinsics)
current_state = State(active_parameters=active_parameters, model_settings=pose_client.model_settings(), anim_gt_array=file_manager.f_anim_gt_array,
future_window_size=pose_client.FUTURE_WINDOW_SIZE, initial_drone_pos=airsim_client.DRONE_INITIAL_POS,
camera_offset_x = camera_offset_x)
potential_states_fetcher = PotentialStatesFetcher(airsim_client=airsim_client, pose_client=pose_client,
active_parameters=active_parameters, loop_mode=loop_mode)
current_state.init_anim_time(airsim_client.default_initial_anim_time, file_manager.anim_num)
set_animation_to_frame(airsim_client, pose_client, current_state, airsim_client.default_initial_anim_time)
if airsim_client.is_using_airsim and not (loop_mode == "flight_mode" or loop_mode == "try_controller_control"):
move_drone_to_front(airsim_client, pose_client, current_state.radius, current_state.C_cam_torch)
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
time.sleep(0.5)
if not calibration_mode:
pose_client.read_bone_lengths_from_file(file_manager, current_state.bone_pos_gt)
file_manager.save_initial_drone_pos(airsim_client)
################
if loop_mode == "flight_mode" or loop_mode == "teleportation_mode":
general_simulation_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters, my_rng)
elif loop_mode == "openpose":
openpose_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, my_rng)
elif loop_mode == "save_gt_poses":
save_gt_poses_loop(current_state, pose_client, airsim_client, file_manager)
elif loop_mode == "create_dataset":
create_test_set(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, my_rng)
elif loop_mode == "try_controller_control":
try_controller_control_loop(current_state, pose_client, airsim_client, file_manager, potential_states_fetcher, loop_mode)
elif loop_mode == "openpose_liftnet":
openpose_liftnet(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters, my_rng)
################
#calculate errors
airsim_client.simPause(False, loop_mode)
average_errors = pose_client.average_errors
ave_current_error, ave_middle_error, ave_pastmost_error, ave_overall_error = pose_client.average_errors[pose_client.CURRENT_POSE_INDEX], pose_client.average_errors[pose_client.MIDDLE_POSE_INDEX], pose_client.average_errors[pose_client.PASTMOST_POSE_INDEX], pose_client.ave_overall_error
plot_distance_values(current_state.distances_travelled, file_manager.plot_loc)
if calibration_mode:
file_manager.save_bone_lengths(pose_client.boneLengths)
print('Experiment ended!')
pose_client.reset(file_manager.plot_loc)
file_manager.close_files()
return ave_current_error, ave_middle_error, ave_pastmost_error, ave_overall_error
