def publish(self, pub): if not self.once: pub.text('info', 'never called yet') return N = self.last_y.size with pub.plot('last_w') as pylab: pylab.plot(self.last_w, 's') x_axis_set(pylab, -1, N) y_axis_set(pylab, -0.1, 1.1) turn_off_bottom_and_top(pylab) gy0 = self.last_gy[0, :] def plot_good_bad(pylab, x, valid): invalid = np.logical_not(valid) pylab.plot(np.nonzero(valid)[0], x[valid], 'ks') pylab.plot(np.nonzero(invalid)[0], x[invalid], 'rs') with pub.plot('last_y') as pylab: plot_good_bad(pylab, self.last_y, self.last_y_valid) y_axis_set(pylab, -0.1, +1.1) x_axis_set(pylab, -1, N) turn_off_bottom_and_top(pylab) with pub.plot('last_y_dot') as pylab: pylab.plot(self.last_y_dot) plot_good_bad(pylab, self.last_y_dot, self.last_y_dot_valid) upper = np.ones(N) * self.max_y_dot lower = -upper pylab.plot(upper, 'r--') pylab.plot(lower, 'r--') x_axis_set(pylab, -1, N) y_axis_balanced(pylab) turn_off_bottom_and_top(pylab) with pub.plot('last_gy') as pylab: plot_good_bad(pylab, gy0, self.last_gy_valid) upper = np.ones(N) * self.max_gy lower = -upper pylab.plot(upper, 'r--') pylab.plot(lower, 'r--') x_axis_set(pylab, -1, N) y_axis_balanced(pylab) turn_off_bottom_and_top(pylab) self.w_stats.publish(pub.section('w_stats'))
def repsec_servo1_generic_vel_field(r, fname, centroid, nmap, vels, area_graphs, normalize=True): f = r.figure(cols=2) omegas = map(angular_from_se2, vels) has_theta = np.any(omegas != 0) figsize = (6, 6) arrow_length = nmap.get_average_interpoint_R2_distance() / 2 print('arrow length: %s' % arrow_length) caption = 'First two components of "%s".' % fname with f.plot('xy_arrows', caption=caption, figsize=figsize) as pylab: nmap.plot_points(pylab) nmap.plot_vels(pylab, vels, normalize, length=arrow_length) plot_style_servo_field_xy(pylab, area_graphs=area_graphs) @contract(pose='SE2', returns='>=0') def distance_to_centroid(pose): t = translation_from_SE2(pose) d = np.linalg.norm(t - centroid) return d poses = nmap.get_poses() derivs = vector_field_derivs(SE2, poses, vels, distance_to_centroid) def deriv2color(s): if s == 0: return 'k' if s > 0: return 'r' if s < 0: return 'g' colors = map(deriv2color, derivs) with f.plot('xy_arrows_colors', caption=caption, figsize=figsize) as pylab: nmap.plot_vels(pylab, vels, normalize, colors=colors) plot_style_servo_field_xy(pylab, area_graphs=area_graphs) if has_theta: caption = 'Third component of "%s".' % fname with f.plot('xy_u_th_sign', caption=caption) as pylab: nmap.plot_scalar_field_sign(pylab, omegas) plot_style_servo_field_xy(pylab, area_graphs=area_graphs) f = r.figure() if has_theta: poses = nmap.get_poses() with f.plot('yt', caption=caption) as pylab: plot_poses_vels_yt(pylab, poses, vels, normalize=True) pylab.xlabel('y (m)') pylab.ylabel('theta (deg)') y_axis_balanced(pylab) x_axis_balanced(pylab) with f.plot('xt', caption=caption) as pylab: plot_poses_vels_xt(pylab, poses, vels, normalize=True) pylab.xlabel('x (m)') pylab.ylabel('theta (deg)') y_axis_balanced(pylab) x_axis_balanced(pylab) with f.plot('tw', caption=caption) as pylab: plot_poses_vels_theta_omega(pylab, poses, vels) pylab.xlabel('theta (deg)') pylab.ylabel('omega') y_axis_balanced(pylab) x_axis_balanced(pylab)