def simple_push(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', 'table2'] place_body(env, cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE), (1.65, -.6, PI / 4), '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 = {'blue_cylinder': 'table1'} goal_constrained = { 'blue_cylinder': ('table2', Point( get_point(env.GetKinBody('blue_cylinder')) + np.array([-1.65, -1., 0.]))) #'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.]))) } return ManipulationProblem(function_name(inspect.stack()), object_names=object_names, table_names=table_names, start_constrained=start_constrained, goal_constrained=goal_constrained)
def decision_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)) place_body_on_floor(box_body(env, .1, .3, .8, name='red3', color=RED), (-.5, 1.5, 0)) place_body_on_floor(box_body(env, .1, .3, .8, name='red4', color=RED), (.5, 1.5, 0)) place_body_on_floor(box_body(env, .1, .3, .8, name='red5', color=RED), (-1, 1.5, 0)) place_body_on_floor(box_body(env, .1, .3, .8, name='red6', color=RED), (1, 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 nonmonotonic(env, N=4, M=3): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'regrasp.xml') #env.Load(ENVIRONMENTS_DIR + '3tables.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) pb, bb = place_body, box_body pb(env, bb(env, .1, 1.5, .2, name='obst1', color=GREY), (1.75, 0, 0), 'table1') pb(env, bb(env, 1.5, .1, .2, name='obst2', color=GREY), (0, -1.75, 0), 'table2') obstacle_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] table_names = ['table1', 'table2'] #, 'table3'] goal_poses = {} for i in range(N): displacement = -.75 + (1.5 * (i + .5)) / N value = .25 + (.75 * (i + 1)) / N if i < M: green = 'green%s' % i pb(env, bb(env, .05, .1, .2, name=green, color=(0, value, 0, 1)), (1.75 - 0.1, displacement, 0), 'table1') goal_poses[green] = Pose( pose_from_quat_point( quat_from_axis_angle(0, 0, -PI / 2), np.array([displacement, -1.75 + 0.1, 0.7709]))) blue = 'blue%s' % i pb(env, bb(env, .05, .1, .2, name=blue, color=(0, 0, value, 1)), (1.75 - 0.2, displacement, 0), 'table1') goal_poses[blue] = 'initial' cyan = 'cyan%s' % i pb(env, bb(env, .05, .1, .2, name=cyan, color=(0, value, value, 1)), (displacement, -1.75 + 0.2, PI / 2), 'table2') goal_poses[cyan] = 'initial' #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') object_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names ] return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, goal_poses=goal_poses)
def move_regrasp(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'regrasp.xml') camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0)) 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') 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': Pose( pose_from_quat_point(quat_from_axis_angle(0, 0, PI / 2), np.array([1.55, -0.25, 0.7709]))), 'blue': 'initial' } return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, 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 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 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 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 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 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 srivastava_table(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae') camera_trans = camera_look_at((1., -3., 4), look_point=(2, 1, 0)) body_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] dx = .5 for body_name in body_names: body = env.GetKinBody(body_name) point = get_point(body) point[0] += dx set_point(body, point) table_names = [ body_name for body_name in body_names if 'table' in body_name ] object_names = [ body_name for body_name in body_names if body_name not in table_names ] for object_name in object_names: body = env.GetKinBody(object_name) point = get_point(body) point[2] += BODY_PLACEMENT_Z_OFFSET set_point(body, point) goal_holding = 'object1' goal_config = 'initial' # None return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding)
def trivial_namo(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'navigation.xml') goal_config = Config(np.array((1.5, 0, PI))) return ManipulationProblem(function_name(inspect.stack()), goal_config=goal_config)
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)