示例#1
0
def create_bottom_walls(x_lim, y_lim, env):
    bottom_wall = box_body(env, x_lim * 2, y_lim * 2, 0.135, name='bottom_wall', color=(.82, .70, .55))
    bottom_wall_x = x_lim / 2.0
    set_xy(bottom_wall, bottom_wall_x, 0)
    env.Add(bottom_wall)

    side_wall = box_body(env, y_lim * 2, 0.01 * 2, 0.2 * 2,
                         name='side_wall_1',
                         color=(.82, .70, .55))
    place_body(env, side_wall, (-x_lim + bottom_wall_x, 0, np.pi / 2), base_name='bottom_wall')

    side_wall = box_body(env, y_lim * 2, 0.01 * 2, 0.2 * 2,
                         name='side_wall_2',
                         color=(.82, .70, .55))
    place_body(env, side_wall, (x_lim + bottom_wall_x, 0, np.pi / 2), base_name='bottom_wall')

    side_wall = box_body(env, x_lim * 2, 0.01 * 2, 0.2 * 2,
                         name='side_wall_3',
                         color=(.82, .70, .55))
    place_body(env, side_wall, (bottom_wall_x, y_lim, 0), base_name='bottom_wall')

    entrance_width = 1.2
    left_door_length = (x_lim - entrance_width / 2.0) - 1
    right_door_length = (x_lim - entrance_width / 2.0) + 1
    entrance_left = box_body(env, left_door_length, 0.01 * 2, 0.2 * 2,
                             name='entrance_left',
                             color=(.82, .70, .55))
    entrance_right = box_body(env, right_door_length, 0.01 * 2, 0.2 * 2,
                              name='entrance_right',
                              color=(.82, .70, .55))

    place_body(env, entrance_left, (bottom_wall_x + (-x_lim - (entrance_width / 2.0 + 1)) / 2.0, -y_lim, 0),
               base_name='bottom_wall')
    place_body(env, entrance_right, (bottom_wall_x + (x_lim + (entrance_width / 2.0 - 1)) / 2.0, -y_lim, 0),
               base_name='bottom_wall')
def simple_push(env):
    assert not REARRANGEMENT
    env.Load(ENVIRONMENTS_DIR + 'room1.xml')

    obstacle_names = [
        str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
    ]
    table_names = ['table1', 'table2']

    place_body(env,
               cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE),
               (1.65, -.6, PI / 4), 'table1')
    object_names = [
        str(body.GetName()) for body in env.GetBodies()
        if not body.IsRobot() and str(body.GetName()) not in obstacle_names
    ]

    start_constrained = {'blue_cylinder': 'table1'}
    goal_constrained = {
        'blue_cylinder': ('table2',
                          Point(
                              get_point(env.GetKinBody('blue_cylinder')) +
                              np.array([-1.65, -1., 0.])))
        #'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
    }

    return ManipulationProblem(function_name(inspect.stack()),
                               object_names=object_names,
                               table_names=table_names,
                               start_constrained=start_constrained,
                               goal_constrained=goal_constrained)
示例#3
0
def create_doors(x_lim, y_lim, door_x, door_y, door_width, th, env):
    if th == 0:
        middle_wall_size = (door_y - door_width / 2.0 - (-y_lim)) / 4.0
        left_wall_size = (y_lim - door_width / 2.0 - door_y) / 2.0
        middle_door_y = door_y - middle_wall_size - (door_width / 2.)
        right_wall_size =(y_lim/2.0 - middle_door_y ) / 2.0

    elif th == np.pi / 2.:
        right_wall_size = (door_x - door_width / 2.0 - (-x_lim)) / 2.0
        left_wall_size = (x_lim - door_width / 2.0 - door_x) / 2.0

    left_wall = box_body(env, 0.04 * 2, left_wall_size * 2, 1 * 2,
                         name='left_wall', color=(0, 0, 0))
    right_wall = box_body(env,
                         0.04 * 2, right_wall_size * 2, 1 * 2,
                          name='right_wall',
                          color=(0, 0, 0))

    middle_wall = box_body(env,
                          0.04 * 2, middle_wall_size * 2, 1 * 2,
                          name='middle_wall',
                          color=(0, 0, 0))
    place_body(env, left_wall, (door_x, door_y + left_wall_size + (door_width / 2.), th),
               base_name='bottom_wall')
    place_body(env, middle_wall, (door_x, door_y - middle_wall_size - (door_width / 2.), th),
               base_name='bottom_wall')
    place_body(env, right_wall, (door_x, middle_door_y-door_width-middle_wall_size-door_width, th), base_name='bottom_wall')

    wall_in_room_l = box_body(env, 0.04 * 2, 1, 2, name='wall_in_room_l', color=(0, 0, 0))
    wall_in_room_r = box_body(env, 0.04 * 2, 1, 2, name='wall_in_room_r', color=(0, 0, 0))
    place_body(env, wall_in_room_l, (3.3,0, np.pi/2), base_name='bottom_wall')
    place_body(env, wall_in_room_r, (5.5,0, np.pi/2), base_name='bottom_wall')
