def plot_rotary_heights(csv_data): keys = ['radius', 'bottom', 'top', 'type', 'name', 'repetition'] llists = expand(extract_keys(csv_data, keys), keys) types = llists[3] names = np.array(llists[4]) print 'Handle bottom edge types', set(types) bottom_pts = np.array([float(f) for f in llists[1]])/100. top_pts = [] for i,f in enumerate(llists[2]): if f == '': f = llists[1][i] top_pts.append(float(f)/100.) top_pts = np.array(top_pts) print 'total number of doors:', len(top_pts) mid_pts = (bottom_pts + top_pts)/2. types_arr = np.array(types) cabinet_mid_pts = mid_pts[np.where(types_arr == 'C')[0]] other_mid_pts = mid_pts[np.where(types_arr != 'C')[0]] other_names = names[np.where(types_arr != 'C')[0]] print 'other names', other_names # Hai, Advait apologizes for changing code without making a copy. # He didn't realise. He'll make a copy from an earlier revision in # subversion soon. ymax = 85 handle_height_histogram_advait(mid_pts, plot_title='Handle Heights', ymax=ymax, label = 'All') mpu.legend(display_mode = 'less_space', handlelength=1.) handle_height_histogram_advait(mid_pts, plot_title='Handle Heights', ymax=ymax, color = 'g', label = 'Cabinets') handle_height_histogram_advait(other_mid_pts, plot_title='Handle Heights', ymax=ymax, color = 'y', new_figure = False, label = 'Other') mpu.legend(display_mode = 'less_space', handlelength=1.)
def check_time_sync(ft_time_list, mechanism_time_list, hand_time_list): mpu.plot_yx(np.zeros(len(ft_time_list))+1, ft_time_list, color = mpu.random_color(), label='ft\_time\_list', axis=None, linewidth=0.5, scatter_size=10) mpu.plot_yx(np.zeros(len(mechanism_time_list))+2, mechanism_time_list, color = mpu.random_color(), label='mechanism\_time\_list', axis=None, linewidth=0.5, scatter_size=10) mpu.plot_yx(np.zeros(len(hand_time_list))+3, hand_time_list, color = mpu.random_color(), label='hand\_time\_list', axis=None, linewidth=0.5, scatter_size=10) mpu.legend()
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_', linewidth=2, scatter_size=10, plot_velocity=False): import matplotlib_util.util as mpu import arm_trajectories as at #if traj.__class__ == at.JointTrajectory: if isinstance(traj,at.JointTrajectory): traj = joint_to_cartesian(traj) pts = np.matrix(traj.p_list).T label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)'] x = pts[xaxis,:].A1.tolist() y = pts[yaxis,:].A1.tolist() if plot_velocity: vels = np.matrix(traj.v_list).T xvel = vels[xaxis,:].A1.tolist() yvel = vels[yaxis,:].A1.tolist() if zaxis == None: mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label, axis = 'equal', xlabel = label_list[xaxis], ylabel = label_list[yaxis],) if plot_velocity: mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]), width = 0.001, scale = 1.) mpu.legend() else: from numpy import array from enthought.mayavi.api import Engine engine = Engine() engine.start() if len(engine.scenes) == 0: engine.new_scene() z = pts[zaxis,:].A1.tolist() time_list = [t-traj.time_list[0] for t in traj.time_list] mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4) mlab.axes() mlab.xlabel(label_list[xaxis]) mlab.ylabel(label_list[yaxis]) mlab.zlabel(label_list[zaxis]) mlab.colorbar(title='Time') # ------------------------------------------- axes = engine.scenes[0].children[0].children[0].children[1] axes.axes.position = array([ 0., 0.]) axes.axes.label_format = '%-#6.2g' axes.title_text_property.font_size=4
def angle_between_hooktip_mechanism_radial_vectors(mech_mat, hand_mat): kin_dict = ke.fit(hand_mat, tup) print 'kin_dict from hook tip:', kin_dict print 'measured radius:', cd['radius'] center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'], kin_dict['cz'])).T kin_dict = ke.fit(mech_mat, tup) print 'kin_dict from mechanism:', kin_dict center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'], kin_dict['cz'])).T # working with the projected coordinates. directions_hand = hand_mat - center_hand directions_hand = directions_hand / ut.norm(directions_hand) directions_mech = mech_mat - center_mech directions_mech = directions_mech / ut.norm(directions_mech) #import pdb; pdb.set_trace() ang = np.degrees(np.arccos(np.sum(np.multiply(directions_mech, directions_hand), 0))).A1 mpu.plot_yx(ang, label = 'angle between hooktip-radial and mechanism radial') mpu.legend() mpu.show()
def plot_eq_pt_motion_tl(): vec_list = [] for i in range(len(ee_tl.p_list)): # for i in range(5): curr_pos_tl = np.matrix(ee_tl.p_list[i]).T eq_pt_tl = np.matrix(eq_tl.p_list[i]).T pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T pts_2d_ts = pts_ts[0:2,:] # rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts, # method='fmin_bfgs',verbose=False) rad_opt = 1.0 cx_ts,cy_ts = 0.5,-1.3 c_ts = np.matrix([cx_ts,cy_ts,0.]).T x,y,a = st.x_list[i],st.y_list[i],st.a_list[i] c_tl = smc.tlTts(c_ts,x,y,a) cx_tl,cy_tl = c_tl[0,0],c_tl[1,0] t0 = time.time() vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts, start_pos_ts,eq_pt_tl,bndry,wrkspc_pts) t1 = time.time() # print 'time to segway_motion_repulse:',t1-t0 nrm = np.linalg.norm(vt) # if nrm > 0.005: vt = vt/nrm vec_list.append(vt.A1.tolist()) v = np.matrix(vec_list).T eq_pts = np.matrix(eq_tl.p_list).T ee_pts = np.matrix(ee_tl.p_list).T mpu.plot_yx(eq_pts[1,:].A1,eq_pts[0,:].A1,linewidth=1,color='g',label='eq') mpu.plot_yx(ee_pts[1,:].A1,ee_pts[0,:].A1,linewidth=1,color='b',label='FK') mpu.plot_yx(bndry[1,:].A1,bndry[0,:].A1,linewidth=0,color='y') mpu.plot_quiver_yxv(eq_pts[1,:].A1,eq_pts[0,:].A1,v,scale=30) mpu.legend() mpu.show()
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()
def plot_radii(csv_data, color='#3366FF'): keys = ['radius', 'type', 'name', 'repetition'] llists = expand(extract_keys(csv_data, keys), keys) rad_list = np.array([float(r) for r in llists[0]]) / 100.0 types = llists[1] names = np.array(llists[2]) all_types = set(types) print 'Radii types', all_types types_arr = np.array(types) # np.where(types_arr == 'C') cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]] others_rad_list = rad_list[np.where(types_arr != 'C')[0]] other_names = names[np.where(types_arr != 'C')[0]] print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>' print 'radii other names' for i, n in enumerate(other_names): print n, others_rad_list[i] print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>' rad_lists = [rad_list, cabinet_rad_list, others_rad_list] titles = ['Radii of Rotary Mechanisms', 'Radii of Cabinets', 'Radii of Other Mechanisms'] bin_width = 0.05 max_radius = np.max(rad_list) print 'MIN RADIUS', np.min(rad_list) print 'MAX RADIUS', max_radius mpu.set_figure_size(5.,5.) for idx, radii in enumerate(rad_lists): f = pb.figure() f.set_facecolor('w') bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width) hist, bin_edges = np.histogram(radii, bins) h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist, width=0.8*bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title=titles[idx], color=color, label='All') pb.xlim(.1, 1.) pb.ylim(0, 55) mpu.legend(display_mode = 'less_space', handlelength=1.) #-- different classes in different colors in the same histogram. f = pb.figure() f.set_facecolor('w') bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width) hist, bin_edges = np.histogram(rad_lists[0], bins) h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist, width=0.8*bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title=titles[1], color='g', label='Cabinets') hist, bin_edges = np.histogram(rad_lists[2], bins) h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist, width=0.8*bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title='Cabinets and Other Mechanisms', color='y', label='Other') pb.xlim(.1, 1.) pb.ylim(0, 55) mpu.legend(display_mode = 'less_space', handlelength=1.) color_list = ['g', 'b', 'r'] marker_list = ['s', '^', 'v'] label_list = ['All', 'Cabinets', 'Other'] scatter_size_list = [8, 5, 5] mpu.set_figure_size(5.,5.) mpu.figure() for idx, radii in enumerate(rad_lists): bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width) hist, bin_edges = np.histogram(radii, bins) bin_midpoints = np.arange(0., max_radius+bin_width, bin_width) mpu.plot_yx(hist, bin_midpoints, color = color_list[idx], alpha = 0.6, marker = marker_list[idx], scatter_size = scatter_size_list[idx], xlabel='Radius (meters)', ylabel='\# of mechanisms', label = label_list[idx]) mpu.legend(display_mode = 'less_space')
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()
def plot_radii(csv_data, color='#3366FF'): keys = ['radius', 'type', 'name', 'repetition'] llists = expand(extract_keys(csv_data, keys), keys) rad_list = np.array([float(r) for r in llists[0]]) / 100.0 types = llists[1] names = np.array(llists[2]) all_types = set(types) print 'Radii types', all_types types_arr = np.array(types) # np.where(types_arr == 'C') cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]] others_rad_list = rad_list[np.where(types_arr != 'C')[0]] other_names = names[np.where(types_arr != 'C')[0]] print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>' print 'radii other names' for i, n in enumerate(other_names): print n, others_rad_list[i] print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>' rad_lists = [rad_list, cabinet_rad_list, others_rad_list] titles = [ 'Radii of Rotary Mechanisms', 'Radii of Cabinets', 'Radii of Other Mechanisms' ] bin_width = 0.05 max_radius = np.max(rad_list) print 'MIN RADIUS', np.min(rad_list) print 'MAX RADIUS', max_radius mpu.set_figure_size(5., 5.) for idx, radii in enumerate(rad_lists): f = pb.figure() f.set_facecolor('w') bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width, bin_width) hist, bin_edges = np.histogram(radii, bins) h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2., hist, width=0.8 * bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title=titles[idx], color=color, label='All') pb.xlim(.1, 1.) pb.ylim(0, 55) mpu.legend(display_mode='less_space', handlelength=1.) #-- different classes in different colors in the same histogram. f = pb.figure() f.set_facecolor('w') bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width, bin_width) hist, bin_edges = np.histogram(rad_lists[0], bins) h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2., hist, width=0.8 * bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title=titles[1], color='g', label='Cabinets') hist, bin_edges = np.histogram(rad_lists[2], bins) h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2., hist, width=0.8 * bin_width, xlabel='Radius (meters)', ylabel='\# of mechanisms', plot_title='Cabinets and Other Mechanisms', color='y', label='Other') pb.xlim(.1, 1.) pb.ylim(0, 55) mpu.legend(display_mode='less_space', handlelength=1.) color_list = ['g', 'b', 'r'] marker_list = ['s', '^', 'v'] label_list = ['All', 'Cabinets', 'Other'] scatter_size_list = [8, 5, 5] mpu.set_figure_size(5., 5.) mpu.figure() for idx, radii in enumerate(rad_lists): bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width, bin_width) hist, bin_edges = np.histogram(radii, bins) bin_midpoints = np.arange(0., max_radius + bin_width, bin_width) mpu.plot_yx(hist, bin_midpoints, color=color_list[idx], alpha=0.6, marker=marker_list[idx], scatter_size=scatter_size_list[idx], xlabel='Radius (meters)', ylabel='\# of mechanisms', label=label_list[idx]) mpu.legend(display_mode='less_space')
def split_forces_hooktip_test(hand_mat): kin_dict = ke.fit(hand_mat, tup) center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'], kin_dict['cz'])).T print 'kin_dict:', kin_dict # radial vectors. radial_mat = hand_mat - center_hand radial_mat = radial_mat / ut.norm(radial_mat) # cannot use hook tip to compute mechanism angle because I # don't have a way of knowing when the hook starts opening the # mechanism. (Think hook makes contact with door, moves in # freespace and then makes contact with the handle.) #start_rad = radial_mat[:,0] #ct = (start_rad.T * radial_mat).A1 #st = ut.norm(np.matrix(np.cross(start_rad.A1, radial_mat.T.A)).T).A1 #mech_angle_l = np.arctan2(st, ct).tolist() _, nrm_hand = project_points_plane(hand_mat) print 'nrm_hand:', nrm_hand.A1 f_cam_l = [] m_cam_l = [] m_base_l = [] frad_l = [] ftan_l = [] hook_num = cd['hook_checker_number'] print 'hook_num:', hook_num for i, f in enumerate(force_mat.T): f = f.T m = moment_mat[:,i] f_cam, m_cam, m_base = ft_to_camera_3(f, m, hook_rot_l[i], hook_num, return_moment_cam = True) f_cam_l.append(f_cam) m_cam_l.append(abs((m_cam.T*nrm_hand)[0,0])) m_base_l.append(abs((m_base.T*nrm_hand)[0,0])) #m_base_l.append(np.linalg.norm(f)) tangential_vec = np.matrix(np.cross(radial_mat[:,i].A1, nrm_hand.A1)).T ftan = (f_cam.T * tangential_vec)[0,0] ftan_l.append(ftan) #frad = np.linalg.norm(f_cam - tangential_vec * ftan) frad = (f_cam.T*radial_mat[:,i])[0,0] frad_l.append(abs(frad)) fig1 = mpu.figure() mech_ang_deg = np.degrees(mech_angle_l) mpu.plot_yx(ftan_l, mech_ang_deg, label='Tangential Force (hook tip)', color='b') mpu.plot_yx(frad_l, mech_ang_deg, label='Radial Force (hook tip)', color='y') mech_pkl_name = glob.glob(opt.dir + '/open_mechanism_trajectories_*.pkl')[0] md = ut.load_pickle(mech_pkl_name) radial_mech = md['force_rad_list'] tangential_mech = md['force_tan_list'] mech_x = np.degrees(md['mechanism_x']) mpu.plot_yx(tangential_mech, mech_x, label='Tangential Force (mechanism checker)', color='g') mpu.plot_yx(radial_mech, mech_x, label='Radial Force (mechanism checker)', color='r') mpu.legend() fig2 = mpu.figure() mpu.plot_yx(m_cam_l, mech_ang_deg, label='\huge{$m_{axis}$}') mpu.plot_yx(m_base_l, mech_ang_deg, label='\huge{$m^s$}', color = 'r') mpu.legend() mpu.show()
# ma4 = compute_mech_angle_1(cd, min_evec) lab1 = 'orientation only' lab2 = 'checker origin position + circle fit' lab3 = 'checker origin position + PCA projection + circle fit' # lab4 = 'PCA projection + orientation' mpu.figure() mpu.plot_yx(np.degrees(ma3), color='r', label=lab3, linewidth = 1, scatter_size = 5) mpu.plot_yx(np.degrees(ma2), color='b', label=lab2, linewidth = 1, scatter_size = 5) mpu.plot_yx(np.degrees(ma1), color='y', label=lab1, linewidth = 1, scatter_size = 5) mpu.legend() vel3 = ma.compute_velocity(ma3, cd['time_list'], 1) vel2 = ma.compute_velocity(ma2, cd['time_list'], 1) vel1 = ma.compute_velocity(ma1, cd['time_list'], 1) mpu.figure() mpu.plot_yx(np.degrees(vel3), np.degrees(ma3), color='r', label=lab3, linewidth = 1, scatter_size = 5) mpu.plot_yx(np.degrees(vel2), np.degrees(ma2), color='b', label=lab2, linewidth = 1, scatter_size = 5) mpu.plot_yx(np.degrees(vel1), np.degrees(ma1), color='y', label=lab1, linewidth = 1, scatter_size = 5) mpu.legend() # acc3 = ma.compute_velocity(vel3, cd['time_list'], 1) # mpu.figure()