def initialize_fluent_streams(problem): fluents = problem.fluents() new_axioms = [] for stream in problem.streams: stream.fluent_domain = tuple(a for a in stream.domain if a.head.function in fluents) stream.domain = tuple(a for a in stream.domain if a not in stream.fluent_domain) stream.predicates = [] for i, o in enumerate(stream.outputs): params = stream.inputs + (X, ) predicate = Predicate(params, name='{}{}'.format(stream.name, i)) stream.predicates.append(predicate) new_axioms.append( Axiom(params, pre=[predicate(*params)] + [Computable(i) for i in stream.inputs], eff=Computable(X))) new_axioms.append(Axiom(X, pre=[Computed(X)], eff=Computable(X))) for action in problem.actions: action.preconditions = action.preconditions + \ tuple(map(Computable, action.parameters)) problem.axioms += new_axioms
def get_axioms(): # Axioms (inference rules that are automatically applied at every state) axioms = [ Axiom(param=[CONTROL, CUP, POSE], pre=[Collision(CONTROL, CUP, POSE), AtPose(CUP, POSE)], eff=Unsafe(CONTROL)), Axiom(param=[CUP, GRASP], pre=[IsGrasp(CUP, GRASP), Grasped(CUP, GRASP)], eff=Holding(CUP)), Axiom(param=[CUP, POSE, BLOCK, POSE2], pre=[BlockSupport(CUP, POSE, BLOCK, POSE2), AtPose(CUP, POSE), AtPose(BLOCK, POSE2)], eff=On(CUP, BLOCK)), ] return axioms
def plan_preimage(universe, evaluations, plan): action_instances = [action.instantiate(args) for action, args in plan] lazy_states = list(literal_sequence(universe.evaluations, action_instances)) real_states = list(literal_sequence(evaluations, action_instances)) axiom_instances = [ axiom.instantiate(args) for axiom, args in universe.axiom_instances() ] preimage = set() remapped_axioms = [] for rs, ls, action in reversed( zip(real_states, lazy_states, action_instances + [Goal(universe.problem.goal)])): preimage -= set(action.effects) derived = filter( lambda a: isinstance(a, Atom) and (a.head.function in universe.axioms_from_derived), action.preconditions) preimage |= (set(action.preconditions) - set(derived)) if derived: derived_map = { f: f.__class__(f.inputs) for f in universe.axioms_from_derived } remap_fn = lambda a: a.__class__( Head(derived_map[a.head.function], a.head.args)) preimage.update(map(remap_fn, derived)) for ax in axiom_instances: preconditions = [] for atom in ax.preconditions: if atom.head.function in derived_map: preconditions.append(remap_fn(atom)) elif ls.get(atom.head, False) != atom.value: break elif (atom.head not in rs) and (not atom.head.function.is_defined()): preconditions.append(atom) else: remapped_axioms.append( Axiom([], preconditions, remap_fn(ax.effect))) return preimage, remapped_axioms
def stream_action_instances(evaluations, bound_streams): actions = [] axioms = [] for bound_stream in bound_streams: instance = bound_stream.stream effort = instance.get_effort() assert effort != INF preconditions = [a for a in instance.domain() if a not in evaluations] effects = [a for a in bound_stream.bound_atoms if a not in evaluations] if instance.stream.eager and (len(effects) == 1): axioms.append(Axiom([], preconditions, effects[0])) else: action = Operator([], preconditions, effects + [Increase(TotalCost(), effort)]) action.bound_stream = bound_stream actions.append(action) return actions, axioms
def create_problem(cupSize=(3, 4), kettleSize=(5, 6), cupInitPos=(5, -9, 0), kettleInitPos=(-5, -9, 0), faucetPos=(5, 1, 0), kettleGoalPos=(-5, -9, 0), goalPosEps=0.5, maxMoveDist=10.0, domain=[(-20, 20), (-9, 20), (-np.pi, np.pi)], verboseFns=True): CUP = '?cup' POS1 = '?pos1' POS2 = '?pos2' POS3 = '?pos3' KETTLE = '?kettle' WATER = '?water' FAUCET = '?faucet' IsCup = Predicate([CUP]) IsKettle = Predicate([KETTLE]) IsWater = Predicate([WATER]) IsFaucet = Predicate([FAUCET]) LegalPos = Predicate([POS1]) CanGetWater = Predicate([POS1]) HoldsWater = Predicate([KETTLE]) CanPour = Predicate([POS1, POS2, POS3]) AtPos = Predicate([CUP, POS1]) WaterInKettle = Predicate([WATER, KETTLE]) WaterBoiled = Predicate([WATER, KETTLE]) HasBoiledWater = Predicate([KETTLE]) CanMove = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)], fn=lambda x, y: (distance(x, y) <= maxMoveDist)) CloseTo = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)], fn=lambda x, y: (distance(x, y) <= goalPosEps)) MoveCost = NonNegFunction([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)], fn=lambda p1, p2: scale_cost(distance(p1, p2))) def getWaterTest(p): (x, y, z) = p result = (faucetPos[0] - 1 <= x <= faucetPos[0] + 1) and (faucetPos[1] - cupSize[1] - 1 <= y <= faucetPos[1] - cupSize[1]) and (-0.05 <= z <= 0.05) if verboseFns and not result: print 'cannot get water from:', p return result def fill_cost(pos): success_cost = 1 fail_cost = 10 p_success = 0.75 if getWaterTest(pos) else 0.25 expected_cost = p_success * success_cost + (1 - p_success) * fail_cost return scale_cost(expected_cost) FillCost = NonNegFunction([POS1], domain=[LegalPos(POS1)], fn=fill_cost) rename_functions(locals()) actions = [ Action(name='Move', param=[CUP, POS1, POS2], pre=[IsCup(CUP), CanMove(POS1, POS2), AtPos(CUP, POS1)], eff=[AtPos(CUP, POS2), ~AtPos(CUP, POS1), Increase(TotalCost(), MoveCost(POS1, POS2))]), Action(name='Fill', param=[CUP, FAUCET, POS1, WATER], pre=[HoldsWater(CUP), IsFaucet(FAUCET), CanGetWater(POS1), IsWater(WATER), AtPos(CUP, POS1), AtPos(FAUCET, faucetPos)], eff=[WaterInKettle(WATER, CUP), Increase(TotalCost(), FillCost(POS1))]), Action(name='Pour', param=[CUP, KETTLE, POS1, POS3, POS2, WATER], pre=[HoldsWater(CUP), HoldsWater(KETTLE), CanPour(POS1, POS3, POS2), IsWater(WATER), AtPos(CUP, POS1), AtPos(KETTLE, POS2), WaterInKettle(WATER, CUP)], eff=[WaterInKettle(WATER, KETTLE), AtPos(CUP, POS3), ~WaterInKettle(WATER, CUP), ~AtPos(CUP, POS1), Increase(TotalCost(), scale_cost(1))]), Action(name='Boil', param=[KETTLE, WATER, POS1], pre=[IsKettle(KETTLE), IsWater(WATER), CloseTo(kettleGoalPos, POS1), AtPos(KETTLE, POS1), WaterInKettle(WATER, KETTLE)], eff=[WaterBoiled(WATER, KETTLE), Increase(TotalCost(), scale_cost(1))]), ] axioms = [ Axiom(param=[WATER, KETTLE], pre=[IsWater(WATER), IsKettle(KETTLE), WaterInKettle(WATER, KETTLE), WaterBoiled(WATER, KETTLE)], eff=HasBoiledWater(KETTLE)), ] def legalTest(rp): (x, y, z) = rp result = (domain[0][0] <= x <= domain[0][1]) and ( domain[1][0] <= y <= domain[1][1]) and (domain[2][0] <= z <= domain[2][1]) if verboseFns and not result: print 'not legal:', rp return result def randPos(): while True: pos = tuple(random.uniform(a[0], a[1]) for a in domain) if verboseFns: print 'randPos:', pos yield (pos,) def genMove(p): while True: pos = tuple(random.uniform(-1, 1) * maxMoveDist + a for a in p) if distance(pos, p) < maxMoveDist and legalTest(pos): if verboseFns: print 'genMove:', pos yield (pos,) def genClosePos(p): while True: pos = tuple(random.uniform(-1, 1) * goalPosEps + a for a in p) if (distance(pos, p) < goalPosEps) and legalTest(pos): if verboseFns: print 'genClosePos:', pos yield (pos,) def genPourPos(kpos): while True: x = kpos[0] + random.uniform((cupSize[0] / 2.0 + kettleSize[0] / 2.0), (cupSize[0] / 2.0 + kettleSize[0] / 2.0 + cupSize[1] / 5.0), ) y = kpos[1] + random.uniform(kettleSize[1] + 2, kettleSize[1] + 5) initpos = (x, y, 0) endpos = (x, y, np.pi / 1.4 * np.sign(x - kpos[0])) if legalTest(initpos) and legalTest(endpos): if verboseFns: print 'genPourPos:', initpos, endpos yield (initpos, endpos) elif verboseFns: print 'genPourPos illegal:', initpos, endpos def genGetWaterPos(): while True: pos = (random.uniform(-1, 1) * goalPosEps + faucetPos[0], random.uniform(-cupSize[1] - 1, -cupSize[1]) + faucetPos[1], 0) if legalTest(pos): if verboseFns: print 'genGetWaterPos:', pos yield (pos,) streams = [ GenStream(inp=[], domain=[], fn=genGetWaterPos, out=[POS2], graph=[LegalPos(POS2), CanGetWater(POS2)]), GenStream(inp=[], domain=[], fn=randPos, out=[POS1], graph=[LegalPos(POS1)]), GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genMove, out=[POS2], graph=[LegalPos(POS2), CanMove(POS1, POS2)]), GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genClosePos, out=[POS2], graph=[LegalPos(POS2), CloseTo(POS1, POS2)]), GenStream(inp=[POS2], domain=[LegalPos(POS2)], fn=genPourPos, out=[POS1, POS3], graph=[CanPour(POS1, POS3, POS2), LegalPos(POS1), LegalPos(POS3)]), TestStream(inp=[POS1], domain=[LegalPos(POS1)], test=getWaterTest, graph=[CanGetWater(POS1)]), ] kettle = 'kettle' cup = 'cup' faucet = 'faucet' water = 'water' initial_atoms = [ IsKettle(kettle), IsCup(cup), IsFaucet(faucet), IsWater(water), HoldsWater(cup), HoldsWater(kettle), LegalPos(cupInitPos), LegalPos(kettleInitPos), LegalPos(kettleGoalPos), LegalPos(faucetPos), AtPos(kettle, kettleInitPos), AtPos(faucet, faucetPos), AtPos(cup, cupInitPos), initialize(TotalCost(), 0), ] goal_literals = [ AtPos(cup, cupInitPos), HasBoiledWater(kettle), ] objective = TotalCost() return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=objective)
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 ss_from_problem(robot, movable=[], bound='shared', teleport=False, movable_collisions=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) rigid = [body for body in get_bodies() if body != robot] fixed = [body for body in rigid if body not in movable] print('Robot:', robot) print('Movable:', movable) print('Fixed:', fixed) conf = BodyConf(robot, get_configuration(robot)) initial_atoms = [ HandEmpty(), CanMove(), IsConf(conf), AtConf(conf), initialize(TotalCost(), 0), ] for body in movable: pose = BodyPose(body, get_pose(body)) initial_atoms += [ IsMovable(body), IsPose(body, pose), AtPose(body, pose) ] for surface in fixed: initial_atoms += [Stackable(body, surface)] if is_placement(body, surface): initial_atoms += [IsSupported(pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: initial_atoms.append(Sink(body)) if 'stove' in name: initial_atoms.append(Stove(body)) body = movable[0] goal_literals = [ AtConf(conf), #Holding(body), #On(body, fixed[0]), #On(body, fixed[2]), #Cleaned(body), Cooked(body), ] PlacedCollision = Predicate([T, O, P], domain=[IsTraj(T), IsPose(O, P)], fn=get_movable_collision_test(), bound=False) actions = [ Action(name='pick', param=[O, P, G, Q, T], pre=[ IsKin(O, P, G, Q, T), HandEmpty(), AtPose(O, P), AtConf(Q), ~Unsafe(T) ], eff=[HasGrasp(O, G), CanMove(), ~HandEmpty(), ~AtPose(O, P)]), Action( name='place', param=[O, P, G, Q, T], pre=[IsKin(O, P, G, Q, T), HasGrasp(O, G), AtConf(Q), ~Unsafe(T)], eff=[HandEmpty(), CanMove(), AtPose(O, P), ~HasGrasp(O, G)]), # Can just do move if we check holding collisions Action(name='move_free', param=[Q, Q2, T], pre=[ IsFreeMotion(Q, Q2, T), HandEmpty(), CanMove(), AtConf(Q), ~Unsafe(T) ], eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]), Action(name='move_holding', param=[Q, Q2, O, G, T], pre=[ IsHoldingMotion(Q, Q2, O, G, T), HasGrasp(O, G), CanMove(), AtConf(Q), ~Unsafe(T) ], eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]), Action( name='clean', param=[O, O2], # Wirelessly communicates to clean pre=[Stackable(O, O2), Sink(O2), ~Cooked(O), On(O, O2)], eff=[Cleaned(O)]), Action( name='cook', param=[O, O2], # Wirelessly communicates to cook pre=[Stackable(O, O2), Stove(O2), Cleaned(O), On(O, O2)], eff=[Cooked(O), ~Cleaned(O)]), ] axioms = [ Axiom(param=[O, G], pre=[IsGrasp(O, G), HasGrasp(O, G)], eff=Holding(O)), Axiom(param=[O, P, O2], pre=[IsPose(O, P), IsSupported(P, O2), AtPose(O, P)], eff=On(O, O2)), ] if movable_collisions: axioms += [ Axiom(param=[T, O, P], pre=[IsPose(O, P), PlacedCollision(T, O, P), AtPose(O, P)], eff=Unsafe(T)), ] streams = [ GenStream(name='grasp', inp=[O], domain=[IsMovable(O)], fn=get_grasp_gen(robot, grasp_name), out=[G], graph=[IsGrasp(O, G)], bound=bound), # TODO: test_support GenStream(name='support', inp=[O, O2], domain=[Stackable(O, O2)], fn=get_stable_gen(fixed=fixed), out=[P], graph=[IsPose(O, P), IsSupported(P, O2)], bound=bound), FnStream(name='inverse_kin', inp=[O, P, G], domain=[IsPose(O, P), IsGrasp(O, G)], fn=get_ik_fn(robot, fixed=fixed, teleport=teleport), out=[Q, T], graph=[IsKin(O, P, G, Q, T), IsConf(Q), IsTraj(T)], bound=bound), FnStream(name='free_motion', inp=[Q, Q2], domain=[IsConf(Q), IsConf(Q2)], fn=get_free_motion_gen(robot, teleport=teleport), out=[T], graph=[IsFreeMotion(Q, Q2, T), IsTraj(T)], bound=bound), FnStream(name='holding_motion', inp=[Q, Q2, O, G], domain=[IsConf(Q), IsConf(Q2), IsGrasp(O, G)], fn=get_holding_motion_gen(robot, teleport=teleport), out=[T], graph=[IsHoldingMotion(Q, Q2, O, G, T), IsTraj(T)], bound=bound), ] return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost())
def ss_from_problem(problem, bound='shared', remote=False, teleport=False, movable_collisions=False): robot = problem.robot #initial_bq = Pose(robot, get_pose(robot)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) initial_atoms = [ CanMove(), IsBConf(initial_bq), AtBConf(initial_bq), initialize(TotalCost(), 0), ] for name in problem.arms: joints = get_arm_joints(robot, name) conf = Conf(robot, joints, get_joint_positions(robot, joints)) initial_atoms += [ IsArm(name), HandEmpty(name), IsAConf(name, conf), AtAConf(name, conf) ] for body in problem.movable: pose = Pose(body, get_pose(body)) initial_atoms += [ IsMovable(body), IsPose(body, pose), AtPose(body, pose), POSE(pose) ] for surface in problem.surfaces: initial_atoms += [Stackable(body, surface)] if is_placement(body, surface): initial_atoms += [IsSupported(pose, surface)] initial_atoms += map(Washer, problem.sinks) initial_atoms += map(Stove, problem.stoves) initial_atoms += [IsConnected(*pair) for pair in problem.buttons] initial_atoms += [IsButton(body) for body, _ in problem.buttons] goal_literals = [] if problem.goal_conf is not None: goal_conf = Pose(robot, problem.goal_conf) initial_atoms += [IsBConf(goal_conf)] goal_literals += [AtBConf(goal_conf)] goal_literals += [Holding(*pair) for pair in problem.goal_holding] goal_literals += [On(*pair) for pair in problem.goal_on] goal_literals += map(Cleaned, problem.goal_cleaned) goal_literals += map(Cooked, problem.goal_cooked) #GraspedCollision = Predicate([A, G, AT], domain=[IsArm(A), POSE(G), IsArmTraj(AT)], # fn=lambda a, p, at: False, bound=False) PlacedCollision = Predicate([AT, P], domain=[IsArmTraj(AT), POSE(P)], fn=lambda at, p: False, bound=False) actions = [ Action(name='pick', param=[A, O, P, G, BQ, AT], pre=[ IsKin(A, O, P, G, BQ, AT), HandEmpty(A), AtPose(O, P), AtBConf(BQ), ~Unsafe(AT) ], eff=[ HasGrasp(A, O, G), CanMove(), ~HandEmpty(A), ~AtPose(O, P), Increase(TotalCost(), 1) ]), Action(name='place', param=[A, O, P, G, BQ, AT], pre=[ IsKin(A, O, P, G, BQ, AT), HasGrasp(A, O, G), AtBConf(BQ), ~Unsafe(AT) ], eff=[ HandEmpty(A), CanMove(), AtPose(O, P), ~HasGrasp(A, O, G), Increase(TotalCost(), 1) ]), Action( name='move', param=[BQ, BQ2, BT], pre=[IsMotion(BQ, BQ2, BT), CanMove(), AtBConf(BQ), ~Unsafe(BT)], eff=[ AtBConf(BQ2), ~CanMove(), ~AtBConf(BQ), Increase(TotalCost(), 1) ]), # TODO: should the robot be allowed to have different carry configs or a defined one? #Action(name='move_arm', param=[A, BQ, BQ2], # pre=[IsAConf(A, BQ), IsAConf(A, BQ), # CanMove(), AtBConf(BQ)], # eff=[AtAConf(BQ2), ~AtAConf(BQ), # Increase(TotalCost(), 1)]), # Why would one want to move to the pick configuration for something anywhere but the goal? # Stream that generates arm motions as long as one is a shared configuration # Maybe I should make a base ] if remote: actions += [ Action( name='clean', param=[O, O2], # Wirelessly communicates to clean pre=[Stackable(O, O2), Washer(O2), ~Cooked(O), On(O, O2)], eff=[Cleaned(O)]), Action( name='cook', param=[O, O2], # Wirelessly communicates to cook pre=[Stackable(O, O2), Stove(O2), Cleaned(O), On(O, O2)], eff=[Cooked(O), ~Cleaned(O)]), ] else: actions += [ Action(name='press_clean', param=[O, O2, A, B, BQ, AT], pre=[ Stackable(O, O2), Washer(O2), IsConnected(B, O2), IsPress(A, B, BQ, AT), ~Cooked(O), On(O, O2), HandEmpty(A), AtBConf(BQ) ], eff=[Cleaned(O), CanMove()]), Action(name='press_cook', param=[O, O2, A, B, BQ, AT], pre=[ Stackable(O, O2), Stove(O2), IsConnected(B, O2), IsPress(A, B, BQ, AT), Cleaned(O), On(O, O2), HandEmpty(A), AtBConf(BQ) ], eff=[Cooked(O), ~Cleaned(O), CanMove()]), ] axioms = [ Axiom(param=[A, O, G], pre=[IsArm(A), IsGrasp(O, G), HasGrasp(A, O, G)], eff=Holding(A, O)), Axiom(param=[O, P, O2], pre=[IsPose(O, P), IsSupported(P, O2), AtPose(O, P)], eff=On(O, O2)), ] if movable_collisions: axioms += [ Axiom(param=[O, P, AT], pre=[IsPose(O, P), PlacedCollision(AT, P), AtPose(O, P)], eff=Unsafe(AT)), ] streams = [ FnStream(name='motion', inp=[BQ, BQ2], domain=[IsBConf(BQ), IsBConf(BQ2)], fn=get_motion_gen(problem, teleport=teleport), out=[BT], graph=[IsMotion(BQ, BQ2, BT), IsBaseTraj(BT)], bound=bound), ListStream(name='grasp', inp=[O], domain=[IsMovable(O)], fn=get_grasp_gen(problem), out=[G], graph=[IsGrasp(O, G), GRASP(G)], bound=bound), # TODO: test_support Stream(name='support', inp=[O, O2], domain=[Stackable(O, O2)], fn=get_stable_gen(problem), out=[P], graph=[IsPose(O, P), IsSupported(P, O2), POSE(P)], bound=bound), GenStream( name='ik_ir', inp=[A, O, P, G], domain=[IsArm(A), IsPose(O, P), IsGrasp(O, G)], fn=get_ik_ir_gen(problem, teleport=teleport), out=[BQ, AT], graph=[IsKin(A, O, P, G, BQ, AT), IsBConf(BQ), IsArmTraj(AT)], bound=bound), GenStream(name='press', inp=[A, B], domain=[IsArm(A), IsButton(B)], fn=get_press_gen(problem, teleport=teleport), out=[BQ, AT], graph=[IsPress(A, B, BQ, AT), IsBConf(BQ), IsArmTraj(AT)], bound=bound), ] return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost())
def main(n=2, verbose=False): Item = Predicate('?b') Part = Predicate('?r') Class = Predicate('?c') IsMovable = Predicate('?i', domain=[Item('?i')]) IsFixed = Predicate('?i', domain=[Item('?i')]) IsClass = Predicate('?i ?c', domain=[Item('?i'), Class('?c')]) IsArm = Predicate('?r', domain=[Part('?r')]) IsStackable = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')]) HandHolding = Predicate('?r ?b', domain=[IsArm('?r'), Item('?b')]) HandEmpty = Predicate('?r', domain=[IsArm('?r')]) On = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')]) Nearby = Predicate('?s', domain=[IsFixed('?s')]) Found = Predicate('?b', domain=[Item('?b')]) Localized = Predicate('?b', domain=[Item('?b')]) Holding = Predicate('?c', domain=[Class('?c')]) rename_functions(locals()) actions = [ Action( name='Pick', param='?a ?i ?s', pre=[ IsArm('?a'), IsStackable('?i', '?s'), HandEmpty('?a'), Nearby('?s'), On('?i', '?s'), Localized('?i') ], eff=[HandHolding('?a', '?i'), ~HandEmpty('?a'), ~On('?i', '?s')]), Action(name='Place', param='?a ?i ?s', pre=[ IsArm('?a'), IsStackable('?i', '?s'), Nearby('?s'), HandHolding('?a', '?i'), Localized('?s') ], eff=[HandEmpty('?a'), On('?i', '?s'), ~HandHolding('?a', '?i')]), Action(name='ScanRoom', param='?s', pre=[IsFixed('?s'), ~Found('?s')], eff=[Found('?s')]), Action(name='ScanFixed', param='?s ?i', pre=[IsStackable('?i', '?s'), ~Found('?i')], eff=[Found('?i'), On('?i', '?s'), Increase(TotalCost(), 1)]), Action(name='Look', param='?i', pre=[Found('?i')], eff=[Localized('?i')]), Action(name='Move', param='?s', pre=[IsFixed('?s')], eff=[Nearby('?s')] + [~Nearby(s) for s in ['table0', 'table1']] + [~Localized(s) for s in ['soup0']]), ] initial_atoms = [ initialize(TotalCost(), 0), HandEmpty('left'), HandEmpty('right'), Found('table0'), Found('table1'), IsClass('table0', 'table'), IsClass('table1', 'table'), IsFixed('table0'), IsFixed('table1'), IsStackable('soup0', 'table0'), IsStackable('soup0', 'table1'), IsClass('soup0', 'soup'), Found('soup0'), On('soup0', 'table0'), IsStackable('green0', 'table0'), IsStackable('green0', 'table1'), IsClass('green0', 'green'), Found('green0'), On('green0', 'table0'), ] axioms = [ Axiom(param='?a ?i ?c', pre=[IsArm('?a'), IsClass('?i', '?c'), HandHolding('?a', '?i')], eff=Holding('?c')), ] goal_literals = [Holding('green'), Holding('soup')] problem = Problem(initial_atoms, goal_literals, actions, axioms, [], objective=TotalCost()) print problem plan, evaluations = incremental(problem, verbose=verbose) print plan
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