00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 import roslib; roslib.load_manifest('2009_humanoids_epc_pull')
00031
00032 import scipy.optimize as so
00033
00034 import math, numpy as np
00035 import pylab as pl
00036 import sys, optparse, time
00037 import copy
00038
00039 from enthought.mayavi import mlab
00040
00041 import mekabot.hrl_robot as hr
00042
00043 import hrl_lib.util as ut, hrl_lib.transforms as tr
00044 import matplotlib_util.util as mpu
00045 import hrl_tilting_hokuyo.display_3d_mayavi as d3m
00046
00047
00048
00049 class JointTrajectory():
00050 ''' class to store joint trajectories.
00051 data only - use for pickling.
00052 '''
00053 def __init__(self):
00054 self.time_list = []
00055 self.q_list = []
00056 self.qdot_list = []
00057
00058 class CartesianTajectory():
00059 ''' class to store trajectory of cartesian points.
00060 data only - use for pickling
00061 '''
00062 def __init__(self):
00063 self.time_list = []
00064 self.p_list = []
00065
00066 class ForceTrajectory():
00067 ''' class to store time evolution of the force at the end effector.
00068 data only - use for pickling
00069 '''
00070 def __init__(self):
00071 self.time_list = []
00072 self.f_list = []
00073
00074
00075 def joint_to_cartesian(traj):
00076 ''' traj - JointTrajectory
00077 returns CartesianTajectory after performing FK on traj.
00078 '''
00079 firenze = hr.M3HrlRobot(connect=False)
00080 pts = []
00081 for q in traj.q_list:
00082 p = firenze.FK('right_arm',q)
00083 pts.append(p.A1.tolist())
00084
00085 ct = CartesianTajectory()
00086 ct.time_list = copy.copy(traj.time_list)
00087 ct.p_list = copy.copy(pts)
00088
00089 return ct
00090
00091
00092 def plot_forces_quiver(pos_traj,force_traj,color='k'):
00093 import arm_trajectories as at
00094
00095 if isinstance(pos_traj,at.JointTrajectory):
00096 pos_traj = joint_to_cartesian(pos_traj)
00097
00098 pts = np.matrix(pos_traj.p_list).T
00099 label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
00100 x = pts[0,:].A1.tolist()
00101 y = pts[1,:].A1.tolist()
00102
00103 forces = np.matrix(force_traj.f_list).T
00104 u = (-1*forces[0,:]).A1.tolist()
00105 v = (-1*forces[1,:]).A1.tolist()
00106 pl.quiver(x,y,u,v,width=0.002,color=color,scale=100.0)
00107
00108 pl.axis('equal')
00109
00110 def plot_cartesian(traj,xaxis=None,yaxis=None,zaxis=None,color='b',label='_nolegend_',
00111 linewidth=2,scatter_size=20):
00112 ''' xaxis - x axis for the graph (0,1 or 2)
00113 zaxis - for a 3d plot. not implemented.
00114 '''
00115
00116 import arm_trajectories as at
00117
00118 if isinstance(traj,at.JointTrajectory):
00119 traj = joint_to_cartesian(traj)
00120
00121 pts = np.matrix(traj.p_list).T
00122 label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
00123 x = pts[xaxis,:].A1.tolist()
00124 y = pts[yaxis,:].A1.tolist()
00125
00126 if zaxis == None:
00127 pl.plot(x,y,c=color,linewidth=linewidth,label=label)
00128 pl.scatter(x,y,c=color,s=scatter_size,label='_nolegend_', linewidths=0)
00129 pl.xlabel(label_list[xaxis])
00130 pl.ylabel(label_list[yaxis])
00131 pl.legend(loc='best')
00132 pl.axis('equal')
00133 else:
00134 from numpy import array
00135 from enthought.mayavi.api import Engine
00136 engine = Engine()
00137 engine.start()
00138 if len(engine.scenes) == 0:
00139 engine.new_scene()
00140
00141 z = pts[zaxis,:].A1.tolist()
00142 time_list = [t-traj.time_list[0] for t in traj.time_list]
00143 mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
00144 mlab.axes()
00145 mlab.xlabel(label_list[xaxis])
00146 mlab.ylabel(label_list[yaxis])
00147 mlab.zlabel(label_list[zaxis])
00148 mlab.colorbar(title='Time')
00149
00150
00151 axes = engine.scenes[0].children[0].children[0].children[1]
00152 axes.axes.position = array([ 0., 0.])
00153 axes.axes.label_format = '%-#6.2g'
00154 axes.title_text_property.font_size=4
00155
00156
00157
00158
00159
00160
00161
00162 def compute_forces(q_actual_traj,q_eq_traj,torque_traj,rel_stiffness_list):
00163 firenze = hr.M3HrlRobot(connect=False)
00164
00165 d_gains_list_mN_deg_sec = [-100,-120,-15,-25,-1.25]
00166 d_gains_list = [180./1000.*s/math.pi for s in d_gains_list_mN_deg_sec]
00167
00168 stiff_list_mNm_deg = [1800,1300,350,600,60]
00169 stiff_list_Nm_rad = [180./1000.*s/math.pi for s in stiff_list_mNm_deg]
00170
00171
00172 dia = np.array(rel_stiffness_list) * np.array(stiff_list_Nm_rad)
00173 k_q = np.matrix(np.diag(dia))
00174 dia_inv = 1./dia
00175 k_q_inv = np.matrix(np.diag(dia_inv))
00176
00177 actual_cart = joint_to_cartesian(q_actual_traj)
00178 eq_cart = joint_to_cartesian(q_eq_traj)
00179 force_traj_jacinv = ForceTrajectory()
00180 force_traj_stiff = ForceTrajectory()
00181 force_traj_torque = ForceTrajectory()
00182
00183 k_cart_list = []
00184
00185 for q_actual,q_dot,q_eq,actual_pos,eq_pos,t,tau_m in zip(q_actual_traj.q_list,q_actual_traj.qdot_list,q_eq_traj.q_list,actual_cart.p_list,eq_cart.p_list,q_actual_traj.time_list,torque_traj.q_list):
00186
00187 q_eq = firenze.clamp_to_physical_joint_limits('right_arm',q_eq)
00188 q_delta = np.matrix(q_actual).T - np.matrix(q_eq).T
00189 tau = k_q * q_delta[0:5,0] - np.matrix(np.array(d_gains_list)*np.array(q_dot)[0:5]).T
00190
00191 x_delta = np.matrix(actual_pos).T - np.matrix(eq_pos).T
00192
00193 jac_full = firenze.Jac('right_arm',q_actual)
00194 jac = jac_full[0:3,0:5]
00195
00196 jac_full_eq = firenze.Jac('right_arm',q_eq)
00197 jac_eq = jac_full_eq[0:3,0:5]
00198 k_cart = np.linalg.inv((jac_eq*k_q_inv*jac_eq.T))
00199 k_cart_list.append(k_cart)
00200
00201 pseudo_inv_jac = np.linalg.inv(jac_full*jac_full.T)*jac_full
00202
00203 tau_full = np.row_stack((tau,np.matrix(tau_m[5:7]).T))
00204
00205 force = -1*pseudo_inv_jac[0:3,0:5]*tau
00206 force_traj_jacinv.f_list.append(force.A1.tolist())
00207 force_traj_stiff.f_list.append((k_cart*x_delta).A1.tolist())
00208 force_traj_torque.f_list.append((pseudo_inv_jac*np.matrix(tau_m).T)[0:3].A1.tolist())
00209
00210 return force_traj_jacinv,force_traj_stiff,force_traj_torque,k_cart_list
00211
00212
00213
00214
00215
00216
00217
00218 def compute_radial_tangential_forces(f_list,p_list,cx,cy):
00219 f_rad_l,f_tan_l = [],[]
00220 for f,p in zip(f_list,p_list):
00221 rad_vec = np.array([p[0]-cx,p[1]-cy])
00222 rad_vec = rad_vec/np.linalg.norm(rad_vec)
00223 f_vec = np.array([f[0],f[1]])
00224 f_rad_mag = np.dot(f_vec,rad_vec)
00225 f_tan_mag = np.linalg.norm(f_vec-rad_vec*f_rad_mag)
00226 f_rad_mag = abs(f_rad_mag)
00227 f_rad_l.append(f_rad_mag)
00228 f_tan_l.append(f_tan_mag)
00229
00230 return f_rad_l,f_tan_l
00231
00232
00233 def plot_error_forces(measured_list,calc_list):
00234 err_x, err_y = [],[]
00235 err_rel_x, err_rel_y = [],[]
00236 mag_x, mag_y = [],[]
00237 for m,c in zip(measured_list,calc_list):
00238 err_x.append(abs(m[0]-c[0]))
00239 err_rel_x.append(abs(m[0]-c[0])/abs(m[0])*100)
00240
00241 mag_x.append(abs(m[0]))
00242 err_y.append(abs(m[1]-c[1]))
00243 err_rel_y.append(abs(m[1]-c[1])/abs(m[1])*100)
00244
00245 mag_y.append(abs(m[1]))
00246
00247 x_idx = range(len(err_x))
00248 zero = [0 for i in x_idx]
00249
00250 fig = pl.figure()
00251 ax1 = fig.add_subplot(111)
00252 ax2 = ax1.twinx()
00253
00254 ax1.plot(zero,c='k',linewidth=1,label='_nolegend_')
00255 l1 = ax1.plot(err_x,c='b',linewidth=1,label='absolute error')
00256 ax1.scatter(x_idx,err_x,c='b',s=10,label='_nolegend_', linewidths=0)
00257 l2 = ax1.plot(mag_x,c='g',linewidth=1,label='magnitude')
00258 ax1.scatter(x_idx,mag_x,c='g',s=10,label='_nolegend_', linewidths=0)
00259 l3 = ax2.plot(err_rel_x,c='r',linewidth=1,label='relative error %')
00260
00261 ax1.set_ylim(0,15)
00262 ax2.set_ylim(0,100)
00263 ax1.set_xlabel('measurement number')
00264 ax1.set_ylabel('x component of force (N)')
00265 ax2.set_ylabel('percentage error')
00266 ax1.yaxis.set_label_coords(-0.3,0.5)
00267 ax2.yaxis.set_label_coords(-0.3,0.5)
00268 leg = pl.legend([l1,l2,l3],['absolute error','magnitude','rel error %'],loc='upper left',
00269 handletextsep=0.015,handlelen=0.003,labelspacing=0.003)
00270
00271 fig = pl.figure()
00272 ax1 = fig.add_subplot(111)
00273 ax2 = ax1.twinx()
00274
00275 ax1.plot(zero,c='k',linewidth=1)
00276 l1 = ax1.plot(err_y,c='b',linewidth=1)
00277 ax1.scatter(x_idx,err_y,c='b',s=10, linewidths=0)
00278 l2 = ax1.plot(mag_y,c='g',linewidth=1)
00279 ax1.scatter(x_idx,mag_y,c='g',s=10,linewidths=0)
00280 l3 = ax2.plot(err_rel_y,c='r',linewidth=1)
00281
00282 ax1.set_ylim(0,15)
00283 ax2.set_ylim(0,100)
00284 ax1.yaxis.set_label_coords(-0.3,0.5)
00285 ax2.yaxis.set_label_coords(-0.3,0.5)
00286 ax1.set_xlabel('measurement number')
00287 ax1.set_ylabel('y component of force (N)')
00288 ax2.set_ylabel('percentage error')
00289
00290 leg = pl.legend([l1,l2,l3],['absolute error','magnitude','rel error %'],loc='upper left',
00291 handletextsep=0.015,handlelen=0.003,labelspacing=0.003)
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307 def plot_stiff_ellipses(k_cart_list,pos_traj,skip=10,subplotnum=111):
00308 import arm_trajectories as at
00309 if isinstance(pos_traj,at.JointTrajectory):
00310 pos_traj = joint_to_cartesian(pos_traj)
00311
00312 pts = np.matrix(pos_traj.p_list).T
00313 x_l = pts[0,:].A1.tolist()
00314 y_l = pts[1,:].A1.tolist()
00315
00316 from pylab import figure, show, rand
00317 from matplotlib.patches import Ellipse
00318
00319 ells = []
00320 scale = 25000.
00321 ratio_list = []
00322 for k_c,x,y in zip(k_cart_list[::skip],x_l[::skip],y_l[::skip]):
00323 w,v = np.linalg.eig(k_c[0:2,0:2])
00324 w_abs = np.abs(w)
00325 major_axis = np.max(w_abs)
00326 minor_axis = np.min(w_abs)
00327 print 'major, minor:',major_axis,minor_axis
00328
00329 ratio_list.append(major_axis/minor_axis)
00330
00331 ells.append(Ellipse(np.array([x,y]),width=w[0]/scale,height=w[1]/scale,angle=math.degrees(math.atan2(v[1,0],v[0,0]))))
00332 ells[-1].set_lw(2)
00333
00334
00335
00336 ax = pl.subplot(subplotnum, aspect='equal')
00337
00338 for e in ells:
00339 ax.add_artist(e)
00340
00341
00342 e.set_facecolor(np.array([1,1,1]))
00343
00344 plot_cartesian(pos_traj,xaxis=0,yaxis=1,color='b',
00345 linewidth=0.0,scatter_size=0)
00346
00347
00348
00349
00350
00351
00352
00353 mean_ratio = np.mean(np.array(ratio_list))
00354 std_ratio = np.std(np.array(ratio_list))
00355
00356 return mean_ratio,std_ratio
00357
00358
00359
00360
00361 def plot_stiffness_field(k_cart,plottitle=''):
00362 n_points = 20
00363 ang_step = math.radians(360)/n_points
00364 x_list = []
00365 y_list = []
00366 u_list = []
00367 v_list = []
00368 k_cart = k_cart[0:2,0:2]
00369
00370 for i in range(n_points):
00371 ang = i*ang_step
00372 for r in [0.5,1.,1.5]:
00373 dx = r*math.cos(ang)
00374 dy = r*math.sin(ang)
00375 dtau = -k_cart*np.matrix([dx,dy]).T
00376 x_list.append(dx)
00377 y_list.append(dy)
00378 u_list.append(dtau[0,0])
00379 v_list.append(dtau[1,0])
00380
00381 pl.figure()
00382
00383 mpu.plot_radii(0,0,1.5,0.,math.radians(360),interval=ang_step,color='r')
00384 pl.quiver(x_list,y_list,u_list,v_list,width=0.002,color='k',scale=None)
00385 pl.axis('equal')
00386 pl.title(plottitle)
00387
00388
00389 def plot_stiff_ellipse_map(stiffness_list,num):
00390 firenze = hr.M3HrlRobot(connect=False)
00391 hook_3dprint_angle = math.radians(20-2.54)
00392 rot_mat = tr.Rz(0.-hook_3dprint_angle)*tr.Ry(math.radians(-90))
00393
00394 d_gains_list_mN_deg_sec = [-100,-120,-15,-25,-1.25]
00395 d_gains_list = [180./1000.*s/math.pi for s in d_gains_list_mN_deg_sec]
00396
00397 stiff_list_mNm_deg = [1800,1300,350,600,60]
00398 stiff_list_Nm_rad = [180./1000.*s/math.pi for s in stiff_list_mNm_deg]
00399 dia = np.array(stiffness_list) * np.array(stiff_list_Nm_rad)
00400 k_q = np.matrix(np.diag(dia))
00401 dia_inv = 1./dia
00402 k_q_inv = np.matrix(np.diag(dia_inv))
00403
00404 s0,s1,s2,s3 = stiffness_list[0],stiffness_list[1],stiffness_list[2],stiffness_list[3]
00405
00406 i=0
00407
00408 for z in np.arange(-0.23,-0.27,-0.05):
00409 pl.figure()
00410 k_cart_list = []
00411 pos_traj = CartesianTajectory()
00412 for x in np.arange(0.25,0.56,0.05):
00413 for y in np.arange(-0.15,-0.56,-0.05):
00414 if math.sqrt(x**2+y**2)>0.55:
00415 continue
00416 q = firenze.IK('right_arm',np.matrix([x,y,z]).T,rot_mat)
00417 if q == None:
00418 continue
00419 jac_full = firenze.Jac('right_arm',q)
00420 jac = jac_full[0:3,0:5]
00421 k_cart = np.linalg.inv((jac*k_q_inv*jac.T))
00422 k_cart_list.append(k_cart)
00423 pos_traj.p_list.append([x,y,z])
00424 pos_traj.time_list.append(0.1)
00425 if len(pos_traj.p_list)>0:
00426 ret = plot_stiff_ellipses(k_cart_list,pos_traj,skip=1)
00427 pl.axis('equal')
00428 pl.legend(loc='best')
00429 title_string = 'z: %.2f stiff:[%.1f,%.1f,%.1f,%.1f]'%(z,s0,s1,s2,s3)
00430 pl.title(title_string)
00431 i+=1
00432 pl.savefig('ellipses_%03d_%03d.png'%(num,i))
00433
00434 return ret
00435
00436
00437 def compute_workspace(z,plot=False,wrist_roll_angle=math.radians(0),subplotnum=None,title=''):
00438 firenze = hr.M3HrlRobot(connect=False)
00439
00440
00441 rot_mat = tr.Rz(wrist_roll_angle)*tr.Ry(math.radians(-90))
00442 x_list,y_list = [],[]
00443 for x in np.arange(0.15,0.65,0.02):
00444 for y in np.arange(-0.05,-0.65,-0.02):
00445 q = firenze.IK('right_arm',np.matrix([x,y,z]).T,rot_mat)
00446 if q != None:
00447 x_list.append(x)
00448 y_list.append(y)
00449
00450 if len(x_list) > 2:
00451 multipoint = sg.Point(x_list[0],y_list[0])
00452 for x,y in zip(x_list[1:],y_list[1:]):
00453 multipoint = multipoint.union(sg.Point(x,y))
00454 hull = multipoint.convex_hull
00455 if plot:
00456 coords_seq = hull.boundary.coords
00457 hull_x_list,hull_y_list = [],[]
00458 for c in coords_seq:
00459 hull_x_list.append(c[0])
00460 hull_y_list.append(c[1])
00461 mpu.plot_yx(y_list,x_list,linewidth=0,subplotnum=subplotnum,axis='equal',
00462 plot_title=title)
00463 mpu.plot_yx(hull_y_list,hull_x_list,linewidth=2,subplotnum=subplotnum,axis='equal')
00464 return hull,len(x_list)
00465 else:
00466 return None,None
00467
00468 def diff_roll_angles():
00469 pl.subplot(211,aspect='equal')
00470
00471
00472
00473 def compute_workspace_z():
00474 n_points_list,area_list,z_list = [],[],[]
00475
00476
00477 for z in np.arange(-0.15,-0.155,-0.01):
00478 pl.figure()
00479 hull,n_points = compute_workspace(z,plot=True)
00480 pl.title('z: %.2f'%(z))
00481 pl.savefig('z_%.2f.png'%(z))
00482
00483 if hull != None:
00484 area_list.append(hull.area)
00485 z_list.append(z)
00486 n_points_list.append(n_points)
00487
00488 coords_seq = hull.boundary.coords
00489 hull_x_list,hull_y_list = [],[]
00490 for c in coords_seq:
00491 hull_x_list.append(c[0])
00492 hull_y_list.append(c[1])
00493
00494 pl.figure()
00495 mpu.plot_yx(area_list,z_list,linewidth=2,label='area')
00496 pl.savefig('hist_area.png')
00497 pl.figure()
00498 mpu.plot_yx(n_points_list,z_list,linewidth=2,color='g',label='n_points')
00499
00500 pl.xlabel('Z coordinate (m)')
00501 pl.ylabel('# points')
00502 pl.savefig('hist_points.png')
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514 def fit_rotary_joint(rad,x_guess,y_guess,pts):
00515 def error_function(params):
00516 center = np.matrix((params[0],params[1])).T
00517
00518
00519
00520 err = ut.norm(pts-center).A1 - rad
00521 res = np.dot(err,err)
00522 return res
00523
00524 params_1 = [x_guess,y_guess]
00525 r = so.fmin_bfgs(error_function,params_1,full_output=1)
00526 opt_params_1,f_opt_1 = r[0],r[1]
00527
00528 params_2 = [x_guess,y_guess+2*rad]
00529 r = so.fmin_bfgs(error_function,params_2,full_output=1)
00530 opt_params_2,f_opt_2 = r[0],r[1]
00531
00532 if f_opt_2<f_opt_1:
00533 return opt_params_2[0],opt_params_2[1]
00534 else:
00535 return opt_params_1[0],opt_params_1[1]
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546 def fit_circle(rad_guess,x_guess,y_guess,pts,method,verbose=True):
00547 def error_function(params):
00548 center = np.matrix((params[0],params[1])).T
00549 rad = params[2]
00550
00551
00552
00553 err = ut.norm(pts-center).A1 - rad
00554 res = np.dot(err,err)
00555 return res
00556
00557 params_1 = [x_guess,y_guess,rad_guess]
00558 if method == 'fmin':
00559 r = so.fmin(error_function,params_1,xtol=0.0002,ftol=0.000001,full_output=1,disp=verbose)
00560 opt_params_1,fopt_1 = r[0],r[1]
00561 elif method == 'fmin_bfgs':
00562 r = so.fmin_bfgs(error_function,params_1,full_output=1,disp=verbose)
00563 opt_params_1,fopt_1 = r[0],r[1]
00564 else:
00565 raise RuntimeError('unknown method: '+method)
00566
00567 params_2 = [x_guess,y_guess+2*rad_guess,rad_guess]
00568 if method == 'fmin':
00569 r = so.fmin(error_function,params_2,xtol=0.0002,ftol=0.000001,full_output=1,disp=verbose)
00570 opt_params_2,fopt_2 = r[0],r[1]
00571 elif method == 'fmin_bfgs':
00572 r = so.fmin_bfgs(error_function,params_2,full_output=1,disp=verbose)
00573 opt_params_2,fopt_2 = r[0],r[1]
00574 else:
00575 raise RuntimeError('unknown method: '+method)
00576
00577 if fopt_2<fopt_1:
00578 return opt_params_2[2],opt_params_2[0],opt_params_2[1]
00579 else:
00580 return opt_params_1[2],opt_params_1[0],opt_params_1[1]
00581
00582
00583
00584 if __name__ == '__main__':
00585 p = optparse.OptionParser()
00586 p.add_option('-f', action='store', type='string', dest='fname',
00587 help='pkl file to use.', default='')
00588 p.add_option('--xy', action='store_true', dest='xy',
00589 help='plot the x and y coordinates of the end effector.')
00590 p.add_option('--yz', action='store_true', dest='yz',
00591 help='plot the y and z coordinates of the end effector.')
00592 p.add_option('--xz', action='store_true', dest='xz',
00593 help='plot the x and z coordinates of the end effector.')
00594 p.add_option('--plot_ellipses', action='store_true', dest='plot_ellipses',
00595 help='plot the stiffness ellipse in the x-y plane')
00596 p.add_option('--pfc', action='store_true', dest='pfc',
00597 help='plot the radial and tangential components of the force.')
00598 p.add_option('--pmf', action='store_true', dest='pmf',
00599 help='plot things with the mechanism alinged with the axes.')
00600 p.add_option('--pff', action='store_true', dest='pff',
00601 help='plot the force field corresponding to a stiffness ellipse.')
00602 p.add_option('--pev', action='store_true', dest='pev',
00603 help='plot the stiffness ellipses for different combinations of the rel stiffnesses.')
00604 p.add_option('--plot_forces', action='store_true', dest='plot_forces',
00605 help='plot the force in the x-y plane')
00606 p.add_option('--plot_forces_error', action='store_true', dest='plot_forces_error',
00607 help='plot the error between the computed and measured (ATI) forces in the x-y plane')
00608 p.add_option('--xyz', action='store_true', dest='xyz',
00609 help='plot in 3d the coordinates of the end effector.')
00610 p.add_option('-r', action='store', type='float', dest='rad',
00611 help='radius of the joint.', default=None)
00612 p.add_option('--rad_fix', action='store_true', dest='rad_fix',
00613 help='do not optimize for the radius.')
00614 p.add_option('--noshow', action='store_true', dest='noshow',
00615 help='do not display the figure (use while saving figures to disk)')
00616 p.add_option('--exptplot', action='store_true', dest='exptplot',
00617 help='put all the graphs of an experiment as subplots.')
00618 p.add_option('--pwf', action='store_true', dest='pwf',
00619 help='plot the workspace at some z coord.')
00620
00621 opt, args = p.parse_args()
00622 fname = opt.fname
00623 xy_flag = opt.xy
00624 yz_flag = opt.yz
00625 xz_flag = opt.xz
00626 plot_forces_flag = opt.plot_forces
00627 plot_ellipses_flag = opt.plot_ellipses
00628 plot_forces_error_flag = opt.plot_forces_error
00629 plot_force_components_flag = opt.pfc
00630 plot_force_field_flag = opt.pff
00631 plot_mechanism_frame = opt.pmf
00632 xyz_flag = opt.xyz
00633 rad = opt.rad
00634 show_fig = not(opt.noshow)
00635 plot_ellipses_vary_flag = opt.pev
00636 expt_plot = opt.exptplot
00637 rad_fix = opt.rad_fix
00638 plot_workspace_flag = opt.pwf
00639
00640
00641 if plot_workspace_flag:
00642 compute_workspace_z()
00643
00644
00645
00646 if plot_ellipses_vary_flag:
00647 show_fig=False
00648 i = 0
00649 ratio_list1 = [0.1,0.3,0.5,0.7,0.9]
00650 ratio_list2 = [0.1,0.3,0.5,0.7,0.9]
00651 ratio_list3 = [0.1,0.3,0.5,0.7,0.9]
00652
00653
00654
00655
00656
00657
00658
00659 inv_mean_list,std_list = [],[]
00660 x_l,y_l,z_l = [],[],[]
00661 s0 = 0.2
00662
00663 for s1 in ratio_list1:
00664 for s2 in ratio_list2:
00665 for s3 in ratio_list3:
00666 i += 1
00667 s_list = [s0,s1,s2,s3,0.8]
00668
00669 print '################## s_list:', s_list
00670 m,s = plot_stiff_ellipse_map(s_list,i)
00671 inv_mean_list.append(1./m)
00672 std_list.append(s)
00673 x_l.append(s1)
00674 y_l.append(s2)
00675 z_l.append(s3)
00676
00677 ut.save_pickle({'x_l':x_l,'y_l':y_l,'z_l':z_l,'inv_mean_list':inv_mean_list,'std_list':std_list},
00678 'stiff_dict_'+ut.formatted_time()+'.pkl')
00679 d3m.plot_points(np.matrix([x_l,y_l,z_l]),scalar_list=inv_mean_list,mode='sphere')
00680 mlab.axes()
00681 d3m.show()
00682
00683 sys.exit()
00684
00685 if fname=='':
00686 print 'please specify a pkl file (-f option)'
00687 print 'Exiting...'
00688 sys.exit()
00689
00690 d = ut.load_pickle(fname)
00691 actual_cartesian = joint_to_cartesian(d['actual'])
00692 eq_cartesian = joint_to_cartesian(d['eq_pt'])
00693
00694 for p in actual_cartesian.p_list:
00695 print p[0],p[1],p[2]
00696
00697
00698 if rad != None:
00699
00700
00701
00702 pts_list = actual_cartesian.p_list
00703 ee_start_pos = pts_list[0]
00704 x_guess = ee_start_pos[0]
00705 y_guess = ee_start_pos[1] - rad
00706 print 'before call to fit_rotary_joint'
00707 pts_2d = (np.matrix(pts_list).T)[0:2,:]
00708 t0 = time.time()
00709
00710 if rad_fix:
00711 rad_guess = 0.9
00712 else:
00713 rad_guess = rad
00714 rad_fmin,cx,cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d[:,0:-4],method='fmin')
00715 t1 = time.time()
00716 rad_opt,cx,cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d[:,0:-4],method='fmin_bfgs')
00717 t2 = time.time()
00718 print 'after fit_rotary_joint'
00719 print 'optimized radius:', rad_opt
00720 print 'optimized radius fmin:', rad_fmin
00721 print 'time to bfgs:', t2-t1
00722 print 'time to fmin:', t1-t0
00723
00724 if rad_fix:
00725 cx,cy = fit_rotary_joint(rad,x_guess,y_guess,pts_2d[:,0:-4])
00726 else:
00727 rad = rad_opt
00728
00729
00730 if plot_mechanism_frame:
00731
00732 if expt_plot:
00733 pl.subplot(231)
00734
00735
00736 start_pt = actual_cartesian.p_list[0]
00737 x_diff = start_pt[0] - cx
00738 y_diff = start_pt[1] - cy
00739 angle = math.atan2(y_diff,x_diff) - math.radians(90)
00740 rot_mat = tr.Rz(angle)[0:2,0:2]
00741 translation_mat = np.matrix([cx,cy]).T
00742
00743 robot_width,robot_length = 0.1,0.2
00744 robot_x_list = [-robot_width/2,-robot_width/2,robot_width/2,robot_width/2,-robot_width/2]
00745 robot_y_list = [-robot_length/2,robot_length/2,robot_length/2,-robot_length/2,-robot_length/2]
00746 robot_mat = rot_mat*(np.matrix([robot_x_list,robot_y_list]) - translation_mat)
00747 mpu.plot_yx(robot_mat[1,:].A1,robot_mat[0,:].A1,linewidth=2,scatter_size=0,
00748 color='k',label='torso')
00749
00750 pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2]
00751 pts2d_actual_t = rot_mat *(pts2d_actual - translation_mat)
00752 mpu.plot_yx(pts2d_actual_t[1,:].A1,pts2d_actual_t[0,:].A1,scatter_size=20,label='FK')
00753
00754 end_pt = pts2d_actual_t[:,-1]
00755 end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0],end_pt[0,0])-math.radians(90))
00756
00757 mpu.plot_circle(0,0,rad,0.,end_angle,label='Actual_opt',color='r')
00758 mpu.plot_radii(0,0,rad,0.,end_angle,interval=math.radians(15),color='r')
00759 pl.legend(loc='best')
00760 pl.axis('equal')
00761
00762 if not(expt_plot):
00763 str_parts = fname.split('.')
00764 fig_name = str_parts[0]+'_robot_pose.png'
00765 pl.savefig(fig_name)
00766 pl.figure()
00767 else:
00768 pl.subplot(232)
00769
00770 pl.text(0.1,0.15,d['info'])
00771 pl.text(0.1,0.10,'control: '+d['strategy'])
00772 pl.text(0.1,0.05,'robot angle: %.2f'%math.degrees(angle))
00773 pl.text(0.1,0,'optimized radius: %.2f'%rad_opt)
00774 pl.text(0.1,-0.05,'radius used: %.2f'%rad)
00775 pl.text(0.1,-0.10,'opening angle: %.2f'%math.degrees(end_angle))
00776 s_list = d['stiffness'].stiffness_list
00777 s_scale = d['stiffness'].stiffness_scale
00778 sl = [min(s*s_scale,1.0) for s in s_list]
00779 pl.text(0.1,-0.15,'stiffness list: %.2f, %.2f, %.2f, %.2f'%(sl[0],sl[1],sl[2],sl[3]))
00780 pl.text(0.1,-0.20,'stop condition: '+d['result'])
00781 time_dict = d['time_dict']
00782 pl.text(0.1,-0.25,'time to hook: %.2f'%(time_dict['before_hook']-time_dict['before_pull']))
00783 pl.text(0.1,-0.30,'time to pull: %.2f'%(time_dict['before_pull']-time_dict['after_pull']))
00784
00785 pl.ylim(-0.45,0.25)
00786 if not(expt_plot):
00787 pl.figure()
00788
00789
00790 if xy_flag:
00791 st_pt = pts_2d[:,0]
00792 start_angle = tr.angle_within_mod180(math.atan2(st_pt[1,0]-cy,st_pt[0,0]-cx) - math.radians(90))
00793 end_pt = pts_2d[:,-1]
00794 end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0]-cy,end_pt[0,0]-cx) - math.radians(90))
00795
00796 print 'start_angle, end_angle:', math.degrees(start_angle), math.degrees(end_angle)
00797 print 'angle through which mechanism turned:', math.degrees(end_angle-start_angle)
00798
00799 if expt_plot:
00800 pl.subplot(233)
00801
00802 plot_cartesian(actual_cartesian,xaxis=0,yaxis=1,color='b',label='End Effector Trajectory')
00803 plot_cartesian(eq_cartesian, xaxis=0,yaxis=1,color='g',label='Eq Point')
00804 mpu.plot_circle(cx,cy,rad,start_angle,end_angle,label='Estimated Kinematics',color='r')
00805
00806
00807
00808 leg = pl.legend(loc='best')
00809 leg.draw_frame(False)
00810
00811 ax = pl.gca()
00812 ax.set_xlim(ax.get_xlim()[::-1])
00813 ax.set_ylim(ax.get_ylim()[::-1])
00814
00815 force_traj = d['force']
00816 forces = np.matrix(force_traj.f_list).T
00817 force_mag = ut.norm(forces)
00818 print 'force_mag:', force_mag.A1
00819
00820 elif yz_flag:
00821 plot_cartesian(actual_cartesian,xaxis=1,yaxis=2,color='b',label='FK')
00822 plot_cartesian(eq_cartesian, xaxis=1,yaxis=2,color='g',label='Eq Point')
00823 elif xz_flag:
00824 plot_cartesian(actual_cartesian,xaxis=0,yaxis=2,color='b',label='FK')
00825 plot_cartesian(eq_cartesian, xaxis=0,yaxis=2,color='g',label='Eq Point')
00826
00827
00828 if plot_forces_flag or plot_forces_error_flag or plot_ellipses_flag or plot_force_components_flag or plot_force_field_flag:
00829 arm_stiffness_list = d['stiffness'].stiffness_list
00830 scale = d['stiffness'].stiffness_scale
00831 asl = [min(scale*s,1.0) for s in arm_stiffness_list]
00832 ftraj_jinv,ftraj_stiff,ftraj_torque,k_cart_list=compute_forces(d['actual'],d['eq_pt'],
00833 d['torque'],asl)
00834 if plot_forces_flag:
00835 plot_forces_quiver(actual_cartesian,d['force'],color='k')
00836 plot_forces_quiver(actual_cartesian,ftraj_jinv,color='y')
00837
00838
00839 if plot_ellipses_flag:
00840
00841 if expt_plot:
00842 subplotnum=234
00843 else:
00844 pl.figure()
00845 subplotnum=111
00846 plot_stiff_ellipses(k_cart_list,eq_cartesian,subplotnum=subplotnum)
00847
00848 if plot_forces_error_flag:
00849 plot_error_forces(d['force'].f_list,ftraj_jinv.f_list)
00850
00851
00852 if plot_force_components_flag:
00853 p_list = actual_cartesian.p_list
00854 frad_list,ftan_list = compute_radial_tangential_forces(d['force'].f_list,p_list,cx,cy)
00855 if expt_plot:
00856 pl.subplot(235)
00857 else:
00858 pl.figure()
00859
00860 time_list = d['force'].time_list
00861 time_list = [t-time_list[0] for t in time_list]
00862 x_coord_list = np.matrix(p_list)[:,0].A1.tolist()
00863 mpu.plot_yx(frad_list,x_coord_list,scatter_size=50,color=time_list,cb_label='time')
00864 pl.xlabel('x coord of end effector (m)')
00865 pl.ylabel('magnitude of radial force (N)')
00866 pl.title(d['info'])
00867 if expt_plot:
00868 pl.subplot(236)
00869 else:
00870 pl.figure()
00871 mpu.plot_yx(ftan_list,x_coord_list,scatter_size=50,color=time_list,cb_label='time')
00872 pl.xlabel('x coord of end effector (m)')
00873 pl.ylabel('magnitude of tangential force (N)')
00874 pl.title(d['info'])
00875
00876 if plot_force_field_flag:
00877 plot_stiffness_field(k_cart_list[0],plottitle='start')
00878 plot_stiffness_field(k_cart_list[-1],plottitle='end')
00879
00880
00881 if expt_plot:
00882 f = pl.gcf()
00883 curr_size = f.get_size_inches()
00884 f.set_size_inches(curr_size[0]*2,curr_size[1]*2)
00885 str_parts = fname.split('.')
00886 if d.has_key('strategy'):
00887 fig_name = str_parts[0]+'_'+d['strategy']+'.png'
00888 else:
00889 fig_name = str_parts[0]+'_res.png'
00890 f.savefig(fig_name)
00891
00892 if show_fig:
00893 pl.show()
00894 else:
00895 print '################################'
00896 print 'show_fig is FALSE'
00897
00898 if xyz_flag:
00899 plot_cartesian(traj, xaxis=0,yaxis=1,zaxis=2)
00900 mlab.show()
00901
00902
00903
00904