def visualize_plan(plan, oracle, display=True, save=None):
  if not display and save is None: return
  if save is not None and not os.path.exists(save): os.makedirs(save)

  def execute():
    oracle.unlock()
    oracle.problem.set_viewer(oracle.env)
    oracle.clear_regions()
    oracle.draw_goals()
    set_state(plan.start, oracle)
    open_gripper(oracle) # TODO - do I always want this?
    update_viewer()
    if display: raw_input('Hit enter to finish' if len(plan.operators) == 0 else 'Hit enter to step')
    else: sleep(1) # NOTE - Gives time for the viewer to update
    if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, 0, 0)) # TODO - sometimes captures images before the viewer is ready
    for i, action in enumerate(plan.operators):
      if isinstance(action, StepExecutable):
        for j, _ in enumerate(action.step(oracle)):
          update_viewer()
          if display: raw_input('Hit enter to finish' if i == len(plan.operators) - 1 else 'Hit enter to step')
          else: sleep(1) # Gives time for the viewer to update
          if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, i+1, j)) # TODO - sometimes captures images before the viewer is ready

  if is_viewer_active(oracle.env): execute()
  else: SetViewerUserThread(oracle.env, 'qtcoin', execute)
def visualize_states(states, oracle, display=True, save=None):
  if not display and save is None: return
  if save is not None and not os.path.exists(save): os.makedirs(save)

  def execute():
    oracle.unlock()
    oracle.problem.set_viewer(oracle.env)
    oracle.clear_regions()
    oracle.draw_goals()
    for i, state in enumerate(states):
      set_state(state, oracle)
      oracle.env.UpdatePublishedBodies() # Updates display if env locked
      if display: raw_input('Hit enter to finish' if i == len(states) - 1 else 'Hit enter to step')
      else: sleep(1) # Gives time for the viewer to update
      if save is not None: save_image(oracle.env, (save + 'image%0' + str(len(str(len(states) - 1))) + 'd.jpg') % i) # Sometimes captures images before the viewer is ready

  if is_viewer_active(oracle.env): execute()
  else: SetViewerUserThread(oracle.env, 'qtcoin', execute)
def execute_plan(plan, oracle, pause=True):
  if PRE_SMOOTH_TRAJECTORIES: smooth_actions(plan, oracle)

  def execute():
    oracle.unlock()
    oracle.problem.set_viewer(oracle.env)
    oracle.clear_regions()
    oracle.draw_goals()
    #oracle.env.StartSimulation(time_step, realtime=False) # realtime=False calls simulate step as fast as possible
    set_state(plan.start, oracle)
    raw_input('Hit enter to finish' if len(plan.operators) == 0 else 'Hit enter to start')
    for i, action in enumerate(plan.operators):
      if isinstance(action, Executable):
        action.execute(oracle)
      if i == len(plan.operators) - 1: raw_input('Hit enter to finish')
      elif pause: raw_input('Hit enter to continue')

  if is_viewer_active(oracle.env): execute()
  else: SetViewerUserThread(oracle.env, 'qtcoin', execute)