]), #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 ############ init['OY'] = (OY_motion, OY_action, OY_task) #======================================================= # Model and task for Pointing Youbot (PY) # pyr2 | # pyr3| # | # pyr1 pyr0| ########### PY motion ########## PY_node = [(-1.3, 1.10, 0.0), (-1.27, -0.1, 0.0), (-0.81, 0.35, 1.5)] PY_label = {
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]
# 'drop': (50, '1', set(['drop'])) #} action = { 'pick_from_floor': (10, '1', set([ 'pick_from_floor', ])), 'reach_max': (10, '1', set([ 'reach_max', ])), 'pick': (10, '1', set([ 'pick', ])) } robot_action = ActionModel(action) #robot_model = [robot_motion, init_pose, robot_action] ############################## # complete robot model # robot_full_model = MotActModel(robot_motion, robot_action) ############################## # specify tasks ########## only hard # hard_task = '<>(r1 && <> (r2 && <> r6)) && (<>[] r6)' #hard_task = '(<>(pick && <> drop)) && ([]<> r3) && ([]<> r6)' #soft_task = None ########## soft and hard # hard_task = '(([]<> r3) && ([]<> r4))'
((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, HEIGHT * 5 * RATE)), ] B_motion.add_un_edges(B_edge_list, unit_cost=0.1) ############################### ########### B action ########## B_action_dict = { 'grasp': (100, 'ball', set(['grasp'])), 'throw': (60, 'basket', set(['throw'])) } B_action = ActionModel(B_action_dict) ############################### ########### B task ############ B_task = '(<> (grasp && <> throw)) && (<>[] r1)' #B_task = '<> grasp && (<>[] r2)' ####################### init['B'] = (B_motion, B_action, B_task) ##################################################### #======================================================= #======================================================= #======================================================= ########### C motion ########## colormap = {'ball': 'red', 'obs': 'black', 'basket': 'yellow'} C_symbols = colormap.keys() WIDTH = 240 # cm