def separate(env, n=7): # Previously 4, 8 assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'tables.xml') set_default_robot_config(env.GetRobots()[0]) table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()]) objects = [] goal_regions = {} for i in range(2*n): objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED)) for i in range(n): name = 'green'+str(i+1) objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN)) goal_regions[name] = 'table1' for i in range(n): name = 'blue'+str(i+1) objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE)) goal_regions[name] = 'table3' object_names = [get_name(body) for body in objects] for obj in randomize(objects): randomly_place_body(env, obj, ['table2', 'table4']) return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_regions=goal_regions)
def two_tables(env, n=2): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + '2tables.xml') set_default_robot_config(env.GetRobots()[0]) table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()]) #m = 4*n objects = [] goal_regions = {} #for i in range(4*m): for i in range(10*n): objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED)) #for i in range(n): for i in range(1): name = 'blue'+str(i+1) objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE)) goal_regions[name] = 'table2' object_names = [get_name(body) for body in objects] for obj in randomize(objects): randomly_place_body(env, obj, ['table1']) return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_regions=goal_regions)
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') set_default_robot_config(env.GetRobots()[0]) set_point(env.GetRobots()[0], (-1.5, 0, 0)) m, n = 3, 3 #m, n = 5, 5 n_obj = 8 side_dim = .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) pose_indices = list(product(range(m), range(n))) colors = {} for r, c in pose_indices: color = np.zeros(4) color[2-r] = 1. colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0]) poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r, c in pose_indices: x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) initial_indices = randomize(pose_indices[:]) initial_poses = {} goal_poses = {} for i, indices in enumerate(pose_indices[:n_obj]): name = 'block%d-%d'%indices color = colors[indices] initial_poses[name] = poses[initial_indices[i]] goal_poses[name] = poses[indices] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, initial_poses[name].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=poses.values())
def get_goal_config_from_theta_list(theta_list): env=Environment() env.Reset() simple_prob=separate(env,0) floor = env.GetKinBody("floorwalls") floor.Enable(False) oracle = ManipulationOracle(simple_prob,env) robot = env.GetRobots()[0] target=env.GetKinBody('ObjToG') target.Enable(False) manipulator = robot.SetActiveManipulator('leftarm_torso') robot.SetActiveDOFs(manipulator.GetArmIndices()) gmodel = databases.grasping.GraspingModel(robot,target) ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,\ iktype=IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() # env.SetViewer('qtcoin') goal_config_list = [] for idx in range(np.shape(theta_list)[0]): print idx base_pose = theta_list[idx,0:3] grasp = theta_list[idx,3:] robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1]) robot.SetActiveDOFValues(base_pose) set_default_robot_config(robot) # set arms to the pregrasp pose Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=False) goal_config = inverse_kinematics(oracle,Tgrasp) if goal_config is not None: import pdb; pdb.set_trace() goal_config_list.append(goal_config) env.Destroy() return goal_config_list
def move_several(env, n): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') box_dims = (.07, .07, .2) #separation = (.08, .08) separation = (.10, .10) length = math.sqrt(n+1)*(box_dims[0] + separation[0]) width = math.sqrt(n+1)*(box_dims[1] + separation[1]) height = .7 table1 = box_body(env, length, width, height, name='table1', color=get_color('tan1')) set_point(table1, (0, 0, 0)) env.Add(table1) table2 = box_body(env, length, width, height, name='table2', color=get_color('tan1')) set_point(table2, (1.5, 0, 0)) env.Add(table2) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) # TODO - place walls and/or a roof to make more similar to pebble graph people objects = [] goal_regions = {} obj = box_body(env, .07, .07, .2, name='blue', color=BLUE) set_point(obj, (0, 0, height + BODY_PLACEMENT_Z_OFFSET)) objects.append(obj) goal_regions[get_name(obj)] = get_name(table2) env.Add(obj) for i in range(n): objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED)) for obj in randomize(objects[1:]): randomly_place_body(env, obj, [get_name(table1)]) return ManipulationProblem(None, object_names=[get_name(body) for body in objects], table_names=[get_name(table) for table in [table1, table2]], goal_regions=goal_regions)
def grid_arrangement(env, m, n): # (Dealing with Difficult Instances of Object Rearrangment) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') box_dims = (.12, .04, .08) #separation = (.08, .08) separation = (.12, .12) #separation = (.16, .16) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) #set_point(table, (1.75, 0, 0)) set_point(table, (0, 0, 0)) env.Add(table) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) objects = [] goal_poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for i in range(m): x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0]) row_color = np.zeros(4) row_color[2-i] = 1. for j in range(n): y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1]) name = 'block%d-%d'%(i, j) color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0]) goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) objects.append(box_body(env, *box_dims, name=name, color=color)) object_names = [get_name(body) for body in objects] for obj in randomize(objects): randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(None, object_names=object_names, table_names=[get_name(table)], goal_poses=goal_poses)
def separate(env, n=7): # Previously 4, 8 env.Load(ENVIRONMENTS_DIR + 'tables.xml') set_default_robot_config(env.GetRobots()[0]) table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()]) objects = [] goal_regions = {} for i in range(2*n): objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED)) for i in range(n): name = 'green'+str(i+1) objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN)) goal_regions[name] = 'table1' for i in range(n): name = 'blue'+str(i+1) objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE)) goal_regions[name] = 'table3' objects.append(box_body(env, .07, .07, .2, name='black', color=BLACK)) object_names = [get_name(body) for body in objects] robot = env.GetRobots()[0] robot.SetActiveManipulator('leftarm') print robot.GetActiveManipulator().GetLocalToolTransform() grasps = {} #for obj_name in object_names: obj_name = 'black' env.Add(objects[-1]) obj = env.GetKinBody(obj_name) with obj: obj.SetTransform(np.eye(4)) obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.GRASP) #obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH) # TOP and SIDE are swapped grasps[get_name(obj)] = obj_grasps for obj in randomize(objects): randomly_place_body(env, obj, ['table2', 'table4']) return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_regions=goal_regions,grasps=grasps)
def srivastava_table(env, n=INF): env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae') set_default_robot_config(env.GetRobots()[0]) body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()] table_names = [body_name for body_name in body_names if 'table' in body_name] dx = .5 for body_name in body_names: body = env.GetKinBody(body_name) set_point(body, get_point(body) + np.array([dx, 0, 0])) objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names] for obj in objects: env.Remove(obj) object_names = [] for obj in take(objects, n): randomly_place_body(env, obj, table_names) object_names.append(get_name(obj)) goal_holding = 'object1' goal_config = 'initial' # None return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding)
def grid_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment) # env.Load(ENVIRONMENTS_DIR + 'empty.xml') env.Load(ENVIRONMENTS_DIR + 'regrasp_one_table.xml') # the environment for HBf pb, bb = place_body, box_body """ pb(env, bb(env, .3, .05, .3, name='obst1', color=GREY), (1.65, .075, 0), 'table1') pb(env, bb(env, .3, .05, .3, name='obst2', color=GREY), (1.65, .425, 0), 'table1') pb(env, bb(env, .05, .4, .3, name='obst3', color=GREY), (1.825, .25, 0), 'table1') """ pb(env, bb(env, .3, .05, .3, name='obst4', color=GREY), (1.65, -.125, 0), 'table1') pb(env, bb(env, .3, .05, .3, name='obst5', color=GREY), (1.65, -.375, 0), 'table1') pb(env, bb(env, .05, .3, .3, name='obst6', color=GREY), (1.825, -.25, 0), 'table1') obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1', 'table2'] """ pb(env, bb(env, .03, .1, .2, name='green', color=GREEN), (1.55, 0.25, 0), 'table1') pb(env, bb(env, .03, .1, .2, name='blue', color=BLUE), (1.5, 0.25, 0), 'table1') pb(env, bb(env, .05, .05, .1, name='red1', color=RED), (.1, -1.8, PI/16), 'table2') pb(env, bb(env, .15, .05, .15, name='red2', color=RED), (-.4, -1.95, PI/5), 'table2') pb(env, bb(env, .07, .07, .07, name='red3', color=RED), (.5, -1.9, PI/3), 'table2') pb(env, bb(env, .1, .1, .25, name='red4', color=RED), (1.9, -0.55, PI/7), 'table1') """ set_default_robot_config(env.GetRobots()[0]) #m, n = 2, 10 m, n = 2, 10 box_dims = (.12, .04, .08) #separation = (.08, .08) separation = (.12, .12) #separation = (.16, .16) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 # table = box_body(env, length, width, height, name='table', color=get_color('tan1')) # set_point(table, (1.75, 0, 0)) # env.Add(table) table = env.GetKinBody('table1') objects = [] goal_poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for i in range(m): x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0]) row_color = np.zeros(4) row_color[2-i] = 1. for j in range(n): y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1]) name = 'block%d-%d'%(i, j) # if i==0 and j==0: # color = np.array([1,0,0,0]) # box_dims = (.12, .06, .08) # else: color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0]) box_dims = (.12, .04, .1) goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) objects.append(box_body(env, *box_dims, name=name, color=color)) object_names = [get_name(body) for body in objects] object_names.append('ObjToG') objects.append(env.GetKinBody('ObjToG')) for obj in randomize(objects): #randomly_place_body(env, obj, [get_name(table)]) randomly_place_body(env, obj, ['table1']) return ManipulationProblem(None, object_names=object_names, table_names=[get_name(table)], goal_poses=goal_poses)
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 main(): theta_values=[] theta_augmented_with_grasp = [] env_file_list = os.listdir(DATA_DIR+'/env_files/') reward_matrix = [] theta_file_template = 'env_' thetas = get_all_thetas(theta_file_template) sio.savemat( SAVE_DIR + 'thetas.mat',{'thetas':thetas}) # goal_config_list = get_goal_config_from_theta_list(thetas) # sio.savemat( SAVE_DIR + 'goal_config_list.mat',{'goal_config_list':goal_config_list}) for env_idx in range(len(env_file_list)): train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx) # load the environment file print train_env_f_name env=Environment() env.Reset() simple_prob=separate(env,0) floor = env.GetKinBody("floorwalls") floor.Enable(False) oracle = ManipulationOracle(simple_prob,env) #env.SetViewer('qtcoin') ## Recovering the environment restore_env(env,train_env_f_name) # set the robot to default configuration robot = env.GetRobots()[0] manipulator = robot.SetActiveManipulator('leftarm_torso') set_default_robot_config(robot) # set arms to the pregrasp pose robot.SetActiveDOFs(manipulator.GetArmIndices()) evaluator = ThetaEvaluator(env,oracle) env_theta_vals = [] env_plan_time = [] env_ik_time= [] tst = time.time() for theta_idx in range(np.shape(thetas)[0]): theta = thetas[theta_idx,:] if len(theta) is not 0: base_pose = theta[0:3] grasp = theta[3:] each = time.time() theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose) else: theta_val = 0 env_theta_vals.append(theta_val) env_plan_time.append(plan_time) env_ik_time.append(ik_time) print time.time() - tst theta_values.append(env_theta_vals) ik_times.append(env_ik_time) plan_times.append(env_plan_time) sio.savemat(SAVE_DIR + 'reward_matrix.mat',{'reward_matrix':theta_values,'ik_times':ik_times,'plan_times':plan_times}) env.Destroy()
def main(): theta_values=[] ik_times = [] plan_times = [] theta_augmented_with_grasp = [] env_file_list = os.listdir(DATA_DIR+'/env_files/') reward_matrix = [] theta_file_template = 'env_' if not os.path.isfile(SAVE_DIR+'thetas.mat'): thetas = get_all_thetas(theta_file_template) sio.savemat( SAVE_DIR + 'thetas.mat',{'thetas':thetas}) else: thetas = sio.loadmat( SAVE_DIR + 'thetas.mat')['thetas'] # goal_config_list = get_goal_config_from_theta_list(thetas) # sio.savemat( SAVE_DIR + 'goal_config_list.mat',{'goal_config_list':goal_config_list}) if len(sys.argv) == 1: print "NEED ENV NUMBER" return 0 env_idx = int(sys.argv[1]) train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx) reward_file = SAVE_DIR + 'reward_matrix' +str(env_idx)+'.mat' if os.path.isfile(reward_file): print train_env_f_name +' already done' return 0 print "SPAWNED: " + train_env_f_name # load the environment file # print train_env_f_name env=Environment() env.Reset() simple_prob=separate(env,0) floor = env.GetKinBody("floorwalls") floor.Enable(False) oracle = ManipulationOracle(simple_prob,env) #env.SetViewer('qtcoin') ## Recovering the environment restore_env(env,train_env_f_name,DATA_DIR) # set the robot to default configuration robot = env.GetRobots()[0] manipulator = robot.SetActiveManipulator('leftarm_torso') set_default_robot_config(robot) # set arms to the pregrasp pose robot.SetActiveDOFs(manipulator.GetArmIndices()) evaluator = ThetaEvaluator(env,oracle) env_theta_vals = [] env_plan_time = [] env_ik_time= [] tst = time.time() for theta_idx in range(np.shape(thetas)[0]): print "Running ENV " + str(theta_idx) theta = thetas[theta_idx,:] if len(theta) is not 0: base_pose = theta[0:3] grasp = theta[3:] each = time.time() theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose) else: theta_val = 0 env_theta_vals.append(theta_val) env_plan_time.append(plan_time) env_ik_time.append(ik_time) theta_values.append(env_theta_vals) ik_times.append(env_ik_time) plan_times.append(env_plan_time) sio.savemat(SAVE_DIR + 'reward_matrix' +str(env_idx)+'.mat',\ {'reward_matrix':theta_values,'ik_times':ik_times,'plan_times':plan_times}) env.Destroy() print "ENV " + str(env_idx) + "FINISHED RUNNING"
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') m, n = 3, 3 #m, n = 5, 5 side_dim = .07 # .05 | .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) #separation = (side_dim/2, side_dim/2) coordinates = list(product(range(m), range(n))) assert n_obj <= len(coordinates) obj_coordinates = sample(coordinates, n_obj) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) #set_base_values(robot, (0, width/2 + .5, math.pi)) #set_base_values(robot, (.35, width/2 + .35, math.pi)) #set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4)) poses = [] z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r in range(m): row = [] x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) for c in range(n): y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))) poses.append(row) initial_poses = {} goal_poses = {} # TODO - randomly assign goal poses here for i, (r, c) in enumerate(obj_coordinates): row_color = np.zeros(4) row_color[2-r] = 1. if i == 0: name = 'goal%d-%d'%(r, c) color = BLUE goal_poses[name] = poses[m/2][n/2] else: name = 'block%d-%d'%(r, c) color = RED initial_poses[name] = poses[r][c] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, poses[r][c].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) known_poses = list(flatten(poses)) #known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses)
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 main(): env_file_order = sio.loadmat(DATA_DIR+'env_file_order.mat')['file_orders'][0] theta_values=[] theta_augmented_with_grasp = [] for env_idx in range(160,len(env_file_order)): # load the environment file env_order = env_file_order[env_idx] train_env_f_name = 'scene'+str(env_order)+'_env.dae' print train_env_f_name #theta_file_template = 'good_thetas_' theta_file_template = 'good_thetas_leslie_' thetas = sio.loadmat(THETA_DIR+theta_file_template+str(env_idx)+'.mat')['thetas'] if theta_file_template is not 'good_thetas_leslie_': #artificial_vals= sio.loadmat(THETA_DIR+'good_thetas_'+\ # str(env_idx)+'.mat')['values'] #thetas = thetas[artificial_vals[0]>0,:] thetas = get_all_thetas(theta_file_template) else: thetas = get_all_thetas(theta_file_template) # sio.savemat('theta_values_leslie.mat',{'thetas':thetas}) env=Environment() env.Reset() simple_prob=separate(env,0) floor = env.GetKinBody("floorwalls") floor.Enable(False) oracle = ManipulationOracle(simple_prob,env) #env.SetViewer('qtcoin') ## Recovering the environment restore_env(env,train_env_f_name,DATA_DIR) # set the robot to default configuration robot = env.GetRobots()[0] manipulator = robot.SetActiveManipulator('leftarm_torso') set_default_robot_config(robot) # set arms to the pregrasp pose robot.SetActiveDOFs(manipulator.GetArmIndices()) evaluator = ThetaEvaluator(env,oracle) env_theta_vals = [] for theta_idx in range(np.shape(thetas)[0]): theta = thetas[theta_idx,:] if theta_file_template == 'good_thetas_leslie_': if len(theta) is not 0: base_pose = theta[0:3] grasp = theta[3:] theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose) else: theta_val = 0 else: theta_val,g = evaluator.get_true_vals(theta) if g is not None: print g theta_augmented_with_grasp.append( np.r_[theta,g] ) else: theta_augmented_with_grasp.append( np.r_[theta] ) env_theta_vals.append(theta_val) theta_values.append(env_theta_vals) if theta_file_template == 'good_thetas_leslie_': sio.savemat('theta_rewards_leslile5.mat',{'reward':theta_values})
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) """
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
env_f_name = "scene" + str(env_order) + "_env.dae" print "generating optimal theta value for n_evals = " + str(n_theta_evals) + " with env file = " + env_f_name print str(float(total_n_thetas * n_theta_evals + env_order + 1) / total_evals * 100) + "% Complete" ## Load the environment env = Environment() env.Reset() simple_prob = separate(env, 0) floor = env.GetKinBody("floorwalls") floor.Enable(False) # env.SetViewer('qtcoin') robot = env.GetRobots()[0] manipulator = robot.SetActiveManipulator("leftarm_torso") set_default_robot_config(robot) # set arms to the pregrasp pose training_env = Environment() training_env.Load(data_dir + "env_files/" + env_f_name) restore_env(env, training_env) training_env.Destroy() ikmodel, gmodel, path_plan_time, goals = load_database(env, robot, manipulator) grasps, grasp_indices = gmodel.computeValidGrasps(checkcollision=False, checkik=False) goal_indices = range(len(goals)) grasp_indices = range(len(grasps)) ## restore the following: [goal,grasp]=(1,1),(1,2),...,(1,n_grasps),(2,1),...,(2,n_grasps),...,(n_goals,n_grasps) optimal_theta_idx = optimal_theta_idx_list[n_theta_evals, env_idx]