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