Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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)
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
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)
Ejemplo n.º 9
0
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)
Ejemplo n.º 10
0
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)
Ejemplo n.º 11
0
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)
Ejemplo n.º 12
0
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)