def plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts, start_pos_ts,eq_pt_tl,bndry,wrkspc_pts): vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts, start_pos_ts,eq_pt_tl,bndry,wrkspc_pts) mpu.plot_yx(eq_pt_tl[1,:].A1, eq_pt_tl[0,:].A1, linewidth=2, color='g', scatter_size=15, label='Eq Pt') mpu.plot_yx(curr_pos_tl[1,:].A1, curr_pos_tl[0,:].A1, linewidth=0, color='b', scatter_size = 15, label = 'FK') mpu.plot_yx(bndry[1,:].A1, bndry[0,:].A1, linewidth=0, color='y', scatter_size=8) mpu.plot_yx([-0.2], [0.], linewidth=0, color='b', scatter_size=2) bndry_dist_eq = smc.dist_from_boundary(eq_pt_tl, bndry, wrkspc_pts) # signed bndry_dist_ee = smc.dist_from_boundary(curr_pos_tl, bndry, wrkspc_pts) # signed if bndry_dist_ee < bndry_dist_eq: p = curr_pos_tl else: p = eq_pt_tl pts_close = smc.pts_within_dist(p[0:2,:],bndry,0.01,0.1) mpu.plot_yx(pts_close[1,:].A1, pts_close[0,:].A1, linewidth=0, color='r', scatter_size = 8) nrm = np.linalg.norm(vt) vt = vt/nrm mpu.plot_quiver_yxv(p[1,:].A1, p[0,:].A1, vt, scale=12) mpu.show()
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_', linewidth=2, scatter_size=10, plot_velocity=False): 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 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 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()
end_angle, label='\huge{Estimated Kinematics}', color='r', alpha=0.7) mpu.pl.xlim(0.28, 0.47) mpu.legend() #mpu.savefig('three.png') mpu.savefig(fig_name + '%d' % fig_number) fig_number += 1 current_pos = pts_2d_zoom[:, -1] radial_vec = current_pos - np.matrix([cx, cy]).T radial_vec = radial_vec / np.linalg.norm(radial_vec) tangential_vec = np.matrix([[0, -1], [1, 0]]) * radial_vec mpu.plot_quiver_yxv([pts_2d_zoom[1, -1]], [pts_2d_zoom[0, -1]], radial_vec, scale=10., width=0.002) rad_text_loc = pts_2d_zoom[:, -1] + np.matrix([0.001, 0.01]).T mpu.pl.text(rad_text_loc[0, 0], rad_text_loc[1, 0], '$\hat v_{rad}$', fontsize=30) mpu.plot_quiver_yxv([pts_2d_zoom[1, -1]], [pts_2d_zoom[0, -1]], tangential_vec, scale=10., width=0.002) tan_text_loc = pts_2d_zoom[:, -1] + np.matrix([-0.012, -0.011]).T mpu.pl.text(tan_text_loc[0, 0], tan_text_loc[1, 0], s='$\hat v_{tan}$',
pts_2d_zoom[0,-1]-cx) - math.pi/2) mpu.plot_circle(cx, cy, rad, start_angle, end_angle, label='\huge{Estimated Kinematics}', color='r', alpha=0.7) mpu.pl.xlim(0.28, 0.47) mpu.legend() #mpu.savefig('three.png') mpu.savefig(fig_name+'%d'%fig_number) fig_number += 1 current_pos = pts_2d_zoom[:,-1] radial_vec = current_pos - np.matrix([cx,cy]).T radial_vec = radial_vec / np.linalg.norm(radial_vec) tangential_vec = np.matrix([[0,-1],[1,0]]) * radial_vec mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]], [pts_2d_zoom[0,-1]], radial_vec, scale=10., width = 0.002) rad_text_loc = pts_2d_zoom[:,-1] + np.matrix([0.001,0.01]).T mpu.pl.text(rad_text_loc[0,0], rad_text_loc[1,0], '$\hat v_{rad}$', fontsize = 30) mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]], [pts_2d_zoom[0,-1]], tangential_vec, scale=10., width = 0.002) tan_text_loc = pts_2d_zoom[:,-1] + np.matrix([-0.012, -0.011]).T mpu.pl.text(tan_text_loc[0,0], tan_text_loc[1,0], s = '$\hat v_{tan}$', fontsize = 30) mpu.pl.xlim(0.28, 0.47) mpu.legend() #mpu.savefig('four.png') mpu.savefig(fig_name+'%d'%fig_number) fig_number += 1