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_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_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 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')