# 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 ############
B_init_pose = (WIDTH*RATE,HEIGHT*RATE);
B_node_dict ={
# lower three rooms
(WIDTH*RATE,HEIGHT*RATE): set(['r1']),
(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,HEIGHT*5*RATE)),
]
B_motion.add_un_edges(B_edge_list,unit_cost=0.1)
# |oyr3
# |
# |oyr0 oyr1
################ OY motion ##################
OY_node = [(-0.65,-1.0,0.0),(-0.65,-1.0,0.0),(0.4,-1.01,0.0),(-0.47,-1.1,-0.9)]
#OY_node=[(-0.5,0.5,0.0),(-0.5,1.0,-1.57),(0.5,1.0,-1.57),(-0.0,0.5,-2.6)]
#OY_node=[(-0.0,1.0,0.0),(-0.0,1.0,-0.0),(0.5,1.3,-1.57),(-0.00,0.5,-2.6)]
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)
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]
(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,
(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