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