def gen_task_file(args, sim, rotation_angle=0):
"""
draw n_examples states from the initial state distribution defined by
sample_rope_state using random intial states from actionfile
writes results to task file name
TODO: Add rotation angle to available perturbation
"""
taskfile = h5py.File(args.gen_tasks.taskfile, 'w')
actionfile = h5py.File(args.gen_tasks.actionfile, 'r')
try:
for i in range(args.gen_tasks.n_examples):
redprint('Creating State {}/{}'.format(i,
args.gen_tasks.n_examples))
(rope_nodes, demo_id) = sample_rope_state(
actionfile,
sim,
args.animation,
human_check=args.interactive,
perturb_points=args.gen_tasks.n_perturb_pts,
min_rad=args.gen_tasks.min_rad,
max_rad=args.gen_tasks.max_rad)
taskfile.create_group(str(i))
taskfile[str(i)]['rope_nodes'] = rope_nodes
taskfile[str(i)]['demo_id'] = str(demo_id)
taskfile.create_group('args')
taskfile['args']['num_examples'] = args.gen_tasks.n_examples
taskfile['args']['actionfname'] = args.gen_tasks.actionfile
taskfile['args']['perturb_bounds'] = (args.gen_tasks.min_rad,
args.gen_tasks.max_rad)
taskfile['args']['num_perturb_pts'] = args.gen_tasks.n_perturb_pts
taskfile['args']['rotation'] = float(rotation_angle)
print ''
except:
print 'encountered exception', sys.exc_info()
raise
finally:
taskfile.close()
actionfile.close()
assert check_task_file(args.gen_tasks.taskfile, args.gen_tasks.n_examples)
def gen_task_file(args, sim, rotation_angle=0):
"""
draw n_examples states from the initial state distribution defined by
sample_rope_state using random intial states from actionfile
writes results to task file name
TODO: Add rotation angle to available perturbation
"""
taskfile = h5py.File(args.gen_tasks.taskfile, 'w')
actionfile = h5py.File(args.gen_tasks.actionfile, 'r')
try:
for i in range(args.gen_tasks.n_examples):
redprint('Creating State {}/{}'.format(i, args.gen_tasks.n_examples))
(rope_nodes, demo_id) = sample_rope_state(actionfile, sim,
args.animation,
human_check=args.interactive,
perturb_points=args.gen_tasks.n_perturb_pts,
min_rad=args.gen_tasks.min_rad,
max_rad=args.gen_tasks.max_rad)
taskfile.create_group(str(i))
taskfile[str(i)]['rope_nodes'] = rope_nodes
taskfile[str(i)]['demo_id'] = str(demo_id)
taskfile.create_group('args')
taskfile['args']['num_examples'] = args.gen_tasks.n_examples
taskfile['args']['actionfname'] = args.gen_tasks.actionfile
taskfile['args']['perturb_bounds'] = (args.gen_tasks.min_rad,
args.gen_tasks.max_rad)
taskfile['args']['num_perturb_pts'] = args.gen_tasks.n_perturb_pts
taskfile['args']['rotation'] = float(rotation_angle)
print ''
except:
print 'encountered exception', sys.exc_info()
raise
finally:
taskfile.close()
actionfile.close()
assert check_task_file(args.gen_tasks.taskfile, args.gen_tasks.n_examples)
def replay_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim):
loadresultfile = h5py.File(args.replay.loadresultfile, 'r')
loadresult_items = eval_util.get_indexed_items(loadresultfile, task_list=args.tasks, task_file=args.taskfile, i_start=args.i_start, i_end=args.i_end)
num_successes = 0
num_total = 0
for i_task, task_info in loadresult_items:
redprint("task %s" % i_task)
for i_step in range(len(task_info)):
redprint("task %s step %i" % (i_task, i_step))
replay_results = eval_util.load_task_results_step(args.replay.loadresultfile, i_task, i_step)
sim_state = replay_results['sim_state']
if i_step > 0: # sanity check for reproducibility
sim_util.reset_arms_to_side(sim)
if sim.simulation_state_equal(sim_state, sim.get_state()):
yellowprint("Reproducible results OK")
else:
yellowprint("The replayed simulation state doesn't match the one from the result file")
sim.set_state(sim_state)
if args.replay.simulate_traj_steps is not None and i_step not in args.replay.simulate_traj_steps:
continue
if i_step in args.replay.compute_traj_steps: # compute the trajectory in this step
best_root_action = replay_results['best_action']
scene_state = replay_results['scene_state']
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(sim.env.plot3(scene_state.cloud[:,:3], 2, scene_state.color if scene_state.color is not None else (0,0,1)))
sim.viewer.Step()
test_aug_traj = reg_and_traj_transferer.transfer(GlobalVars.demos[best_root_action], scene_state, plotting=args.plotting)
else:
test_aug_traj = replay_results['aug_traj']
feasible, misgrasp = lfd_env.execute_augmented_trajectory(test_aug_traj, step_viewer=args.animation, interactive=args.interactive, check_feasible=args.eval.check_feasible)
if replay_results['knot']:
num_successes += 1
num_total += 1
redprint('REPLAY Successes / Total: ' + str(num_successes) + '/' + str(num_total))
def setup_log_file(args):
if args.log:
redprint("Writing log to file %s" % args.log)
GlobalVars.exec_log = task_execution.ExecutionLog(args.log)
atexit.register(GlobalVars.exec_log.close)
GlobalVars.exec_log(0, "main.args", args)
def setup_log_file(args):
if args.log:
redprint("Writing log to file %s" % args.log)
GlobalVars.exec_log = task_execution.ExecutionLog(args.log)
atexit.register(GlobalVars.exec_log.close)
GlobalVars.exec_log(0, "main.args", args)
def eval_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim):
"""TODO
Args:
action_selection: ActionSelection
reg_and_traj_transferer: RegistrationAndTrajectoryTransferer
lfd_env: LfdEnvironment
sim: DynamicSimulation
"""
holdoutfile = h5py.File(args.eval.holdoutfile, 'r')
holdout_items = eval_util.get_indexed_items(holdoutfile, task_list=args.tasks, task_file=args.taskfile, i_start=args.i_start, i_end=args.i_end)
rope_params = sim_util.RopeParams()
if args.eval.rope_param_radius is not None:
rope_params.radius = args.eval.rope_param_radius
if args.eval.rope_param_angStiffness is not None:
rope_params.angStiffness = args.eval.rope_param_angStiffness
num_successes = 0
num_total = 0
for i_task, demo_id_rope_nodes in holdout_items:
redprint("task %s" % i_task)
init_rope_nodes = demo_id_rope_nodes["rope_nodes"][:]
rope = RopeSimulationObject("rope", init_rope_nodes, rope_params)
sim.add_objects([rope])
sim.settle(step_viewer=args.animation)
for i_step in range(args.eval.num_steps):
redprint("task %s step %i" % (i_task, i_step))
sim_util.reset_arms_to_side(sim)
if args.animation:
sim.viewer.Step()
sim_state = sim.get_state()
sim.set_state(sim_state)
scene_state = lfd_env.observe_scene()
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(sim.env.plot3(scene_state.cloud[:,:3], 2, scene_state.color if scene_state.color is not None else (0,0,1)))
sim.viewer.Step()
eval_stats = eval_util.EvalStats()
start_time = time.time()
if len(scene_state.cloud) == 0:
redprint("Detected 0 points in scene")
break
try:
(agenda, q_values_root), goal_found = action_selection.plan_agenda(scene_state, i_step)
except ValueError: #e.g. if cloud is empty - any action is hopeless
redprint("**Raised Value Error during action selection")
break
eval_stats.action_elapsed_time += time.time() - start_time
eval_stats.generalized = True
num_actions_to_try = MAX_ACTIONS_TO_TRY if args.eval.search_until_feasible else 1
for i_choice in range(num_actions_to_try):
if q_values_root[i_choice] == -np.inf: # none of the demonstrations generalize
eval_stats.generalized = False
break
redprint("TRYING %s"%agenda[i_choice])
best_root_action = str(agenda[i_choice])
start_time = time.time()
try:
test_aug_traj = reg_and_traj_transferer.transfer(GlobalVars.demos[best_root_action], scene_state, plotting=args.plotting)
except ValueError: # If something is cloud/traj is empty or something
redprint("**Raised value error during traj transfer")
break
eval_stats.feasible, eval_stats.misgrasp = lfd_env.execute_augmented_trajectory(test_aug_traj, step_viewer=args.animation, interactive=args.interactive, check_feasible=args.eval.check_feasible)
eval_stats.exec_elapsed_time += time.time() - start_time
if not args.eval.check_feasible or eval_stats.feasible: # try next action if TrajOpt cannot find feasible action and we care about feasibility
break
else:
sim.set_state(sim_state)
knot = is_knot(rope.rope.GetControlPoints())
results = {'scene_state':scene_state, 'best_action':best_root_action, 'values':q_values_root, 'aug_traj':test_aug_traj, 'eval_stats':eval_stats, 'sim_state':sim_state, 'knot':knot, 'goal_found': goal_found}
eval_util.save_task_results_step(args.resultfile, i_task, i_step, results)
if not eval_stats.generalized:
assert not knot
break
if args.eval.check_feasible and not eval_stats.feasible:
# Skip to next knot tie if the action is infeasible -- since
# that means all future steps (up to 5) will have infeasible trajectories
assert not knot
break
if knot:
num_successes += 1
break;
sim.remove_objects([rope])
num_total += 1
redprint('Eval Successes / Total: ' + str(num_successes) + '/' + str(num_total))
redprint('Success Rate: ' + str(float(num_successes)/num_total))
