def save_task_follow_traj_inputs(fname, sim_env, task_index, step_index, choice_index, miniseg_index, manip_name,
new_hmats, old_traj):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
step_index = str(step_index)
choice_index = str(choice_index)
miniseg_index = str(miniseg_index)
assert task_index in result_file, "Must call save_task_results_int() before save_task_follow_traj_inputs()"
if step_index not in result_file[task_index]:
result_file[task_index].create_group(step_index)
if 'plan_traj' not in result_file[task_index][step_index]:
result_file[task_index][step_index].create_group('plan_traj')
if choice_index not in result_file[task_index][step_index]['plan_traj']:
result_file[task_index][step_index]['plan_traj'].create_group(choice_index)
if miniseg_index not in result_file[task_index][step_index]['plan_traj'][choice_index]:
result_file[task_index][step_index]['plan_traj'][choice_index].create_group(miniseg_index)
manip_g = result_file[task_index][step_index]['plan_traj'][choice_index][miniseg_index].create_group(manip_name)
manip_g['rope_nodes'] = sim_env.sim.rope.GetControlPoints()
trans, rots = sim_util.get_rope_transforms(sim_env)
manip_g['trans'] = trans
manip_g['rots'] = rots
manip_g['dof_inds'] = sim_env.robot.GetActiveDOFIndices()
manip_g['dof_vals'] = sim_env.robot.GetDOFValues()
manip_g.create_group('new_hmats')
for (i_hmat, new_hmat) in enumerate(new_hmats):
manip_g['new_hmats'][str(i_hmat)] = new_hmat
manip_g['old_traj'] = old_traj
result_file.close()
def replay_on_holdout(args, sim_env):
holdoutfile = h5py.File(args.holdoutfile, 'r')
tasks = eval_util.get_specified_tasks(args.tasks, args.taskfile, args.i_start, args.i_end)
loadresultfile = h5py.File(args.loadresultfile, 'r')
loadresult_items = eval_util.get_holdout_items(loadresultfile, tasks)
num_successes = 0
num_total = 0
for i_task, demo_id_rope_nodes in loadresult_items:
print "task %s" % i_task
sim_util.reset_arms_to_side(sim_env)
redprint("Replace rope")
rope_nodes, rope_params, _, _ = eval_util.load_task_results_init(args.loadresultfile, i_task)
# uncomment if the results file don't have the right rope nodes
#rope_nodes = demo_id_rope_nodes["rope_nodes"][:]
if args.replay_rope_params:
rope_params = args.replay_rope_params
# don't call replace_rope and sim.settle() directly. use time machine interface for deterministic results!
time_machine = sim_util.RopeSimTimeMachine(rope_nodes, sim_env)
if args.animation:
sim_env.viewer.Step()
eval_util.save_task_results_init(args.resultfile, sim_env, i_task, rope_nodes, rope_params)
for i_step in range(len(loadresultfile[i_task]) - (1 if 'init' in loadresultfile[i_task] else 0)):
print "task %s step %i" % (i_task, i_step)
sim_util.reset_arms_to_side(sim_env)
redprint("Observe point cloud")
new_xyz = sim_env.sim.observe_cloud()
eval_stats = eval_util.EvalStats()
best_action, full_trajs, q_values, trans, rots = eval_util.load_task_results_step(args.loadresultfile, sim_env, i_task, i_step)
time_machine.set_checkpoint('depth_0_%i'%i_step, sim_env)
time_machine.restore_from_checkpoint('depth_0_%i'%i_step, sim_env, sim_util.get_rope_params(rope_params))
eval_stats.success, eval_stats.feasible, eval_stats.misgrasp, full_trajs = simulate_demo_traj(sim_env, new_xyz, GlobalVars.actions[best_action], full_trajs, animate=args.animation, interactive=args.interactive)
print "BEST ACTION:", best_action
replay_trans, replay_rots = sim_util.get_rope_transforms(sim_env)
if np.linalg.norm(trans - replay_trans) > 0 or np.linalg.norm(rots - replay_rots) > 0:
yellowprint("The rope transforms of the replay rope doesn't match the ones in the original result file by %f and %f" % (np.linalg.norm(trans - replay_trans), np.linalg.norm(rots - replay_rots)))
else:
yellowprint("Reproducible results OK")
eval_util.save_task_results_step(args.resultfile, sim_env, i_task, i_step, eval_stats, best_action, full_trajs, q_values)
if is_knot(sim_env.sim.rope.GetControlPoints()):
break;
if is_knot(sim_env.sim.rope.GetControlPoints()):
num_successes += 1
num_total += 1
redprint('REPLAY Successes / Total: ' + str(num_successes) + '/' + str(num_total))
def save_task_results_step(fname,
sim_env,
task_index,
step_index,
eval_stats,
best_root_action,
full_trajs,
q_values_root,
floating=False):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
step_index = str(step_index)
assert task_index in result_file, "Must call save_task_results_init() before save_task_results_step()"
if step_index not in result_file[task_index]:
result_file[task_index].create_group(step_index)
result_file[task_index][step_index][
'misgrasp'] = 1 if eval_stats.misgrasp else 0
result_file[task_index][step_index][
'infeasible'] = 1 if not eval_stats.feasible else 0
result_file[task_index][step_index][
'rope_nodes'] = sim_env.sim.rope.GetControlPoints()
trans, rots = sim_util.get_rope_transforms(sim_env)
result_file[task_index][step_index]['trans'] = trans
result_file[task_index][step_index]['rots'] = rots
result_file[task_index][step_index]['best_action'] = str(best_root_action)
if not floating:
full_trajs_g = result_file[task_index][step_index].create_group(
'full_trajs')
for (i_traj, (traj, dof_inds)) in enumerate(full_trajs):
full_traj_g = full_trajs_g.create_group(str(i_traj))
# current version of h5py can't handle empty arrays, so don't save them if they are empty
if np.all(traj.shape):
full_traj_g['traj'] = traj
if len(dof_inds) > 0:
full_traj_g['dof_inds'] = dof_inds
else:
gripper_trajs_g = result_file[task_index][step_index].create_group(
'gripper_trajs')
for (i_traj, gripper_traj) in enumerate(full_trajs):
gripper_traj_g = gripper_trajs_g.create_group(str(i_traj))
# Make numpy array from a list of numpy arrays (hmats)
# (go the other direction by calling list() on the resulting numpy array)
gripper_traj_g['l_hmats'] = np.asarray(gripper_traj['l'])
gripper_traj_g['r_hmats'] = np.asarray(gripper_traj['r'])
result_file[task_index][step_index]['values'] = q_values_root
result_file[task_index][step_index][
'action_time'] = eval_stats.action_elapsed_time
result_file[task_index][step_index][
'exec_time'] = eval_stats.exec_elapsed_time
result_file.close()
def save_task_results_init(fname, sim_env, task_index, rope_nodes, rope_params=None):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
if task_index in result_file:
del result_file[task_index]
result_file.create_group(task_index)
result_file[task_index].create_group('init')
trans, rots = sim_util.get_rope_transforms(sim_env)
result_file[task_index]['init']['rope_nodes'] = rope_nodes
if rope_params:
result_file[task_index]['init']['rope_params'] = rope_params
result_file[task_index]['init']['trans'] = trans
result_file[task_index]['init']['rots'] = rots
result_file.close()
def save_task_results_init(fname,
sim_env,
task_index,
rope_nodes,
rope_params=None):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
if task_index in result_file:
del result_file[task_index]
result_file.create_group(task_index)
result_file[task_index].create_group('init')
trans, rots = sim_util.get_rope_transforms(sim_env)
result_file[task_index]['init']['rope_nodes'] = rope_nodes
if rope_params:
result_file[task_index]['init']['rope_params'] = rope_params
result_file[task_index]['init']['trans'] = trans
result_file[task_index]['init']['rots'] = rots
result_file.close()
def save_task_follow_traj_inputs(fname, sim_env, task_index, step_index,
choice_index, miniseg_index, manip_name,
new_hmats, old_traj):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
step_index = str(step_index)
choice_index = str(choice_index)
miniseg_index = str(miniseg_index)
assert task_index in result_file, "Must call save_task_results_int() before save_task_follow_traj_inputs()"
if step_index not in result_file[task_index]:
result_file[task_index].create_group(step_index)
if 'plan_traj' not in result_file[task_index][step_index]:
result_file[task_index][step_index].create_group('plan_traj')
if choice_index not in result_file[task_index][step_index]['plan_traj']:
result_file[task_index][step_index]['plan_traj'].create_group(
choice_index)
if miniseg_index not in result_file[task_index][step_index]['plan_traj'][
choice_index]:
result_file[task_index][step_index]['plan_traj'][
choice_index].create_group(miniseg_index)
manip_g = result_file[task_index][step_index]['plan_traj'][choice_index][
miniseg_index].create_group(manip_name)
manip_g['rope_nodes'] = sim_env.sim.rope.GetControlPoints()
trans, rots = sim_util.get_rope_transforms(sim_env)
manip_g['trans'] = trans
manip_g['rots'] = rots
manip_g['dof_inds'] = sim_env.robot.GetActiveDOFIndices()
manip_g['dof_vals'] = sim_env.robot.GetDOFValues()
manip_g.create_group('new_hmats')
for (i_hmat, new_hmat) in enumerate(new_hmats):
manip_g['new_hmats'][str(i_hmat)] = new_hmat
manip_g['old_traj'] = old_traj
result_file.close()
def save_task_results_step(fname, sim_env, task_index, step_index, eval_stats, best_root_action, full_trajs, q_values_root, floating=False):
if fname is None:
return
result_file = h5py.File(fname, 'a')
task_index = str(task_index)
step_index = str(step_index)
assert task_index in result_file, "Must call save_task_results_init() before save_task_results_step()"
if step_index not in result_file[task_index]:
result_file[task_index].create_group(step_index)
result_file[task_index][step_index]['misgrasp'] = 1 if eval_stats.misgrasp else 0
result_file[task_index][step_index]['infeasible'] = 1 if not eval_stats.feasible else 0
result_file[task_index][step_index]['rope_nodes'] = sim_env.sim.rope.GetControlPoints()
trans, rots = sim_util.get_rope_transforms(sim_env)
result_file[task_index][step_index]['trans'] = trans
result_file[task_index][step_index]['rots'] = rots
result_file[task_index][step_index]['best_action'] = str(best_root_action)
if not floating:
full_trajs_g = result_file[task_index][step_index].create_group('full_trajs')
for (i_traj, (traj, dof_inds)) in enumerate(full_trajs):
full_traj_g = full_trajs_g.create_group(str(i_traj))
# current version of h5py can't handle empty arrays, so don't save them if they are empty
if np.all(traj.shape):
full_traj_g['traj'] = traj
if len(dof_inds) > 0:
full_traj_g['dof_inds'] = dof_inds
else:
gripper_trajs_g = result_file[task_index][step_index].create_group('gripper_trajs')
for (i_traj, gripper_traj) in enumerate(full_trajs):
gripper_traj_g = gripper_trajs_g.create_group(str(i_traj))
# Make numpy array from a list of numpy arrays (hmats)
# (go the other direction by calling list() on the resulting numpy array)
gripper_traj_g['l_hmats'] = np.asarray(gripper_traj['l'])
gripper_traj_g['r_hmats'] = np.asarray(gripper_traj['r'])
result_file[task_index][step_index]['values'] = q_values_root
result_file[task_index][step_index]['action_time'] = eval_stats.action_elapsed_time
result_file[task_index][step_index]['exec_time'] = eval_stats.exec_elapsed_time
result_file.close()
def replay_on_holdout(args, sim_env):
holdoutfile = h5py.File(args.holdoutfile, 'r')
tasks = eval_util.get_specified_tasks(args.tasks, args.taskfile,
args.i_start, args.i_end)
loadresultfile = h5py.File(args.loadresultfile, 'r')
loadresult_items = eval_util.get_holdout_items(loadresultfile, tasks)
num_successes = 0
num_total = 0
for i_task, demo_id_rope_nodes in loadresult_items:
print "task %s" % i_task
sim_util.reset_arms_to_side(sim_env)
redprint("Replace rope")
rope_nodes, rope_params, _, _ = eval_util.load_task_results_init(
args.loadresultfile, i_task)
# uncomment if the results file don't have the right rope nodes
#rope_nodes = demo_id_rope_nodes["rope_nodes"][:]
if args.replay_rope_params:
rope_params = args.replay_rope_params
# don't call replace_rope and sim.settle() directly. use time machine interface for deterministic results!
time_machine = sim_util.RopeSimTimeMachine(rope_nodes, sim_env)
if args.animation:
sim_env.viewer.Step()
eval_util.save_task_results_init(args.resultfile, sim_env, i_task,
rope_nodes, rope_params)
for i_step in range(
len(loadresultfile[i_task]) -
(1 if 'init' in loadresultfile[i_task] else 0)):
print "task %s step %i" % (i_task, i_step)
sim_util.reset_arms_to_side(sim_env)
redprint("Observe point cloud")
new_xyz = sim_env.sim.observe_cloud()
eval_stats = eval_util.EvalStats()
best_action, full_trajs, q_values, trans, rots = eval_util.load_task_results_step(
args.loadresultfile, sim_env, i_task, i_step)
time_machine.set_checkpoint('depth_0_%i' % i_step, sim_env)
time_machine.restore_from_checkpoint(
'depth_0_%i' % i_step, sim_env,
sim_util.get_rope_params(rope_params))
eval_stats.success, eval_stats.feasible, eval_stats.misgrasp, full_trajs = simulate_demo_traj(
sim_env,
new_xyz,
GlobalVars.actions[best_action],
full_trajs,
animate=args.animation,
interactive=args.interactive)
print "BEST ACTION:", best_action
replay_trans, replay_rots = sim_util.get_rope_transforms(sim_env)
if np.linalg.norm(trans - replay_trans) > 0 or np.linalg.norm(
rots - replay_rots) > 0:
yellowprint(
"The rope transforms of the replay rope doesn't match the ones in the original result file by %f and %f"
% (np.linalg.norm(trans - replay_trans),
np.linalg.norm(rots - replay_rots)))
else:
yellowprint("Reproducible results OK")
eval_util.save_task_results_step(args.resultfile, sim_env, i_task,
i_step, eval_stats, best_action,
full_trajs, q_values)
if is_knot(sim_env.sim.rope.GetControlPoints()):
break
if is_knot(sim_env.sim.rope.GetControlPoints()):
num_successes += 1
num_total += 1
redprint('REPLAY Successes / Total: ' + str(num_successes) + '/' +
str(num_total))
