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: execute_viewer(oracle.env, execute)
def solve_deterministic(env, hand='left', no_window=True): test = test0 # test0 | testBusy exp, goal = test() print SEPARATOR test = PlanTest(exp, noWindow=no_window) test.load() # TODO - do I need to pass regions here? #test_obj = test.realWorld.objectConfs.keys()[0] #test.realWorld.getObjectPose() #print 'Initial collision:', test.realWorld.checkRobotCollision(test.realWorld.robotConf, test.realWorld.objectShapes[test_obj]) print 'Initial collisions:', world_check_collisions(test.realWorld) manip_problem = real_manipulation_problem(env, test.realWorld, goal, hand) #manip_problem.set_viewer(env) oracle = ManipulationOracle(manip_problem, env, active_arms=[hand], reset_robot=False) stream_problem = compile_problem(oracle) oracle.draw_goals( ) # NOTE - this must be after compile_problem to ensure the goal_poses convert if is_viewer_active(oracle.env): raw_input('Start?') search_fn = get_fast_downward( 'eager', verbose=False ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = focused_planner(stream_problem, search=search_fn, greedy=False, stream_cost=10, verbose=False, debug=False) #plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False) if plan is None: return print plan if no_window: visualize_plan(Plan( convert_state(oracle, stream_problem.initial_atoms), executable_plan(oracle, plan)), oracle, display=True, save=None)
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: execute_viewer(oracle.env, 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: execute_viewer(oracle.env, execute)
def solve_tamp(env, mproblem, display, incremental, focused, movie): RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug) mproblem.set_viewer(env) mproblem.load_env(env) oracle = ManipulationOracle(mproblem, env) stream_problem = compile_problem(oracle) print stream_problem oracle.draw_goals() # NOTE - this must be after compile_problem to ensure the goal_poses convert if is_viewer_active(oracle.env): raw_input('Start?') search_fn = get_fast_downward('eager', verbose=False) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy verbose = False debug = False profile = False assert incremental != focused planner = INCREMENTAL if incremental else FOCUSED #planner = 'hierarchy' # 'hierarchy' | 'serial' t0 = time() def solve(): if planner == INCREMENTAL: return incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, verbose=verbose, debug=debug) # 1 | 20 | 100 | INF elif planner == FOCUSED: return focused_planner(stream_problem, search=search_fn, greedy=False, stream_cost=10, verbose=verbose, debug=debug) elif planner == 'serial': return goal_serialization(stream_problem, verbose=verbose, debug=debug) elif planner == 'hierarchy': selector = first_selector # first_selector | all_selector return replan_hierarchy(stream_problem, selector=selector, first_only=False, execute=True, verbose=verbose) else: raise ValueError(planner) if profile: from misc.profiling import run_profile (plan, universe), prof = run_profile(solve) else: (plan, universe), prof = solve(), None if prof is not None: from misc.profiling import str_profile print SEPARATOR print str_profile(prof) universe.print_statistics() #print_plan_stats(plan, universe) print SEPARATOR print 'Planner:', planner print 'Search:', FAST_DOWNWARD print 'Solved:', plan is not None print 'Length:', len(plan) if plan is not None else None print 'Time:', time() - t0 if plan is not None: print 'Plan:' #, convert_plan(plan) for i, (action, args) in enumerate(convert_plan(plan)): print '%s) %s%s'%(i, action, tuple(args)) if plan is None: return #if is_viewer_active(oracle.env): # raw_input('Done!') #return #trace_plan(stream_problem, plan) #print get_raw_states(stream_problem, plan) images_directory = os.path.join(get_directory_path(mproblem.name, os.path.basename(__file__)), 'images') if movie else None if display: #display_solution(oracle, plan, directory_path, 'plan', movie) #states = map(lambda s: convert_state(oracle, s), get_states(pddl_problem, plan)) #visualize_states(states, oracle, display=True, save=images_directory) start = convert_state(oracle, stream_problem.initial_atoms) executable = executable_plan(oracle, plan) visualize_plan(Plan(start, executable), oracle, display=True, save=images_directory)
def solve_constraint_tamp(env, manip_problem, display, focused, movie, execute = True, verbose=False): #RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug) manip_problem.set_viewer(env) manip_problem.load_env(env) oracle = ManipulationOracle(manip_problem, env) fts_problem = tamp_problem(oracle) print print fts_problem stream_problem = constraint_to_stripstream(fts_problem) print print stream_problem oracle.draw_goals() # NOTE - this must be after compile_problem to ensure the goal_poses convert if is_viewer_active(oracle.env): raw_input('Start?') search = get_fast_downward('eager') # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy planner = FOCUSED if focused else INCREMENTAL print SEPARATOR t0 = time() if planner == FOCUSED: # TODO - why do I need make_stream_instances=True here? plan, _ = focused_planner(stream_problem, search=search, check_feasible=False, greedy=False, dfs=True, verbose=verbose) elif planner == INCREMENTAL: frequency = 1 # 1 | 20 | 100 | INF # NOTE - the versions I submitted to RSS had implicitly waves=False plan, _ = incremental_planner(stream_problem, search=search, frequency=frequency, waves=True, verbose=verbose) else: raise ValueError('Must specify a planner') print SEPARATOR print 'Planner:', planner print 'Search:', FAST_DOWNWARD print 'Solved:', plan is not None print 'Length:', len(plan) if plan is not None else None print 'Time:', time() - t0 if plan is None: return # NOTE - need to set constants #PRE_SMOOTH_TRAJECTORIES = True #REPLAN_TRAJECTORIES = True #SMOOTH_TRAJECTORIES = identity_trajectories # identity_trajectories | smooth_trajectories #print oracle.robot.GetAffineTranslationMaxVels(), oracle.robot.GetAffineRotationAxisMaxVels() #oracle.robot.SetAffineTranslationMaxVels(5*oracle.robot.GetAffineTranslationMaxVels()) #SetAffineRotationAxisMaxVels(oracle.robot) # SetAffineRotationAxisMaxVels | SetAffineRotation3DMaxVels #SetAffineTranslationMaxVels(oracle.robot) # SetActiveDOFVelocities, SetDOFVelocities # SetDOFAccelerationLimits # GetActiveDOFMaxAccel but not SetActiveDOFMaxAccel #oracle.robot.SetActiveDOFVelocities(5*oracle.robot.GetActiveDOFVelocities()) #action.base_trajs[0].cspace.set_active() #indices = oracle.robot.GetActiveDOFIndices() #print indices #print oracle.robot.GetActiveDOFMaxAccel(indices) #oracle.robot.SetActiveDOFMaxAccel(5*oracle.robot.GetActiveDOFMaxAccel(indices), indices) # NOTE - not sure why the focused algorithm isn't working that well here anymore images_directory = None if movie: directory_path = get_directory_path(manip_problem.name, 'fts') images_directory = os.path.join(directory_path, 'images/') print 'Save:', images_directory if display or movie: Plan = namedtuple('Plan', ['start', 'operators']) start = convert_state(oracle, get_single_state(fts_problem.initial_state)) executables = convert_plan(oracle, plan) if execute: if movie: from misc.utils import launch_quicktime launch_quicktime() # TODO - produces missing value if quicktime is already enabled execute_plan(Plan(start, executables), oracle, pause=False) else: visualize_plan(Plan(start, executables), oracle, display=display, save=images_directory)
def policy(self, belief, goal): # The positions of objects don't change as the robot moves (i.e they are in a global frame) # The variances grow though as the robot moves # The robot config has no uncertainty # Thus, things are sort of relative to the robot # Sample poses that are reachable given the uncertainty and can see the table # Can ignore transition uncertainty for action effects # Only need to keep track of uncertainty along hte first move action as a precondition # Can either sample robot trajectory rollouts or object poses as the robot moves with increased uncertainty # Just do large objects for base collisions from manipulation.primitives.transforms import vector_trans from manipulation.bodies.robot import approach_vector_from_object_trans DISPLAY_GRASPS_HPN = False if DISPLAY_GRASPS_HPN: obj = 'objA' conf = belief.pbs.getConf( ) # belief.conf # Default conf (i.e. base is wrong) grasp_desc = belief.objects[obj].attrs['graspDesc'] shape = belief.objects[obj].attrs['shape'] print shape.parts()[0].vertices() import util.windowManager3D as wm win = 'W' for i in range(len(grasp_desc)): wm.getWindow(win).clear() obj_trans = get_object_frame(conf, self.hand, grasp_desc[i]) #approach_trans = get_approach_frame(conf, self.hand) #print np.round(approach_trans.matrix, 2) approach_vector = or_from_hpn_approach_vector( conf.robot, self.hand, grasp_desc[i]) transformed_vector = approach_vector_from_object_trans( obj_trans.matrix, approach_vector) obj_trans = hu.Transform( vector_trans(obj_trans.matrix, transformed_vector)) #temp_matrix = obj_trans.matrix.copy() # NOTE - this works as well? #temp_matrix[:3, 3] = approach_trans.matrix[:3, 3] #obj_trans = hu.Transform(temp_matrix) #print shape.origin() # Origin #print np.round(obj_trans.matrix, 2) shape.applyTrans(obj_trans).draw(win, color='blue') conf.draw(win, color='red') raw_input('Continue?') return None assert not check_belief_collisions(belief, .9) if satisfies(belief, goal): return [] if self.plan is None: env = get_env() manip_problem = or_manipulation_problem(env, belief, goal) #manip_problem.set_viewer(env) self.oracle = ManipulationOracle( manip_problem, env, active_arms=[self.hand], reset_robot=False ) # TODO - need to avoid resetting the original configuration # TODO - try just replacing the current problem DISPLAY_GRASPS_OR = False if DISPLAY_GRASPS_OR: # NOTE - OR Grasps are from Manip to Pose (not Gripper) from manipulation.bodies.robot import get_manip_trans, approach_vector_from_object_trans from manipulation.primitives.transforms import object_trans_from_manip_trans, set_trans, point_from_trans from manipulation.grasps.grasps import get_grasps from manipulation.primitives.display import draw_arrow obj = 'objA' #set_trans(self.oracle.bodies[obj], np.eye(4)) #print self.oracle.bodies[obj].ComputeAABB() #for grasp in get_grasps(self.oracle, obj): # NOTE - this randomizes the grasps for grasp in get_or_grasps(self.oracle, obj): manip_trans = get_manip_trans(self.oracle) #grasp_trans = or_from_hpn_grasp(belief.conf.robot, self.hand, grasp_desc[i]).matrix obj_trans = object_trans_from_manip_trans( manip_trans, grasp.grasp_trans) set_trans(self.oracle.bodies[obj], obj_trans) approach_vector = approach_vector_from_object_trans( obj_trans, grasp.approach_vector) approach_trans = vector_trans(obj_trans, approach_vector) _ = draw_arrow(env, point_from_trans(approach_trans), point_from_trans(obj_trans)) print print grasp.approach_vector print grasp.grasp_index print np.round(obj_trans, 2) raw_input('Continue?') return None # TODO - check that the plan is still good by converting the state stream_problem = compile_problem(self.oracle) self.oracle.draw_goals( ) # NOTE - this must be after compile_problem to ensure the goal_poses convert if is_viewer_active(self.oracle.env): raw_input('Start?') search_fn = get_fast_downward( 'eager', verbose=False ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = focused_planner(stream_problem, search=search_fn, greedy=False, stream_cost=10, verbose=False, debug=False) #plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False) if plan is None: plan = [] #visualize_plan(Plan(convert_state(self.oracle, stream_problem.initial_atoms), # executable_plan(self.oracle, plan)), self.oracle, display=True, save=None) #for state in get_states(universe, plan): # or_state = convert_state(self.oracle, state) # set_state(or_state, self.oracle) # raw_input('Continue?') look_iterations = 3 self.plan = [] state_sequence = get_states(universe, plan) for i, (action, args) in enumerate(convert_plan(plan)): or_state = convert_state(self.oracle, state_sequence[i]) set_state(or_state, self.oracle) if action.name in ['move']: last_config = self.oracle.get_robot_config() for _ in range(look_iterations): for obj in belief.objects: if or_state.get(obj, True) is not None: result = look_at_ik(self.oracle, obj) if result is not None: look_config = self.oracle.get_robot_config( ) self.plan += [ ('move_no_base', (last_config, look_config)), ('look', (obj, look_config)), ] last_config = look_config # TODO - does this mean I have to change the next action config? #print 'Looking at', obj #raw_input('Pause') self.plan.append((action.name, args)) elif action.name in ['pick', 'place']: obj, pose, grasp, approach_config, pap = args self.plan += [ ('move_no_base', (approach_config, pap.vector_config)), ('move_no_base', (pap.vector_config, pap.grasp_config)), (action.name, args), ('move_no_base', (pap.grasp_config, pap.vector_config)), ('move_no_base', (pap.vector_config, approach_config)), ] if self.replan: break else: self.plan.append((action.name, args)) #raw_input('Continue?') print self.plan if not self.plan: return None print SEPARATOR action, args = self.plan.pop(0) print action, args if not self.plan and self.replan: self.plan = None return [self.convert_action(belief, action, args) ] # NOTE - need to return a list for now