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