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 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 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 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 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 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 solve_online(env): # TODO: simulate the work for real robot, occupancy = load_env(env) arm = robot.GetActiveManipulator() available_actions = ['pick', 'place', 'move_head', 'move_base', 'scan_room', 'scan_table', 'register'] get_world = get_lis_world # get_world_1 | get_world_2 | get_world_3 | get_world_4 | get_world_5 | get_world_6 object_meshes = get_object_meshes(MESHES_DIR) world, task = get_world(object_meshes) #add_world(robot, object_meshes, world) initialize_world(robot, object_meshes, world) initial_tform = robot.GetTransform() print world print task #prior = unknown(world) # TODO: start off not facing the object prior = initial_surface_belief(world) belief = belief_from_task(prior, task) #belief = update_belief(robot, world, belief, []) # Updates with initial observations #belief = observable_belief(robot, world) history = [] while True: #belief = belief_from_task(belief, task) print SEPARATOR print belief saver = EnvironmentStateSaver(env) handles = [ draw_circle_2d(env, get_point(robot), MAX_LOOK_DISTANCE, color=np.array([1, 0, 0, 1])), draw_circle_2d(env, get_point(robot), MAX_REG_DISTANCE, color=np.array([0, 1, 0, 1])), draw_affine_limits(robot)] + draw_viewcone(robot, z=2.5) # TODO: draw view cone with env: #plan = solve_tamp(arm, object_meshes, belief, task, occupancy, # available_actions=available_actions) plan = solve_hierarchical_tamp(arm, object_meshes, belief, task, occupancy, initial_tform, available_actions=available_actions) # TODO: some sort of shift when this happens if plan is None: raise RuntimeError('Unable to find a plan') if not plan: break plan_prefix = get_plan_prefix(convert_plan(plan)) print plan_prefix trajectories = [process_plan(arm, [action]) for action in plan_prefix] print trajectories saver.Restore() raw_input('Execute?') for action, trajs in zip(plan_prefix, trajectories): simulate_trajectories(env, trajs, time_step=0.01, realtime=False) belief = update_belief(robot, world, belief, [action]) #belief = observable_belief(robot, world) history += plan_prefix 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 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 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 update_best_plan(universe, search, best_plan, best_cost, max_time): if max_time is None: return best_plan, best_cost t0 = time() plan = search(universe, max_time, best_cost) universe.search_time += time() - t0 if plan is not None: cost = plan_cost(universe, plan) # plan_cost | plan_length if cost < best_cost: best_plan, best_cost = plan, cost if universe.verbose: print best_cost, convert_plan(best_plan) return best_plan, best_cost
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 main(argv): testNum = int(argv[0]) if argv else 0 print 'Test number', testNum print 'Args = ', testArgs[testNum] problem = create_problem(**testArgs[testNum]) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search = get_fast_downward('eager') #plan, _ = incremental_planner(problem, search=search, # frequency=10, waves=False) plan, _ = focused_planner(problem, search=search, greedy=False) print '\nPlan:', convert_plan(plan)
def main(): """ Creates and solves the 1D task and motion planning STRIPStream problem. """ pr = cProfile.Profile() pr.enable() 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) #plan, _ = simple_focused(problem, search=search) plan, _ = signed_focused(problem, search=search, verbose=True) print print 'Plan:', convert_plan(plan) pr.disable() pstats.Stats(pr).sort_stats('cumtime').print_stats(10) # tottime
def main(): """ Creates and solves the 1D car STRIPStream problem. """ problem = create_problem(p_goal=10) #plan, _ = simultaneous(problem, frequency=10) plan, _ = progression(problem, frequency=10, useful_actions=True, verbose=False) search = get_fast_downward( 'eager' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy #plan, _ = incremental_planner(problem, search=search, frequency=1, waves=True) #plan, _ = focused_planner(problem, search=search, greedy=False) 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 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 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 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_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?')
def simple_focused(problem, search=DEFAULT_SEARCH, max_time=INF, max_iterations=INF, optimal=False, stream_cost=10, check_feasible=False, max_level=0, greedy=True, shared=True, dfs=True, verbose=False, debug=False): universe = Universe( problem, use_ground=False, make_stream_instances=True, make_action_instances=True) # Need to assign a cost to all #universe = Universe(problem, use_ground=False, make_stream_instances=True, make_action_instances=not optimal) initialize_universe(universe) universe.action_to_function = get_stream_functions( universe) if stream_cost is not None else {} #################### for _ in irange(0, max_iterations): if verbose: print print_status(universe) if (time() - universe.start_time) >= max_time: break universe.iterations += 1 if debug: focused_debug(universe) if check_feasible: # Checks if the problem is feasible with no parameters plan = solve_real(universe, search, max_time) if plan is not None: assert is_real_solution(universe, plan) return plan, universe make_streams(universe, max_level) # TODO - can also do these in iterations/waves if debug: abstract_focused_debug(universe) #################### atom_nodes = determine_reachable(universe) for instance in universe.action_instances: # TODO - do I need to reset this ever? if isinstance(instance, ActionInstance): add_stream_cost( universe, atom_nodes, instance, stream_cost ) # NOTE - need to add these before the state resets plan, goals = solve_abstract(universe, atom_nodes, search, max_time) if verbose: print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan( plan) # TODO - use an upper bound and continue? raw_input('Continue?') if plan is None: if not universe.temp_blocked: break universe.temp_blocked = set() universe.resets += 1 continue #################### constants = set_union(set(args) for _, args in plan) abstract_constants = filter(lambda c: isinstance(c, AbstractConstant), constants) new_goals = goals.copy() new_goals.update( map(Concrete, abstract_constants) ) # NOTE - could do this for all supporting goals without distinction order = extract_streams(atom_nodes, new_goals) #visualize_streams(goals, abstract_constants) if verbose: print 'Goals:', len(goals) print 'Abstract constants:', len(abstract_constants) print 'Order:', order #################### bound_plan = produce_bindings(universe, order, plan, greedy, shared, dfs) if verbose: print 'Bound plan:', bound_plan if bound_plan is not None: assert is_real_solution(universe, bound_plan) return bound_plan, universe return None, universe
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 simple_focused(problem, search=DEFAULT_SEARCH, max_time=INF, max_iterations=INF, optimal=False, stream_cost=10, check_feasible=False, max_level=0, greedy=True, shared=True, dfs=True, verbose=False, debug=False): universe = Universe(problem, use_ground=False, make_stream_instances=True, make_action_instances=True) initialize_universe(universe) universe.action_to_function = get_stream_functions( universe) if stream_cost is not None else {} for _ in irange(0, max_iterations): if verbose: print print_status(universe) if time() - universe.start_time >= max_time: break universe.iterations += 1 if debug: focused_debug(universe) if check_feasible: plan = solve_real(universe, search, max_time) if plan is not None: assert is_real_solution(universe, plan) return plan, universe make_streams(universe, max_level) if debug: abstract_focused_debug(universe) atom_nodes = determine_reachable(universe) for instance in universe.action_instances: if isinstance(instance, ActionInstance): add_stream_cost(universe, atom_nodes, instance, stream_cost) plan, goals = solve_abstract(universe, atom_nodes, search, max_time, stream_cost) if verbose: print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan(plan) raw_input('Continue?') if plan is None: if not universe.temp_blocked: break universe.temp_blocked = set() universe.resets += 1 continue constants = set_union(set(args) for _, args in plan) abstract_constants = filter(lambda c: isinstance(c, AbstractConstant), constants) new_goals = goals.copy() new_goals.update(map(Concrete, abstract_constants)) order = extract_streams(atom_nodes, new_goals) if verbose: print 'Goals:', len(goals) print 'Abstract constants:', len(abstract_constants) print 'Order:', order bound_plan = produce_bindings(universe, order, plan, greedy, shared, dfs) if verbose: print 'Bound plan:', bound_plan if bound_plan is not None: assert is_real_solution(universe, bound_plan) return bound_plan, universe return None, universe
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?')