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)