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
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)))
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() """
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
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
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')
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]
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
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) """