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 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
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