示例#4
0
def create_shelf(env, obst_x, obst_width, obst_height, name_idx, stacked_obj_name, table_name):
    width = 0.4
    length = 0.01
    height = obst_height
    top_wall_width = 0.001
    bottom_wall_width = 0.0001

    table_pos = aabb_from_body(env.GetKinBody(table_name)).pos()
    table_x = table_pos[0]
    table_y = table_pos[1]
    place_body(env,
               box_body(env,
                        width, length, height,
                        name='right_wall_' + str(name_idx),
                        color=OBST_COLOR,
                        transparency=OBST_TRANSPARENCY),
               (table_x + .0, table_y + obst_x - (obst_width - .05) / 2, 0),
               stacked_obj_name)
    place_body(env,
               box_body(env,
                        width, length, height,
                        name='left_wall_' + str(name_idx),
                        color=OBST_COLOR,
                        transparency=OBST_TRANSPARENCY),
               (table_x + .0, table_y + obst_x + (obst_width - .05) / 2, 0),
               stacked_obj_name)
    place_body(env,
               box_body(env,
                        length, obst_width - 0.05, height,
                        name='back_wall_' + str(name_idx),
                        color=OBST_COLOR,
                        transparency=OBST_TRANSPARENCY),
               (table_x + .225, table_y + obst_x, 0),
               stacked_obj_name)
    place_body(env,
               box_body(env,
                        width, obst_width - 0.05, top_wall_width,
                        name='top_wall_' + str(name_idx),
                        color=OBST_COLOR,
                        transparency=OBST_TRANSPARENCY),
               (table_x + 0, table_y + obst_x, 0),
               'back_wall_' + str(name_idx))
    place_body(env,
               box_body(env,
                        width, obst_width - 0.05, bottom_wall_width,
                        name='bottom_wall_' + str(name_idx),
                        color=OBST_COLOR,
                        transparency=0.5),
               (table_x + 0, table_y + obst_x, 0),
               stacked_obj_name)

    region = create_region(env, 'place_region_' + str(name_idx),
                           ((-1.0, 1.0), (-0.85, 0.85)),
                           'bottom_wall_' + str(name_idx), color=np.array((0, 0, 0, .5)))
    region.draw(env)
    return region
def decision_wall_namo(
        env, rows, cols
):  # TODO - could make variant that requires replacing things as well
    assert not REARRANGEMENT
    env.Load(ENVIRONMENTS_DIR + 'decision_2tables.xml')
    camera_trans = camera_look_at(CAMERA_POINT_SCALE *
                                  np.array([-1.5, 1.5, 3]),
                                  look_point=(1.5, -1.5, 0))

    obstacle_names = [
        str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
    ]
    table_names = ['table1', 'table2']

    for x in range(cols):
        for y in range(rows):
            #if y >= rows/2 and x >= cols/2: continue
            if y >= rows / 2 and x >= cols - 1:
                continue  # NOTE - makes one side harder than the other
            place_body_on_floor(
                box_body(env, .1, .1, .8, name='red%s-%s' % (x, y), color=RED),
                (-.25 + x * .75 / cols, -2.5 + (y + .5) * 5 / rows, 0))
    place_body(env, box_body(env, .07, .07, .2, name='blue0', color=BLUE),
               (1.75, +0.2, 0), 'table1')
    place_body(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE),
               (1.75, -0.2, 0), 'table1')

    object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
                    str(body.GetName()) not in obstacle_names and 'blue' in str(body.GetName())]
    floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
                    str(body.GetName()) not in obstacle_names and 'red' in str(body.GetName())]

    goal_config = 'initial'
    goal_holding = None  # False | None
    goal_regions = {
        'blue0': 'table2',
        'blue1': 'table2',
    }

    return ManipulationProblem(function_name(inspect.stack()),
                               camera_trans=camera_trans,
                               floor_object_names=floor_object_names,
                               object_names=object_names,
                               table_names=table_names,
                               goal_config=goal_config,
                               goal_holding=goal_holding,
                               goal_regions=goal_regions)
