# Model and task for Observing Youbot (OY) # | oyr2 # |oyr3 # | # |oyr0 oyr1 ################ OY motion ################## OY_node = [(0.8, 0.72, 3.0), (0.89, -0.24, 2.6)] OY_label = { OY_node[0]: set(['oyr2']), OY_node[1]: set([ 'oyr1', ]), #oyball } OY_init_pose = OY_node[0] OY_symbols = set(['oyr0', 'oyr1']) OY_motion = MotionFts(OY_label, OY_symbols, 'OY-workspace') OY_motion.set_initial(OY_init_pose) #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] OY_edge = [ (OY_node[0], OY_node[1]), ] OY_motion.add_un_edges(OY_edge, unit_cost=10.0) ########### OY action ########## OY_action_label = { 'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])), } OY_action = ActionModel(OY_action_label) ########### OY task ############ #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' OY_task = '(<> (oyr1 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)' ########### OY initialize ############
(WIDTH * 3 * RATE, HEIGHT * RATE): set(['r2']), (WIDTH * 5 * RATE, HEIGHT * RATE): set(['r3']), # cooridor three parts (WIDTH * RATE, HEIGHT * 3 * RATE): set(['c1', 'ball']), (WIDTH * 3 * RATE, HEIGHT * 3 * RATE): set([ 'c2', ]), (WIDTH * 5 * RATE, HEIGHT * 3 * RATE): set(['c3']), # upper three rooms (WIDTH * RATE, HEIGHT * 5 * RATE): set([ 'r4', ]), (WIDTH * 3 * RATE, HEIGHT * 5 * RATE): set(['r5', 'basket']), (WIDTH * 5 * RATE, HEIGHT * 5 * RATE): set(['r6']), } B_motion = MotionFts(B_node_dict, B_symbols, 'office') B_motion.set_initial(B_init_pose) B_edge_list = [ # 1st column ((WIDTH * RATE, HEIGHT * RATE), (WIDTH * RATE, HEIGHT * 3 * RATE)), ((WIDTH * RATE, HEIGHT * 3 * RATE), (WIDTH * RATE, HEIGHT * 5 * RATE)), # 2nd row ((WIDTH * RATE, HEIGHT * 3 * RATE), (WIDTH * 3 * RATE, HEIGHT * 3 * RATE)), ((WIDTH * 3 * RATE, HEIGHT * 3 * RATE), (WIDTH * 5 * RATE, HEIGHT * 3 * RATE)), # 2nd column ((WIDTH * 3 * RATE, HEIGHT * RATE), (WIDTH * 3 * RATE, HEIGHT * 3 * RATE)), ((WIDTH * 3 * RATE, HEIGHT * 3 * RATE), (WIDTH * 3 * RATE, HEIGHT * 5 * RATE)), # 3rd column ((WIDTH * 5 * RATE, HEIGHT * RATE), (WIDTH * 5 * RATE, HEIGHT * 3 * RATE)), ((WIDTH * 5 * RATE, HEIGHT * 3 * RATE), (WIDTH * 5 * RATE,
def __init__(self, _file): self.file = _file stream = file(self.file, 'r') data_file = yaml.load(stream) FTS = data_file['FTS'] _map = data_file['Map'] stream.close() regions = {} ap = set() ############################## # motion FTS for i in range(0, len(FTS)): #ap.update({FTS.keys()[i]}) for j in range(0, len(FTS[FTS.keys()[i]]['propos'])): if FTS[FTS.keys()[i]]['propos'][j] not in ap: ap.update({FTS[FTS.keys()[i]]['propos'][j]}) regions.update({ (tuple(FTS[FTS.keys()[i]]['pose']['position']), tuple(FTS[FTS.keys()[i]]['pose']['orientation'])): set(FTS[FTS.keys()[i]]['propos']) }) init_pose = ((7.77, 7.00, 0.0), (0.0, 0.0, 0.0, 1.0)) robot_motion = MotionFts(regions, ap, _map) robot_motion.set_initial(init_pose) for i in range(0, len(FTS)): for j in range(0, len(FTS[FTS.keys()[i]]['edges'])): robot_motion.add_un_edges( [[(tuple(FTS[FTS.keys()[i]]['pose']['position']), tuple(FTS[FTS.keys()[i]]['pose']['orientation'])), (tuple(FTS[FTS[FTS.keys()[i]]['edges'][j]['target']] ['pose']['position']), tuple(FTS[FTS[FTS.keys()[i]]['edges'][j]['target']] ['pose']['orientation']))]], unit_cost=FTS[FTS.keys()[i]]['edges'][j]['cost']) ############################## # action FTS ############# no action model # action = dict() ############# with action # for supported actions in play_motion # see http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/play_motion############################## action = {} robot_action = ActionModel(action) ############################## # specify tasks robot_hard_task = '' robot_soft_task = '' robot_task = [robot_hard_task, robot_soft_task] self.robot_model = [robot_motion, init_pose, robot_action, robot_task]
# Model and task for Observing Youbot (OY) # | oyr2 # |oyr3 # | # |oyr0 oyr1 ################ OY motion ################## OY_node=[(-0.7,0.8,0.0),(-0.7,1.5,-1.57),(-0.2,1.5,-1.57),(-0.91,1.08,-0.79+1.57+0.3)] OY_label={ OY_node[0] : set(['oyr0']), OY_node[1]: set(['oyr1',]), OY_node[2]: set(['oyr2',]), OY_node[3]: set(['oyr3',]), } OY_init_pose=OY_node[0] OY_symbols=set(['oyr0','oyr1','oyr2','oyr3']) OY_motion=MotionFts(OY_label,OY_symbols,'OY-workspace') OY_motion.set_initial(OY_init_pose) #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] OY_edge=[(OY_node[0],OY_node[1]),(OY_node[1],OY_node[2]),(OY_node[2],OY_node[3]), (OY_node[1],OY_node[3]),] OY_motion.add_un_edges(OY_edge,unit_cost=10.0) ########### OY action ########## OY_action_label={ 'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])), } OY_action = ActionModel(OY_action_label) ########### OY task ############ #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' OY_task = '<>(oyr1 && <> (oyr2 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)' ########### OY initialize ############ init['OY']=(OY_motion, OY_action, OY_task)
(14.5, 4.5, 0.5), (18.5, 4.5, 0.5), (3.5, 8.5, 0.5), (11.5, 8.5, 0.5), (1.0, 5.0, 1.0), (11.0, 4.0, 1.0), (17.0, 4.0, 1.0), (8.0, 7.0, 1.0), ] regions = dict() for k in range(len(ap)): regions[loc[k]] = set([ ap[k], ]) init_pose = loc[0] robot_motion = MotionFts(regions, set(ap), 'hotel') robot_motion.set_initial(list(init_pose)) edges = [(0, 1), (0, 2), (0, 4), (0, 9), (1, 4), (1, 2), (2, 4), (3, 11), (5, 8), (5, 11), (5, 12), (6, 11), (7, 9), (7, 12), (8, 12), (8, 11), (9, 12), (11, 12)] edge_list = [(loc[e[0]], loc[e[1]]) for e in edges] robot_motion.add_un_edges(edge_list, unit_cost=2) ############################## # action FTS ############# no action model action = dict() robot_action = ActionModel(action) robot_model = [robot_motion, init_pose, robot_action]
for i in range(0, len(data)): test_ap.update({data.keys()[i]}) #test.update( {tuple(data[data.keys()[i]]['position']): set([data.keys()[i]])}) test.update({ (tuple(data[data.keys()[i]]['pose']['position']), tuple(data[data.keys()[i]]['pose']['orientation'])): set([data.keys()[i]]) }) print('test') print(test) print(test_ap) init_pose = ((7.77, 7.00, 0.0), (0.0, 0.0, 0.0, 1.0)) #robot_motion = MotionFts(regions, ap, 'pal_office' ) robot_motion = MotionFts(test, test_ap, 'pal_office') robot_motion.set_initial(init_pose) #robot_motion.add_full_edges(unit_cost = 0.1) for i in range(0, len(data)): for j in range(0, len(data[data.keys()[i]]['edges'])): print(data[data.keys()[i]]['edges'][j]['cost']) #robot_motion.add_un_edges([[tuple(data[data.keys()[i]]['position']), tuple(data[data[data.keys()[i]]['edges'][j]['target']]['position'])]], unit_cost = data[data.keys()[i]]['edges'][j]['cost']) robot_motion.add_un_edges( [[(tuple(data[data.keys()[i]]['pose']['position']), tuple(data[data.keys()[i]]['pose']['orientation'])), (tuple(data[data[data.keys()[i]]['edges'][j]['target']]['pose'] ['position']), tuple(data[data[data.keys()[i]]['edges'][j]['target']]['pose'] ['orientation']))]], unit_cost=data[data.keys()[i]]['edges'][j]['cost'])
import time ############################## # motion FTS ap = {'r1', 'r2', 'r3', } regions = { (0.57, -10.01, 0.95): set(['r1',]), (-3.25, -0.31, 0.82): set(['r2',]), (-2.89, -9.01, 0.52): set(['r3',]), } init_pose = (0.57, -10.01, 0.95) robot_motion = MotionFts(regions, ap, 'office' ) robot_motion.set_initial((0.57, -10.01, 0.95)) robot_motion.add_full_edges(unit_cost = 0.1) ############################## # action FTS ############# no action model # action = dict() ############# with action # for supported actions in play_motion # see http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/play_motion # action = { 'pick': (100, 'r', set(['pick'])), # 'drop': (50, '1', set(['drop'])) # }
loc[0]: set([ 'r0', ]), loc[1]: set([ 'r1', ]), loc[2]: set([ 'r2', ]), loc[3]: set([ 'r3', ]), } init_pose = loc[0] robot_motion = MotionFts(regions, set(ap), 'office') robot_motion.set_initial(list(init_pose)) edge_list = [(loc[0], loc[1]), (loc[0], loc[2]), (loc[1], loc[2]), (loc[2], loc[3]), (loc[3], loc[0])] robot_motion.add_un_edges(edge_list, unit_cost=0.1) ############################## # action FTS ############# no action model action = dict() robot_action = ActionModel(action) robot_model = [robot_motion, init_pose, robot_action] ############################## # complete robot model