Ejemplo n.º 1
0
def cached_region_placements(oracle, body_name, region_names, order=None, random=False):
  if random:
    poses = randomize(reduce(operator.add, (oracle.random_placement_cache(body_name, region_name) for region_name in region_names)))
  else:
    poses = randomize(reduce(operator.add, (oracle.grid_placement_cache(body_name, region_name) for region_name in region_names)))
  # TODO - merge them
  if order is not None: poses = order(poses)
  body_saver = oracle.body_saver(body_name)
  for pose in poses:
    oracle.set_pose(body_name, pose)
    if not hasattr(pose, 'obstacle_collision'):
      pose.obstacle_collision = obstacle_collision(oracle, body_name)
    if not (pose.obstacle_collision or object_collision(oracle, body_name)): # TODO - cache object_collision as well
      body_saver.Restore()
      yield pose
Ejemplo n.º 2
0
def center_stackings(oracle, body_name, stackings, max_failures=INF): # NOTE - aligns the center of the bounding boxes
  body_saver = oracle.body_saver(body_name)
  failures = 0
  while failures <= max_failures:
    failures += 1
    quat = quat_from_z_rot(uniform(0, 2*PI))
    aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
    base_name, base_pose = choice(stackings)
    base_aabb = oracle.get_aabb(base_name, trans_from_pose(base_pose.value))
    point = base_aabb.pos() + (base_aabb.extents()[2]+aabb.extents()[2]+REGION_PLACEMENT_Z_OFFSET)*unit_z() - aabb.pos()
    pose = Pose(pose_from_quat_point(quat, point))
    oracle.set_pose(body_name, pose)
    if not (obstacle_collision(oracle, body_name) or object_collision(oracle, body_name)):
      body_saver.Restore()
      yield pose
      failures = 0
Ejemplo n.º 3
0
def random_edge_placements(oracle, body_name, region_name, z=None, use_quat=None, bias_point=None, max_failures=INF):
  body_saver = oracle.body_saver(body_name)
  edge_dist = ClosestEdgeDistribution(oracle.get_region(region_name), bias_point) if bias_point is not None \
      else UniformEdgeDistribution(oracle.get_region(region_name))
  failures = 0
  while failures <= max_failures:
    failures += 1
    quat = quat_from_z_rot(uniform(0, 2*PI)) if use_quat is None else use_quat
    aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
    point = edge_dist.sample() + (aabb.extents()[2]+REGION_PLACEMENT_Z_OFFSET)*unit_z() - aabb.pos()
    if z is not None: point[2] = z
    pose = pose_from_quat_point(quat, point)
    set_pose(oracle.get_body(body_name), pose)
    if not (obstacle_collision(oracle, body_name) or object_collision(oracle, body_name)):
      body_saver.Restore()
      yield Pose(pose)
      failures = 0
Ejemplo n.º 4
0
def random_region_placements(oracle, body_name, region_names, region_weights=False, max_failures=INF):
  if region_weights is True: region_weights = weight_regions(oracle, body_name, region_names)
  sample_region = lambda: (choice(region_names) if region_weights is False else sample_categorical(region_weights))

  body_saver = oracle.body_saver(body_name)
  failures = 0
  while failures <= max_failures:
    failures += 1
    quat = quat_from_z_rot(uniform(0, 2*PI))
    aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
    region = oracle.get_region(sample_region())
    cspace = region.cspace(aabb)
    if cspace is None: continue
    point = np.array([uniform(*range) for range in cspace] +
                     [region.z + aabb.extents()[2] + REGION_PLACEMENT_Z_OFFSET]) - aabb.pos() # NOTE - okay because the aabb is relative to unit_trans()
    pose = pose_from_quat_point(quat, point)
    set_pose(oracle.get_body(body_name), pose)
    if not (obstacle_collision(oracle, body_name) or object_collision(oracle, body_name)):
      body_saver.Restore()
      yield Pose(pose)
      failures = 0
Ejemplo n.º 5
0
def grid_search_region_placement(oracle, object_name, region_name, attempts=INF, dtheta=PI/16):
  obj = oracle.bodies[object_name]
  region = oracle.regions[region_name]
  with oracle.body_saver(object_name):
    orientations = [] #deque()
    for theta in irange(2*pi, step=dtheta): # TODO - binary search over successively smaller dthetas?
      set_quat(obj, quat_from_axis_angle(0, 0, theta))
      aabb = aabb_from_body(obj)
      ranges = []
      for k in range(2):
        low, high = region.box[k][0] + aabb.extents()[k], region.box[k][1] - aabb.extents()[k]
        if low > high: break
        ranges.append((low, high))
      if len(ranges) == 2:
        orientations.append((theta, tuple(ranges)))

    while len(orientations) != 0:
      theta, ranges = choice(orientations)
      set_quat(obj, quat_from_axis_angle(0, 0, uniform(0, 2*PI)))
      point = [uniform(low, high) for low, high in ranges] + [region.z + aabb.extents()[2] + REGION_PLACEMENT_Z_OFFSET]
      set_point(obj, point + get_point(obj) - aabb.pos())
      if not (obstacle_collision(oracle, object_name) or object_collision(oracle, object_name)):
        return oracle.get_pose(object_name)
  return None