def multi_robot_test(): import blast_world_test world_i = blast_world_test.make_table_top_world(".", True) stair5 = blast_world.BlastRobot("stair5", blast_world.BlastPt(10.000, 40.957, 0.148, "clarkcenterfirstfloor"), world_i.types.get_robot("pr2-cupholder")) world_i.append_robot(stair5) world_i.take_action("stair5", "tuck-both-arms", {}) #To debug with arms tucked. world = blast_planner.BlastPlannableWorld(world_i) #print "-"*180 #print world.world.to_text() #print "-"*180 world.append_plan([blast_world.BlastCodeStep(None, "CALLSUB", {'sub': 'hunt_objects', 'object_types': "coffee_cup", 'holder': 'stair4.cupholder'}, 'plan_return'), blast_world.BlastCodeStep(None, "IF", {"condition": blast_world.BlastParameterPtr('plan_return'), 'label_true': "success", 'label_false': 'failure'}), blast_world.BlastCodeStep("success", "RETURN"), blast_world.BlastCodeStep("failure", "FAIL"),], ["stair4", "stair5",]) #r = world.plan_hunt("stair4", "cupholder", "coffee_cup") world.try_exec() #print "-"*180 #print world.world.to_text() #print "-"*180 return False
def test_multi_robot(): import blast_world_test man = BlastManager(["test_actions",], blast_world_test.make_table_top_world(False)) w = open("table_1_objects.txt", "w") w.close() w = open("table_2_objects.txt", "w") w.write("coffee_cup\n") w.write("coffee_cup\n") w.close() stair5 = blast_world.BlastRobot("stair5", blast_world.BlastPt(10.000, 40.957, 0.148, "clarkcenterfirstfloor"), man.world.world.types.get_robot("pr2-cupholder")) man.world.world.append_robot(stair5) man.world.world.take_action("stair5", "tuck-both-arms", {}) #To debug with arms tucked. print man.world.world.to_text() if not man.plan_hunt("stair4", "cupholder", "coffee_cup"): return False #print man.world.world.to_text() #if not man.plan_hunt("stair5", "cupholder", "coffee_cup"): # return False print man.world.world.to_text() return True
def coffee_hunt_test(): import blast_world_test world = blast_planner.BlastPlannableWorld(blast_world_test.make_table_top_world(".", False)) initial_pickup_point = blast_world.BlastPt(17.460, 38.323, -2.330, "clarkcenterfirstfloor") objects_to_add = {"table_1": 1, "table_2": 1, "first_scan": None} def ac(r, a, p): #Test add the cups tn = p.get("table", None) if type(tn) == blast_world.BlastSurface: tn = tn.name if r == "stair4" and a == "table-coffee-scan": if not objects_to_add["first_scan"]: objects_to_add["first_scan"] = tn if objects_to_add[tn] > 0: world.world.add_surface_object(tn, "coffee_cup", blast_world.BlastPos(0.6602, 0.0, 0.762, 0.0, 0.0, 0.0)) objects_to_add[tn] -= 1 if r == "stair4" and a == "table-pick-left" and tn == objects_to_add["first_scan"]: print "THERE IS NO OBJECT - THE PHANTOM CUP" return "no_object" if a == "table-place-left": time.sleep(30) return True world.action_default = True world.action_callback = ac world.append_plan([blast_world.BlastCodeStep(None, "CALLSUB", {'sub': 'hunt_objects', 'object_types': "coffee_cup", 'holder': 'stair4.cupholder'}, 'plan_return'), blast_world.BlastCodeStep(None, "IF", {"condition": ('?', blast_world.BlastParameterPtr('plan_return')), 'label_true': "success", 'label_false': 'failure'}), blast_world.BlastCodeStep("success", "GETOBJECT", {'holder': 'stair4.cupholder'}, 'object_n'), #blast_world.BlastCodeStep(None, "PLAN", {'extra_steps': [("stair4", "table-place-left", {"table": "table_1", "position": "table_1, Pos(0.6602, 0.10398, 0.762, 0.0, 0.0, 0.0)"}),], # }, 'plan_return'), blast_world.BlastCodeStep(None, "PLAN", {'world_limits': {'place-objects': [{'object': blast_world.BlastParameterPtr('object_n'), 'surface': 'table_1', 'position': 'Pos(0.6602, 0.10398, 0.762, 0.0, 0.0, 0.0)', } ], }}, 'plan_return'), blast_world.BlastCodeStep(None, "IF", {"condition": ("?", blast_world.BlastParameterPtr('plan_return')), 'label_true': "success2", 'label_false': 'failure'}), blast_world.BlastCodeStep("success2", "RETURN"), blast_world.BlastCodeStep("failure", "FAIL"),], ["stair4",]) world.run(True) world.print_old() return True
def overplan(): import blast_world_test world_i = blast_world_test.make_table_top_world(".", False) stair5 = blast_world.BlastRobot("stair5", blast_world.BlastPt(10.000, 40.957, 0.148, "clarkcenterfirstfloor"), world_i.types.get_robot("pr2-cupholder")) world_i.append_robot(stair5) world_i.take_action("stair5", "tuck-both-arms", {}) #To debug with arms tucked. world = blast_planner.BlastPlannableWorld(world_i) world.no_exec_debug = True #Plan to location. world.append_plan([blast_world.BlastCodeStep(None, "PLAN", {'world_limits': {"robot-location": {"stair5": blast_world.BlastPt(15.000, 20.957, 0.148, "clarkcenterfirstfloor")}, }, 'extra_goals': {'Pt': [blast_world.BlastPt(15.000, 20.957, 0.148, "clarkcenterfirstfloor")]}}, 'plan_return'), blast_world.BlastCodeStep(None, "IF", {"condition": blast_world.BlastParameterPtr('plan_return'), 'label_true': "success", 'label_false': 'failure'}), blast_world.BlastCodeStep("success", "RETURN"), blast_world.BlastCodeStep("failure", "FAIL"), ], ["stair5",]) #Make stair5 tuck arms but order stair4 to move into position before it can happen world.append_plan([blast_world.BlastCodeStep(None, "PLAN", {'world_limits': {"robot-location": {"stair4": blast_world.BlastPt(20.000, 20.957, 0.148, "clarkcenterfirstfloor")}, }, 'extra_steps': [("stair5", "tuck-both-arms", {})], 'extra_goals': {'Pt': [blast_world.BlastPt(20.000, 20.957, 0.148, "clarkcenterfirstfloor")]}}, 'plan_return'), blast_world.BlastCodeStep(None, "IF", {"condition": blast_world.BlastParameterPtr('plan_return'), 'label_true': "success", 'label_false': 'failure'}), blast_world.BlastCodeStep("success", "RETURN"), blast_world.BlastCodeStep("failure", "FAIL"), ], ["stair4", "stair5",]) world.try_exec() #r = world.plan_to_location("stair5", blast_world.BlastPt(15.000, 20.957, 0.148, "clarkcenterfirstfloor")) #if not r: return False #r = world.plan_action("stair5", "tuck-both-arms", {}, {"robot-location": {"stair4": blast_world.BlastPt(20.000, 20.957, 0.148, "clarkcenterfirstfloor")}}) #if not r: return False r = False return r
def test_place(): import blast_world_test w = open("table_1_objects.txt", "w") w.close() w = open("table_2_objects.txt", "w") w.write("coffee_cup\n") w.close() man = BlastManager(["test_actions",], blast_world_test.make_table_top_world()) print "-"*120 print " "*60, "BEFORE PICK" print man.world.world.to_text() print "-"*120 if not man.plan_hunt("stair4", "cupholder", "coffee_cup"): return False print "-"*120 print " "*60, "AFTER PICK" print man.world.world.to_text() print "-"*120 #man.plan_action("stair4", "unstash-cupholder", {}) object_uid = man.world.world.robots["stair4"].holders["cupholder"].uid print "Hold onto the uid of", object_uid r = man.plan_action("stair4", "table-place-left", {"table": "table_1", "position": ("table_1", "Pos(0.6602, 0.10398, 0.762, 0.0, 0.0, 0.0)")}, {"robot-holders": {"stair4": {"left-arm": object_uid}}}) if not r: return False print "-"*120 print " "*60, "AFTER PLACE" print man.world.world.to_text() print "-"*120 a = "937e356e0aebbd33554a3380547260698f20b9ca7dc2c6f23361598c994f85648b069fa200933a1f2a7c8f352" a = a + "d13d8ca3db1b0a1dc50f28e238cd4177368f587817290159b54892eb02743b464ca71a606d13a1d" if a != man.world.world.get_hex_hash_state(): print "Invalid state, failed test" return False return True