Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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,