def visualize_base_confs(world, name, base_confs, **kwargs):
handles = []
if not base_confs:
return handles
z = get_floor_z(world)
# for x, y in base_points:
# handles.extend(draw_point(Point(x, y, z), color=color))
vertices = grow_polygon(base_confs, radius=GROW_INVERSE_BASE)
points = [Point(x, y, z) for x, y, in vertices]
handles.extend(add_segments(points, closed=True, **kwargs))
cx, cy = convex_centroid(vertices)
centroid = [cx, cy, z]
# draw_point(centroid, color=color)
handles.append(add_text(name, position=centroid, **kwargs))
return handles
def draw_names(world, **kwargs):
# TODO: adjust the colors?
handles = []
if not DEBUG:
return handles
with ClientSaver(world.perception.client):
for name, body in world.perception.sim_bodies.items():
#add_body_name(body, **kwargs)
with PoseSaver(body):
set_pose(body, unit_pose())
lower, upper = get_aabb(
body
) # TODO: multi-link bodies doesn't seem to update when moved
handles.append(
add_text(name, position=upper, parent=body,
**kwargs)) # parent_link=0,
#handles.append(draw_pose(get_pose(body)))
#handles.extend(draw_base_limits(get_base_limits(world.pr2), color=(1, 0, 0)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_VIS_DISTANCE, color=(0, 0, 1)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_REG_DISTANCE, color=(0, 0, 1)))
#from plan_tools.debug import test_voxels
#test_voxels(world)
return handles
def label_nodes(node_points, **kwargs):
return [
add_text(node, position=point, **kwargs)
for node, point in enumerate(node_points)
]
def label_element(element_bodies, element):
element_body = element_bodies[element]
return [
add_text(element[0], position=(0, 0, -0.02), parent=element_body),
add_text(element[1], position=(0, 0, +0.02), parent=element_body),
]