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 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)
