def plan_commands(problem, search='ff-astar', max_time=60, verbose=True): ss_problem = ss_from_problem(problem, remote=True, teleport=False) print(ss_problem) # ss_problem.dump() t0 = time.time() pr = cProfile.Profile() pr.enable() # plan, evaluations = incremental(ss_problem, planner=search, max_time=max_time, # verbose=verbose, terminate_cost=terminate_cost) plan, evaluations = dual_focused(ss_problem, planner=search, max_time=max_time, effort_weight=None, verbose=verbose) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) # tottime | cumtime print('Time:', time.time() - t0) # print_plan(plan, evaluations) if plan is None: return None return post_process(problem, plan)
def main(n=2, bound='unique'): initial_conf = CONF(-1) blocks = ['b{}'.format(i) for i in xrange(n)] initial_poses = {b: POSE(b, i) for i, b in enumerate(blocks)} goal_poses = {'b0': POSE('b0', 1)} Block = Predicate('?b') IsPose = Predicate('?b ?p') Conf = Predicate('?q') Kin = Predicate('?b ?q ?p') Collision = Predicate('?b ?p ?b2 ?p2', domain=[IsPose('?b', '?p'), IsPose('?b2', '?p2')], fn=lambda b, p, b2, p2: p.value == p2.value, eager=True, bound=False) AtConf = Predicate('?q') AtPose = Predicate('?b ?p') Holding = Predicate('?b') HandEmpty = Predicate('') Unsafe = Predicate('?b ?p') rename_functions(locals()) streams = [ Stream(name='placement', inp=['?b'], domain=[Block('?b')], fn=PoseGen, out='?p', graph=[IsPose('?b', '?p')], bound=bound), FnStream(name='ik', inp='?b ?p', domain=[IsPose('?b', '?p')], fn=lambda b, p: (CONF(p.value), ), out='?q', graph=[Kin('?b', '?q', '?p'), Conf('?q')], bound=bound), ] actions = [ Action(name='Move', param='?q1 ?q2', pre=[Conf('?q1'), Conf('?q2'), AtConf('?q1')], eff=[AtConf('?q2'), ~AtConf('?q1'), Increase(TotalCost(), 1)]), Action(name='Pick', param='?b ?p ?q', pre=[ Kin('?b', '?q', '?p'), AtPose('?b', '?p'), HandEmpty(), AtConf('?q') ], eff=[ Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(), Increase(TotalCost(), 1) ]), Action(name='Place', param='?b ?p ?q', pre=[ Kin('?b', '?q', '?p'), Holding('?b'), AtConf('?q'), ~Unsafe('?b', '?p') ], eff=[ AtPose('?b', '?p'), HandEmpty(), ~Holding('?b'), Increase(TotalCost(), 1) ]), ] axioms = [ Axiom(param='?b ?p ?b2 ?p2', pre=[Collision('?b', '?p', '?b2', '?p2'), AtPose('?b2', '?p2')], eff=Unsafe('?b', '?p')), ] initial_atoms = [ Conf(initial_conf), AtConf(initial_conf), HandEmpty(), initialize(TotalCost(), 0), ] for b, p in initial_poses.items(): initial_atoms += [Block(b), IsPose(b, p), AtPose(b, p)] for b, p in goal_poses.items(): initial_atoms += [IsPose(b, p)] goal_literals = [AtPose(b, p) for b, p in goal_poses.items()] problem = Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost()) print problem pr = cProfile.Profile() pr.enable() plan, evaluations = dual_focused(problem, terminate_cost=INF, verbose=False, bind=True, use_context=True, temp_dir='fish/', clean=True) print plan pr.disable() pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
def main(n=5, bound='unique'): initial_conf = 0 initial_poses = {'b{}'.format(i): i for i in xrange(n)} goal_poses = {'b1': 2} Block = Predicate('?b') Pose = Predicate('?p') Conf = Predicate('?q') Kin = Predicate('?q ?p') CFree = Predicate('?p1 ?p2') AtConf = Predicate('?q') AtPose = Predicate('?b ?p') Holding = Predicate('?b') HandEmpty = Predicate('') Safe = Predicate('?b ?p') rename_functions(locals()) streams = [ Stream(name='placement', inp=[], domain=[], fn=lambda: ([(p, )] for p in xrange(n, 100)), out='?p', graph=[Pose('?p')], bound=bound), FnStream(name='ik', inp='?p', domain=[Pose('?p')], fn=lambda p: (p, ), out='?q', graph=[Kin('?q', '?p'), Conf('?q')], bound=bound), TestStream(name='collision', inp='?p1 ?p2', domain=[Pose('?p1'), Pose('?p2')], test=lambda p1, p2: p1 != p2, graph=[CFree('?p1', '?p2')], eager=True, bound=bound), ] actions = [ Action(name='Move', param='?q1 ?q2', pre=[Conf('?q1'), Conf('?q2'), AtConf('?q1')], eff=[AtConf('?q2'), ~AtConf('?q1'), Increase(TotalCost(), 1)]), Action(name='Pick', param='?b ?p ?q', pre=[ Block('?b'), Kin('?q', '?p'), AtPose('?b', '?p'), HandEmpty(), AtConf('?q') ], eff=[ Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(), Increase(TotalCost(), 1) ]), ] for b in initial_poses: actions += [ Action(name='Place-' + b, param='?p ?q', pre=[Block(b), Kin('?q', '?p'), Holding(b), AtConf('?q')] + [Safe(b2, '?p') for b2 in initial_poses if b2 != b], eff=[ AtPose(b, '?p'), HandEmpty(), ~Holding(b), Increase(TotalCost(), 1) ]) ] axioms = [ Axiom(param='?p1 ?b2 ?p2', pre=[Block('?b2'), CFree('?p1', '?p2'), AtPose('?b2', '?p2')], eff=Safe('?b2', '?p1')), ] initial_atoms = [ Conf(initial_conf), AtConf(initial_conf), HandEmpty(), initialize(TotalCost(), 0), ] for b, p in initial_poses.items(): initial_atoms += [Block(b), Pose(p), AtPose(b, p)] for b, p in goal_poses.items(): initial_atoms += [Pose(p)] goal_literals = [AtPose(b, p) for b, p in goal_poses.items()] problem = Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost()) print problem pr = cProfile.Profile() pr.enable() plan, evaluations = dual_focused(problem, terminate_cost=INF, verbose=True) print plan pr.disable() pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
def main(execute='execute'): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') args = parser.parse_args() #display = args.display display = True connect(use_gui=args.viewer) robot, block = load_world() #robot2 = clone_body(robot) #block2 = clone_body(block) #dump_world() saved_world = WorldSaver() dump_world() ss_problem = ss_from_problem(robot, movable=[block], teleport=False, movable_collisions=True) #ss_problem = ss_problem.debug_problem() #print(ss_problem) t0 = time.time() plan, evaluations = dual_focused(ss_problem, verbose=True) # plan, evaluations = incremental(ss_problem, verbose=True) print_plan(plan, evaluations) print(time.time() - t0) if (not display) or (plan is None): disconnect() return paths = [] for action, params in plan: if action.name == 'place': paths += params[-1].reverse().body_paths elif action.name in ['move_free', 'move_holding', 'pick']: paths += params[-1].body_paths print(paths) command = Command(paths) if not args.viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() saved_world.restore() user_input('Execute?') if execute == 'control': command.control() elif execute == 'execute': command.refine(num_steps=10).execute(time_step=0.001) elif execute == 'step': command.step() else: raise ValueError(execute) #dt = 1. / 240 # Bullet default #p.setTimeStep(dt) wait_for_interrupt() disconnect()
def main(): initial_bq = BConf(0, 1) initial_poses = { 'green': [10, 10, 30], 'table': [10, 20, 30], } movable = {'green'} initial_p_from_name = {} class_from_name = {} for cl in initial_poses: for i, x in enumerate(initial_poses[cl]): name = cl + str(i) class_from_name[name] = cl initial_p_from_name[name] = Pose(name, x) print initial_p_from_name print class_from_name O = '?o' S = '?s' R = '?r' P = '?p' P2 = '?p2' G = '?g' Q = '?q' Q2 = '?q2' C = '?c' base = 'base' head = 'head' left = 'left' right = 'right' IsPose = Predicate([O, P]) IsGrasp = Predicate([O, G]) IsConf = Predicate([P, Q]) IsMovable = Predicate([O]) IsFixed = Predicate([O]) IsKin = Predicate([R, O, P, G, Q]) IsSupported = Predicate([P, P2]) IsArm = Predicate([R]) IsClass = Predicate([O, C]) AtPose = Predicate([O, P]) AtConf = Predicate([R, Q]) HasGrasp = Predicate([R, O, G]) HandEmpty = Predicate([R]) Holding = Predicate([O]) On = Predicate([O, S]) base_constant_cost = 1 def get_circle(value): if isinstance(value, BConf): return value.x, 0 if isinstance(value, Pose): return value.x, 0 if isinstance(value, InputOutputSet): if value.stream.name == 'IK': _, _, p, _ = value.inputs x, r = get_circle(p) return x, r + 0 if value.stream.name == 'placement': _, _, p = value.inputs x, r = get_circle(p) return x, r + 0 raise ValueError(value) def distance_bound_fn(q1, q2): x1, r1 = get_circle(q1) x2, r2 = get_circle(q2) return max(abs(x2 - x1) - (r1 + r2), 0) + base_constant_cost Distance = Function([Q, Q2], domain=[IsConf(base, Q), IsConf(base, Q2)], fn=lambda q1, q2: abs(q2.x - q1.x) + abs(q2.y - q1.y) + base_constant_cost, bound=distance_bound_fn) rename_functions(locals()) bound = 'shared' streams = [ FnStream(name='grasp', inp=[O], domain=[IsMovable(O)], fn=lambda o: (Grasp(o), ), out=[G], graph=[IsGrasp(O, G)], bound=bound), ListStream(name='IK', inp=[R, O, P, G], domain=[IsArm(R), IsPose(O, P), IsGrasp(O, G)], fn=lambda r, o, p, g: [(BConf(p.x, +1), )], out=[Q], graph=[IsKin(R, O, P, G, Q), IsConf(base, Q)], bound=bound), FnStream(name='placement', inp=[O, S, P], domain=[IsMovable(O), IsFixed(S), IsPose(S, P)], fn=lambda o, s, p: (Pose(o, p.x), ), out=[P2], graph=[IsPose(O, P2), IsSupported(P2, P)], bound=bound), ] actions = [ Action(name='pick', param=[R, O, P, G, Q], pre=[ IsKin(R, O, P, G, Q), HandEmpty(R), AtPose(O, P), AtConf(base, Q) ], eff=[HasGrasp(R, O, G), ~HandEmpty(R)]), Action(name='place', param=[R, O, P, G, Q], pre=[IsKin(R, O, P, G, Q), HasGrasp(R, O, G), AtConf(base, Q)], eff=[HandEmpty(R), AtPose(O, P), ~HasGrasp(R, O, G)]), Action( name='move_base', param=[Q, Q2], pre=[IsConf(base, Q), IsConf(base, Q2), AtConf(base, Q)], eff=[AtConf(base, Q2), ~AtConf(base, Q), Increase(TotalCost(), 1)]), Action(name='move_head', param=[Q, Q2], pre=[IsConf(head, Q), IsConf(head, Q2), AtConf(head, Q)], eff=[AtConf(head, Q2), ~AtConf(head, Q)]), ] axioms = [ Axiom(param=[R, O, G], pre=[IsArm(R), IsGrasp(O, G), HasGrasp(R, O, G)], eff=Holding(O)), Axiom(param=[O, P, S, P2], pre=[ IsPose(O, P), IsPose(S, P2), IsSupported(P, P2), AtPose(O, P), AtPose(S, P2) ], eff=On(O, S)), ] initial_atoms = [ IsArm(left), HandEmpty(left), HandEmpty(right), IsConf(base, initial_bq), AtConf(base, initial_bq), initialize(TotalCost(), 0), ] for n, p in initial_p_from_name.items(): initial_atoms += [IsPose(n, p), AtPose(n, p)] if class_from_name[n] not in movable: continue for n2, p2 in initial_p_from_name.items(): if class_from_name[n2] in movable: continue if p.x == p2.x: initial_atoms.append(IsSupported(p, p2)) for n, cl in class_from_name.items(): if cl in movable: initial_atoms.append(IsMovable(n)) else: initial_atoms.append(IsFixed(n)) goal_literals = [On('green0', 'table1'), On('green1', 'table1')] problem = Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost()) print problem pr = cProfile.Profile() pr.enable() plan, _ = dual_focused(problem) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: print plan return print 'Length', len(plan) for i, act in enumerate(plan): print i, act