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 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_through_door(env, obj_length, n=7): # Previously 4, 8 env.Load(ENVIRONMENTS_DIR + 'two_tables'+str(obj_length)+'.xml') #env.Load(ENVIRONMENTS_DIR + 'two_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 = [] # objects.append(env.GetKinBody("ObjToG")) 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']) # randomly_place_body(env, obj, ['table2']) return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_regions=goal_regions)
def two_tables_through_door(env, obj_length, n=7): # Previously 4, 8 env.Load(ENVIRONMENTS_DIR + 'two_tables'+str(obj_length)+'.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 = [] # objects.append(env.GetKinBody("ObjToG")) 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 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 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 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 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))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not env.CheckCollision(body): return get_body_xytheta(body) return None
def shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment) env.Load(ENVIRONMENTS_DIR + 'empty.xml') #m, n = 2, 10 m, n = 2, 4 box_dims = (.07, .07, .2) #separation = (.08, .08) separation = (.15, .15) 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) # TODO - place walls and/or a roof to make more similar to pebble graph people 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 shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment) env.Load(ENVIRONMENTS_DIR + 'empty.xml') #m, n = 2, 10 m, n = 2, 4 box_dims = (.07, .07, .2) #separation = (.08, .08) separation = (.15, .15) 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) # TODO - place walls and/or a roof to make more similar to pebble graph people 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] print object_names 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 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 get_grasps(env, robot, body, grasp_approach, grasp_type): Options = get_grasp_options(body, grasp_approach) grasp_options = Options.default(grasp_type, body) filename = GRASP_FILENAME%positive_hash(grasp_options) # TODO - get rid of the negative numbers """ print 'Creating', get_name(body), GRASP_TYPES.names[grasp_type], 'database' # TODO - move this to create_grasp_database c_grasps = hacked_create_grasp_database(env, grasp_options) o_grasps = load_grasp_database(robot,filename) return c_grasps #import pdb;pdb.set_trace() """ try: return load_grasp_database(robot, filename) except IOError: print 'Creating', get_name(body), GRASP_TYPES.names[grasp_type], 'database' # TODO - move this to create_grasp_database grasps = hacked_create_grasp_database(env, grasp_options) #save_grasp_database(filename, grasps) # TODO - not saving right not because incomplete return grasps
def gaussian_randomly_place_in_region(env, body, region, center, var): if env.GetKinBody(get_name(body)) is None: env.Add(body) for i in range(1000): xytheta = np.random.normal(center, var) set_obj_xytheta(xytheta, body) if not body_collision(env, body): return xytheta import pdb;pdb.set_trace() for i in range(1000): set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return get_body_xytheta(body) return None
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 get_body_name(self, geom_hash): for body in self.env.GetBodies(): if geometry_hash(body) == geom_hash: return get_name(body) return None
def rearrangement_problem(env,obj_poses=None): # import random # seed = 50 # random.seed(seed) # np.random.seed(seed) MIN_DELTA = .01 # .01 | .02 ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml' env.Load(ENV_FILENAME) robot = env.GetRobots()[0] TABLE_NAME = 'table1' NUM_OBJECTS = 8 WIDTH = .05 # .07 | .1 TARGET_WIDTH = 0.07 OBJ_HEIGHT = 0.2 objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \ color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)] target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \ color=(1, 1.5, 0)) #TODO: Place obstacle that prevent you to reach from top OBST_X = 0 OBST_WIDTH = 1. OBST_COLOR = (1, 0, 0) OBST_HEIGHT = 0.4 OBST_TRANSPARENCY = .25 place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME) place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME) place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.225, OBST_X, 0), TABLE_NAME) # roof OBST_Z = OBST_HEIGHT place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (0, OBST_X, OBST_Z,0), TABLE_NAME) # I think this can be done once and for all REST_TORSO = [.15] robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')]) l_model = databases.linkstatistics.LinkStatisticsModel(robot) if not l_model.load(): l_model.autogenerate() l_model.setRobotWeights() min_delta = 0.01 l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T robot.SetAffineTranslationLimits(*extrema) X_PERCENT = 0.0 # define intial region for movable objects init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5))) #init_region.draw(env) # define target object region target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \ TABLE_NAME, color=np.array((0, 0, 0, 0.9))) target_obj_region.draw(env) # place object if obj_poses is not None: for obj in objects: set_pose(obj,obj_poses[get_name(obj)]) set_quat(obj, quat_from_z_rot(0)) if env.GetKinBody(get_name(obj)) is None: env.Add(obj) set_pose( target_obj, obj_poses[get_name(target_obj)] ) set_quat( target_obj, quat_from_z_rot(0) ) if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj) else: for obj in objects: randomly_place_region(env, obj, init_region) set_quat(obj, quat_from_z_rot(0)) randomly_place_region(env, target_obj, target_obj_region) set_quat(target_obj, quat_from_z_rot(0)) object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left) set_xy(robot, -.75, 0) # LEFT_ARM_CONFIG = HOLDING_LEFT_ARM robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices()) robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices()) grasps = {} for obj_name in object_names: obj = env.GetKinBody(obj_name) obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \ + get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH) grasps[get_name(obj)] = obj_grasps target_obj = env.GetKinBody('target_obj') target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\ get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) grasps['target_obj'] = target_obj_grasps initial_poses={} for obj in objects: initial_poses[get_name(obj)] = get_pose(obj) initial_poses[get_name(target_obj)] = get_pose(target_obj) return ManipulationProblem(None, object_names=object_names, table_names='table1', goal_regions=init_region,grasps=grasps, initial_poses = initial_poses)
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 get_grasps(env, robot, body, grasp_approach, grasp_type): Options = get_grasp_options(body, grasp_approach) grasp_options = Options.default(grasp_type, body) print 'Creating', get_name(body), GRASP_TYPES.names[ grasp_type], 'database' # TODO - move this to create_grasp_database return hacked_create_grasp_database(env, grasp_options)
def robot_collision(oracle, body_name=None, check_self=True): if body_name is None: return collision(oracle, get_name(oracle.robot)) or ( not check_self and self_collision(oracle, get_name(oracle.robot))) return collision(oracle, get_name(oracle.robot), body_name)