def main(): """ Creates and solves the 1D car STRIPStream problem. """ problem = create_problem(p_goal=5) # 0 | 1 | 2 | 5 | 10 #print problem, '\n' #problem.replace_axioms() #print problem #plan, _ = progression(problem, frequency=10, useful_actions=True, verbose=False) #plan, _ = simultaneous(problem, frequency=10, verbose=False, debug=False) #plan, _ = simultaneous_strips(problem, frequency=10, verbose=False, debug=False) from misc.profiling import run_profile, str_profile #solve = lambda: progression(problem, frequency=INF, useful_actions=True, verbose=False) # frequency = 10 | INF #solve = lambda: simultaneous(problem, frequency=10, verbose=False, debug=False) #solve = lambda: simultaneous_strips(problem, frequency=10, verbose=False, debug=False) #(plan, _), prof = run_profile(solve) #print str_profile(prof) search = get_fast_downward('eager') # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1, waves=True) #plan, _ = incremental_planner(problem, search=search, frequency=100, waves=False) #plan, _ = focused_planner(problem, search=search, greedy=False) print print 'Plan:', convert_plan(plan)
def fn(belief): problem = compile_belief(belief, goal) if FOCUSED: search = get_fast_downward( 'astar', verbose=True ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy #plan, universe = focused_planner(problem, search=search, stream_cost=0, verbose=True, optimal=True, debug=False) # stream_cost = 0 instead of None plan, universe = simple_focused(problem, search=search, stream_cost=0, max_level=0, verbose=True, optimal=True, debug=False) else: search = get_fast_downward( 'wastar1' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = incremental_planner(problem, search=search, frequency=1, optimal=True, waves=True, debug=False, max_calls=5000) print print 'Cost:', plan_cost(universe, plan) print 'Length:', plan_length(universe, plan) plan = convert_plan(plan) print 'Plan:', plan if plan is None or not plan: return None action, params = plan[0] return OPERATOR_MAP[action.name](operators, *params)
def run_observable(): problem_fn = test8 # test0 | test1 | test8 env, start, goal = problem_fn() global operatorDict operatorDict = makeOperators(glob.failProbs) print for name, operator in operatorDict.iteritems(): print 'Name:', name print 'Args:', [ arg for i, arg in enumerate(operator.args) if i not in operator.ignorableArgs ] print 'Pre:', operator.preconditions print 'Eff:', operator.results print 'Side Eff:', operator.sideEffects print problem = observable_problem(env, start, goal) search = get_fast_downward( 'eager' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1) print convert_plan(plan)
def run_test(self, problem_fn, planner, search): print SEPARATOR print problem_fn.__name__, self.planner, self.search tamp_problem = problem_fn(p=self.p) stream_problem = compile_problem(tamp_problem) print stream_problem if search == BFS: search_fn = get_bfs() elif search == FAST_DOWNWARD: # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search_fn = get_fast_downward('eager') else: raise ValueError(search) if planner == INCREMENTAL: plan, _ = incremental_planner(stream_problem, search=search_fn, waves=True, frequency=1, verbose=False) elif planner == FOCUSED: plan, _ = simple_focused(stream_problem, search=search_fn, check_feasible=False, greedy=False, dfs=True, verbose=False) else: raise ValueError(planner) self.assertTrue(plan is not None)
def flat_plan(initial, goal): #problem = create_abstract_problem(initial, goal) problem = create_real_problem(initial, goal) print problem search = get_fast_downward(config='ff-astar', remove=False, verbose=False) plan, _ = incremental_planner(problem, search=search) return convert_plan(plan)
def fn(belief): problem = compile_belief(belief, goal) search = get_fast_downward( 'eager' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1) print convert_plan(plan) # TODO - need to apply axioms to do this # TODO - need to also be careful that constants contains all the necessary constants if using quantifiers """ if goal_formula.holds(initial_atoms, constants): return [] if bias_plan is not None: # NOTE - testing if any subplan works is NP-Complete #candidate_plans = [bias_plan[1:], bias_plan] candidate_plans = [bias_plan[1:]] for candidate_plan in candidate_plans: instantiated_plan = [action.instantiate(args) for action, args in candidate_plan] if len(instantiated_plan) >= 1: if not isinstance(instantiated_plan[0], RefinableOperator) and \ is_valid_plan(initial_atoms, goal_formula, constants, instantiated_plan): return candidate_plan """ plan = convert_plan(plan) if plan is None or not plan: return None online_policy.last_plan = plan # TODO - use the next action on the last plan if it still is valid action, params = plan[0] return action.executable(operators, params)
def solve_single(env): use_viewer = (env.GetViewer() is not None) robot, occupancy = load_env(env) arm = robot.GetActiveManipulator() #get_problem = get_problem_1_0 #get_problem = get_problem_1_1 #get_problem = get_problem_1_2 get_problem = get_problem_1_3 #get_problem = get_problem_2_1 #get_problem = get_problem_2_2 #get_problem = get_problem_3_2 #get_problem = get_problem_4 object_meshes = get_object_meshes(MESHES_DIR) belief, task = get_problem(object_meshes) print SEPARATOR print 'Belief:', belief print 'Task:', task avaliable_actions = None #avaliable_actions = ['pick'] stream_problem, belief_from_name = get_ground_problem( arm, object_meshes, belief, task, occupancy, available_actions=avaliable_actions) print stream_problem env.Lock() plan, universe = incremental_planner(stream_problem, search=get_fast_downward( 'astar', verbose=False), optimal=False, frequency=1, waves=True, debug=False, max_time=5.0) env.Unlock() print SEPARATOR length = plan_length(universe, plan) cost = plan_cost(universe, plan) plan = convert_plan(plan) print 'Plan: {}\nLength: {} | Cost: {}'.format(plan, length, cost) if use_viewer and (plan is not None): trajectories = process_plan(arm, belief_from_name, plan) print trajectories raw_input('Start?') while True: with EnvironmentStateSaver(env): simulate_trajectories(env, trajectories) response = get_input('Replay?', ('y', 'n')) if response != 'y': break # simulate_single(arm, plan) if use_viewer: raw_input('Finish?')
def run_observable_2(): problem_fn = test8 # test0 | test1 | test8 env, start, goal = problem_fn() print print convert_start(env, start) print convert_goal(goal) #operators = makeOperators(glob.failProbs) #for name, operator in operators.iteritems(): # print 'Name:', name # print 'Args:', [arg for i, arg in enumerate(operator.args) if i not in operator.ignorableArgs] # print 'Pre:', operator.preconditions # print 'Eff:', operator.results # print 'Side Eff:', operator.sideEffects # print #problem = make_problem(convert_start(env, start), goal) problem = make_problem(maximum_likelihood_obs(start.details), goal) #problem = make_problem(sample_obs(start.details), goal) #problem = make_problem(sample_consistent_obs(start.details), goal) print problem search = get_fast_downward( 'dijkstra' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1) print convert_plan(plan)
def main(): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), .2, .2)] goal = create_box((.9, .9), .2, .2) constraint_problem = create_problem(goal, obstacles) print print constraint_problem stream_problem = constraint_to_stripstream(constraint_problem) print print stream_problem search = get_fast_downward('astar') # dijkstra | astar plan, _ = incremental_planner(stream_problem, search=search, frequency=25, waves=False) plan = convert_plan(plan) print 'Plan:', plan draw_solution(plan, goal, obstacles) raw_input('Finish?')
def main(): """ Creates and solves a blocksworld STRIPStream problem. """ problem = create_problem() print problem plan, _ = incremental_planner(problem) print print 'Plan:', convert_plan(plan)
def main(): """ Creates and solves the 1D task and motion planning STRIPStream problem. """ problem = create_problem() print problem plan, _ = incremental_planner(problem) print print 'Plan:', convert_plan(plan)
def main(): #planning_problem = get_problem() planning_problem = create_problem() print planning_problem t0 = time() plan, _ = incremental_planner(planning_problem, frequency=1) # 20 | 100 | INF print print 'Plan:', convert_plan(plan) print 'Time:', time() - t0
def main(): """ Creates and solves the 1D task and motion planning STRIPStream problem. """ problem = create_problem() print problem search = get_fast_downward('eager', max_time=10, verbose=True) # dijkstra | astar plan, _ = incremental_planner(problem, search=search, max_time=30, optimal=False) print print 'Plan:', convert_plan(plan)
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)
def hierarchy(initial, goal): abstract_problem = create_abstract_problem(initial, goal) abstract_plan, abstract_universe = incremental_planner( abstract_problem, search=get_fast_downward(config='ff-astar', remove=False, verbose=False)) print_plan(convert_plan(abstract_plan)) goal_sequence = preimage_sequence(abstract_universe, abstract_plan) #goal_sequence = [action.instantiate(args).condition for action, args in abstract_plan[1:]] + \ # [abstract_problem.goal_literals] # TODO: this doesn't really work because it doesn't accomplish the necessary subgoals #goal_sequence = list(And(*atoms) for atoms in get_states(abstract_universe, abstract_plan))[1:] # TODO: note that the subgoal actually has quite a few literals but most of them are satisfied # Each abstract operator reflects out confidence that each abstract operator represents the ability to go between conditions state = initial history = [] for goal_formula in goal_sequence: print SEPARATOR problem = create_real_problem(state, goal) problem.goal_literals = goal_formula print state print problem.goal_literals plan, universe = incremental_planner(problem, search=get_fast_downward( config='ff-astar', remove=False, verbose=False)) print_plan(convert_plan(plan)) if plan is None: return None state = execute(state, convert_plan( plan)) # TODO: would be better if directly applied the actions history += plan return convert_plan(history)
def main(): """ Creates and solves the Tsiolkovsky rocket problem. """ problem = create_problem(goal_p=100) print problem #plan, _ = simultaneous(problem, frequency=10, verbose=False, debug=False) search = get_fast_downward( 'eager' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1) print print 'Plan:', convert_plan(plan)
def fn(belief): problem = observable_problem(belief, goal) #print problem # NOTE - the cost sensitivity starts to factor into this... # NOTE - max cost prevents extremely large cost actions search = get_fast_downward('astar', verbose=False, max_cost=max_cost) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = incremental_planner(problem, search=search, frequency=1) cost = plan_cost(universe, plan) plan = convert_plan(plan) print 'Plan:', plan print 'Cost:', cost print 'Length:', len(plan) if plan is None or not plan: return None action, params = plan[0] print 'Action:', action, params print return OPERATOR_MAP[action.name](operators, *params)
def policy(estimator): problem, command_from_action = compile_problem(estimator, task) print problem plan, universe = incremental_planner( problem, search=get_fast_downward('astar'), frequency=100, waves=False, optimal=True, debug=False ) # TODO: need to make sure I do all costs (they are always eager) print 'Plan: {}\nLength: {} | Cost: {}'.format( convert_plan(plan), plan_length(universe, plan), plan_cost(universe, plan)) if plan is None: raise RuntimeError('Unable to find a plan') if not plan: return None return command_from_action(plan[0])
def main(): """ Creates and solves the 1D task and motion planning FTSProblem problem. """ constraint_problem = create_problem() print print constraint_problem stream_problem = constraint_to_stripstream(constraint_problem) print print stream_problem # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search_fn = get_fast_downward('eager') plan, _ = incremental_planner(stream_problem, search=search_fn) print print 'Plan:', convert_plan(plan)
def replan(initial, goal): state = initial history = [] while True: problem = create_real_problem(state, goal) search = get_fast_downward(config='ff-astar', remove=False, verbose=False) plan, _ = incremental_planner(problem, search=search) plan = convert_plan(plan) print_plan(plan) raw_input('awefwaef') if plan is None: return None if not plan: return convert_plan(history) plan_prefix = plan[:1] state = execute(state, plan_prefix) history += plan_prefix print SEPARATOR
def fn(belief): problem = make_problem(maximum_likelihood_obs(belief), goal) #problem = make_problem(sample_obs(belief), goal) #problem = make_problem(sample_consistent_obs(belief), goal) #print problem search = get_fast_downward( 'astar', verbose=False ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = incremental_planner(problem, search=search, frequency=1) plan = convert_plan(plan) print 'Plan:', plan print 'Length:', len(plan) if plan is None or not plan: return None action, params = plan[0] print 'Action:', action, params print return OPERATOR_MAP[action.name](operators, *params)
def main(): """ Creates and solves a generic non-holonomic motion planning problem """ init_state = (0, 0) problem = create_problem(.5, DGoalTest, DClearTest, DCalcDiffSystem, init_state) search = get_fast_downward( 'eager' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, _ = incremental_planner(problem, search=search, frequency=1, waves=True, debug=True) #plan, _ = focused_planner(problem, search=search, greedy=False) print print 'Plan:', convert_plan(plan)
def main(): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), .2, .2)] #goal = (1, 1) goal = create_box((.8, .8), .4, .4) problem, roadmap = create_problem(goal, obstacles) print problem search = get_fast_downward('astar') # dijkstra | astar plan, _ = incremental_planner(problem, search=search, frequency=1, waves=False) plan = convert_plan(plan) print 'Plan:', plan print 'Roadmap', len(roadmap) draw_roadmap(roadmap, goal, obstacles) raw_input('Continue?') draw_solution(plan, goal, obstacles) raw_input('Finish?')
def main(optimal=True, max_time=20): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), .2, .2)] #goal = (1, 1) goal = create_box((.8, .8), .4, .4) problem = create_problem(goal, obstacles) print problem search = get_fast_downward('ff-astar', max_time=10, verbose=True) # dijkstra | astar plan, _ = incremental_planner(problem, search=search, frequency=10, waves=False, optimal=optimal, max_time=max_time) plan = convert_plan(plan) print 'Plan:', plan draw_solution(plan, goal, obstacles) raw_input('Finish?')
def solve_continuous_tamp(planner, search, visualize, display, verbose=False, deterministic=False): # if deterministic: # set_deterministic() # sample_tamp_problem | sample_grasp_problem | sample_namo_problem tamp_problem = sample_tamp_problem() # stream_problem = compile_problem(tamp_problem, # streams_fn=generative_streams) # constant_streams | implicit_streams | # generative_streams stream_problem = compile_problem(tamp_problem) print stream_problem viewer = None if visualize: viewer = visualize_initial(tamp_problem, stream_problem) raw_input('Continue?') # viewer.save('initial') if search == DEFAULT: search_fn = DEFAULT_SEARCH elif search == BFS: search_fn = get_bfs() elif search == FAST_DOWNWARD: # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search_fn = get_fast_downward('eager') else: raise ValueError(search) t0 = time() if planner == INCREMENTAL: plan, universe = incremental_planner( stream_problem, search=search_fn, frequency=1, verbose=verbose) # 1 | 20 | 100 | INF elif planner == FOCUSED: #plan, universe = focused_planner(stream_problem, search=search_fn, greedy=False, verbose=verbose) plan, universe = simple_focused(stream_problem, search=search_fn, greedy=True, optimal=False, verbose=verbose) #plan, universe = plan_focused(stream_problem, search=search_fn, greedy=True, optimal=False, verbose=verbose) else: raise ValueError(planner) print SEPARATOR print 'Planner:', planner print 'Search:', search print 'Plan:', convert_plan(plan) print 'Solved:', plan is not None print 'Length:', len(plan) if plan is not None else None print 'Time:', time() - t0 # TODO - sometimes the movement actions, especially for the lazy # algorithm, screw up in the display for some reason... if display and plan is not None: if viewer is None: viewer = visualize_initial(tamp_problem, stream_problem) print '\nExecuting' # TODO - need to return the problem states = get_states(universe, plan) for i, state in enumerate(states): viewer.clear_state() visualize_atoms(viewer, state) raw_input('%s) %s?' % (i, 'Continue' if i != len(states) - 1 else 'Finish')) elif viewer is not None: raw_input('\nFinish?')
def solve_tamp(env, use_focused): use_viewer = env.GetViewer() is not None problem = PROBLEM(env) # TODO: most of my examples have literally had straight-line motions plans robot, manipulator, base_manip, ir_model = initialize_openrave( env, ARM, min_delta=MIN_DELTA) set_manipulator_conf(ir_model.manip, CARRY_CONFIG) bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names} surfaces = {surface.name: surface for surface in problem.surfaces} open_gripper(manipulator) initial_q = Conf(robot.GetConfigurationValues()) initial_poses = { name: Pose(name, get_pose(body)) for name, body in bodies.iteritems() } # TODO: just keep track of the movable degrees of freedom # GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues saver = EnvironmentStateSaver(env) #cfree_pose = cfree_pose_fn(env, bodies) #cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies) #base_generator = base_generator_fn(ir_model) #base_values = base_values_from_full_config(initial_q.value) #goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value) #goal_conf = Conf(goal_values) #return #################### # TODO - should objects contain their geometry cond_streams = [ EasyListGenStream(inputs=[O, P, G], outputs=[Q, T], conditions=[IsPose(O, P), IsGrasp(O, G)], effects=[GraspMotion(O, P, G, Q, T)], generator=sample_grasp_traj_fn( base_manip, ir_model, bodies, CARRY_CONFIG)), EasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_base_motion_fn(base_manip, bodies), order=1, max_level=0), EasyTestStream( inputs=[O, P, O2, P2], conditions=[IsPose(O, P), IsPose(O2, P2)], effects=[CFreePose(P, P2)], test=lambda *args: True, #cfree_pose, eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=lambda *args: True), #test=cfree_traj), EasyListGenStream(inputs=[O], outputs=[G], conditions=[], effects=[IsGrasp(O, G)], generator=grasp_generator_fn(bodies, TOP_GRASPS, SIDE_GRASPS, MAX_GRASPS)), EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[], effects=[IsPose(O, P), Stable(P, S)], generator=pose_generator_fn(bodies, surfaces)), #EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)], # effects=[], generator=base_generator), ] #################### constants = [] initial_atoms = [ConfEq(initial_q), HandEmpty()] for name in initial_poses: initial_atoms += body_initial_atoms(name, initial_poses, bodies, surfaces) goal_formula = And( ConfEq(initial_q), *[ OnSurface(obj, surface) for obj, surface in problem.goal_surfaces.iteritems() ]) #goal_formula = ConfEq(goal_conf) #obj, _ = problem.goal_surfaces.items()[0] #goal_formula = And(Holding(obj)) #goal_formula = Holding(obj) # TODO: this cause a bug stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) print stream_problem handles = draw_affine_limits(robot) if use_viewer: for surface in problem.surfaces: surface.draw(env) raw_input('Start?') max_time = INF search_fn = get_fast_downward('eager', max_time=10, verbose=False) if use_focused: solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=max_time) else: solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=10, waves=False, debug=False, max_time=max_time) with env: plan, universe = solve() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### if plan is not None: commands = process_plan(robot, bodies, plan) if use_viewer: print SEPARATOR saver.Restore() visualize_solution(commands, step=False) raw_input('Finish?')
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 solve_incrementally(): O = Param(OBJECT) O1, O2 = Param(OBJECT), Param(OBJECT) L = Param(LOCATION) L_s, L_g = Param(LOCATION), Param(LOCATION) # generic location Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param( STOVE_L_G) # locations for stove and sink Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G) actions = [ Action(name='wash', parameters=[O], condition=And(InSink(O)), effect=And(Clean(O))), Action(name='cook', parameters=[O], condition=And(InStove(O), Clean(O)), effect=And(Cooked(O))), Action(name='pickplace', parameters=[O, L_s, L_g], condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)), effect=And(AtPose(O, L_g), Not(AtPose(O, L_s)))) # You should delete! ] axioms = [ # For all objects in the world, either object is O1 or if not, then it is not in the region Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\ Or(Equal(O,O2),\ Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))), # Object is in the stove if it is at pose L for which Ls and Lg define stove Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))), Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))), ] cond_streams = [ EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\ generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))), EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS), # Generate static predicates that object is contained in sink for which Ls and Lg define the sink. If L was not continuous value, # then we would define this in the intial condition and would not be changed by any of the actions (hence static predicate) EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS), EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS), # OutsideRegion tests if O2 is is outside of the region (Ls,Lg) EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS), ] #################### tamp_problem = sample_one_d_kitchen_problem() # instantiate the environment region? constants = [ STOVE_L_S(tamp_problem.stove_region_s), STOVE_L_G(tamp_problem.stove_region_g), SINK_L_S(tamp_problem.sink_region_s), SINK_L_G(tamp_problem.sink_region_g), ] # define initial state using initial poses of objects initial_atoms = [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [ IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g) ] # initial_atoms = static predicates, but on steroid goal_literals = [] subgoal_list = [ InSink(tamp_problem.target_obj), \ InStove(tamp_problem.target_obj), Cooked(tamp_problem.target_obj) ] for i in range(len(subgoal_list)): goal_literals = [subgoal_list[0]] stream_problem = STRIPStreamProblem(initial_atoms, goal_literals, \ actions+axioms, cond_streams, constants) search = get_fast_downward('eager') plan, universe = incremental_planner(stream_problem, search=search,\ frequency=1, verbose=True, max_time=200) plan = convert_plan(plan) # move the object to the new locations; todo: add new predicates if len(plan) > 0: # if no action needed, keep last initial atoms initial_atoms = [] for action in plan: action_name = action[0].name action_args = action[1] if action_name == 'pickplace': O = action_args[0].name pick_l = action_args[1] place_l = action_args[2] block = [b for b in tamp_problem.blocks if b.name == O][0] initial_atoms += [AtPose(block, place_l)] if len(initial_atoms) == 1: block = [b for b in tamp_problem.blocks if b.name != O][0] initial_atoms += [AtPose(block, tamp_problem.initial_poses[block])] initial_atoms += [ IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g) ] initial_atoms += [ IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g) ]
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] initialize_openrave(env, ARM, min_delta=.01) manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} all_bodies = bodies.values() assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1 # NOTE - assuming all objects has the same geometry body1 = all_bodies[-1] # Generic object 1 body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS] poses = problem.known_poses if problem.known_poses else [] open_gripper(manipulator) initial_conf = Conf(robot.GetConfigurationValues()[manipulator.GetArmIndices()]) def _enable_all(enable): # Enables or disables all bodies for collision checking for body in all_bodies: body.Enable(enable) #################### def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2 body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose _enable_all(False) body2.Enable(True) set_pose(body2, pose.value) for conf in traj.path(): set_manipulator_conf(manipulator, conf) if env.CheckCollision(robot, body2): return False return True def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose _enable_all(False) body1.Enable(True) body2.Enable(True) set_pose(body2, pose.value) for conf in traj.path(): set_manipulator_conf(manipulator, conf) manip_trans = manipulator.GetTransform() set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans)) if env.CheckCollision(body1, body2): return False return True def cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose if DISABLE_TRAJ_COLLISIONS: return True return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose)) #################### def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp _enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration if grasp_conf is None: return if DISABLE_TRAJECTORIES: yield [(Conf(grasp_conf), object())] return set_manipulator_conf(manipulator, grasp_conf) robot.Grab(body1) grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp #grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration yield [(pregrasp_conf, grasp_traj)] def sample_free_motion(conf1, conf2): # Sample motion while not holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return _enable_all(False) set_manipulator_conf(manipulator, conf1.value) #traj = motion_plan(env, cspace, conf2.value, self_collisions=True) traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10) if not traj: return traj.grasp = None yield [(traj,)] def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return _enable_all(False) body1.Enable(True) set_manipulator_conf(manipulator, conf1.value) manip_trans = manipulator.GetTransform() set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans)) robot.Grab(body1) #traj = motion_plan(env, cspace, conf2.value, self_collisions=True) traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10) robot.Release(body1) if not traj: return traj.grasp = grasp yield [(traj,)] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[], effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj), # Move trajectory EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0), EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[], effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0), # Collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)], test=cfree_pose, eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=cfree_traj), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ConfEq(initial_conf), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ConfEq(initial_conf), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: env.UpdatePublishedBodies() raw_input('Start?') search_fn = get_fast_downward('eager', max_time=10) solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) #solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR #universe.print_statistics() plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i+1, action, args else: print 'Failure' #################### def _execute_traj(traj): #for j, conf in enumerate(traj.path()): #for j, conf in enumerate([traj.end()]): path = list(sample_manipulator_trajectory(manipulator, traj.traj())) for j, conf in enumerate(path): set_manipulator_conf(manipulator, conf) env.UpdatePublishedBodies() raw_input('%s/%s) Step?'%(j, len(path))) if viewer and plan is not None: print SEPARATOR # Resets the initial state #set_manipulator_conf(manipulator, initial_conf.value) set_manipulator_conf(manipulator, initial_conf) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) env.UpdatePublishedBodies() if not DISABLE_TRAJECTORIES: for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move': _, _, traj = args _execute_traj(traj) elif action.name == 'move_holding': _, _, traj, _, _ = args _execute_traj(traj) elif action.name == 'pick': obj, _, _, _, traj = args _execute_traj(traj.reverse()) robot.Grab(bodies[obj]) _execute_traj(traj) elif action.name == 'place': obj, _, _, _, traj = args _execute_traj(traj.reverse()) robot.Release(bodies[obj]) _execute_traj(traj) else: raise ValueError(action.name) env.UpdatePublishedBodies() else: for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move': _, q2, _ = args set_manipulator_conf(manipulator, q2) elif action.name == 'move_holding': _, q2, _, _, _ = args set_manipulator_conf(manipulator, q2) elif action.name == 'pick': obj, _, _, _, traj = args robot.Grab(bodies[obj]) elif action.name == 'place': obj, _, _, _, traj = args robot.Release(bodies[obj]) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')
SEARCHES = [ #('BFS', get_bfs()), #('FastDownward-A*', get_fast_downward('astar', verbose=False)), #('FastDownward-Eager', get_fast_downward('eager', verbose=False)), #('FastForward', get_fast_forward(verbose=False)), #('LAPTK-BFS-F', get_lakpt(search='bfs_f', verbose=False)), #('LAPTK-SIW', get_lakpt(search='siw', verbose=False)), #('YAHSP', get_yahsp(verbose=False)), #('pyplanners', get_pyplanners(verbose=False)), ] GREEDY, DFS = True, True PLANNERS = [ ('incremental', lambda p, s: incremental_planner(p, search=s, frequency=1, verbose=False) ), # Focused planners ('focused', lambda p, s: focused_planner(p, search=s, check_feasible=False, greedy=GREEDY, dfs=DFS, verbose=True)), ('simple_focused', lambda p, s: simple_focused(p, search=s, check_feasible=False, greedy=GREEDY, dfs=DFS, verbose=False)), ('plan_focused', lambda p, s: plan_focused(p,