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