def sample_goal_region_paps(oracle): # TODO - base this off of the area of the region
  if DEBUG: print function_name(inspect.stack())
  for object_name, region_name in oracle.problem.goal_regions.items():
    if object_name in oracle.reachable_objects:
      random_sample_regions(oracle, object_name, [region_name], num_poses=NUM_GOAL_REGION_POSES,
                            num_grasps=NUM_GRASPS, max_failures=NUM_GOAL_REGION_FAILURES)
    if DEBUG: print object_name, sum(1 for pap in oracle.get_paps(object_name) if oracle.region_contains(region_name, object_name, pap.pose)), 'placements'
def sample_start_paps(oracle):
  if DEBUG: print function_name(inspect.stack())
  important_objects = set(oracle.problem.goal_regions.keys() + oracle.problem.goal_poses.keys())
  oracle.reachable_objects = set()
  for object_name in oracle.get_objects():
    max_failures = N_START_FAILURES if object_name not in important_objects else N_IMPORTANT_START_FAILURES
    if sample_pose(oracle, object_name, oracle.initial_poses[object_name], num_grasps=NUM_GRASPS, max_failures=max_failures):
      oracle.reachable_objects.add(object_name)
    else:
      #set_color(oracle.get_body(object_name), np.array([0, 0, 0, 1])) # TODO - remove
      if DEBUG: print object_name + ' is not reachable'
def sample_general_paps(oracle, max_failures=10):
  if DEBUG: print function_name(inspect.stack())
  counter_objects = CountDict([oracle.get_geom_hash(obj) for obj in oracle.get_counter_objects() if obj in oracle.reachable_objects])
  floor_objects = CountDict([oracle.get_geom_hash(obj) for obj in oracle.get_floor_objects() if obj in oracle.reachable_objects])

  for geom_hash, num in counter_objects.iteritems():
    cached_sample_regions(oracle, oracle.get_body_name(geom_hash), oracle.get_counters(),
                        num_poses=NUM_COUNTER_POSES(num), num_grasps=NUM_GRASPS, max_failures=NUM_COUNTER_FAILURES,
                        avoid_initial=AVOID_INITIAL, avoid_goal=AVOID_GOAL)
  for geom_hash, num in floor_objects.iteritems():
    cached_sample_regions(oracle, oracle.get_body_name(geom_hash), oracle.get_floors(),
                        num_poses=NUM_FLOOR_POSES(num), num_grasps=NUM_GRASPS, max_failures=NUM_FLOOR_FAILURES,
                        avoid_initial=AVOID_INITIAL, avoid_goal=AVOID_GOAL)
Example #4
0
def push_wall(env):
  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)
Example #5
0
def distractions(env):
  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 product((-.15, 0, .15), repeat=2):
    if not (dx == 0 and dy == 0):
      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 in irange(-.65, .75, .15):
    for y in irange(-1.95, -1.5, .15):
      pb(env, box_body(env, .05, .05, .15, 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)
Example #6
0
def dinner(env):
  env.Load(ENVIRONMENTS_DIR + 'kitchen.xml')

  obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
  table_names = ['table1']
  sink_names = ['washer']
  stove_names = ['heater']

  regions = [
    create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))),
    create_region(env, 'green_goal', ((-1, 1), (.5, 1)), 'table1', color=np.array((0, 1, 0, .5)))
  ]

  pb = place_body
  pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE), (1.65, 0.115, PI/3), 'table1')
  pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1')
  pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1')
  pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.415, PI/8), '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': 'blue_goal',
    'green': 'green_goal'
  }
  goal_cleaned = ['blue']
  goal_cooked = ['green']

  return ManipulationProblem(function_name(inspect.stack()),
      object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions,
      goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
Example #7
0
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)
Example #8
0
def move_regrasp(env):
  env.Load(ENVIRONMENTS_DIR + 'regrasp.xml') # the environment for HBf
  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)
Example #9
0
def stacking(env):
  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 sample_ffrob(oracle, start, goal, max_attempts=INF):
  t0, attempts = time(), Counter()
  if DEBUG: print SEPARATOR
  while int(attempts) < max_attempts:
    next(attempts)
    if DEBUG: print '%s attempt %d'%(function_name(inspect.stack()), int(attempts))
    sample_start_paps(oracle)
    sample_general_paps(oracle)
    sample_special_paps(oracle)
    sample_goal_pose_paps(oracle)
    sample_goal_region_paps(oracle)

    if len(oracle.floor_objects) != 0: construct_namo_crg(oracle)
    else: construct_crg(oracle)

    create_reachable_operators(oracle)
    create_sas_operators(oracle)
    if DEBUG: print 'compute_costs'
    with oracle.env: # NOTE - for drawing
      if not CHECK_FEASIBILITY or goal in compute_costs(start, goal, oracle)[0]: break
    if DEBUG: print 'relaxed plan is infeasible\n' # TODO - diagnose which samples are needed

  oracle.ffrob_sample_time, oracle.ffrob_sample_attempts = time() - t0, int(attempts)
  #print '%d Base Roadmap Vertices'%len(oracle.base_roadmap)
  if DEBUG: print '%d CRG Vertices | %d CRG Edges\n%f seconds | %d attempts\n'%(len(oracle.crg), len(oracle.crg.edges),
      oracle.ffrob_sample_time, oracle.ffrob_sample_attempts)
  if DRAW_CRG: draw_crg(oracle)
Example #11
0
def simple_namo(env):
  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))
  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)
Example #12
0
def simple_push(env):
  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)
Example #13
0
def simple(env):
  env.Load(ENVIRONMENTS_DIR + 'room1.xml')

  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, .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',
    'green_box': 'green_goal'
  }

  return ManipulationProblem(function_name(inspect.stack()),
      object_names=object_names, table_names=table_names, regions=regions,
      goal_regions=goal_regions)
def sample_goal_pose_paps(oracle):
  if DEBUG: print function_name(inspect.stack())
  for object_name, object_pose in oracle.problem.goal_poses.items():
    if object_name in oracle.reachable_objects:
      sample_pose(oracle, object_name, object_pose, num_grasps=NUM_GRASPS, max_failures=NUM_GOAL_POSE_FAILURES)
def sample_special_paps(oracle):
  if DEBUG: print function_name(inspect.stack())
  for region_name in oracle.sinks + oracle.stoves:
    for object_name in oracle.get_counter_objects():
      if object_name in oracle.reachable_objects:
        random_sample_regions(oracle, object_name, [region_name], num_poses=NUM_SPECIAL_POSES, num_grasps=NUM_GRASPS, max_failures=NUM_SPECIAL_FAILURES)