示例#6
0
def simple_push(env):
  env.Load(ENVIRONMENTS_DIR + 'room1.xml')

  obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
  table_names = ['table1', 'table2']

  place_body(env, cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE), (1.65, -.6, PI/4), 'table1')
  object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]

  start_constrained = {'blue_cylinder': 'table1'}
  goal_constrained = {
    'blue_cylinder': ('table2', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-1.65,-1., 0.])))
    #'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
  }

  return ManipulationProblem(function_name(inspect.stack()),
      object_names=object_names, table_names=table_names,
      start_constrained=start_constrained,
      goal_constrained=goal_constrained)
示例#7
0
def set_fixed_object_poses(env, x_lim, y_lim):
    objects = [env.GetKinBody('shelf1'), env.GetKinBody('shelf2'), env.GetKinBody('computer_table'),
               env.GetKinBody('table2')]
    place_body(env, env.GetKinBody('shelf1'), (x_lim + x_lim / 2.0 - 0.5, y_lim - 0.2, np.pi * 3 / 2),
               base_name='bottom_wall')
    place_body(env, env.GetKinBody('shelf2'), (x_lim + x_lim / 2.0 - 1.5, y_lim - 0.2, np.pi * 3 / 2),
               base_name='bottom_wall')
    place_body(env, env.GetKinBody('table2'), (x_lim + x_lim / 2.0 - 0.5, y_lim - 2, np.pi * 3 / 2),
               base_name='bottom_wall')
    #place_body(env, env.GetKinBody('computer_chair'), (4.2, -1.5, 0), base_name='bottom_wall')
    obj_poses = {obj.GetName(): get_pose(obj) for obj in objects}
    return obj_poses
示例#8
0
def create_doors(x_lim, y_lim, door_x, door_y, door_width, th, env):
    if th == 0:
        right_wall_size = (door_y - door_width / 2.0 - (-y_lim)) / 2.0
        left_wall_size = (y_lim - door_width / 2.0 - door_y) / 2.0
    elif th == np.pi / 2.:
        right_wall_size = (door_x - door_width / 2.0 - (-x_lim)) / 2.0
        left_wall_size = (x_lim - door_width / 2.0 - door_x) / 2.0

    left_wall = box_body(env,
                         0.04 * 2,
                         left_wall_size * 2,
                         1 * 2,
                         name='left_wall',
                         color=(.82, .70, .55))
    right_wall = box_body(env,
                          0.04 * 2,
                          right_wall_size * 2,
                          1 * 2,
                          name='right_wall',
                          color=(.82, .70, .55))
    if th == 0:
        place_body(env,
                   left_wall,
                   (door_x, door_y + left_wall_size + (door_width / 2.), th),
                   base_name='bottom_wall')
        place_body(env,
                   right_wall,
                   (door_x, door_y - right_wall_size - (door_width / 2.), th),
                   base_name='bottom_wall')
    else:
        place_body(env,
                   left_wall,
                   (door_x + left_wall_size + (door_width / 2.), door_y, th),
                   base_name='bottom_wall')
        place_body(env,
                   right_wall,
                   (door_x - right_wall_size - (door_width / 2.), door_y, th),
                   base_name='bottom_wall')

    set_body_transparency(left_wall, 0.3)
    set_body_transparency(right_wall, 0.3)
