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 grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500): if goal in self: self[goal].retrace() if self.collision(goal): return None nodes1, new_nodes1 = list(take(randomize(self.nodes.values()), max_tree_size)), [] nodes2, new_nodes2 = [], [TreeNode(goal)] for _ in irange(iterations): if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2): nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 s = self.sample() last1 = argmin(lambda n: self.distance(n.config, s), nodes1 + new_nodes1) for q in self.extend(last1.config, s): if self.collision(q): break last1 = TreeNode(q, parent=last1) new_nodes1.append(last1) last2 = argmin(lambda n: self.distance(n.config, last1.config), nodes2 + new_nodes2) for q in self.extend(last2.config, last1.config): if self.collision(q): break last2 = TreeNode(q, parent=last2) new_nodes2.append(last2) else: if len(nodes1) == 0: nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 last1, last2 = last2, last1 path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1] for p, n in pairs(path2): n.parent = p if len(path2) == 0: # TODO - still some kind of circular error for n in new_nodes2: if n.parent == last2: n.parent = path1[-1] else: path2[0].parent = path1[-1] path = path1 + path2 if store in [ts.ALL, ts.SUCCESS]: self.add(*(new_nodes1 + new_nodes2[:-1])) elif store == ts.PATH: new_nodes_set = set(new_nodes1 + new_nodes2[:-1]) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes1) 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] 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 grow(self, goal_sample, iterations=50, goal_probability=.2, store=ts.PATH, max_tree_size=500): if not callable(goal_sample): goal_sample = lambda: goal_sample nodes, new_nodes = list(take(randomize(self.nodes.values()), max_tree_size)), [] for i in irange(iterations): goal = random() < goal_probability or i == 0 s = goal_sample() if goal else self.sample() last = argmin(lambda n: self.distance(n.config, s), nodes + new_nodes) for q in self.extend(last.config, s): if self.collision(q): break last = TreeNode(q, parent=last) new_nodes.append(last) else: if goal: path = last.retrace() if store in [ts.ALL, ts.SUCCESS]: self.add(*new_nodes) elif store == ts.PATH: new_nodes_set = set(new_nodes) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes) 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)