def plot_workspace(pts, ha, z):
if pts.shape[1] == 0:
return
mpu.figure()
good_location = pts.mean(1)
mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, label="ha:%.1f" % (math.degrees(ha)), axis="equal", linewidth=0)
mpu.plot_yx(good_location[1, :].A1, good_location[0, :].A1, axis="equal", linewidth=0, scatter_size=90, color="k")
mpu.savefig("z%.2f_ha%.1f.png" % (z, math.degrees(ha)))
def plot_workspace(pts, ha, z):
if pts.shape[1] == 0:
return
mpu.figure()
good_location = pts.mean(1)
mpu.plot_yx(pts[1, :].A1,
pts[0, :].A1,
label='ha:%.1f' % (math.degrees(ha)),
axis='equal',
linewidth=0)
mpu.plot_yx(good_location[1, :].A1,
good_location[0, :].A1,
axis='equal',
linewidth=0,
scatter_size=90,
color='k')
mpu.savefig('z%.2f_ha%.1f.png' % (z, math.degrees(ha)))
def plot_radial_tangential(mech_dict, savefig, fig_name=''):
radial_mech = mech_dict['force_rad_list']
tangential_mech = mech_dict['force_tan_list']
typ = mech_dict['mech_type']
mech_x = mech_dict['mechanism_x']
if typ == 'rotary':
mech_x_degrees = np.degrees(mech_x)
xlabel = 'angle (degrees)'
else:
mech_x_degrees = mech_x
xlabel = 'distance (meters)'
mpu.pl.clf()
mpu.plot_yx(radial_mech, mech_x_degrees, axis=None, label='radial force',
xlabel=xlabel, ylabel='N', color='r')
mpu.plot_yx(tangential_mech, mech_x_degrees, axis=None, label='tangential force',
xlabel=xlabel, ylabel='N', color='g')
mpu.legend()
if typ == 'rotary':
mpu.figure()
rad = mech_dict['radius']
torques_1 = rad * np.array(tangential_mech)
torques_3 = np.array(mech_dict['moment_tip_list']) + torques_1
mpu.plot_yx(torques_1, mech_x_degrees, axis=None,
label='torque from tangential',
xlabel=xlabel, ylabel='moment', color='r')
mpu.plot_yx(torques_3, mech_x_degrees, axis=None,
label='total torque',
xlabel=xlabel, ylabel='moment', color='y')
mpu.legend()
if savefig:
mpu.savefig(opt.dir+'/%s_force_components.png'%fig_name)
else:
mpu.show()
end_angle,
label='\huge{Estimated Kinematics}',
color='r',
alpha=0.7)
mpu.plot_radii(cx,
cy,
rad,
start_angle,
end_angle,
interval=end_angle - start_angle,
color='r',
alpha=0.7)
mpu.legend()
fig_name = 'epc'
fig_number = 1
mpu.savefig(fig_name + '%d' % fig_number)
fig_number += 1
# now zoom in to a small region to show the force
# decomposition.
zoom_location = 10
pts_2d_zoom = pts_2d[:, :zoom_location]
cep_2d_zoom = cep_2d[:, :zoom_location]
# image_name = 'anim'
# for i in range(zoom_location):
# mpu.figure()
# mpu.plot_yx(pts_2d_zoom[1,:].A1, pts_2d_zoom[0,:].A1, color='w',
# axis = 'equal', alpha = 1.0)
# mpu.plot_yx(cep_2d_zoom[1,:].A1, cep_2d_zoom[0,:].A1, color='w',
# axis = 'equal', alpha = 1.0)
marker_edge_width = 1.5)
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Estimated Kinematics', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()
mpu.savefig('one.png')
# now zoom in to a small region to show the force
# decomposition.
zoom_location = 10
pts_2d_zoom = pts_2d[:,:zoom_location]
cep_2d_zoom = cep_2d[:,:zoom_location]
mpu.figure()
mpu.plot_yx(pts_2d_zoom[1,:].A1, pts_2d_zoom[0,:].A1, color='b',
label = 'FK', axis = 'equal', alpha = 1.0,
scatter_size=7, linewidth=0, marker='x',
marker_edge_width = 1.5)
mpu.plot_yx(cep_2d_zoom[1,:].A1, cep_2d_zoom[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
cd = ut.load_pickle(pkl_name)
cd['hook_checker_number'] = md['checkerboard_number']
cd['radius'] = md['radius']
rad, tan, ang, typ = al.compute_mechanism_properties(cd)
rad, tan_b, ang, typ = al.compute_mechanism_properties(cd,
bias_ft=True)
#------------ plot spring scale and FT data -------------
spring_pkl = glob.glob(opt.dir+'/spring_scale*.pkl')[0]
spring_list = ut.load_pickle(spring_pkl)
mpu.plot_yx(spring_list, label='Spring Scale', color='b', axis=None)
mpu.plot_yx(rad, label='Measured Radial force (unbiased)', color='r',
xlabel='Reading number', ylabel='Force (N)', axis=None)
mpu.plot_yx(tan, label='Measured Tangential force (unbiased)', color='g',
xlabel='Reading number', ylabel='Force (N)', axis=None)
mpu.plot_yx(tan_b, label='Measured Tangential force (biased)', color='y',
xlabel='Reading number', ylabel='Force (N)',
axis=None, plot_title=opt.dir)
mpu.legend()
mpu.savefig(opt.dir.split('/')[0]+'.png')
mpu.show()
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = '\huge{Equilibrium Point Trajectory}', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='\huge{Estimated Kinematics}', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()
fig_name = 'epc'
fig_number = 1
mpu.savefig(fig_name+'%d'%fig_number)
fig_number += 1
# now zoom in to a small region to show the force
# decomposition.
zoom_location = 10
pts_2d_zoom = pts_2d[:,:zoom_location]
cep_2d_zoom = cep_2d[:,:zoom_location]
# image_name = 'anim'
# for i in range(zoom_location):
# mpu.figure()
# mpu.plot_yx(pts_2d_zoom[1,:].A1, pts_2d_zoom[0,:].A1, color='w',
# axis = 'equal', alpha = 1.0)
# mpu.plot_yx(cep_2d_zoom[1,:].A1, cep_2d_zoom[0,:].A1, color='w',
# axis = 'equal', alpha = 1.0)
poses_pkl = glob.glob(opt.dir + '/poses_dict*.pkl')[0]
ft_dict = ut.load_pickle(ft_pkl)
poses_dict = ut.load_pickle(poses_pkl)
mechanism_dict = poses_dict['mechanism']
hand_dict = poses_dict['hand']
ft_time_list = ft_dict['time_list']
mechanism_time_list = mechanism_dict['time_list']
hand_time_list = hand_dict['time_list']
if opt.tc:
check_time_sync(ft_time_list, mechanism_time_list, hand_time_list)
if opt.savefig:
mpu.savefig(opt.dir+'/time_check.png')
else:
mpu.show()
if opt.sync:
print 'Begin synchronize'
d = synchronize(ft_dict, mechanism_dict, hand_dict)
print 'End synchronize'
#ut.save_pickle(d, opt.dir+'/combined_log'+ut.formatted_time()+'.pkl')
ut.save_pickle(d, opt.dir+'/combined_log.pkl')
print 'Saved pickle'
if opt.cd:
cd = ut.load_pickle(glob.glob(opt.dir + '/combined_log*.pkl')[0])
plot(cd, opt.savefig)
#------------ plot spring scale and FT data -------------
spring_pkl = glob.glob(opt.dir + '/spring_scale*.pkl')[0]
spring_list = ut.load_pickle(spring_pkl)
mpu.plot_yx(spring_list, label='Spring Scale', color='b', axis=None)
mpu.plot_yx(rad,
label='Measured Radial force (unbiased)',
color='r',
xlabel='Reading number',
ylabel='Force (N)',
axis=None)
mpu.plot_yx(tan,
label='Measured Tangential force (unbiased)',
color='g',
xlabel='Reading number',
ylabel='Force (N)',
axis=None)
mpu.plot_yx(tan_b,
label='Measured Tangential force (biased)',
color='y',
xlabel='Reading number',
ylabel='Force (N)',
axis=None,
plot_title=opt.dir)
mpu.legend()
mpu.savefig(opt.dir.split('/')[0] + '.png')
mpu.show()
marker_edge_width = 1.5)
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Estimated Kinematics', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()
mpu.savefig('one.png')
# now zoom in to a small region to show the force
# decomposition.
zoom_location = 10
pts_2d_zoom = pts_2d[:,:zoom_location]
cep_2d_zoom = cep_2d[:,:zoom_location]
mpu.figure()
mpu.plot_yx(pts_2d_zoom[1,:].A1, pts_2d_zoom[0,:].A1, color='b',
label = 'FK', axis = 'equal', alpha = 1.0,
scatter_size=7, linewidth=0, marker='x',
marker_edge_width = 1.5)
mpu.plot_yx(cep_2d_zoom[1,:].A1, cep_2d_zoom[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
