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 simple_namo(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'navigation.xml') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] place_body_on_floor(box_body(env, .1, .3, .8, name='red1', color=RED), (0, -1.5, 0)) place_body_on_floor(box_body(env, .1, .3, .8, name='red2', color=RED), (0, 1.5, 0)) floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \ str(body.GetName()) not in obstacle_names] goal_config = Config(np.array((1.5, 0, PI))) goal_holding = False # False | None goal_poses = { # 'blue': Pose(pose_from_base_values([0, 1.5, 0], z=BODY_PLACEMENT_Z_OFFSET)) } return ManipulationProblem(function_name(inspect.stack()), floor_object_names=floor_object_names, goal_config=goal_config, goal_holding=goal_holding, goal_poses=goal_poses)
def distractions(env): env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1', 'table2'] regions = [ create_region(env, 'green_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 1, 0, .5))), ] pb = place_body center = np.array([1.75, 0.25, 0]) pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center, 'table1') id = count(1) for dx, dy in product((-.15, 0, .15), repeat=2): if not (dx == 0 and dy == 0): pb(env, box_body(env, .07, .05, .2, name='red_box%d'%next(id), color=RED), center + np.array([dx, dy, 0]), 'table1') #for x in irange(-.7, .7, .15): # for y in irange(-2.0, -1.5, .15): for x in irange(-.65, .75, .15): for y in irange(-1.95, -1.5, .15): pb(env, box_body(env, .05, .05, .15, name='red_box%d'%next(id), color=RED), np.array([x, y, 0]), 'table2') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names] goal_regions = { 'green_box': 'green_goal', } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, regions=regions, goal_regions=goal_regions)
def dinner(env): env.Load(ENVIRONMENTS_DIR + 'kitchen.xml') obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1'] sink_names = ['washer'] stove_names = ['heater'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))), create_region(env, 'green_goal', ((-1, 1), (.5, 1)), 'table1', color=np.array((0, 1, 0, .5))) ] pb = place_body pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE), (1.65, 0.115, PI/3), 'table1') pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1') pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1') pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.415, PI/8), 'table1') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names] goal_regions = { 'blue': 'blue_goal', 'green': 'green_goal' } goal_cleaned = ['blue'] goal_cooked = ['green'] return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions, goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
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 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 pap_ir_oracle(env): # TODO - vary object and table geometry from manipulation.problems.fixed import ENVIRONMENTS_DIR # NOTE - prevents circular imports from manipulation.oracle import ManipulationOracle from manipulation.problems import ManipulationProblem env.Load(ENVIRONMENTS_DIR + 'empty.xml') table_name = 'table' env.Add(box_body(env, 1., 1., .75, name=table_name)) body_name = 'body' env.Add(box_body(env, .07, .07, .2, name=body_name)) return ManipulationOracle(ManipulationProblem(None, object_names=[body_name], table_names=[table_name]), env, preload_databases=False, debug=False)
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 distract_n(env, n): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] regions = [ create_region(env, 'green_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 1, 0, .5))), ] pb = place_body center = np.array([1.75, 0.25, 0]) pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center, 'table1') id = count(1) for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]: pb(env, box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED), center + np.array([dx, dy, 0]), 'table1') #for x in irange(-.7, .7, .15): # for y in irange(-2.0, -1.5, .15): for x, y in islice( product(irange(-.65, .75, .15), irange(-1.95, -1.5, .15)), n): pb(env, box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED), np.array([x, y, 0]), 'table2') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_regions = { 'green_box': 'green_goal', } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, regions=regions, goal_regions=goal_regions)
def kitchen(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'kitchen.xml') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1'] sink_names = ['washer'] #stove_names = ['heater'] stove_names = ['microwave'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))), create_region(env, 'green_goal', ((-1, 1), (.5, 1)), 'table1', color=np.array((0, 1, 0, .5))) ] pb = place_body pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE), (1.65, 0.115, PI / 3), 'table1') pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN), (1.55, -0.215, 7 * PI / 6), 'table1') pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1') pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.415, PI / 8), 'table1') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_regions = {'blue': 'blue_goal', 'green': 'green_goal'} goal_cleaned = ['blue'] goal_cooked = ['green'] return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions, goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
def stacking(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))), create_region(env, 'green_goal', ((-1, -.5), (-1, 1)), 'table2', color=np.array((0, 1, 0, .5))) ] pb = place_body pb(env, box_body(env, .07, .07, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI / 3), 'table1') pb(env, box_body(env, .07, .07, .2, name='green_box', color=GREEN), (1.55, -0.215, 7 * PI / 6), 'table1') pb(env, box_body(env, .07, .07, .2, name='purple_box', color=BLACK), (.15, -1.8, 7 * PI / 6), 'table2') pb(env, box_body(env, .07, .07, .2, name='red_box1', color=RED), (1.55, -0.215, 0), 'green_box') #pb(env, box_body(env, .07, .07, .2, name='red_box2', color=RED), (1.65, .6, PI/4), 'table1') #pb(env, box_body(env, .07, .07, .2, name='red_box3', color=RED), (.3, -1.9, PI/12), 'table2') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_stackings = { # 'purple_box': 'blue_box' # target_cylinder } goal_regions = {'blue_box': 'blue_goal', 'green_box': 'green_goal'} return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, regions=regions, goal_stackings=goal_stackings, goal_regions=goal_regions)
def create_obstacles(env, loading_regions): NUM_OBSTACLES = 4 OBSTACLES = [] obstacle_poses = {} obstacle_shapes = {} i = 0 while len(OBSTACLES) < 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 new_body = box_body(env,width,length,height,\ name='obst%s'%len(OBSTACLES),\ color=(0, (i+.5)/NUM_OBSTACLES, 1)) trans = np.eye(4) trans[2, -1] = 0.075 env.Add(new_body) new_body.SetTransform(trans) xytheta = randomly_place_in_region(env,new_body,\ loading_regions[np.random.randint(len(loading_regions))]) 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) return OBSTACLES, obstacle_shapes, obstacle_poses
def pap_ir_oracle(env): # TODO - vary object and table geometry from manipulation.problems.fixed import ENVIRONMENTS_DIR # NOTE - prevents circular imports from manipulation.oracle import ManipulationOracle from manipulation.problems import ManipulationProblem env.Load(ENVIRONMENTS_DIR + 'empty.xml') table_name = 'table' #env.Add(box_body(env, 1., 1., .75, name=table_name)) # NOTE - .75 is too tall for top grasps env.Add(box_body(env, 1., 1., .7, name=table_name)) body_name = 'body' env.Add(box_body(env, .07, .07, .2, name=body_name)) return ManipulationOracle(ManipulationProblem(None, object_names=[body_name], table_names=[table_name]), env, preload_databases=False, debug=False)
def create_objects(env, conveyor_belt): num_objects = 5 objects = [] obj_shapes = {} obj_poses = {} for i in range(num_objects): if i > 10 and i < 15: min_width = 0.7 max_width = 0.7 min_length = 0.6 else: min_width = 0.2 max_width = 0.6 min_length = 0.2 width = np.random.rand(1) * (max_width - min_width) + min_width length = np.random.rand(1) * (max_width - min_length) + min_length height = np.random.rand(1) * (max_height - min_height) + min_height new_body = box_body(env, width, length, height, \ name='obj%s' % i, \ color=(0, (i + .5) / num_objects, 0)) trans = np.eye(4) trans[2, -1] = 0.075 env.Add(new_body) new_body.SetTransform(trans) xytheta = randomly_place_in_region(env, new_body, conveyor_belt) objects.append(new_body) obj_shapes['obj%s' % i] = [width[0], length[0], height[0]] obj_poses['obj%s' % i] = xytheta return objects, obj_shapes, obj_poses
def decision_wall_namo( env, rows, cols ): # TODO - could make variant that requires replacing things as well assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'decision_2tables.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] for x in range(cols): for y in range(rows): #if y >= rows/2 and x >= cols/2: continue if y >= rows / 2 and x >= cols - 1: continue # NOTE - makes one side harder than the other place_body_on_floor( box_body(env, .1, .1, .8, name='red%s-%s' % (x, y), color=RED), (-.25 + x * .75 / cols, -2.5 + (y + .5) * 5 / rows, 0)) place_body(env, box_body(env, .07, .07, .2, name='blue0', color=BLUE), (1.75, +0.2, 0), 'table1') place_body(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE), (1.75, -0.2, 0), 'table1') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \ str(body.GetName()) not in obstacle_names and 'blue' in str(body.GetName())] floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \ str(body.GetName()) not in obstacle_names and 'red' in str(body.GetName())] goal_config = 'initial' goal_holding = None # False | None goal_regions = { 'blue0': 'table2', 'blue1': 'table2', } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, floor_object_names=floor_object_names, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding, goal_regions=goal_regions)
def replace(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] pb = place_body id = count(1) center = np.array([1.75, 0.25, 0]) pb(env, box_body(env, .07, .05, .2, name='green', color=GREEN), center, 'table1') for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]: pb(env, box_body(env, .07, .05, .2, name='red%d' % next(id), color=RED), center + np.array([dx, dy, 0]), 'table1') center = np.array([.25, -1.75, 0]) pb(env, box_body(env, .07, .05, .2, name='blue', color=BLUE), center, 'table1') for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]: pb(env, box_body(env, .07, .05, .2, name='red%d' % next(id), color=RED), center + np.array([dx, dy, 0]), 'table2') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_poses = { 'green': 'blue', 'blue': 'green', } #for obj in object_names: # This make the problem highly non-monotonic and thus hard (still solved it though) # if 'red' in obj: # goal_poses[obj] = 'initial' return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, goal_poses=goal_poses)
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 push_wall(env): env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1, 1, 3]), look_point=(3, 0, 0)) obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1', 'table2'] place_body(env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN), (1.65, -.5, PI/4), 'table1') #place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1') #set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z()) place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED), (1.50, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED), (1.6, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED), (1.7, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED), (1.8, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED), (1.9, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED), (2.0, 0, 0), 'table1') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names] start_constrained = {'green_cylinder': 'table1'} goal_constrained = { 'green_cylinder': ('table1', Point(get_point(env.GetKinBody('green_cylinder')) + np.array([-.2, 1.0, 0.]))) } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, start_constrained=start_constrained, goal_constrained=goal_constrained)
def stacking(env): env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1', 'table2'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))), create_region(env, 'green_goal', ((-1, -.5), (-1, 1)), 'table2', color=np.array((0, 1, 0, .5))) ] pb = place_body pb(env, box_body(env, .07, .07, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI/3), 'table1') pb(env, box_body(env, .07, .07, .2, name='green_box', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1') pb(env, box_body(env, .07, .07, .2, name='purple_box', color=BLACK), (.15, -1.8, 7*PI/6), 'table2') pb(env, box_body(env, .07, .07, .2, name='red_box1', color=RED), (1.55, -0.215, 0), 'green_box') pb(env, box_body(env, .07, .07, .2, name='red_box2', color=RED), (1.65, .6, PI/4), 'table1') pb(env, box_body(env, .07, .07, .2, name='red_box3', color=RED), (.3, -1.9, PI/12), 'table2') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names] goal_stackings = { 'purple_box': 'blue_box' # target_cylinder } goal_regions = { 'blue_box': 'blue_goal', 'green_box': 'green_goal' } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, regions=regions, goal_stackings=goal_stackings, goal_regions=goal_regions)
def simple(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'room1.xml') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] #table_names = ['table1'] table_names = ['table1', 'table2'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))), ] pb = place_body pb(env, box_body(env, .07, .05, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI / 3), 'table1') pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.55, -0.215, 7 * PI / 6), 'table1') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_regions = { 'blue_box': 'blue_goal', } #goal_config = 'initial' # None goal_config = None return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, regions=regions, goal_config=goal_config, goal_regions=goal_regions)
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 easy_dinner(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'dinner.xml') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'shelves', 'plate1', 'kitchen_table'] sink_names = ['washer'] stove_names = ['microwave'] pb = place_body pb(env, box_body(env, .07, .07, .2, name='cyan2', color=CYAN), (1.7, 0.0, PI / 4), 'table1') pb(env, box_body(env, .1, .1, .1, name='green1', color=GREEN), (-1.7, 1.8, 0), 'shelves') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_regions = { 'green1': 'plate1', } goal_cleaned = ['cyan2'] goal_cooked = ['green1'] return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
def simple(env): env.Load(ENVIRONMENTS_DIR + 'room1.xml') obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()] table_names = ['table1', 'table2'] regions = [ create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))), create_region(env, 'green_goal', ((-1, -.5), (-1, 1)), 'table2', color=np.array((0, 1, 0, .5))) ] pb = place_body pb(env, box_body(env, .07, .05, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI/3), 'table1') pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1') object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names] goal_regions = { 'blue_box': 'blue_goal', 'green_box': 'green_goal' } return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, regions=regions, goal_regions=goal_regions)
def create_obstacles(env, loading_regions): num_obstacles = 3 obstacles = [] obstacle_poses = {} obstacle_shapes = {} i = 0 min_width = 0.2 max_width = 0.4 min_length = 0.2 max_length = 3 max_height = 0.5 while len(obstacles) < num_obstacles: if len(obstacles) == 0: min_length = 1.8 max_length = 1.8 else: min_length = 0.2 max_length = 0.8 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 new_body = box_body(env, width, length, height, name='obst%s' % len(obstacles), color=(0, (i + .5) / num_obstacles, 1)) trans = np.eye(4) trans[2, -1] = 0.075 env.Add(new_body) new_body.SetTransform(trans) xytheta = randomly_place_in_region(env, new_body, loading_regions) #loading_regions[np.random.randint(len(loading_regions))]) 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: raise ValueError, 'Not enough spot for obstacles' #env.Remove(new_body) return obstacles, obstacle_shapes, obstacle_poses
def load_objects(env, obj_shapes, obj_poses, color): objects = [] i = 0 nobj = len(obj_shapes.keys()) for obj_name in obj_shapes.keys(): xytheta = obj_poses[obj_name].squeeze() width, length, height = obj_shapes[obj_name] quat = quat_from_z_rot(xytheta[-1]) new_body = box_body(env, width, length, height, \ name=obj_name, \ color=np.array(color) / float(nobj - i)) i += 1 env.Add(new_body) set_point(new_body, [xytheta[0], xytheta[1], 0.075]) set_quat(new_body, quat) objects.append(new_body) return objects
def draw_q_value_rod_for_action(action_idx, base_pose, q_val, penv, maxQ): normalized_q = q_val / float(maxQ) width = 0.2 length = 0.2 height = normalized_q if height == 0.5: color = (0, 0, 1) else: color = (0.8, 0.2, 0.5) new_body = box_body(penv.env, width, length, height, name='q_value_obj%s' % action_idx, color=color) penv.env.Add(new_body) if base_pose is not None: set_obj_xytheta(base_pose, new_body)
def restore_env(curr_env, past_env): assert len(curr_env.GetBodies()) == 7 # make sure the curr env is "empty" with only ObjToG in it curr_env_objtog = curr_env.GetKinBody("ObjToG") curr_env_objtog.SetTransform(past_env.GetKinBody("ObjToG").GetTransform()) new_obj_idx = 1 for past_env_body in past_env.GetBodies(): if ( past_env_body.GetName() == "pr2" or past_env_body.GetName() == "floorwalls" or past_env_body.GetName() == "table2" or past_env_body.GetName() == "table1" or past_env_body.GetName() == "table4" or past_env_body.GetName() == "table3" or past_env_body.GetName() == "ObjToG" ): continue new_obj = box_body(curr_env, 0.07, 0.07, 0.2, name="red" + str(new_obj_idx), color=RED) new_obj.SetTransform(past_env_body.GetTransform()) new_obj_idx += 1 curr_env.AddKinBody(new_obj)
def push_wall(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'room1.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1, 1, 3]), look_point=(3, 0, 0)) obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] place_body( env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN), (1.65, -.5, PI / 4), 'table1') #place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1') #set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z()) place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED), (1.50, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED), (1.6, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED), (1.7, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED), (1.8, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED), (1.9, 0, 0), 'table1') place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED), (2.0, 0, 0), 'table1') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] start_constrained = {'green_cylinder': 'table1'} goal_constrained = { 'green_cylinder': ('table1', Point( get_point(env.GetKinBody('green_cylinder')) + np.array([-.2, 1.0, 0.]))) } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, start_constrained=start_constrained, goal_constrained=goal_constrained)
def create_objects(env, conveyor_belt): NUM_OBJECTS = 5 OBJECTS = [] obj_shapes = {} obj_poses = {} for i in range(NUM_OBJECTS): width = np.random.rand(1) * (max_width - min_width) + min_width length = np.random.rand(1) * (max_width - min_length) + min_length height = np.random.rand(1) * (max_height - min_height) + min_height new_body = box_body(env,width,length,height,\ name='obj%s'%i,\ color=(0, (i+.5)/NUM_OBJECTS, 0)) trans = np.eye(4) trans[2, -1] = 0.075 env.Add(new_body) new_body.SetTransform(trans) xytheta = randomly_place_in_region(env, new_body, conveyor_belt, np.array([0])) OBJECTS.append(new_body) obj_shapes['obj%s' % i] = [width[0], length[0], height[0]] obj_poses['obj%s' % i] = xytheta return OBJECTS, obj_shapes, obj_poses
def create_objects(env, conveyor_belt, num_objects): objects = [] obj_shapes = {} obj_poses = {} for i in range(num_objects): #if 0 <= i < 0: if i == 0 or i == 1: min_width = 0.6 max_width = 0.7 min_length = 0.7 name = 'big_obj' else: min_width = 0.4 max_width = 0.5 min_length = 0.6 name = 'small_obj' width = np.random.rand(1) * (max_width - min_width) + min_width length = np.random.rand(1) * (max_width - min_length) + min_length height = np.random.rand(1) * (max_height - min_height) + min_height new_body = box_body(env, width, length, height, name=name + '%s' % i, color=(0, 0.5, 0)) trans = np.eye(4) trans[2, -1] = 0.075 env.Add(new_body) new_body.SetTransform(trans) xytheta = randomly_place_in_region(env, new_body, conveyor_belt) objects.append(new_body) obj_shapes['obj%s' % i] = [width[0], length[0], height[0]] obj_poses['obj%s' % i] = xytheta return objects, obj_shapes, obj_poses
def dinner(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'dinner.xml') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'shelves', 'plate1', 'plate2', 'kitchen_table'] sink_names = ['washer'] stove_names = ['microwave'] regions = [ #create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))), #create_shelf_region(env, 'shelf', ((-1, 1), (-1, 1)), 'shelves', .99), ] pb = place_body #pb(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE), (1.5, -0.4, 0), 'table1') #pb(env, box_body(env, .07, .07, .2, name='blue2', color=BLUE), (1.5, 0.4, 0), 'table1') pb(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE), (1.6, -0.1, PI / 3), 'table1') pb(env, box_body(env, .07, .07, .2, name='blue2', color=BLUE), (1.5, 0.15, PI / 6), 'table1') #pb(env, box_body(env, .07, .07, .2, name='cyan1', color=CYAN), (1.55, -0.5, PI/5), 'table1') # NOTE - just need to be cleaned pb(env, box_body(env, .07, .07, .2, name='cyan2', color=CYAN), (1.7, 0.0, PI / 4), 'table1') pb(env, box_body(env, .1, .1, .1, name='green1', color=GREEN), (-1.7, 1.8, 0), 'shelves') pb(env, box_body(env, .1, .1, .1, name='green2', color=GREEN), (-1.3, 1.8, 0), 'shelves') #set_body(env, box_body(env, .1, .1, .15, name='green1', color=GREEN), (-1.7, 1.85, .991), 0) #set_body(env, box_body(env, .1, .1, .15, name='green2', color=GREEN), (-1.3, 1.85, .991), 0) pb(env, box_body(env, .07, .07, .07, name='pink1', color=PINK), (-1.7, 1.7, 0), 'shelves') pb(env, box_body(env, .07, .07, .07, name='pink2', color=PINK), (-1.3, 1.7, 0), 'shelves') pb(env, box_body(env, .07, .07, .07, name='pink3', color=PINK), (-1.8, 1.8, 0), 'shelves') pb(env, box_body(env, .07, .07, .07, name='pink4', color=PINK), (-1.2, 1.8, 0), 'shelves') #pb(env, box_body(env, .1, .1, .1, name='red1', color=RED), (-1.5, 1.8, 0), 'shelves') # NOTE - next to green #pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1') #pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.215, PI/8), 'table1') #set_body(env, box_body(env, .07, .07, .2, name='red3', color=RED), (-2, 0.1, 1.07), 0) #set_body(env, box_body(env, .07, .07, .2, name='red4', color=RED), (-2, -0.1, .79), 0) #set_body(env, box_body(env, .07, .07, .2, name='red3', color=RED), (-2, 0.1, .99), 0) #set_body(env, box_body(env, .07, .07, .2, name='red4', color=RED), (-2, -0.1, .99), 0) object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] goal_poses = { 'blue1': Pose( pose_from_quat_point(quat_from_axis_angle(0, 0, 0), np.array([1.5, -0.4, .701]))), 'blue2': Pose( pose_from_quat_point(quat_from_axis_angle(0, 0, 0), np.array([1.5, 0.4, .701]))), #'blue1': 'initial', #'blue2': 'initial', 'pink1': 'initial', 'pink2': 'initial', 'pink3': 'initial', 'pink4': 'initial', } # TODO - food table # TODO - Pantry table goal_regions = { #'blue1': 'blue_goal', #'green1': 'green_goal', 'green1': 'plate1', 'green2': 'plate2', } goal_cleaned = ['blue1', 'blue2'] + ['cyan2'] #goal_cleaned = ['blue1', 'blue2', 'cyan1', 'cyan2'] goal_cooked = ['green1', 'green2'] return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions, goal_poses=goal_poses, goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
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 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 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 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 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)