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