def plot_stiffness_field(k_cart,plottitle=''): n_points = 20 ang_step = math.radians(360)/n_points x_list = [] y_list = [] u_list = [] v_list = [] k_cart = k_cart[0:2,0:2] for i in range(n_points): ang = i*ang_step for r in [0.5,1.,1.5]: dx = r*math.cos(ang) dy = r*math.sin(ang) dtau = -k_cart*np.matrix([dx,dy]).T x_list.append(dx) y_list.append(dy) u_list.append(dtau[0,0]) v_list.append(dtau[1,0]) pl.figure() # mpu.plot_circle(0,0,1.0,0.,math.radians(360)) mpu.plot_radii(0,0,1.5,0.,math.radians(360),interval=ang_step,color='r') pl.quiver(x_list,y_list,u_list,v_list,width=0.002,color='k',scale=None) pl.axis('equal') pl.title(plottitle)
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]
mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b', label = 'FK', axis = 'equal', alpha = 1.0, scatter_size=7, linewidth=0, marker='x', 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)
robot_width,robot_length = 0.1,0.2 robot_x_list = [-robot_width/2,-robot_width/2,robot_width/2,robot_width/2,-robot_width/2] robot_y_list = [-robot_length/2,robot_length/2,robot_length/2,-robot_length/2,-robot_length/2] robot_mat = rot_mat*(np.matrix([robot_x_list,robot_y_list]) - translation_mat) mpu.plot_yx(robot_mat[1,:].A1,robot_mat[0,:].A1,linewidth=2,scatter_size=0, color='k',label='torso') pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2] pts2d_actual_t = rot_mat *(pts2d_actual - translation_mat) mpu.plot_yx(pts2d_actual_t[1,:].A1,pts2d_actual_t[0,:].A1,scatter_size=20,label='FK') end_pt = pts2d_actual_t[:,-1] end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0],end_pt[0,0])-math.radians(90)) mpu.plot_circle(0,0,rad,0.,end_angle,label='Actual_opt',color='r') mpu.plot_radii(0,0,rad,0.,end_angle,interval=math.radians(15),color='r') pl.legend(loc='best') pl.axis('equal') if not(expt_plot): str_parts = fname.split('.') fig_name = str_parts[0]+'_robot_pose.png' pl.savefig(fig_name) pl.figure() else: pl.subplot(232) pl.text(0.1,0.15,d['info']) pl.text(0.1,0.10,'control: '+d['strategy']) pl.text(0.1,0.05,'robot angle: %.2f'%math.degrees(angle)) pl.text(0.1,0,'optimized radius: %.2f'%rad_opt)
mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b', label = '\huge{End Effector Trajectory}', axis = 'equal', alpha = 1.0, scatter_size=7, linewidth=0, marker='x', 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 = '\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):