]), #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