def push_wall(env):
    assert not REARRANGEMENT
    env.Load(ENVIRONMENTS_DIR + 'room1.xml')
    camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1, 1, 3]),
                                  look_point=(3, 0, 0))

    obstacle_names = [
        str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
    ]
    table_names = ['table1', 'table2']

    place_body(
        env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN),
        (1.65, -.5, PI / 4), 'table1')
    #place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
    #set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
    place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED),
               (1.50, 0, 0), 'table1')
    place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED),
               (1.6, 0, 0), 'table1')
    place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED),
               (1.7, 0, 0), 'table1')
    place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED),
               (1.8, 0, 0), 'table1')
    place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED),
               (1.9, 0, 0), 'table1')
    place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED),
               (2.0, 0, 0), 'table1')
    object_names = [
        str(body.GetName()) for body in env.GetBodies()
        if not body.IsRobot() and str(body.GetName()) not in obstacle_names
    ]

    start_constrained = {'green_cylinder': 'table1'}
    goal_constrained = {
        'green_cylinder': ('table1',
                           Point(
                               get_point(env.GetKinBody('green_cylinder')) +
                               np.array([-.2, 1.0, 0.])))
    }

    return ManipulationProblem(function_name(inspect.stack()),
                               camera_trans=camera_trans,
                               object_names=object_names,
                               table_names=table_names,
                               start_constrained=start_constrained,
                               goal_constrained=goal_constrained)
示例#10
0
def push_wall(env):
  env.Load(ENVIRONMENTS_DIR + 'room1.xml')
  camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1, 1, 3]), look_point=(3, 0, 0))

  obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
  table_names = ['table1', 'table2']

  place_body(env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN), (1.65, -.5, PI/4), 'table1')
  #place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
  #set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
  place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED), (1.50, 0, 0), 'table1')
  place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED), (1.6, 0, 0), 'table1')
  place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED), (1.7, 0, 0), 'table1')
  place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED), (1.8, 0, 0), 'table1')
  place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED), (1.9, 0, 0), 'table1')
  place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED), (2.0, 0, 0), 'table1')
  object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]

  start_constrained = {'green_cylinder': 'table1'}
  goal_constrained = {
    'green_cylinder': ('table1', Point(get_point(env.GetKinBody('green_cylinder')) + np.array([-.2, 1.0, 0.])))
  }

  return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans,
      object_names=object_names, table_names=table_names,
      start_constrained=start_constrained,
      goal_constrained=goal_constrained)
