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)
