def draw_primal(self, tube):
self.poly_handles = []
colors = [(0.5, 0.5, 0., 0.3), (0., 0.5, 0.5, 0.3)]
if tube.start_stance.label.startswith('SS'):
colors.reverse()
for (i, vertices) in enumerate(tube.primal_vrep):
color = colors[i]
if len(vertices) == 1:
self.poly_handles.append(
draw_point(vertices[0], color=color, pointsize=0.01))
else:
self.poly_handles.extend(
draw_polyhedron(vertices, '*.-#', color=color))
def draw_vrp_com_cone(contact_set, vrp, gain):
"""
Draw the cone of COM positions sustainable under a fixed VRP.
Parameters
----------
contact_set : pymanoid.ContactSet
Set of contacts.
vrp : array, shape=(3,)
Virtual Repellent Point coordinates in the world frame.
Returns
-------
handles : list of openravepy.GraphHandle
OpenRAVE graphical handles. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
polytope = contact_set.compute_vrp_com_cone(vrp, gain)
polytope.compute_vrep()
return draw_polyhedron(
polytope.vertices +
[vrp + 1 * r / dot(r, [0, 0, 1]) for r in polytope.rays])