示例#1
0
def create_environment_region(name, xy, extents, z=None):
    if z is None:
        z = 0.138

    region = AARegion(name, ((xy[0] - extents[0], xy[0] + extents[0]),
                             (xy[1] - extents[1], xy[1] + extents[1])),
                      z,
                      color=np.array((1, 1, 0, 0.25)))
    return region
示例#2
0
def randomly_place_body(env, body, table_names):
#  import pdb; pdb.set_trace()
  if env.GetKinBody(get_name(body)) is None: 
	env.Add(body)
  while True:
    set_quat(body, quat_from_z_rot(uniform(0, 2*PI)))
    aabb = aabb_from_body(body)
    region = AARegion.create_on_body(env.GetKinBody(choice(table_names)))
    cspace = region.cspace(aabb)
    if cspace is None: continue
    set_point(body, np.array([uniform(*range) for range in cspace] +
		     [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
    if not body_collision(body): return
    def __init__(self, problem_idx):
        Mover.__init__(self, problem_idx)
        self.operator_names = ['one_arm_pick', 'one_arm_place']
        set_robot_config([4.19855789, 2.3236321, 5.2933337], self.robot)
        set_obj_xytheta([3.35744004, 2.19644156, 3.52741118], self.objects[1])
        self.boxes = self.objects
        self.objects = self.problem_config['shelf_objects']
        self.objects = [k for v in self.objects.values() for k in v]
        self.objects[0], self.objects[1] = self.objects[1], self.objects[0]

        self.target_box = self.env.GetKinBody('rectangular_packing_box1')
        # utils.randomly_place_region(self.target_box, self.regions['home_region'])
        kitchen = AARegion('kitchen', ((1.29, 2.8), (-3.16, 3.16)), z=0.135, color=np.array((1, 1, 0, 0.25)))
        utils.randomly_place_region(self.target_box, kitchen)

        self.regions['rectangular_packing_box1_region'] = self.compute_box_region(self.target_box)
        self.shelf_regions = self.problem_config['shelf_regions']
        self.target_box_region = self.regions['rectangular_packing_box1_region']
        self.regions.update(self.shelf_regions)
        self.entity_names = [obj.GetName() for obj in self.objects] + ['rectangular_packing_box1_region',
                                                                       'center_shelf_region']
        self.name = 'one_arm_mover'
        self.init_saver = utils.CustomStateSaver(self.env)

        self.object_names = self.entity_names

        # fix incorrectly named regions
        self.regions = {
            region.name: region
            for region in self.regions.values()
        }

        self.placement_regions = {
            region.name: region
            for region in [self.target_box_region] + list(self.shelf_regions.values())
        }
    n_samples = 1
    for _ in range(300):
        robot_xy = sample_base_locations(arm_len, obj, env)[:-1]
        angle = sample_angle_facing_given_transform(obj_xy, robot_xy)  # angle around z
        set_robot_config(np.r_[robot_xy, angle], robot)
        if (not env.CheckCollision(robot)) and np.any([r.contains(robot.ComputeAABB()) for r in multiple_regions]):
            return np.array([robot_xy[0], robot_xy[1], angle])
    return None


def create_region(env, region_name, ((nx, px), (ny, py)), table_name, color=None):
    table_aabb = aabb_from_body(env.GetKinBody(table_name))
    return AARegion(region_name,
                    ((table_aabb.pos()[0] + nx * table_aabb.extents()[0],
                      table_aabb.pos()[0] + px * table_aabb.extents()[0]),
                     (table_aabb.pos()[1] + ny * table_aabb.extents()[1],
                      table_aabb.pos()[1] + py * table_aabb.extents()[1])),
                    table_aabb.pos()[2] + table_aabb.extents()[2] + REGION_Z_OFFSET, color=color)


def simulate_base_path(robot, path):
    for p in path:
        # set_config(robot, p, get_active_arm_indices(robot))
        set_robot_config(p, robot)
        sleep(0.001)


def randomly_place_in_region(env, body, region):
    if env.GetKinBody(get_name(body)) is None: env.Add(body)
    for i in range(1000):
        set_quat(body, quat_from_z_rot(uniform(0, 2 * PI)))
示例#5
0
    def __init__(self, env):
        x_extents = 3.5
        y_extents = 3.16

        door_width = 1.5  # 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
        door_th = 0

        # todo move all the kitchen objects by 0.5

        fdir = os.path.dirname(os.path.abspath(__file__))
        env.Load(fdir + '/resources/mover_env.xml')
        # set_xy(env.GetKinBody('kitchen'), 0, 0.5)

        robot = env.GetRobots()[0]
        # 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()

        create_bottom_walls(x_extents, y_extents, env)
        create_doors(x_extents, y_extents, door_x, door_y, door_width, door_th,
                     env)
        set_config(robot, FOLDED_LEFT_ARM,
                   robot.GetManipulator('leftarm').GetArmIndices())
        set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM),
                   robot.GetManipulator('rightarm').GetArmIndices())

        fixed_obj_poses = set_fixed_object_poses(env, x_extents, y_extents)
        shelf_shapes, shelf_xs = generate_shelf_shapes()
        shelf_regions = create_shelves(env, shelf_shapes, shelf_xs, 'table2')

        obj_shapes = generate_shelf_obj_shapes()
        shelf_objects = create_shelf_objs(env, obj_shapes)
        shelf_obj_poses = generate_poses_and_place_shelf_objs(
            shelf_objects, shelf_regions, env)
        for region_name, region in zip(shelf_regions.keys(),
                                       shelf_regions.values()):
            region.name = region_name + '_shelf_region'

        home_region_xy = [x_extents / 2.0, 0]
        home_region_xy_extents = [x_extents, y_extents]
        home_region = AARegion(
            'home_region',
            ((-x_extents + x_extents / 2.0, x_extents + x_extents / 2.0),
             (-y_extents, y_extents)),
            z=0.135,
            color=np.array((1, 1, 0, 0.25)))

        loading_region_xy = [1.8, -6.7]
        loading_region_xy_extents = [2.5, 1.85]
        loading_region = AARegion(
            'loading_region',
            ((loading_region_xy[0] - loading_region_xy_extents[0],
              loading_region_xy[0] + loading_region_xy_extents[0]),
             (loading_region_xy[1] - loading_region_xy_extents[1],
              loading_region_xy[1] + loading_region_xy_extents[1])),
            z=0.135,
            color=np.array((1, 1, 0, 0.25)))
        bridge_region_name = 'bridge_region'
        bridge_region_xy = [0.7, -4.1]
        bridge_region_extents = [1, 1.0]
        bridge_region = create_environment_region(bridge_region_name,
                                                  bridge_region_xy,
                                                  bridge_region_extents)

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

        packing_box_names = [
            'square_packing_box1', 'rectangular_packing_box1',
            'square_packing_box2', 'rectangular_packing_box2',
            'square_packing_box3', 'rectangular_packing_box3',
            'square_packing_box4', 'rectangular_packing_box4'
        ]
        packing_boxes = [env.GetKinBody(pname) for pname in packing_box_names]

        place_objs_in_region(packing_boxes, loading_region, env)
        place_objs_in_region([robot], loading_region, env)
        open_gripper(robot)
        """
        box_regions = {}
        for box in packing_boxes:
            box_region = AARegion.create_on_body(box)
            box_region.color = (1., 1., 0., 0.25)
            box_regions[box.GetName()] = box_region
            if box == packing_boxes[0]:
                xytheta = get_body_xytheta(box)
                set_obj_xytheta([xytheta[0, 0], xytheta[0, 1], 0], box)
                box_region.draw(env)
        """

        temp_objects_to_pack = [
            body for body in env.GetBodies()
            if body.GetName().find('box') == -1 and body.GetName().find(
                'wall') == -1 and body.GetName().find('sink') == -1
            and body.GetName().find('kitchen') == -1 and body.GetName().find(
                'entrance') == -1 and body.GetName().find('pr2') == -1
            and body.GetName().find('floorwalls') == -1 and body.GetName(
            ).find('table') == -1 and body.GetName().find('obst') == -1
        ]

        # packing boxes are packed in the order given in packing_boxes
        # 1. packing boxes in the home
        # 2. big objects in the truck
        # 3. small objects in the box
        # 4. shelf objects in the box
        # 5. boxes in the truck

        big_objects_to_pack = [
            body for body in env.GetBodies()
            if body.GetName().find('chair') != -1
            or body.GetName().find('shelf') != -1
        ]
        objects_to_pack = [
            obj for obj in temp_objects_to_pack
            if obj not in big_objects_to_pack
        ]
        objects = objects_to_pack + big_objects_to_pack
        self.problem_config = {
            'shelf_objects': shelf_objects,
            'packing_boxes': packing_boxes,
            'objects_to_pack': objects_to_pack,
            'big_objects_to_pack': big_objects_to_pack,
            'home_region': home_region,
            'loading_region': loading_region,
            'entire_region': entire_region,
            'entire_region_xy': entire_region_xy,
            'entire_region_extents': entire_region_xy_extents,
            'bridge_region': bridge_region,
            'bridge_region_xy': bridge_region_xy,
            'bridge_region_extents': bridge_region_extents,
            'env': env,
            'loading_region_xy': loading_region_xy,
            'loading_region_extents': loading_region_xy_extents,
            'home_region_xy': home_region_xy,
            'home_region_extents': home_region_xy_extents,
            'shelf_regions': shelf_regions,
            'objects': objects
        }
        # import pdb;pdb.set_trace()
        """
示例#6
0
def create_conveyor_belt_problem(env, obj_setup=None):
    if obj_setup is not None:
        obj_shapes = obj_setup['object_shapes']
        obj_poses = obj_setup['object_poses']
        obst_shapes = obj_setup['obst_shapes']
        obst_poses = obj_setup['obst_poses']

    fdir = os.path.dirname(os.path.abspath(__file__))
    env.Load(fdir + '/convbelt_env.xml')
    robot = env.GetRobots()[0]
    set_default_robot_config(robot)

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

    # left arm IK
    robot.SetActiveManipulator('leftarm')
    manip = robot.GetActiveManipulator()
    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()
    ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
                                                                  iktype=IkParameterization.Type.Transform6D, \
                                                                  forceikfast=True, freeindices=None, \
                                                                  freejoints=None, manip=None)
    if not ikmodel2.load():
        ikmodel2.autogenerate()

    # loading areas

    loading_region = AARegion('loading_area', ((-3.51, -0.81), (-2.51, 2.51)),
                              z=0.01,
                              color=np.array((1, 1, 0, 0.25)))

    # converyor belt region
    conv_x = 3
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',
                             ((-1 + conv_x, 20 * max_width + conv_x),
                              (-0.4 + conv_y, 0.5 + conv_y)),
                             z=0.01,
                             color=np.array((1, 0, 0, 0.25)))

    all_region = AARegion('all_region',
                          ((-3.51, 20 * max_width + conv_x), (-2.51, 2.51)),
                          z=0.01,
                          color=np.array((1, 1, 0, 0.25)))

    if obj_setup is None:
        objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt)
        obstacles, obst_shapes, obst_poses = create_obstacles(
            env, loading_region)
    else:
        objects = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0))
        obstacles = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1))

    initial_saver = DynamicEnvironmentStateSaver(env)
    initial_state = (initial_saver, [])
    #set_obj_xytheta([-1, -1, 1], obstacles[0])
    #set_obj_xytheta([-2, 2.3, 0], obstacles[1])
    init_base_conf = np.array([0, 1.05, 0])
    set_robot_config(np.array([0, 1.05, 0]), robot)
    #obst_poses = [randomly_place_in_region(env, obj, loading_region) for obj in obstacles]
    #obst_poses = [get_body_xytheta(obj) for obj in obstacles]
    """
    tobj = env.GetKinBody('tobj3')
    tobj_xytheta = get_body_xytheta(tobj.GetLinks()[1])
    tobj_xytheta[0, -1] = (160 / 180.0) * np.pi
    set_obj_xytheta(tobj_xytheta, tobj.GetLinks()[1])
    objects = []
    for tobj in env.GetBodies():
        if tobj.GetName().find('tobj') == -1: continue
        randomly_place_in_region(env, tobj, conveyor_belt)
        objects.append(tobj)
    """

    problem = {
        'initial_state': initial_state,
        'obstacles': obstacles,
        'objects': objects,
        'conveyor_belt_region': conveyor_belt,
        'loading_region': loading_region,
        'env': env,
        'obst_shapes': obst_shapes,
        'obst_poses': obst_poses,
        'obj_shapes': obj_shapes,
        'obj_poses': obj_poses,
        'entire_region': all_region,
        'init_base_conf': init_base_conf
    }
    return problem  # the second is for indicating 0 placed objs
示例#7
0
def two_tables_through_door(env,
                            obj_shapes=None,
                            obj_poses=None,
                            obst_shapes=None,
                            obst_poses=None):
    env.Load('env.xml')
    robot = env.GetRobots()[0]
    set_default_robot_config(robot)
    region = create_region(env, 'goal', ((-1, 1), (-.3, .3)), \
                           'floorwalls', color=np.array((0, 0, 1, .25)))

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

    # 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()

    # loading areas
    init_loading_region = AARegion('init_loading_area',\
                          ((-2.51,-0.81),(-2.51,0)),\
                          z=0.0001,color=np.array((1,0,1,0.25)))
    init_loading_region.draw(env)
    init_loading_region2 = AARegion('init_loading_area2',\
                           ((-2.51,-0.81),(1.7,2.6)),\
                           z=0.0001,color=np.array((1,0,1,0.25)))
    init_loading_region2.draw(env)
    init_loading_region4 = AARegion('init_loading_area4',\
                           ((-2.51,-1.5),(-0.1,2)),\
                            z=0.0001,color=np.array((1,0,1,0.25)))
    init_loading_region4.draw(env)
    loading_regions =[init_loading_region,init_loading_region2,\
                      init_loading_region4]

    loading_region = AARegion('loading_area',\
                    ((-2.51,-0.81),(-2.51,2.51)),\
                    z=0.0001,color=np.array((1,1,0,0.25)))
    loading_region.draw(env)

    # converyor belt region
    conv_x = 2
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',\
                    ((-1+conv_x,10*max_width+conv_x),\
                    (-0.4+conv_y,0.5+conv_y)),\
                    z=0.0001,color=np.array((1,0,0,0.25)))
    conveyor_belt.draw(env)

    all_region = AARegion('all_region',\
                 ((-2.51,10*max_width+conv_x),(-3.51,3.51)),\
                 z=0.0001,color=np.array((1,1,0,0.25)))

    if obj_shapes == None:
        OBJECTS, obj_shapes, obj_poses = create_objects(env, conveyor_belt)
    else:
        OBJECTS = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0))

    if obst_shapes == None:
        OBSTACLES, obst_shapes, obst_poses = create_obstacles(
            env, loading_regions)
    else:
        OBSTACLES = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1))

    initial_saver = DynamicEnvironmentStateSaver(env)
    initial_state = (initial_saver, [])
    init_base_conf = np.array([0, 1.05, 0])

    problem = {'initial_state':initial_state,\
               'obstacles':OBSTACLES,\
               'objects':OBJECTS,\
               'loading_region':loading_region,\
               'env':env,\
               'obst_shapes':obst_shapes,\
               'obst_poses':obst_poses,\
               'obj_shapes':obj_shapes,\
               'obj_poses':obj_poses,\
               'all_region':all_region,\
               'init_base_conf':init_base_conf}
    return problem  # the second is for indicating 0 placed objs
示例#8
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')
示例#9
0
    def set_goal(self, goal_objects, goal_region):
        self.goal_objects = goal_objects
        [utils.set_color(o, [1, 0, 0]) for o in self.goal_objects]
        if 40000 <= self.problem_idx < 50000:
            entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
            non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
                                           color=np.array((1, 1, 0, 0.25)))
            # move objects out of the entrance region
            utils.randomly_place_region(self.robot, non_entrance_region)
            [utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]

            # try to put three objs near the entrance
            objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
            for obj in objs_to_move_near_entrance:
                utils.randomly_place_region(obj, entrance_region, n_limit=100)

            region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
                                                     color=np.array((1, 1, 0, 0.25)))

            object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance and obj.GetName() not in goal_objects][0:3]
            for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
                                                                           n_limit=100)

            # surround the robot?
            # Force the goal object to be around the robot
            #utils.randomly_place_region(self.robot, robot_region)
            radius = 1
            center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
            xmin = center[0] - radius
            xmax = center[0]
            ymin = center[1]
            ymax = center[1] + radius
            goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
                                    color=np.array((1, 1, 0, 0.25)))

            # doing the below because we ran n_objs_pack=1 and n_objs_pack=4 on different commits
            if len(goal_objects) == 4:
                for obj in goal_objects[0:1]:
                    utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
            else:
                for obj in goal_objects:
                    utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)

        elif 50000 <= self.problem_idx < 60000:
            # hard problems for both RSC and Greedy
            # hard for RSC: make sure the goal object needs to be moved twice
            # dillema: if I put the goal object near the entrance, then I can just move that to the goal region
            # I must surround the robot with the goal object such that the shortest path always go
            # through the goal object
            # another option is to block the object in the entrance region with the goal object
            entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
            non_entrance_region = AARegion('non_entrance_region',
                                           # xmin,xmax    ymin, ymax
                                           ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
                                           color=np.array((1, 1, 0, 0.25)))
            # move objects out of the entrance region
            utils.randomly_place_region(self.robot, non_entrance_region)
            [utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]

            # try to put three objs near the entrance
            objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
            for obj in objs_to_move_near_entrance:
                utils.randomly_place_region(obj, entrance_region, n_limit=100)

            region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
                                                     color=np.array((1, 1, 0, 0.25)))

            object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance
                                      if obj.GetName() not in goal_objects][0:3]
            # object_around_entrance = np.array(object_around_entrance)[
            #    np.random.choice(range(len(object_around_entrance)), 3, replace=False)]
            for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
                                                                           n_limit=100)

            # xmin,xmax    ymin, ymax
            robot_region = AARegion('robot_region', ((3.0, 4.29), (-8.0, -6.0)), z=0.135,
                                    color=np.array((1, 1, 0, 0.25)))
            utils.randomly_place_region(self.robot, robot_region)

            [utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects
             if obj not in object_around_entrance + objs_to_move_near_entrance]

            # surround the robot?
            # Force the goal object to be around the robot
            utils.randomly_place_region(self.robot, robot_region)
            radius = 1
            center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
            xmin = center[0] - radius
            xmax = center[0] + radius
            ymin = center[1] - radius
            ymax = center[1] + radius
            goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
                                       color=np.array((1, 1, 0, 0.25)))
            for obj in goal_objects:
                utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region)
        elif self.problem_idx >= 60000:
            entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
            non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
                                           color=np.array((1, 1, 0, 0.25)))
            # move objects out of the entrance region
            utils.randomly_place_region(self.robot, non_entrance_region)
            [utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]

            # try to put three objs near the entrance
            objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
            for obj in objs_to_move_near_entrance:
                utils.randomly_place_region(obj, entrance_region, n_limit=100)

            region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
                                                     color=np.array((1, 1, 0, 0.25)))

            object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance][0:3]
            for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
                                                                           n_limit=100)

        self.initial_robot_base_pose = get_body_xytheta(self.robot)
        self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
        self.init_saver = CustomStateSaver(self.env)

        self.goal_region = goal_region
        self.goal_entities = self.goal_objects + [self.goal_region]
示例#10
0
def create_conveyor_belt_problem(env, obj_setup=None, problem_idx=0):
    if obj_setup is not None:
        obj_shapes = obj_setup['object_shapes']
        obj_poses = obj_setup['object_poses']
        obst_shapes = obj_setup['obst_shapes']
        obst_poses = obj_setup['obst_poses']

    fdir = os.path.dirname(os.path.abspath(__file__))

    if problem_idx == 0 or problem_idx == 1:
        env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
    else:
        env.Load(fdir + '/convbelt_env_diffcult_shapes_two_rooms.xml')
    """
    if problem_idx == 0:
        env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
    else:
        env.Load(fdir + '/convbelt_env.xml')
    """

    robot = env.GetRobots()[0]
    set_default_robot_config(robot)

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

    # left arm IK
    robot.SetActiveManipulator('leftarm')
    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')
    ikmodel2 = databases.inversekinematics.InverseKinematicsModel(
        robot=robot,
        iktype=IkParameterization.Type.Transform6D,
        forceikfast=True,
        freeindices=None,
        freejoints=None,
        manip=None)
    if not ikmodel2.load():
        ikmodel2.autogenerate()

    # loading areas
    """
    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]),
                                 (-y_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))
    """
    init_base_conf = np.array([0, 1.05, 0])
    set_robot_config(np.array([0, 1.05, 0]), robot)

    # converyor belt region
    conv_x = 3
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',
                             ((-1 + conv_x, 20 * max_width + conv_x),
                              (-0.4 + conv_y, 0.5 + conv_y)),
                             z=0.01,
                             color=np.array((1, 0, 0, 0.25)))

    y_extents = 5.0
    x_extents = 3.01
    entire_region = AARegion('entire_region',
                             ((-7.4, 20 * max_width + conv_x),
                              (-y_extents - 2.5, y_extents - 2)),
                             z=0.01,
                             color=np.array((1, 1, 0, 0.25)))
    loading_region = AARegion('loading_area', ((-7.4, -0.5), (-7.5, 3.0)),
                              z=0.01,
                              color=np.array((1, 1, 0, 0.25)))

    big_region_1 = AARegion('big_region_1', ((-5, -0.5), (-7.5, -0.4)),
                            z=0.01,
                            color=np.array((1, 1, 0, 0.25)))
    big_region_2 = AARegion('big_region_2', ((-7.4, -4.0), (-7.5, 3.0)),
                            z=0.01,
                            color=np.array((1, 1, 0, 0.25)))

    if problem_idx == 0 or problem_idx == 1:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2 + i, 1.05, 0], tobj)
            objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env,
                                                               conveyor_belt,
                                                               num_objects=10)
        objects += square_objects
    else:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2 + i, 1.05, get_body_xytheta(tobj)[0, -1]], tobj)
            #objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env,
                                                               conveyor_belt,
                                                               num_objects=20)
        for obj in square_objects:
            set_obj_xytheta([2 + i, 1.05, get_body_xytheta(obj)[0, -1]], obj)
            objects.append(obj)
            i += 1.1

        #objects += square_objects
    """
    if problem_idx == 0:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2+i, 1.05, 0], tobj)
            objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=10)
        objects += square_objects
    else:
        objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=20)
    """

    initial_saver = DynamicEnvironmentStateSaver(env)
    initial_state = (initial_saver, [])
    problem = {
        'initial_state': initial_state,
        'objects': objects,
        'conveyor_belt_region': conveyor_belt,
        'loading_region': loading_region,
        'big_region_1': big_region_1,
        'big_region_2': big_region_2,
        'env': env,
        'entire_region': entire_region,
        'init_base_conf': init_base_conf
    }
    return problem  # the second is for indicating 0 placed objs
 def compute_box_region(self, box):
     box_region = AARegion.create_on_body(box)
     box_region.color = (1., 1., 0., 0.25)
     box_region.draw(self.env)
     box_region.name += '_region'
     return box_region
示例#12
0
def main():
  env=Environment()
  env.Load('env.xml')
  robot = env.GetRobots()[0]
  #set_default_robot_config(robot)
  robot_initial_pose = get_pose(robot)
  leftarm_manip = robot.GetManipulator('leftarm')
  robot.SetActiveManipulator('leftarm')

  ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
                                  iktype=IkParameterization.Type.Transform6D, \
                                  forceikfast=True, freeindices=None, \
                                  freejoints=None, manip=None)
  if not ikmodel1.load():
    ikmodel1.autogenerate()

  rightarm_manip = robot.GetManipulator('rightarm')
  rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
  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()

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

  robot_initial_config = np.array([-1,1,0])
  set_robot_config(robot_initial_config,robot)
  env.SetViewer('qtcoin')

  target = env.GetKinBody('target')
  width = 0.1  #np.random.rand(1)*(max_width-min_width)+min_width
  length = 0.8 #np.random.rand(1)*(max_length-min_length) + min_length
  height = 1.0   #np.random.rand(1)*(max_height-min_height)+min_height
  new_body = box_body(env,width,length,height,\
                      name='obst',\
                      color=(0, 5, 1))
  trans = np.eye(4); 
  trans[2,-1] = 0.075
  #env.Add(new_body); 
  #new_body.SetTransform(trans)
 
  all_region = region = AARegion('all_region',((-2.51,2.51),(-2.51,2.51)),\
                                    z=0.0001,color=np.array((1,1,0,0.25)))

  activate_arms_torso_base(robot)
  # Try picking different objects
  pick_obj = lambda obj_name: pick(obj_name,env,robot,all_region)
  import pdb;pdb.set_trace()
  pbase,grasp,g_config = sample_pick(env.GetRobot('valkyrie'),robot,env,all_region)
  pick_obj(env.GetRobot('valkyrie'),robot,g_config,leftarm_manip,rightarm_manip) 

  import pdb;pdb.set_trace()
  path,tpath,status = get_motion_plan(robot,robot_xytheta,env,maxiter=10,n_node_lim=5000)
 
  import pdb;pdb.set_trace()
  sample_pick(target,robot,env,all_region)
  import pdb;pdb.set_trace()
def two_tables_through_door(env):  # Previously 4, 8
    env.Load('env.xml')
    robot = env.GetRobots()[0]
    set_default_robot_config(robot)
    region = create_region(env, 'goal', ((-1, 1), (-.3, .3)), \
                           'floorwalls', color=np.array((0, 0, 1, .25)))

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

    # 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()

    # obj definitions
    min_height = 0.4
    max_height = 1

    min_width = 0.2
    max_width = 0.6

    min_length = 0.2
    max_length = 0.6

    # loading areas
    #rightmost one
    init_loading_region = AARegion('init_loading_area',
                                   ((-2.51, -0.81), (-2.51, -1)),
                                   z=0.0001,
                                   color=np.array((1, 0, 1, 0.25)))
    init_loading_region.draw(env)
    init_loading_region2 = AARegion('init_loading_area2',
                                    ((-2.51, -0.81), (1.7, 2.6)),
                                    z=0.0001,
                                    color=np.array((1, 0, 1, 0.25)))
    init_loading_region2.draw(env)
    init_loading_region3 = AARegion('init_loading_area3',
                                    ((-1.3, -0.81), (-1, 0)),
                                    z=0.0001,
                                    color=np.array((1, 0, 1, 0.25)))
    init_loading_region3.draw(env)
    init_loading_region4 = AARegion('init_loading_area4',
                                    ((-2.51, -2), (-1, 0)),
                                    z=0.0001,
                                    color=np.array((1, 0, 1, 0.25)))
    init_loading_region4.draw(env)
    loading_regions =[init_loading_region,init_loading_region2,\
                      init_loading_region3,init_loading_region4]

    loading_region = AARegion('loading_area', ((-2.51, -0.81), (-2.51, 2.51)),
                              z=0.0001,
                              color=np.array((1, 1, 0, 0.25)))
    loading_region.draw(env)

    # converyor belt region
    conv_x = 2
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',
                             ((-1 + conv_x, 10 * max_width + conv_x),
                              (-0.4 + conv_y, 0.5 + conv_y)),
                             z=0.0001,
                             color=np.array((1, 0, 0, 0.25)))
    conveyor_belt.draw(env)

    all_region = AARegion('all_region',
                          ((-2.51, 10 * max_width + conv_x), (-3.51, 3.51)),
                          z=0.0001,
                          color=np.array((1, 1, 0, 0.25)))
    """
  obj1 = box_body(env,0.5,0.5,0.5,\
                      name='obst1',\
                      color=(0, 1, 1))
  env.Add(obj1)
  obj2 = box_body(env,0.5,0.5,0.5,\
                      name='obst2',\
                      color=(0, 1, 1))
  env.Add(obj2)
  set_point(obj1,[-1,-1,0.75])

  set_point(obj1,[-1.9,-0.5,0.01])
  set_point(obj2,[-1.,-0.5,0.01])
  set_point(obj2,[-1,0.7,0.01])
  """

    NUM_OBSTACLES = 4
    OBSTACLES = []
    obstacle_poses = {}
    obstacle_shapes = {}
    i = 0
    for i in range(NUM_OBSTACLES):
        width = np.random.rand(1) * (max_width - min_width) + min_width
        length = np.random.rand(1) * (max_length - min_length) + min_length
        height = np.random.rand(1) * (max_height - min_height) + min_height
        trans = np.eye(4)
        trans[2, -1] = 0.075
        new_body = box_body(env,width,length,height,\
                            name='obj%s'%i,\
                            color=(0, (i+.5)/NUM_OBSTACLES, 0))
        env.Add(new_body)
        new_body.SetTransform(trans)
        xytheta = randomly_place_in_region(
            env, new_body, loading_regions[np.random.randint(4)])

        if not (xytheta is None):
            obstacle_shapes['obst%s' %
                            len(OBSTACLES)] = [width[0], length[0], height[0]]
            obstacle_poses['obst%s' % len(OBSTACLES)] = xytheta
            OBSTACLES.append(new_body)
        else:
            env.Remove(new_body)
    goal_base_pose = np.array([-2, -2, 5 * PI / 4])
    robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
                        [0, 0, 1])
    import pdb
    pdb.set_trace()
    n_node_lim_list = [3000, 4000, 5000, 6000, 7000]  #,8000,9000,1000]
    stime = time.time()
    n_node_lim = np.inf
    for n_node_lim in n_node_lim_list:
        path, tpath2, status2 = get_motion_plan(robot,
                                                goal_base_pose,
                                                env,
                                                maxiter=20,
                                                n_node_lim=n_node_lim)
        if status2 is "HasSolution":
            print n_node_lim
            break
    print time.time() - stime
    import pdb
    pdb.set_trace()
    set_robot_config(goal_base_pose, robot)
    """