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))