Exemplo n.º 1
0
def get_kitchen_task(arm='left', grasp_type='top'):
    with HideOutput():
        pr2 = create_pr2(use_drake=USE_DRAKE_PR2)
    set_arm_conf(pr2, arm, get_carry_conf(arm, grasp_type))
    open_arm(pr2, arm)
    other_arm = get_other_arm(arm)
    set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
    close_arm(pr2, other_arm)

    table, cabbage, sink, stove = create_kitchen()
    floor = get_bodies()[1]
    class_from_body = {
        table: 'table',
        cabbage: 'cabbage',
        sink: 'sink',
        stove: 'stove',
    } # TODO: use for debug
    movable = [cabbage]
    surfaces = [table, sink, stove]
    rooms = [floor]

    return BeliefTask(robot=pr2, arms=[arm], grasp_types=[grasp_type],
                      class_from_body=class_from_body,
                      movable=movable, surfaces=surfaces, rooms=rooms,
                      #goal_localized=[cabbage],
                      #goal_registered=[cabbage],
                      #goal_holding=[(arm, cabbage)],
                      #goal_on=[(cabbage, table)],
                      goal_on=[(cabbage, sink)],
                      )
Exemplo n.º 2
0
def packed(arm='left', grasp_type='top', num=5):
    # TODO: packing problem where you have to place in one direction
    base_extent = 5.0

    base_limits = (-base_extent / 2. * np.ones(2),
                   base_extent / 2. * np.ones(2))
    block_width = 0.07
    block_height = 0.1
    #block_height = 2*block_width
    block_area = block_width * block_width

    #plate_width = 2*math.sqrt(num*block_area)
    plate_width = 0.27
    #plate_width = 0.28
    #plate_width = 0.3
    print('Width:', plate_width)
    plate_width = min(plate_width, 0.6)
    plate_height = 0.001

    other_arm = get_other_arm(arm)
    initial_conf = get_carry_conf(arm, grasp_type)

    add_data_path()
    floor = load_pybullet("plane.urdf")
    pr2 = create_pr2()
    set_arm_conf(pr2, arm, initial_conf)
    open_arm(pr2, arm)
    set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
    close_arm(pr2, other_arm)
    set_group_conf(pr2, 'base',
                   [-1.0, 0, 0])  # Be careful to not set the pr2's pose

    table = create_table()
    plate = create_box(plate_width, plate_width, plate_height, color=GREEN)
    plate_z = stable_z(plate, table)
    set_point(plate, Point(z=plate_z))
    surfaces = [table, plate]

    blocks = [
        create_box(block_width, block_width, block_height, color=BLUE)
        for _ in range(num)
    ]
    initial_surfaces = {block: table for block in blocks}

    min_distances = {block: 0.05 for block in blocks}
    sample_placements(initial_surfaces, min_distances=min_distances)

    return Problem(
        robot=pr2,
        movable=blocks,
        arms=[arm],
        grasp_types=[grasp_type],
        surfaces=surfaces,
        #goal_holding=[(arm, block) for block in blocks])
        goal_on=[(block, plate) for block in blocks],
        base_limits=base_limits)
Exemplo n.º 3
0
def problem1(n_rovers=1, n_objectives=1, n_rocks=2, n_soil=2, n_stores=1):
    base_extent = 5.0
    base_limits = (-base_extent / 2. * np.ones(2),
                   base_extent / 2. * np.ones(2))

    floor = create_box(base_extent, base_extent, 0.001,
                       color=TAN)  # TODO: two rooms
    set_point(floor, Point(z=-0.001 / 2.))

    add_data_path()
    lander = load_pybullet(HUSKY_URDF, scale=1)
    lander_z = stable_z(lander, floor)
    set_point(lander, Point(-2, -2, lander_z))
    #wait_for_user()

    mount_width = 0.5
    mound_height = mount_width
    mound1 = create_box(mount_width, mount_width, mound_height, color=GREY)
    set_point(mound1, [+2, 1.5, mound_height / 2.])
    mound2 = create_box(mount_width, mount_width, mound_height, color=GREY)
    set_point(mound2, [-2, 1.5, mound_height / 2.])

    initial_surfaces = {}

    rover_confs = [(+1, -1.75, np.pi), (-1, -1.75, 0)]
    assert n_rovers <= len(rover_confs)

    #body_names = map(get_name, env.GetBodies())
    landers = [lander]
    stores = ['store{}'.format(i) for i in range(n_stores)]

    #affine_limits = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
    rovers = []
    for i in range(n_rovers):
        robot = create_pr2()
        dump_body(robot)  # camera_rgb_optical_frame
        rovers.append(robot)
        set_group_conf(robot, 'base', rover_confs[i])
        for arm in ARM_NAMES:
            set_arm_conf(robot, arm, arm_conf(arm, REST_LEFT_ARM))
            close_arm(robot, arm)

    obj_width = 0.07
    obj_height = 0.2

    objectives = []
    for _ in range(n_objectives):
        body = create_box(obj_width, obj_width, obj_height, color=BLUE)
        objectives.append(body)
        initial_surfaces[body] = mound1
    for _ in range(n_objectives):
        body = create_box(obj_width, obj_width, obj_height, color=RED)
        initial_surfaces[body] = mound2

    # TODO: it is moving to intermediate locations attempting to reach the rocks
    rocks = []
    for _ in range(n_rocks):
        body = create_box(0.075, 0.075, 0.05, color=BLACK)
        rocks.append(body)
        initial_surfaces[body] = floor
    soils = []
    for _ in range(n_soil):
        body = create_box(0.1, 0.1, 0.025, color=BROWN)
        soils.append(body)
        initial_surfaces[body] = floor
    sample_placements(initial_surfaces)

    #for name in rocks + soils:
    #    env.GetKinBody(name).Enable(False)
    return RoversProblem(rovers, landers, objectives, rocks, soils, stores,
                         base_limits)
