def observable_belief(robot, world):
holding = None
surfaces = []
items = []
surface_types = {s.type: s for s in world.surfaces}
item_types = {i.type: i for i in world.items}
grab_info_from_name = {info._grabbedname: info for info in robot.GetGrabbedInfo()}
for body in robot.GetEnv().GetBodies():
if body.IsRobot():
continue
name = get_name(body)
ty, _ = name.split('-')
if name in grab_info_from_name:
assert (holding is None) and (ty in item_types)
#info = grab_info_from_name[name]
arm = robot.GetActiveManipulator()
#link = robot.GetActiveManipulator().GetEndEffector()
#world_from_gripper = link.GetTransform()
world_from_gripper = arm.GetTransform()
world_from_body = body.GetTransform()
grasp = Grasp(np.linalg.inv(world_from_gripper).dot(world_from_body))
#grasp = Grasp(np.linalg.inv(world_from_body).dot(world_from_gripper))
#print link, info._robotlinkname # Equal
#grasp = Grasp(np.linalg.inv(info._trelative))
holding = Object(ty, grasp, True)
else:
pose = pose_from_trans(body.GetTransform())
if ty in surface_types:
[mesh] = [l.GetCollisionData() for l in body.GetLinks()]
surfaces.append(Surface(ty, pose, mesh.vertices))
elif ty in item_types:
items.append(Object(ty, pose, True))
else:
raise ValueError(ty)
return Belief(holding, surfaces, items)
def update_belief(robot, world, belief, plan):
# TODO: before or after movement?
holding, surfaces, items = belief.holding, belief.surfaces, belief.items
surface_types = {s.type: s for s in world.surfaces}
item_types = {i.type: i for i in world.items}
head = robot.GetManipulator('head')
for action, args in plan:
if action.name == 'scan_room':
# TODO: observe objects under some conditions
surfaces = world.surfaces[:]
elif action.name == 'move_head':
#items = world.items[:]
# TODO: should this be a sense action?
pass
elif action.name == 'move_base':
items = [Object(ty, p, False) for ty, p, _ in items]
elif action.name == 'register':
pass
elif action.name == 'pick':
if holding is not None: print holding
assert holding is None
_, p, g, bg, _ = args # TODO: use g.value
item = items.pop(get_pose_index(items, p.value))
holding = Object(item.type, g, False)
elif action.name == 'place':
assert holding is not None
_, p, g, bg, _ = args
items.append(Object(holding.type, p.value, False))
holding = None
else:
raise ValueError(action.name)
# TODO: add noise to these?
#for name, pose in observe_env(robot, p_hit_vis=0.25).items():
visible = observe_env(robot, p_hit_vis=1)
print 'Visible:', visible.keys()
for name, pose in visible.items():
ty, _ = name.split('-')
if ty in item_types:
# TODO: probabilities for detect and register
for index, item in reversed(list(enumerate(items))):
if ty == item.type:
point = point_from_pose(item.pose) if is_oriented(item.pose) else item.pose
if (point is None) or np.allclose(point_from_pose(pose), point):
items.pop(index)
#index = get_point_index(items, pose)
distance = length(get_point(head)[:2] - point_from_pose(pose)[:2])
pose = pose if (distance <= MAX_REG_DISTANCE) else point_from_pose(pose)
items.append(Object(ty, pose, is_oriented(pose)))
elif ty in surface_types:
[matched] = [s for s in surfaces if (s.type == ty) and (s.pose is not None) and
np.allclose(s.pose, pose)]
matched.observations += 1
return Belief(holding, surfaces, items)
def get_world_5(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
soup_z = np.min(object_meshes['soup'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2)),
Surface('table', get_table_pose(0, TABLE_X, TABLE_Z),
get_rectangle(1.2, 0.6))
]
items = [
Object('block', get_table_pose(TABLE_X, 0.3, TABLE_Z - block_z), None),
Object('soup', get_table_pose(TABLE_X, -0.3, TABLE_Z - soup_z), None)
]
return World(None, surfaces, items), Task(holding='block')
def get_problem_5_3(object_meshes):
oil_z = np.min(object_meshes['oil'].vertices[:, 2])
block_z = np.min(object_meshes['block'].vertices[:, 2])
soup_z = np.min(object_meshes['soup'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(2, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [
Object('oil', get_table_pose(TABLE_X, -0.25, TABLE_Z - oil_z), True),
Object('block', get_table_pose(TABLE_X, 0, TABLE_Z - block_z), True),
Object('soup', get_table_pose(TABLE_X, 0.25, TABLE_Z - soup_z), True)
]
return Belief(None, surfaces, items), Task(holding='block')
def get_problem_1_2(object_meshes):
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [Object('block', None, False)]
return Belief(None, surfaces, items), Task(holding='block')
def get_problem_4(object_meshes):
table_x = 0.5
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(table_x, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [Object('block', get_table_pose(table_x, 0.2, TABLE_Z - block_z))]
return Belief(None, surfaces, items), Task(holding='block')
def get_world_3(object_meshes):
soup_z = np.min(object_meshes['soup'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [
Object('soup', get_table_pose(TABLE_X, 0, TABLE_Z - soup_z), None)
]
return World(None, surfaces, items), Task(holding='soup')
def get_problem_3_2(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [
Object('block', get_table_pose(TABLE_X, 0, TABLE_Z - block_z), True)
]
return Belief(None, surfaces,
items), Task(object_surfaces=[('block', 'table')])
def get_world_4(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2))
]
items = [
Object('block', get_table_pose(TABLE_X, 0.3, TABLE_Z - block_z), None)
]
#return World(None, surfaces, items), Task(localized_items=['block'])
return World(None, surfaces, items), Task(registered_items=['block'])
def get_world_7(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(0, -TABLE_X, TABLE_Z),
get_rectangle(1.2, 0.6)),
Surface('table', get_table_pose(0, TABLE_X, TABLE_Z),
get_rectangle(1.2, 0.6))
]
items = [
Object('block', get_table_pose(0.3, -TABLE_X, TABLE_Z - block_z), None)
]
return World(None, surfaces, items), Task(left_items=['block'])
def get_world_2(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(TABLE_X, 0, TABLE_Z),
get_rectangle(0.6, 1.2)),
Surface('dinner', get_table_pose(0, TABLE_X, TABLE_Z),
get_rectangle(1.2, 0.6))
]
items = [
Object('block', get_table_pose(TABLE_X, 0, TABLE_Z - block_z), None)
]
return World(None, surfaces,
items), Task(object_surfaces=[('block', 'dinner')])
def get_lis_world(object_meshes):
block_z = np.min(object_meshes['block'].vertices[:, 2])
surfaces = [
Surface('table', get_table_pose(1.5, 0.5, TABLE_Z),
get_rectangle(0.6, 1.2)),
Surface('table', get_table_pose(0, -1.5, TABLE_Z),
get_rectangle(1.2, 0.6)),
#Surface('table', get_table_pose(-1.5, 1.5, TABLE_Z, np.pi/4), get_rectangle(1.2, 0.6)),
]
items = [
Object('block', get_table_pose(1.25, 0.0, TABLE_Z - block_z), None)
]
return World(None, surfaces, items), Task(right_items=['block'])
def update_world(robot, world, belief, action, args):
# Using the robot state (and octree) as ground truth
if action.name == 'scan_room':
#new_world = copy.copy(world)
new_belief = Belief(belief.holding, world.surfaces[:], belief.items[:])
return world, new_belief
#elif action.name == 'scan_table':
# new_belief = Belief(belief.holding, belief.surfaces[:], world.items[:])
# return world, new_belief
elif action.name == 'move_head':
_, q2 = args
set_conf(robot, 'head', q2.value)
point = get_point(robot.GetManipulator('head'))
# TODO: condition here
# TODO: could update the measurement here
#max_distance = MAX_REG_DISTANCE
max_distance = 10
items = [
Object(ty, p, (norm(point_from_pose(p) - point) <= max_distance))
for ty, p, _ in world.items
]
return world, Belief(belief.holding, belief.surfaces[:], items)
elif action.name == 'move_base':
_, q2 = args
set_conf(robot, 'base', q2.value)
items = [Object(ty, p, False) for ty, p, _ in world.items]
return world, Belief(belief.holding, belief.surfaces[:], items)
elif action.name == 'pick':
_, p, g, bg, _ = args # TODO: use g.value
world_index = get_pose_index(world.items, p.value)
world_items = world.items[:]
world_item = world_items.pop(world_index)
belief_index = get_pose_index(belief.items, p.value)
belief_items = belief.items[:]
belief_item = belief_items.pop(belief_index)
#assert world_item == belief_item # Maybe it's good to not check
return World(Object(world_item.type, g, None), world.surfaces, world_items), \
Belief(Object(belief_item.type, g, False), world.surfaces, belief_items)
elif action.name == 'place':
_, p, g, bg, _ = args
world_items = world.items[:] + [
Object(world.holding.type, p.value, None)
]
belief_items = belief.items[:] + [
Object(belief.holding.type, p.value, False)
]
return World(None, world.surfaces, world_items), \
Belief(None, world.surfaces, belief_items)
else:
raise ValueError(action.name)
def get_problem_2_1(_):
surfaces = [Surface('table', None, None), Surface('dinner', None, None)]
items = [Object('block', None, False)]
return Belief(None, surfaces,
items), Task(object_surfaces=[('block', 'dinner')])
def get_problem_1_1(object_meshes):
surfaces = [Surface('table', None, None)]
items = [Object('block', None, False)]
return Belief(None, surfaces, items), Task(holding='block')
def initial_visible_belief(world):
items = [Object(item.type, point_from_pose(item), False) for item in world.items]
return Belief(world.holding, world.surfaces, items)
