def srivastava_table(env): 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 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)