def run_experiment(oracle, configuration):
if hasattr(oracle, 'crg'): delattr(oracle, 'crg') # TODO - reuse the crg for some experiments
# search is set by configuration? NOt sure what a configuration is
search, successor = configuration
initial, goal = get_representation(oracle, MANIPULATION_PLANNERS.FFROB)
# This one gets the plan from the initial state to the goal
plan, state_space = search(initial, goal, ffrob_generator(initial, goal, oracle, successor()))
if plan is not None:
plan = Plan(plan.cost, plan.length, sum(get_sample_time(operator) for operator in plan.operators))
return Outcome(plan, state_space.time_elapsed(), state_space.iterations, len(state_space),
state_space.num_expanded(), state_space.num_generations(), oracle.ffrob_sample_time, oracle.ffrob_sample_attempts)
def run_experiment(oracle, configuration):
search, control = configuration
initial, goal = get_representation(oracle, MANIPULATION_PLANNERS.HBF)
reachability_graph = lambda state: RelaxedPlanGraph(state, goal,
lambda rg: GeneratingScheduler(rg, [SP(oracle) for SP in USE_SUBPLANNERS]))
generator = lambda vertex: reset_reachability_generator(vertex, oracle, reachability_graph, *control,
only_initial=initial, initial_reset_time=INITIAL_RESET_TIME, reset_time=RESET_TIME)
#generator = lambda vertex: reachability_generator(vertex, reachability_graph(vertex.state), *control)
plan, state_space = resetting_search(oracle, lambda: search(initial, goal, generator), max_time=TIMEOUT)
if plan is not None:
plan = Plan(plan.cost, plan.length, sum(get_sample_time(operator) for operator in plan.operators))
return Outcome(plan, state_space.time_elapsed(), state_space.iterations, len(state_space),
state_space.num_expanded(), state_space.num_generations(), getattr(state_space, 'resets', 0))
def solve(env, problem, planner, search, display, movie, profile, log, debug):
RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug)
problem.set_viewer(env)
problem.load_env(env)
oracle = ManipulationOracle(problem, env)
start, goal = get_representation(oracle, planner)
generator = get_generator(start, goal, oracle, planner)
oracle.draw_goals()
dt = datetime.datetime.now()
planner_name = MANIPULATION_PLANNERS.names[planner].lower()
directory = DIRECTORY + '{}/{}/{}/{}/'.format(problem.name, planner_name, dt.strftime('%Y-%m-%d'), dt.strftime('%H-%M-%S'))
print SEPARATOR + '\nSolving manipulation problem %s using %s'%(problem.name, planner_name)
def execute():
if debug: debug_fn = openrave_debug_fn(oracle)
else: debug_fn = simple_debug
start_time = time()
try:
if search == 'RRT': output = plan_rrt(start, goal, generator, oracle)
else: output = default_plan(start, goal, generator, debug=debug_fn)
except KeyboardInterrupt:
output = None, 'Time: %.3f'%(time() - start_time)
if log or movie:
make_dir(directory)
print 'Created directory:', directory
return output
if profile: (plan, state_space), pr = run_profile(execute)
else: plan, state_space = execute()
data = (str(plan) if plan is not None else 'Infeasible') + '\n\n' + str(state_space)
if plan is not None:
sample_time = sum(get_sample_time(operator) for operator in plan.operators)
data += 'Sample time: %.5f | Sample ratio %.3f%%'%(sample_time,
100*sample_time/state_space.time_elapsed()) # TODO - should really freeze the time
print SEPARATOR
print data
if log:
write(directory + DATA_FILENAME, data)
print '\nWrote', directory + DATA_FILENAME
if plan is not None and display is not None:
print SEPARATOR
images_directory = directory+'images/' if movie else None
if display == 'states':
visualize_states(map(lambda tup: tup[1], plan.plan), oracle, display=True, save=images_directory)
elif display == 'plan':
visualize_plan(plan, oracle, display=True, save=images_directory)
elif display == 'execute':
if movie: launch_quicktime() # TODO - produces missing value if quicktime is already enabled
execute_plan(plan, oracle, pause=False)
else:
print display, ' is not a valid display method'
if profile:
print SEPARATOR, str_profile(pr)
if log:
write_profile(pr, directory + PROFILE_FILENAME)
print '\nWrote', directory + PROFILE_FILENAME
print SEPARATOR,