示例#11
0
    def __init__(self, env, problem_idx, problem_config=None):
        self.env = env
        fdir = os.path.dirname(os.path.abspath(__file__))
        self.env.Load(fdir + '/resources/mover_env.xml')
        self.robot = self.env.GetRobots()[0]
        set_xy(self.env.GetKinBody('kitchen'), -0.3, 1.3)

        # bottom wall creation
        x_extents = 4
        y_extents = 4
        create_bottom_walls(x_extents, y_extents, self.env)

        # door creation
        # self.env.Remove(self.env.GetKinBody('left_wall')); self.env.Remove(self.env.GetKinBody('right_wall'))
        door_width = 1.  # generate_rand(1, 1.5)
        door_x = (-x_extents + 1.5 + x_extents - 1.5) / 2.0 - x_extents * 0.3 + 4
        door_y = (-y_extents + 1.5 + y_extents - 1.5) / 2.0 + 2
        door_th = 0
        create_doors(x_extents, y_extents, door_x, door_y, door_width, door_th, self.env)

        set_config(self.robot, FOLDED_LEFT_ARM, self.robot.GetManipulator('leftarm').GetArmIndices())
        set_config(self.robot, mirror_arm_config(FOLDED_LEFT_ARM),
                   self.robot.GetManipulator('rightarm').GetArmIndices())

        fixed_obj_poses = set_fixed_object_poses(self.env, x_extents, y_extents)

        shelf_table = env.GetKinBody('table2')
        set_obj_xytheta([-1.5, -3, np.pi/2], shelf_table)
        shelf_shapes, shelf_xs = generate_shelf_shapes()
        shelf_regions = create_shelves(self.env, shelf_shapes, shelf_xs, 'table2')
        obj_shapes = generate_shelf_obj_shapes()
        shelf_objects = create_shelf_objs(self.env, obj_shapes)
        shelf_obj_poses = generate_poses_and_place_shelf_objs(shelf_objects, shelf_regions, self.env)

        self.kitchen_region = AARegion('kitchen_region',
                                       ((-x_extents + 2, x_extents * 0.2 + 2), (1 - y_extents / 2, 1 + y_extents / 2)),
                                       z=0.135, color=np.array((1, 1, 0, 0.25)))
        set_xy(self.env.GetKinBody('computer_table'), 5.5, -2.5)


        x_size = x_extents
        y_size = y_extents
        self.entire_region_xy = [x_size / 2.0, 0]
        self.entire_region_xy_extents = [x_size, y_size]
        self.entire_region = AARegion('entire_region',
                                    ((-x_size + self.entire_region_xy[0], x_size + self.entire_region_xy[0]),
                                     (-y_size, y_size)), z=0.135, color=np.array((1, 1, 0, 0.25)))

        # allow it to be entire region
        # but if not in home_region or in forbidden region, reject in motion planning stage
        self.home_region_xy = [x_extents / 2.0, 0]
        self.home_region_xy_extents = [x_extents,y_extents]
        self.home_region = AARegion('entire_region', ((-x_extents + self.home_region_xy[0], x_extents + self.home_region_xy[0]), (-x_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))

        #randomly_place_region(self.env, self.robot, self.home_region)


        # randomly placed objects
        kitchen_chairs = [body for body in self.env.GetBodies() if body.GetName().find('chair') != -1
                          and body.GetName().find('computer') == -1]
        packing_boxes = [b for b in self.env.GetBodies() if b.GetName().find('packing_box') != -1]
        computer_chair = self.env.GetKinBody('computer_chair')
        table = self.env.GetKinBody('table')
        # kitchen table location

        set_obj_xytheta([2, -3.2, -np.pi/2], env.GetKinBody('computer_table'))
        set_obj_xytheta([1, -3.2, 0], env.GetKinBody('computer_chair'))

        self.is_new_env = problem_config is None
        # computer chair location

        # place other objects

        place_objs_in_region(packing_boxes, self.kitchen_region, self.env)


        place_objs_in_region([self.robot], self.entire_region, self.env)
        place_objs_in_region(kitchen_chairs, self.kitchen_region, self.env)

        self.forbidden_region_xy = [x_extents+0.3, 0]
        self.forbidden_region_xy_extents = [x_extents/2.0-0.3, y_extents]
        self.forbidden_region = AARegion('forbidden_region',
                                     ((-self.forbidden_region_xy_extents[0] + self.forbidden_region_xy[0],
                                       self.forbidden_region_xy_extents[0] + self.forbidden_region_xy[0]),
                                     (-y_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))

        robot=self.robot
        # left arm IK
        robot.SetActiveManipulator('leftarm')
        manip = robot.GetActiveManipulator()
        ee = manip.GetEndEffector()
        ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot,
                                                                      iktype=IkParameterization.Type.Transform6D,
                                                                      forceikfast=True, freeindices=None,
                                                                      freejoints=None, manip=None)
        if not ikmodel1.load():
            ikmodel1.autogenerate()

        # right arm torso IK
        robot.SetActiveManipulator('rightarm_torso')
        manip = robot.GetActiveManipulator()
        ee = manip.GetEndEffector()
        ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
                                                                      iktype=IkParameterization.Type.Transform6D, \
                                                                      forceikfast=True, freeindices=None, \
                                                                      freejoints=None, manip=None)
        if not ikmodel2.load():
            ikmodel2.autogenerate()



        self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
        self.movable_objects = [computer_chair] + packing_boxes + kitchen_chairs

        self.problem_idx = problem_idx

        self.goal_base_config = np.array([-0,2.5,np.pi/2.])
        self.init_base_config = np.array([-1.5, -1.5, np.pi/2.])
        set_robot_config(self.init_base_config, self.robot)
        set_obj_xytheta([3, 2, 43 * np.pi / 180.], env.GetKinBody('shelf1'))
        set_obj_xytheta([3, -1.8, 43 * np.pi / 180.], env.GetKinBody('shelf2'))
        set_obj_xytheta([-0.13, 0.45, 3], packing_boxes[2])
        self.set_obj_poses(problem_idx)

        if self.problem_idx == 1:
            env.Remove(env.GetKinBody('shelf1'))
            env.Remove(env.GetKinBody('shelf2'))

        else:
            wall_in_kitchen = box_body(env, 0.04 * 2, 1.3, 2, name='wall_in_kitchen', color=(0, 0, 0))
            place_body(env, wall_in_kitchen, (2.1, 0, np.pi / 2), base_name='bottom_wall')
