def create_test_set(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, my_rng):
date_time_name = time.strftime("%Y-%m-%d-%H-%M")
print("experiment began at:", date_time_name)
#synth_dataset_client = Synth_Dataset_Client(airsim_client.length_of_simulation, file_manager.test_sets_loc)
files = get_synth_dataset_filenames(file_manager.test_sets_loc, file_manager.anim_num)
gt_poses_file = open( files["f_groundtruth_poses"], "w")
gt_poses_file.write("linecount\tanim_time\n")
camera_pos_file = open(files["f_camera_pos"], "w")
file_manager.init_photo_loc_dir(files["image_main_dir"])
for viewpoint in range(18):
if not os.path.exists(files["image_main_dir"]+"/camera_"+str(viewpoint)):
os.makedirs(files["image_main_dir"]+"/camera_"+str(viewpoint))
file_manager.save_intrinsics(airsim_client.intrinsics, files["f_intrinsics"])
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
anim_time = airsim_client.getAnimationTime()
prev_drone_pose = torch.zeros([3,1])
num_of_loc = len(potential_states_fetcher.potential_trajectory_list)
while airsim_client.linecount < airsim_client.length_of_simulation:
file_manager.save_gt_values_dataset(airsim_client.linecount, anim_time, current_state.bone_pos_gt, gt_poses_file)
for trajectory_ind in range(num_of_loc):
potential_states_fetcher.restart_trajectory()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(trajectory_ind)
set_position(goal_traj, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
take_photo(airsim_client, pose_client, current_state, file_manager, trajectory_ind)
new_drone_pose = current_state.C_drone_gt.clone()
assert not torch.allclose(prev_drone_pose, new_drone_pose)
prev_drone_pose = new_drone_pose.clone()
file_manager.prepare_test_set_gt(current_state, airsim_client.linecount, trajectory_ind, camera_pos_file)
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
update_animation(airsim_client, pose_client, current_state)
anim_time = airsim_client.getAnimationTime()
def save_gt_poses_loop(current_state, pose_client, airsim_client, file_manager):
prev_pose_3d_gt = np.zeros([3, pose_client.num_of_joints])
while airsim_client.linecount < 280:
#update state values read from AirSim and take picture
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
take_photo(airsim_client, pose_client, current_state, file_manager)
assert not np.allclose(prev_pose_3d_gt, current_state.bone_pos_gt)
anim_time = airsim_client.getAnimationTime()
file_manager.write_gt_pose_values(anim_time, current_state.bone_pos_gt)
print(current_state.bone_pos_gt[:,0])
prev_pose_3d_gt = current_state.bone_pos_gt.copy()
start1=time.time()
update_animation(airsim_client, pose_client, current_state, delta_t=0.1)
print("updating animation took", time.time()-start1)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
def try_controller_control_loop(current_state, pose_client, airsim_client, file_manager, potential_states_fetcher, loop_mode):
airsim_client.simPause(False, loop_mode)
airsim_client.moveToZAsync(z=-40, velocity=5, timeout_sec=10).join()
airsim_client.simPause(True, loop_mode)
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
FUTURE_WINDOW_SIZE = 3
pose_client.is_calibrating_energy = True
motion_predictor = potential_states_fetcher.motion_predictor
errors = []
prev_pos = np.zeros((0,3))
x_curr = np.zeros((0,3))
v_curr = np.zeros((0,3))
v_final_arr = np.zeros((0,3))
directions_arr = np.zeros((0,3))
x_actual = np.zeros((0,3))
delta_ts = np.zeros((0))
for ind in range(500):
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
current_drone_pos = np.squeeze(current_state.C_drone_gt.numpy())
current_drone_vel = current_state.current_drone_vel
if ind == 0:
choose_sth_random = True
else:
choose_sth_random = False
x_goal_desired, chosen_dir = generate_new_goal_pos_random(current_drone_pos, motion_predictor.prev_direction, potential_states_fetcher.TOP_SPEED, choose_sth_random)
new_directions = motion_predictor.determine_new_direction(x_goal_desired)
goal_trajectory = Potential_Trajectory(0, FUTURE_WINDOW_SIZE, new_directions)
potential_pos = motion_predictor.predict_potential_positions_func(x_goal_desired, current_drone_pos, current_drone_vel)
actual_pos = np.zeros([FUTURE_WINDOW_SIZE,3])
x_curr = np.concatenate((current_drone_pos[np.newaxis].repeat(3, axis=0), x_curr), axis=0)
v_curr = np.concatenate((current_drone_vel[np.newaxis].repeat(3, axis=0), v_curr), axis=0)
directions_arr = np.concatenate((new_directions, directions_arr), axis=0)
delta_ts = np.concatenate((np.array([0.6, 0.4, 0.2]), delta_ts), axis=0)
potential_states_fetcher.motion_predictor.future_ind=FUTURE_WINDOW_SIZE-1
for i in range(FUTURE_WINDOW_SIZE):
set_position(goal_trajectory, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode)
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
actual_pos[FUTURE_WINDOW_SIZE-i-1, :] = np.squeeze(current_state.C_drone_gt.numpy())
errors.append(np.linalg.norm(actual_pos[FUTURE_WINDOW_SIZE-i-1, :] - potential_pos[FUTURE_WINDOW_SIZE-i-1, :]))
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
x_actual = np.concatenate((actual_pos, x_actual), axis=0)
plot_flight_positions_and_error(file_manager.plot_loc, prev_pos, current_drone_pos, x_goal_desired, potential_pos, actual_pos, airsim_client.linecount, errors, chosen_dir)
prev_pos = np.concatenate((prev_pos, actual_pos), axis=0)
if ind % 100 == 0:
file_manager.save_flight_curves(x_curr, v_curr, directions_arr, delta_ts, x_actual)
def openpose_liftnet_other(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters):
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
num_of_loc = len(potential_states_fetcher.potential_trajectory_list)
len_of_sim = 80
validation_anims = ["28_19", "06_13", "13_06"]
openpose_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 2, 15])
gt_2d_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 2, 15])
lift_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 3, 15])
gt_lift_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 3, 15])
for anim_num in range(len(validation_anims)):
parameters["ANIMATION_NUM"] = validation_anims[anim_num]
file_manager = FileManager(parameters, get_bone_len_file_name(pose_client.modes))
current_state.update_animation_gt_array(file_manager.f_anim_gt_array)
airsim_client.changeAnimation(ANIM_TO_UNREAL[file_manager.anim_num])
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)
prev_drone_pose = torch.zeros([3,1])
airsim_client.linecount = 0
while airsim_client.linecount < len_of_sim and anim_num != 0:
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
anim_time = airsim_client.getAnimationTime()
for trajectory_ind in range(num_of_loc):
# print(" ** traj ind", trajectory_ind)
index = anim_num*len_of_sim*num_of_loc+airsim_client.linecount*num_of_loc+trajectory_ind
#move
potential_states_fetcher.restart_trajectory()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(trajectory_ind)
set_position(goal_traj, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
#update state values read from AirSim and take picture
take_photo(airsim_client, pose_client, current_state, file_manager)
new_drone_pose = current_state.C_drone_gt.clone()
assert not torch.allclose(prev_drone_pose, new_drone_pose)
prev_drone_pose = new_drone_pose.clone()
#find 2d pose (using openpose and gt)
pose_2d_cropped, pose_2d_gt_cropped, heatmap_2d, cropped_image = determine_2d_positions(pose_client=pose_client,
current_state=current_state, my_rng=None,
file_manager=file_manager, linecount=airsim_client.linecount)
openpose_array[index, :, :] = pose_2d_cropped.numpy().copy()
gt_2d_array[index, :, :] = pose_2d_gt_cropped.numpy().copy()
#find relative 3d pose using liftnet
pose_client.modes["mode_lift"] = "lift" #set this back
pose3d_lift_directions = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=None, pose_2d=pose_2d_cropped, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
lift_array[index, :, :] = pose3d_lift_directions.numpy().copy()
pose_client.modes["mode_lift"] = "gt"
#find relative 3d pose using gt
pose3d_lift_directions_gt = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=None, pose_2d=pose_2d_cropped, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
gt_lift_array[index, :, :] = pose3d_lift_directions_gt.numpy().copy()
file_manager.save_openpose_and_gt2d(openpose_array, gt_2d_array)
file_manager.save_lift_and_gtlift(lift_array, gt_lift_array)
update_animation(airsim_client, pose_client, current_state, delta_t=0.2)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
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