def parse_data(test_trajectories_file, panda_scene_manager):
results = {}
test_trajectories_by_path_id = deserialize_uncompress(
test_trajectories_file)
for path_id in test_trajectories_by_path_id:
is_successful = test_trajectories_by_path_id[path_id][1]
endpoints = test_trajectories_by_path_id[path_id][0]
non_virtual_endpoints = [
PandaGameSubgoal.virtual_to_real_state(e, panda_scene_manager)
for e in endpoints
]
start = non_virtual_endpoints[0]
goal = non_virtual_endpoints[-1]
results[path_id] = (start, goal, is_successful)
return results
from path_helper import deserialize_uncompress
file_path = '/home/tom/SGT-PG/data/sgt/disks/test_trajectories/2020_04_19_14_42_28/level1_all.txt'
paths = deserialize_uncompress(file_path)
for path_id in paths:
print(f'path id {path_id}, is successful {paths[path_id][1]}')
print(f'path')
print(paths[path_id][0])
def show_trajectory(endpoints, panda_scene_manager):
# set the robot to the initial location
panda_scene_manager.change_robot_joints(endpoints[0])
panda_scene_manager.set_camera(3.5, angle, -20, [0., 0., 0.])
panda_scene_manager.simulation_step()
# assert the teleportation was successful
assert panda_scene_manager.is_close(endpoints[0])
assert not panda_scene_manager.is_collision()
sum_free, sum_collision = panda_scene_manager.smooth_walk(
endpoints[-1], max_target_distance=1., sensitivity=0.01)
print('sum collision {}'.format(sum_collision))
assert sum_collision > 0.0
test_trajectories_by_path_id = deserialize_uncompress(test_trajectories_file)
panda_scene_manager = PandaSceneManager.get_scene_manager(scenario=scenario,
use_ui=True)
for path_id in path_ids:
is_successful = test_trajectories_by_path_id[path_id][1]
print('path_id {} is successful? {}'.format(path_id, is_successful))
if is_successful:
print('showing trajectory')
endpoints = test_trajectories_by_path_id[path_id][0]
non_virtual_endpoints = [
PandaGameSubgoal.virtual_to_real_state(e, panda_scene_manager)
for e in endpoints
]
output_dir = os.path.join(get_base_directory(), scenario, 'visualizations',
source_trajectories, model_name)
obstacles_definitions_location = os.path.join(get_base_directory(),
'scenario_params', scenario,
'params.pkl')
with open(obstacles_definitions_location, 'rb') as pickle_file:
obstacles_definitions = pickle.load(pickle_file)
for cycle in cycles:
cycle_output_dir = os.path.join(output_dir, cycle)
init_dir(cycle_output_dir)
point_robot_manager = PointRobotManager(obstacles_definitions)
trajectory_file = os.path.join(trajectories_dir, '{}.txt'.format(cycle))
data_by_path = deserialize_uncompress(trajectory_file)
if 0 < num_trajectories < len(data_by_path):
# sample if the number of trajectories is bigger than zero and less than total number of trajectories
data_by_path = {
k: data_by_path[k]
for k in random.sample(data_by_path.keys(), num_trajectories)
}
for path_id in data_by_path:
current_item = data_by_path[path_id]
status = 'success' if current_item[1] else 'collision'
trajectory = current_item[0]
output_file_location = os.path.join(
cycle_output_dir, '{}_{}.png'.format(path_id, status))
point_robot_manager.plot(