示例#12
0
def rearrangement_problem(env,obj_poses=None):
#  import random 
#  seed = 50
#  random.seed(seed)
#  np.random.seed(seed)

  MIN_DELTA = .01 # .01 | .02
  ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml' 
  env.Load(ENV_FILENAME)
  robot = env.GetRobots()[0]

  TABLE_NAME = 'table1'

  NUM_OBJECTS = 8
  WIDTH = .05 # .07 | .1
  TARGET_WIDTH = 0.07
  OBJ_HEIGHT = 0.2
  objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \
              color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)]
  target_obj =  box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \
                           color=(1, 1.5, 0)) 

  #TODO: Place obstacle that prevent you to reach from top
  
  OBST_X = 0
  OBST_WIDTH = 1.
  OBST_COLOR = (1, 0, 0)
  OBST_HEIGHT = 0.4
  OBST_TRANSPARENCY = .25
  place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
             (.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
  place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
             (.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
  place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
             (.225, OBST_X, 0), TABLE_NAME)
  # roof
  OBST_Z = OBST_HEIGHT
  place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
           (0, OBST_X, OBST_Z,0), TABLE_NAME)

  # I think this can be done once and for all
  REST_TORSO = [.15]
  robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')])
  l_model = databases.linkstatistics.LinkStatisticsModel(robot)
  if not l_model.load(): l_model.autogenerate()
  l_model.setRobotWeights()
  min_delta = 0.01
  l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object
  extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
  robot.SetAffineTranslationLimits(*extrema)

  X_PERCENT = 0.0 

  # define intial region for movable objects
  init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5)))
  #init_region.draw(env)
  
  # define target object region
  target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \
                        TABLE_NAME, color=np.array((0, 0, 0, 0.9)))
  target_obj_region.draw(env)

  # place object
  if obj_poses is not None:
    for obj in objects:
      set_pose(obj,obj_poses[get_name(obj)])
      set_quat(obj, quat_from_z_rot(0))
      if env.GetKinBody(get_name(obj)) is None: env.Add(obj)
    set_pose( target_obj, obj_poses[get_name(target_obj)] )
    set_quat( target_obj, quat_from_z_rot(0) ) 
    if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj)
  else:
    for obj in objects:
      randomly_place_region(env, obj, init_region)
      set_quat(obj, quat_from_z_rot(0))
    randomly_place_region(env, target_obj, target_obj_region)
    set_quat(target_obj, quat_from_z_rot(0))

  object_names = [get_name(body) for body in objects]           # (tothefront,totheback), (to_the_right,to_the_left)


  set_xy(robot, -.75, 0)

#  LEFT_ARM_CONFIG = HOLDING_LEFT_ARM
  robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices())
  robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices())
  
  grasps = {}
  for obj_name in object_names:
    obj = env.GetKinBody(obj_name)
    obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \
                + get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)
    grasps[get_name(obj)] = obj_grasps

  target_obj = env.GetKinBody('target_obj')
  target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\
                      get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH)
  grasps['target_obj'] = target_obj_grasps

  initial_poses={}
  for obj in objects:
    initial_poses[get_name(obj)] = get_pose(obj)
  initial_poses[get_name(target_obj)] = get_pose(target_obj)

  return ManipulationProblem(None,
      object_names=object_names, table_names='table1',
      goal_regions=init_region,grasps=grasps,
      initial_poses = initial_poses)