def on_tick(self, sim):
if fsm.cur_stance.is_single_support:
ss_stance = fsm.cur_stance
ds_stance = fsm.next_stance
else: # fsm.cur_stance.is_double_support:
ss_stance = fsm.next_stance
ds_stance = fsm.cur_stance
sep_height = preview_buffer.com.z - RobotModel.leg_length
self.ss_handles = draw_polygon(
[(x[0], x[1], sep_height) for x in ss_stance.sep],
normal=[0, 0, 1], color='c')
self.ds_handles = draw_polygon(
[(x[0], x[1], sep_height) for x in ds_stance.sep],
normal=[0, 0, 1], color='y')
def draw_full_support_area(contacts, n, d, alpha=0.5, plot_type=7,
draw_lines=False, draw_gen_polygons=True,
color=(0., 0.5, 0.), linewidth=1.):
"""
Draw the full ZMP support area in a virtual plane.
The plane is defined by the equation
dot(p, n) = d
contacts -- any iterable set of pymanoid.Contact objects
n -- plane normal, a numpy.array with shape (3,)
d -- plane coordinate, a real value
alpha -- transparency of output surfaces
plot_type -- bitmask with 1 for vertices, 2 for edges and 4 for surface
"""
handles = []
rgba = list(color) + [alpha]
# total_time = 0.
# t0 = time.time()
if draw_lines:
pos_poly, neg_poly = compute_full_support_area(contacts, n, d, handles)
else:
pos_poly, neg_poly = compute_full_support_area(contacts, n, d)
# total_time += time.time() - t0
if pos_poly and not neg_poly:
handles.append(pymanoid.draw_polygon(
pos_poly, n, plot_type=plot_type, color=rgba, linewidth=linewidth))
return handles # , total_time
elif neg_poly and not pos_poly:
handles.append(pymanoid.draw_polygon(
neg_poly, n, plot_type=plot_type, color=rgba, linewidth=linewidth))
return handles, total_time
# t0 = time.time()
pos_v, pos_r = compute_polygon_shadow(n, neg_poly, pos_poly)
neg_v, neg_r = compute_polygon_shadow(n, pos_poly, neg_poly)
# total_time += time.time() - t0
if draw_gen_polygons:
handles.append(pymanoid.draw_polygon(
pos_poly, n, plot_type=plot_type, color=(0.5, 0., 0.5, alpha),
linewidth=linewidth))
handles.append(pymanoid.draw_polygon(
neg_poly, n, plot_type=plot_type, color=(0.5, 0., 0.5, alpha),
linewidth=linewidth))
handles.append(pymanoid.draw_2d_cone(
pos_v, pos_r, n, plot_type=plot_type, color=rgba))
handles.append(pymanoid.draw_2d_cone(
neg_v, neg_r, n, plot_type=plot_type, color=rgba))
return handles # , total_time
def draw_static_polygon(contact_set, p=[0, 0, 0], color=None, combined='g-#'):
color = color if color else (0., 0.5, 0., 0.5)
vertices = compute_static_polygon_cdd_hull(contact_set, p)
return pymanoid.draw_polygon([(x[0] + p[0], x[1] + p[1], p[2])
for x in vertices],
normal=[0, 0, 1],
color=color,
combined=combined)
def draw_com_polygon(vertices, color):
return pymanoid.draw_polygon([(x[0], x[1], com_target.z)
for x in vertices],
normal=[0, 0, 1],
combined='m.-#',
color=color,
pointsize=0.02,
linewidth=3.)
def on_tick(self, sim):
if fsm.next_stance is None:
return
if fsm.cur_stance.is_single_support:
ss_stance = fsm.cur_stance
ds_stance = fsm.next_stance
else: # fsm.cur_stance.is_double_support:
ss_stance = fsm.next_stance
ds_stance = fsm.cur_stance
sep_height = 3. # [m]
self.ss_handles = draw_polygon([(x[0], x[1], sep_height)
for x in ss_stance.sep],
normal=[0, 0, 1],
color='c')
self.ds_handles = draw_polygon([(x[0], x[1], sep_height)
for x in ds_stance.sep],
normal=[0, 0, 1],
color='y')
def draw_static_stability_area(self, plot_type=6, alpha=0.5):
vertices = self.compute_static_stability_area()
self.com_polygon = vertices
if not vertices:
return []
handle = pymanoid.draw_polygon(
self.env, vertices, array([0., 0., 1.]),
plot_type=plot_type, color=(0., 0.5, 0., alpha))
return [handle]
def recompute_polygon():
global polygon_handle
vertices = contacts.compute_static_equilibrium_polygon()
polygon_handle = pymanoid.draw_polygon(
[(x[0], x[1], z_polygon) for x in vertices],
normal=[0, 0, 1], color=(0.5, 0., 0.5, 0.5))
