def compile_problem(tamp_problem): """ Constructs a STRIPStream problem for the countable TMP problem. :param tamp_problem: a :class:`.TMPProblem` :return: a :class:`.STRIPStreamProblem` """ B1, B2 = Param(BLOCK), Param(BLOCK) P1, P2 = Param(POSE), Param(POSE) Q1, Q2 = Param(CONF), Param(CONF) actions = [ Action(name='pick', parameters=[B1, P1, Q1], condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)), effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))), Action(name='place', parameters=[B1, P1, Q1], condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1), ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))), effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))), Action(name='move', parameters=[Q1, Q2], condition=AtConf(Q1), effect=And(AtConf(Q2), Not(AtConf(Q1)))), ] axioms = [ Axiom(effect=Safe(B2, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))), ] cond_streams = [ EasyGenStream(inputs=[], outputs=[P1], conditions=[], effects=[], generator=lambda: irange(0, NUM_POSES)), EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)], generator=lambda p: iter([p])), EasyTestStream(inputs=[P1, P2], conditions=[], effects=[CollisionFree(P1, P2)], test=lambda p1, p2: p1 != p2, eager=EAGER_TESTS), ] constants = [] initial_atoms = [ AtConf(tamp_problem.initial_config), ] + [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] if tamp_problem.initial_holding is False: initial_atoms.append(HandEmpty()) else: initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK)) goal_literals = [] if tamp_problem.goal_holding is False: goal_literals.append(HandEmpty()) elif tamp_problem.goal_holding is not None: goal_literals.append(Holding(tamp_problem.goal_holding)) for block, goal in tamp_problem.goal_poses.iteritems(): goal_literals.append(AtPose(block, goal)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
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 compile_problem(oracle): problem = oracle.problem for obj in problem.goal_poses: if problem.goal_poses[obj] == 'initial': problem.goal_poses[obj] = oracle.initial_poses[obj] elif problem.goal_poses[ obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial) problem.goal_poses[obj] = oracle.initial_poses[ problem.goal_poses[obj]] # oracle.initial_config = ??? # TODO - set the initial configuration transit_conf = Config(oracle.default_left_arm_config) #################### O, P, G, T = Param(OBJ), Param(POSE), Param(GRASP), Param(TRAJ) OB, R = Param(OBJ), Param(REG) BQ, MQ = Param(BASE_CONF), Param(MANIP_CONF) BQ2, MQ2 = Param(BASE_CONF), Param(MANIP_CONF) BT, MT = Param(BASE_TRAJ), Param(MANIP_TRAJ) rename_easy(locals()) # Separate arm and base configs and trajectories? # Make a fixed configuration for arm stuff # If I separate arm and base poses, I can avoid worrying about collision when the base and things # Would I ever want to actually separate these and use the same grasp at a different config? # - Maybe for two arms? # - I don't want to reuse trajectories at different base configs. That wouldn't make sense # - Although if I manipulate two configs at once, maybe it would! # - Make constant arm transit configs? # TODO - maybe make an axiom to handle BT and MT when doing these things? # TODO - I could equip a manip conf with a base conf at all times if I really don't want them separate # NOTE - put then would need to update them whenever moving the base #actions = [ # Action(name='pick', parameters=[O, P, G, BQ, MQ], # condition=And(PoseEq(O, P), HandEmpty(), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(MQ)), # #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))), # effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))), # # Action(name='place', parameters=[O, P, G, BQ, MQ], # condition=And(GraspEq(O, G), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(MQ)), # #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))), # effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), # # Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT], # condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT)), # effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))), # # Action(name='move_base', parameters=[BQ, BQ2, MQ, BT], # TODO - put the arm motion stuff in an axiom # condition=And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT)), # effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))), # ] # TODO - should I include the grasp in move backwards trajectory? actions = [ abs_action( name='pick', parameters=[O, P, G, BQ, MQ], conditions=[ And(PoseEq(O, P), HandEmpty(), Kin(O, P, G, BQ, MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))), And(BaseEq(BQ), ManipEq(MQ)) ], effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))), abs_action( name='place', parameters=[O, P, G, BQ, MQ], conditions=[ And(GraspEq(O, G), Kin(O, P, G, BQ, MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))), And(BaseEq(BQ), ManipEq(MQ)) ], effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), # NOTE - the hierarchical versions of this abs_action( name='move_arm', parameters=[MQ, MQ2, BQ, MT], # TODO - Or(TransitManip(MQ), TransitManip(MQ2)) conditions=[ And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ)), #And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), Or(TransitManip(MQ), TransitManip(MQ2))) # This does something odd #And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), # Or(TransitManip(MQ), LegalConf(BQ, MQ)), # Or(TransitManip(MQ2), LegalConf(BQ, MQ2))) # This does something odd ], # TODO - put an Or(Pair(MQ, BQ), Pair(MQ2, BQ)) or something effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))), abs_action(name='move_base', parameters=[BQ, BQ2, BT], conditions=[ And(BaseEq(BQ), SafeManip(BT), BaseMotion(BQ, BQ2, BT)) ], effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))), #abs_action(name='move_base', parameters=[BQ, BQ2, MQ, BT], # conditions=[ # And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT))], # effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))), ] # NOTE - the parameters really aren't necessary # TODO - could remove any dependence on tests because there are so cheap with the evaluation (already can't use them with axioms) # TODO - could also just make the cost only depend on the introduced objects within the effects axioms = [ Axiom(effect=Holding(O), condition=Exists([G], And(GraspEq(O, G), LegalGrasp(O, G)))), Axiom(effect=InRegion(O, R), condition=Exists([P], And(PoseEq(O, P), Contained(R, O, P)))), #Axiom(effect=Safe(O, T), condition=Exists([P], And(PoseEq(O, P), CFree(O, P, T)))), #Axiom(effect=SafeArm(O, MQ, T), condition=Exists([P, MQ], And(PoseEq(O, P), ManipEq(MQ), CFree(O, P, T)))), Axiom(effect=SafeManip(BT), condition=Exists( [MQ], And(ManipEq(MQ), TransitManip(MQ)))), # TODO - add condition here ] # TODO - include parameters in STRIPS axiom? #################### def get_base_conf(conf): return Config(base_values_from_full_config(conf.value)) def get_arm_conf(conf): return Config( arm_from_full_config(oracle.robot.GetActiveManipulator(), conf.value)) def sample_poses(o, num_samples=1): while True: if AVOID_INITIAL: oracle.set_all_object_poses(oracle.initial_poses) else: oracle.set_all_object_poses({o: oracle.initial_poses[o]}) yield list( islice( random_region_placements(oracle, o, oracle.get_counters(), region_weights=True), num_samples)) def sample_grasps(o): yield get_grasps(oracle, o) def sample_region(o, r, num_samples=1): while True: oracle.set_all_object_poses({o: oracle.initial_poses[o]}) yield list( islice( random_region_placements(oracle, o, [r], region_weights=True), num_samples)) def sample_motion(o, p, g, max_calls=1, max_failures=50): oracle.set_all_object_poses( {o: p}) # TODO - saver for the initial state as well? if oracle.approach_collision(o, p, g): return for i in range(max_calls): pap = PickAndPlace(oracle.get_geom_hash(o), p, g) if not pap.sample(oracle, o, max_failures=max_failures, sample_vector=False, sample_arm=False, check_base=False): break #pap.obj = o yield [(get_base_conf(pap.grasp_config), get_arm_conf(pap.grasp_config))] def collision_free(o, p, t): if p is None or o == t.obj: return True holding = ObjGrasp(t.obj, t.grasp) #holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp) if not DO_ARM_MOTION: return not oracle.holding_collision(t.grasp_config, o, p, holding) return not oracle.traj_holding_collision(t.approach_config, t.trajs, o, p, holding) cond_streams = [ EasyListGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses), EasyListGenStream(inputs=[O], outputs=[G], conditions=[], effects=[LegalGrasp(O, G)], generator=sample_grasps), EasyListGenStream(inputs=[O, R], outputs=[P], conditions=[], effects=[LegalPose(O, P), Contained(R, O, P)], generator=sample_region), #MultiEasyGenStream(inputs=[O, P, G], outputs=[BQ, MQ], conditions=[LegalPose(O, P), LegalGrasp(O, G)], # effects=[Kin(O, P, G, BQ, MQ)], generator=sample_motion), EasyListGenStream(inputs=[O, P, G], outputs=[BQ, MQ], conditions=[LegalPose(O, P), LegalGrasp(O, G)], effects=[Kin(O, P, G, BQ, MQ), LegalConf(BQ, MQ)], generator=sample_motion), # TODO - this doesn't work for constants # TODO - make a condition that MQ, MQ2 both work for BQ. Otherwise, it will still create a ton of streams #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[], # effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1), EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[TransitManip(MQ), LegalConf(BQ, MQ2)], effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1), EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[LegalConf(BQ, MQ), TransitManip(MQ2)], effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1), #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[LegalConf(BQ, MQ), LegalConf(BQ, MQ2)], # effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1), EasyGenStream(inputs=[BQ, BQ2], outputs=[BT], conditions=[], effects=[BaseMotion(BQ, BQ2, BT)], generator=lambda *args: [None], order=1), #effects=[BaseMotion(BQ, BQ2, BT)], generator=lambda *args: [None], order=2), # NOTE - causes a bug! #EasyTestStream(inputs=[O, P, T], conditions=[LegalPose(O, P)], effects=[CFree(O, P, T)], # test=collision_free, eager=EAGER_TESTS), ] #################### #print transit_conf.value #print oracle.initial_config.value initial_base = get_base_conf(oracle.initial_config) initial_manip = get_arm_conf(oracle.initial_config) print initial_base.value print initial_manip.value constants = [] initial_atoms = [ BaseEq(initial_base), ManipEq(initial_manip), HandEmpty(), TransitManip(transit_conf), LegalConf(initial_base, initial_manip), ] for obj, pose in oracle.initial_poses.iteritems(): initial_atoms += [PoseEq(obj, pose), LegalPose(obj, pose)] # TODO - serialize goals by object name goal_literals = [] if problem.goal_holding is not None: if problem.goal_holding is False: goal_literals.append(HandEmpty()) elif isinstance(problem.goal_holding, ObjGrasp): goal_literals.append( GraspEq(problem.goal_holding.object_name, problem.goal_holding.grasp)) elif problem.goal_holding in oracle.get_objects(): goal_literals.append(Holding(problem.goal_holding)) else: raise Exception() for obj, pose in problem.goal_poses.iteritems(): goal_literals.append(PoseEq(obj, pose)) initial_atoms.append(LegalPose(obj, pose)) for obj, region in problem.goal_regions.iteritems(): goal_literals.append(InRegion(obj, region)) #goal_formula = And(*goal_literals) #goal_formula = AbsCondition(map(And, goal_literals)) # TODO - bug where goals must have And goal_formula = AbsCondition( goal_literals) # TODO - bug where goals must have And return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
def compile_problem(tamp_problem): 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))))))), 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), 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), 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), # OutsideRegion tests if the block at L is outside of the region (Ls,Lg) ] #################### # 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) ] # static predicate but on steroid # define goal state as target object to be cooked - can you infer that target object is on Stove goal_literals = [] # goal_literals.append( AtPose(tamp_problem.blue_obj,1) ) #NOTE: This works; so planner knows how to clear the area # goal_literals.append( AtPose(tamp_problem.target_obj,3.0) ) #NOTE: But doing this does not work goal_literals.append(Cooked(tamp_problem.target_obj)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants), static_pred_names
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 amber_stripstream(use_window=True, step=False): problem = dantam2() window_3d = Window3D(title='World', windowWidth=500, noWindow=(not use_window), viewport=problem.workspace.flatten('F')) if is_window_active(window_3d): draw_window_3d( window_3d, problem.initial_conf, [body.applyTrans(pose) for body, pose in problem.initial_poses]) raw_input('Start?') robot = problem.initial_conf.robot name_to_body = {get_name(body): body for body, _ in problem.initial_poses} static_bodies = [ body.applyTrans(pose) for body, pose in problem.initial_poses if get_name(body) not in problem.movable_names ] block = name_to_body[problem.movable_names[-1]] # Generic object #################### # NOTE - can either modify the world state or create new bodies # Collision free test between an object at pose1 and an object at pose2 def cfree_pose(pose1, pose2): return not ((pose1 == pose2) or block.applyTrans(pose1).collides( block.applyTrans(pose2))) 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 placed = [body.applyTrans(pose)] attached = None if traj.grasp is not None: attached = { ARM: body.applyTrans( get_wrist_frame(traj.grasp, robot, ARM).inverse()) } # TODO - cache bodies for these configurations collision_fn = get_collision_fn(placed, attached) return not any(collision_fn(q) for q in traj.path) #################### # Sample pregrasp config and motion plan that performs a grasp def sample_grasp_traj(pose, grasp): attached = { ARM: body.applyTrans(get_wrist_frame(grasp, robot, ARM).inverse()) } collision_fn = get_collision_fn(static_bodies, attached) manip_frame = manip_from_pose_grasp(pose, grasp, robot, ARM) grasp_conf = next( inverse_kinematics(problem.initial_conf, manip_frame, ARM), None) # Grasp configuration if grasp_conf is None or collision_fn(grasp_conf): return # NOTE - I could also do this in the opposite order pregrasp_frame = approach_from_manip(manip_frame) pregrasp_conf = next( inverse_kinematics(grasp_conf, pregrasp_frame, ARM)) # Pregrasp configuration if pregrasp_conf is None or collision_fn(pregrasp_conf): return if DISABLE_GRASP_TRAJECTORIES: yield (pregrasp_conf, Traj([grasp_conf], grasp)) return grasp_path = direct_path( grasp_conf, pregrasp_conf, # Trajectory from grasp configuration to pregrasp get_extend_fn([robot.armChainNames[ARM]], STEP_SIZE), collision_fn) if grasp_path is None: return yield (pregrasp_conf, Traj(grasp_path, grasp)) def sample_free_motion(conf1, conf2): # Sample motion while not holding if DISABLE_MOTION_TRAJECTORIES: yield (Traj([conf2]), ) return active_chains = [robot.armChainNames[ARM]] path = birrt(conf1, conf2, get_distance_fn(active_chains), get_sample_fn(conf1, active_chains), get_extend_fn(active_chains, STEP_SIZE), get_collision_fn(static_bodies), restarts=0, iterations=50, smooth=None) if path is None: return yield (Traj(path), ) def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding if DISABLE_MOTION_TRAJECTORIES: yield (Traj([conf2], grasp), ) return active_chains = [robot.armChainNames[ARM]] attached = { ARM: body.applyTrans(get_wrist_frame(grasp, robot, ARM).inverse()) } path = birrt(conf1, conf2, get_distance_fn(active_chains), get_sample_fn(conf1, active_chains), get_extend_fn(active_chains, STEP_SIZE), get_collision_fn(static_bodies, attached), restarts=0, iterations=50, smooth=None) if path is None: return yield (Traj(path, grasp), ) #################### cond_streams = [ # Pick/place trajectory EasyGenStream(inputs=[P, G], outputs=[Q, T], conditions=[], effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj), # Move trajectory EasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0), EasyGenStream(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, TOP_GRASPS[:MAX_GRASPS]) + map( POSE, problem.known_poses) initial_atoms = [ ConfEq(problem.initial_conf), HandEmpty(), ] + [ PoseEq(get_name(body), pose) for body, pose in problem.initial_poses if get_name(body) in problem.movable_names ] goal_literals = [ PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems() ] if problem.goal_conf is not None: goal_literals.append(ConfEq(problem.goal_conf)) stream_problem = STRIPStreamProblem(initial_atoms, And(*goal_literals), actions + axioms, cond_streams, constants) search_fn = get_fast_downward('eager', max_time=10, verbose=False) plan, _ = incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action else: print 'Failure' if is_window_active(window_3d) and plan is not None: robot_conf = problem.initial_conf placements = { get_name(body): body.applyTrans(pose) for body, pose in problem.initial_poses } attached = {'left': None, 'right': None} def _execute_traj(path): for j, conf in enumerate(path): draw_window_3d(window_3d, conf, placements.values(), attached) if step: raw_input('%s/%s) Step?' % (j, len(path))) else: window_3d.update() sleep(0.01) draw_window_3d(window_3d, robot_conf, placements.values()) raw_input('Start?') for i, (action, args) in enumerate(plan): if action.name == 'move': _, _, traj = args _execute_traj(traj.path) elif action.name == 'move_holding': _, _, traj, _, _ = args _execute_traj(traj.path) elif action.name == 'pick': obj, _, grasp, _, traj = args _execute_traj(traj.path[::-1]) attached[ARM] = name_to_body[obj].applyTrans( get_wrist_frame(grasp, robot, ARM).inverse()) del placements[obj] _execute_traj(traj.path) elif action.name == 'place': obj, pose, _, _, traj = args _execute_traj(traj.path[::-1]) placements[obj] = name_to_body[obj].applyTrans(pose) attached[ARM] = None _execute_traj(traj.path) else: raise ValueError(action.name) raw_input('Finish?')
def compile_problem(tamp_problem): """ Constructs a STRIPStream problem for the continuous TMP problem. :param tamp_problem: a :class:`.TMPProblem` :return: a :class:`.STRIPStreamProblem` """ B1, B2 = Param(BLOCK), Param(BLOCK) P1, P2 = Param(POSE), Param(POSE) Q1, Q2 = Param(CONF), Param(CONF) R = Param(REGION) actions = [ Action(name='pick', parameters=[B1, P1, Q1], condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)), effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))), Action(name='place', parameters=[B1, P1, Q1], condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1), ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))), Action(name='move', parameters=[Q1, Q2], condition=AtConf(Q1), effect=And(AtConf(Q2), Not(AtConf(Q1)))), Action(name='clean', parameters=[B1, R], condition=And(InRegion(B1, R), IsSink(R)), effect=And(Cleaned(B1))), Action(name='cook', parameters=[B1, R], condition=And(InRegion(B1, R), IsStove(R)), effect=And(Cooked(B1))), ] axioms = [ Axiom(effect=InRegion(B1, R), condition=Exists([P1], And(AtPose(B1, P1), Contained(B1, P1, R)))), Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), ] cond_streams = [ EasyGenStream(inputs=[R, B1], outputs=[P1], conditions=[CanPlace(B1, R)], effects=[Contained(B1, P1, R)], generator=lambda r, b: (sample_region_pose(r, b) for _ in irange(0, INF))), EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)], generator=lambda p: iter([inverse_kinematics(p)])), EasyTestStream(inputs=[R, B1, P1], conditions=[], effects=[Contained(B1, P1, R)], test=in_region, eager=EAGER_TESTS, plannable=False), EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)], test=lambda *args: not are_colliding(*args), eager=EAGER_TESTS), ] constants = [ REGION(tamp_problem.env_region), ] initial_atoms = [ AtConf(tamp_problem.initial_config), ] + [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] + [ CanPlace(block, tamp_problem.env_region) for block in tamp_problem.initial_poses ] if tamp_problem.initial_holding is None: initial_atoms.append(HandEmpty()) else: initial_atoms.append(Holding(tamp_problem.initial_holding)) goal_literals = [] if tamp_problem.goal_config is not None: goal_literals.append(AtConf(tamp_problem.goal_config)) if tamp_problem.goal_holding is False: goal_literals.append(HandEmpty()) elif tamp_problem.goal_holding is not None: goal_literals.append(Holding(tamp_problem.goal_holding)) for block, goal in tamp_problem.goal_poses: goal_literals.append(AtPose(block, goal)) for block, goal in tamp_problem.goal_regions: initial_atoms.append(CanPlace(block, goal)) goal_literals.append(InRegion(block, goal)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
def compile_problem(tamp_problem): """ Constructs a STRIPStream problem for the countable TMP problem. :param tamp_problem: a :class:`.TMPProblem` :return: a :class:`.STRIPStreamProblem` """ # NOTE - the simple focused algorithm gives "Could not find instantiation for PNE!" when this is moved outside B1, B2 = Param(BLOCK), Param(BLOCK) P1, P2 = Param(POSE), Param(POSE) Q1, Q2 = Param(CONF), Param(CONF) actions = [ Action(name='pick', parameters=[B1, P1, Q1], condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)), effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))), Action( name='place', parameters=[B1, P1, Q1], condition=And( Holding(B1), AtConf(Q1), LegalKin(P1, Q1), #ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))), #*[Or(Equal(B1, BLOCK(b2)), Safe(b2, P1)) for b2 in tamp_problem.initial_poses]), effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))), Action(name='move', parameters=[Q1, Q2], condition=AtConf(Q1), effect=And(AtConf(Q2), Not(AtConf(Q1)))), ] axioms = [ #Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), Axiom(effect=Safe(B2, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))), ] # NOTE - this needs to be inside the method so you make new streams each time cond_streams = [ #EasyGenStream(inputs=[P2], outputs=[P1], conditions=[], effects=[], # generator=lambda a: irange(0, NUM_POSES)), EasyGenStream(inputs=[], outputs=[P1], conditions=[], effects=[], generator=lambda: irange(0, NUM_POSES)), EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)], generator=lambda p: iter([p])), #EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)], # test=lambda b1, p1, b2, p2: p1 != p2, eager=EAGER_TESTS), EasyTestStream(inputs=[P1, P2], conditions=[], effects=[CollisionFree(P1, P2)], test=lambda p1, p2: p1 != p2, eager=EAGER_TESTS), ] constants = [] initial_atoms = [ AtConf(tamp_problem.initial_config), ] + [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] if tamp_problem.initial_holding is False: initial_atoms.append(HandEmpty()) else: initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK)) goal_literals = [] if tamp_problem.goal_holding is False: goal_literals.append(HandEmpty()) elif tamp_problem.goal_holding is not None: goal_literals.append(Holding(tamp_problem.goal_holding)) for block, goal in tamp_problem.goal_poses.iteritems(): goal_literals.append(AtPose(block, goal)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)