def sample_edge_point(polygon, radius): edges = zip(polygon, polygon[-1:] + polygon[:-1]) edge_weights = { i: max(get_length(v2 - v1) - 2 * radius, 0) for i, (v1, v2) in enumerate(edges) } # TODO: fail if no options while True: index = sample_categorical(edge_weights) v1, v2 = edges[index] t = np.random.uniform(radius, get_length(v2 - v1) - 2 * radius) yield t * get_unit_vector(v2 - v1) + v1
def get_position_waypoints(start_point, direction, quat, step_size=0.01): distance = get_length(direction) unit_direction = get_unit_vector(direction) for t in np.arange(0, distance, step_size): point = start_point + t*unit_direction yield (point, quat) yield (start_point + direction, quat)
def get_closest_edge_point(polygon, point): # TODO: always pick perpendicular to the edge edges = zip(polygon, polygon[-1:] + polygon[:-1]) best = None for v1, v2 in edges: proj = (v2 - v1)[:2].dot((point - v1)[:2]) if proj <= 0: closest = v1 elif get_length((v2 - v1)[:2]) <= proj: closest = v2 else: closest = proj * get_unit_vector((v2 - v1)) if (best is None) or (get_length((point - closest)[:2]) < get_length( (point - best)[:2])): best = closest return best
def sample_edge_pose(polygon, world_from_surface, mesh): radius = max(get_length(v[:2]) for v in mesh.vertices) origin_from_base = Pose(Point(z=p.min(mesh.vertices[:, 2]))) for point in sample_edge_point(polygon, radius): theta = np.random.uniform(0, 2 * np.pi) surface_from_origin = Pose(point, Euler(yaw=theta)) yield multiply(world_from_surface, surface_from_origin, origin_from_base)