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_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))