def compute_geom_reachable_polygon(robot, stance, xlim, ylim, draw=True): init_com = stance.com.p feasible_points = [] handles = [] points = generate_2d_grid(xlim, ylim, xres=4, yres=6) for point in points: stance.com.set_x(init_com[0] + point[0]) stance.com.set_y(init_com[1] + point[1]) robot.ik.solve(warm_start=True) # self.draw_polytope_slice() # draw_polygon() if norm(robot.com - stance.com.p) < 0.02: feasible_points.append(robot.com) if draw: handles.append(draw_point(robot.com, pointsize=5e-3)) # handle.append(draw_point(stance.com.p)) feasible = [array([p[0], p[1]]) for p in feasible_points] hull = ConvexHull(feasible) vertices = [feasible[i] for i in hull.vertices] if draw: z_avg = pylab.mean([p[2] for p in feasible_points]) vertices_3d = [array([v[0], v[1], z_avg]) for v in vertices] handles.extend([draw_point(v) for v in vertices_3d]) stance.com.set_pos(init_com) return vertices
def draw(self): com, comd = self.com_state.p, self.com_state.pd cp = com + comd / self.omega + gravity / self.omega2 zmp = self.zmp_state.p handles = [ draw_point(com, color='r', pointsize=0.025), draw_point(cp, color='b', pointsize=0.025), draw_point(zmp, color='r', pointsize=0.025), draw_line(com, cp, color='b', linewidth=1), draw_line(zmp, com, color='r', linewidth=4) ] return handles
def play(self, sim, slowdown=1., wrench_drawer=None, post_preview_duration=None, callback=None): handles = None com, comd = self.P[0], self.V[0] if wrench_drawer is not None: wrench_drawer.point_mass.set_pos(com) wrench_drawer.point_mass.set_vel(comd) wrench_drawer.point_mass.set_transparency(0.5) t, k, rem_time = 0., -1, -1., duration = 2 * self.duration if post_preview_duration is None else \ self.duration + post_preview_duration while t <= duration: if rem_time < 1e-10: if k < self.nb_steps - 1: k += 1 rem_time = self.get_dT(k) omega2 = self.get_omega2(k) cp = com + comd / sqrt(omega2) + sim.gravity / omega2 zmp = self.get_zmp(k) if k < self.nb_steps else cp comdd = omega2 * (com - zmp) + sim.gravity handles = [ draw_point(zmp, color='r'), draw_line(zmp, com, color='r', linewidth=4), draw_point(cp, color='b'), draw_line(cp, com, color='b', linewidth=2), draw_point(com, color='r', pointsize=0.05) ] if wrench_drawer is not None: try: wrench_drawer.recompute(self.get_contact(k), comdd, am=None) except ValueError: print "Warning: wrench validation failed at t = %.2f s" % t dt = min(rem_time, sim.dt) com, comd = integrate_fip(com, comd, zmp, dt, omega2) if wrench_drawer is not None: wrench_drawer.point_mass.set_pos(com) wrench_drawer.point_mass.set_vel(comd) sleep(slowdown * dt) if callback is not None: callback() rem_time -= dt t += dt if wrench_drawer is not None: wrench_drawer.handles = [] return handles
def draw(self, dcm, cop): """ Draw extra points to illustrate stabilizer behavior. Parameters ---------- dcm : (3,) array Divergent component of motion. cop : (3,) array Center of pressure. """ n = self.pendulum.contact.n dcm_ground = dcm - dot(n, dcm) * n self.handles = [ draw_point(dcm_ground, color='b'), draw_point(cop, color='g')]
def compute(self, draw_height=None): assert len(self.working_set) > 0 and len(self.working_set[0]) == 2 self.all_vertices = [] for i_cur, p_cur in enumerate(self.working_set): p_cur = array(p_cur) A_voronoi, b_voronoi = [], [] for i_other, p_other in enumerate(self.working_set): if i_other == i_cur: continue p_other = array(p_other) p_mid = 0.5 * (p_other + p_cur) a = p_other - p_cur A_voronoi.append(a) b_voronoi.append(dot(a, p_mid)) A_voronoi = vstack(A_voronoi) b_voronoi = hstack(b_voronoi) self.stance.com.set_x(p_cur[0]) self.stance.com.set_y(p_cur[1]) self.robot.ik.solve(warm_start=True) proj_vertices = compute_local_actuation_dependent_polygon( self.robot, self.stance, method="bretl") A_proj, b_proj = compute_polytope_halfspaces(proj_vertices) A = vstack([A_proj, A_voronoi]) b = hstack([b_proj, b_voronoi]) if draw_height is not None and (dot(A, p_cur) > b).any(): self.sample_handles.append( draw_point([p_cur[0], p_cur[1], draw_height], color='r', pointsize=5e-3)) continue elif draw_height is not None: self.sample_handles.append( draw_point([p_cur[0], p_cur[1], draw_height], color='g', pointsize=5e-3)) vertices = pypoman.compute_polytope_vertices(A, b) if draw_height is not None: self.polygons.append( draw_horizontal_polygon(vertices, draw_height, combined='b-#')) self.all_vertices.extend(vertices) return self.all_vertices
def draw(self, color='b', pointsize=0.005): handles = [] if self.is_empty: return handles com, comd = self.P[0], self.V[0] handles.append(draw_point(com, color, pointsize)) for k in xrange(self.nb_steps): com_prev = com dT = self.get_dT(k) omega2 = self.get_omega2(k) zmp = self.get_zmp(k) com, comd = integrate_fip(com, comd, zmp, dT, omega2) handles.append(draw_point(com, color, pointsize)) handles.append(draw_line(com_prev, com, color, linewidth=3)) com_prev = com com = com + comd / sqrt(omega2) # stationary value at CP handles.append(draw_point(com, color, pointsize)) handles.append(draw_line(com_prev, com, color, linewidth=3)) return handles
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_polytope(vertices, '*.-#', color=color))
def on_tick(self, sim): """ Entry point called at each simulation tick. Parameters ---------- sim : Simulation Instance of the current simulation. """ if preview_buffer.is_empty: return com_pre, comd_pre = com_target.p, com_target.pd com_free, comd_free = com_target.p, com_target.pd self.handles = [] self.handles.append( draw_point(com_target.p, color='m', pointsize=0.007)) for preview_index in range(preview_buffer.nb_steps): com_pre0 = com_pre j = 3 * preview_index comdd = preview_buffer._U[j:j + 3] dT = preview_buffer._dT[preview_index] com_pre = com_pre + comd_pre * dT + comdd * .5 * dT ** 2 comd_pre += comdd * dT color = \ 'b' if preview_index <= preview_buffer.switch_step \ else 'y' self.handles.append( draw_point(com_pre, color=color, pointsize=0.005)) self.handles.append( draw_line(com_pre0, com_pre, color=color, linewidth=3)) if self.draw_free_traj: com_free0 = com_free com_free = com_free + comd_free * dT self.handles.append( draw_point(com_free, color='g', pointsize=0.005)) self.handles.append( draw_line(com_free0, com_free, color='g', linewidth=3))
def draw(self): """ Draw the interpolated foot path. Returns ------- handle : openravepy.GraphHandle OpenRAVE graphical handle. Must be stored in some variable, otherwise the drawn object will vanish instantly. """ foot_path = self.retimed_traj.path if foot_path is None: return [] ds = self.discrtimestep handles = [draw_point(foot_path.Eval(0), color='m', pointsize=0.007)] for s in arange(ds, foot_path.duration + ds, ds): handles.append( draw_point(foot_path.Eval(s), color='b', pointsize=0.01)) handles.append( draw_line(foot_path.Eval(s - ds), foot_path.Eval(s), color='b', linewidth=3)) return handles
def on_tick(self, sim): from pymanoid.gui import draw_point self.handles = [ draw_point(stab.dcm, color=stab.pendulum.color, pointsize=0.01) for stab in self.stabilizers if stab.pendulum.is_visible]