Exemplo n.º 4
0
def blocked(arm='left', grasp_type='side', num=1):
    x_extent = 10.0

    base_limits = (-x_extent / 2. * np.ones(2), x_extent / 2. * np.ones(2))
    block_width = 0.07
    #block_height = 0.1
    block_height = 2 * block_width
    #block_height = 0.2
    plate_height = 0.001
    table_x = (x_extent - 1) / 2.

    other_arm = get_other_arm(arm)
    initial_conf = get_carry_conf(arm, grasp_type)

    add_data_path()
    floor = load_pybullet("plane.urdf")
    pr2 = create_pr2()
    set_arm_conf(pr2, arm, initial_conf)
    open_arm(pr2, arm)
    set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
    close_arm(pr2, other_arm)
    set_group_conf(
        pr2, 'base',
        [x_extent / 4, 0, 0])  # Be careful to not set the pr2's pose

    table1 = create_table()
    set_point(table1, Point(x=+table_x, y=0))
    table2 = create_table()
    set_point(table2, Point(x=-table_x, y=0))
    #table3 = create_table()
    #set_point(table3, Point(x=0, y=0))

    plate = create_box(0.6, 0.6, plate_height, color=GREEN)
    x, y, _ = get_point(table1)
    plate_z = stable_z(plate, table1)
    set_point(plate, Point(x=x, y=y - 0.3, z=plate_z))
    #surfaces = [table1, table2, table3, plate]
    surfaces = [table1, table2, plate]

    green1 = create_box(block_width, block_width, block_height, color=BLUE)
    green1_z = stable_z(green1, table1)
    set_point(green1, Point(x=x, y=y + 0.3, z=green1_z))
    # TODO: can consider a fixed wall here instead

    spacing = 0.15

    #red_directions = [(-1, 0), (+1, 0), (0, -1), (0, +1)]
    red_directions = [(-1, 0)]
    #red_directions = []
    red_bodies = []
    for red_direction in red_directions:
        red = create_box(block_width, block_width, block_height, color=RED)
        red_bodies.append(red)
        x, y = get_point(green1)[:2] + spacing * np.array(red_direction)
        z = stable_z(red, table1)
        set_point(red, Point(x=x, y=y, z=z))

    wall1 = create_box(0.01, 2 * spacing, block_height, color=GREY)
    wall2 = create_box(spacing, 0.01, block_height, color=GREY)
    wall3 = create_box(spacing, 0.01, block_height, color=GREY)
    z = stable_z(wall1, table1)
    x, y = get_point(green1)[:2]
    set_point(wall1, Point(x=x + spacing, y=y, z=z))
    set_point(wall2, Point(x=x + spacing / 2, y=y + spacing, z=z))
    set_point(wall3, Point(x=x + spacing / 2, y=y - spacing, z=z))

    green_bodies = [
        create_box(block_width, block_width, block_height, color=BLUE)
        for _ in range(num)
    ]
    body_types = [(b, 'green')
                  for b in [green1] + green_bodies]  #  + [(table1, 'sink')]

    movable = [green1] + green_bodies + red_bodies
    initial_surfaces = {block: table2 for block in green_bodies}
    sample_placements(initial_surfaces)

    return Problem(
        robot=pr2,
        movable=movable,
        arms=[arm],
        grasp_types=[grasp_type],
        surfaces=surfaces,
        #sinks=[table1],
        #goal_holding=[(arm, '?green')],
        #goal_cleaned=['?green'],
        goal_on=[('?green', plate)],
        body_types=body_types,
        base_limits=base_limits,
        costs=True)