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)
