00001
00002 import scipy.optimize as so
00003
00004 import math, numpy as np
00005 import pylab as pl
00006 import sys, optparse, time
00007 import copy
00008
00009 from enthought.mayavi import mlab
00010
00011
00012
00013 import roslib; roslib.load_manifest('doors_forces_kinematics')
00014 roslib.load_manifest('epc_core')
00015 import cody_arms.arms as ca
00016
00017 import cody_arms.coord_frames as mcf
00018
00019 import hrl_lib.util as ut, hrl_lib.transforms as tr
00020
00021 roslib.load_manifest('hrl_tilting_hokuyo')
00022 import hrl_tilting_hokuyo.display_3d_mayavi as d3m
00023
00024 roslib.load_manifest('epc_door_opening')
00025 import epc_door_opening.segway_motion_calc as smc
00026
00027 class JointTrajectory():
00028 ''' class to store joint trajectories.
00029 data only - use for pickling.
00030 '''
00031 def __init__(self):
00032 self.time_list = []
00033 self.q_list = []
00034 self.qdot_list = []
00035 self.qdotdot_list = []
00036
00037
00038
00039 class PlanarTrajectory():
00040 def __init__(self):
00041 self.time_list = []
00042 self.x_list = []
00043 self.y_list = []
00044 self.a_list = []
00045
00046 class CartesianTajectory():
00047 ''' class to store trajectory of cartesian points.
00048 data only - use for pickling
00049 '''
00050 def __init__(self):
00051 self.time_list = []
00052 self.p_list = []
00053 self.v_list = []
00054
00055 class ForceTrajectory():
00056 ''' class to store time evolution of the force at the end effector.
00057 data only - use for pickling
00058 '''
00059 def __init__(self):
00060 self.time_list = []
00061 self.f_list = []
00062
00063
00064
00065
00066
00067 def joint_to_cartesian(traj, arm):
00068 firenze = ca.M3HrlRobot(end_effector_length = 0.17318)
00069
00070 pts = []
00071 cart_vel = []
00072 for i in range(len(traj.q_list)):
00073 q = traj.q_list[i]
00074 p = firenze.FK(arm, q)
00075 pts.append(p.A1.tolist())
00076
00077 if traj.qdot_list != [] and traj.qdot_list[0] != None:
00078 qdot = traj.qdot_list[i]
00079 jac = firenze.Jac(arm, q)
00080 vel = jac * np.matrix(qdot).T
00081 cart_vel.append(vel.A1[0:3].tolist())
00082
00083 ct = CartesianTajectory()
00084 ct.time_list = copy.copy(traj.time_list)
00085 ct.p_list = copy.copy(pts)
00086 ct.v_list = copy.copy(cart_vel)
00087
00088 return ct
00089
00090 def plot_forces_quiver(pos_traj,force_traj,color='k'):
00091 import arm_trajectories as at
00092
00093 if isinstance(pos_traj,at.JointTrajectory):
00094 pos_traj = joint_to_cartesian(pos_traj)
00095
00096 pts = np.matrix(pos_traj.p_list).T
00097 label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
00098 x = pts[0,:].A1.tolist()
00099 y = pts[1,:].A1.tolist()
00100
00101 forces = np.matrix(force_traj.f_list).T
00102 u = (-1*forces[0,:]).A1.tolist()
00103 v = (-1*forces[1,:]).A1.tolist()
00104 pl.quiver(x,y,u,v,width=0.002,color=color,scale=100.0)
00105
00106 pl.axis('equal')
00107
00108
00109
00110
00111 def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_',
00112 linewidth=2, scatter_size=10, plot_velocity=False):
00113 import matplotlib_util.util as mpu
00114 import arm_trajectories as at
00115
00116 if isinstance(traj,at.JointTrajectory):
00117 traj = joint_to_cartesian(traj)
00118
00119 pts = np.matrix(traj.p_list).T
00120 label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
00121 x = pts[xaxis,:].A1.tolist()
00122 y = pts[yaxis,:].A1.tolist()
00123
00124 if plot_velocity:
00125 vels = np.matrix(traj.v_list).T
00126 xvel = vels[xaxis,:].A1.tolist()
00127 yvel = vels[yaxis,:].A1.tolist()
00128
00129 if zaxis == None:
00130 mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label,
00131 axis = 'equal', xlabel = label_list[xaxis],
00132 ylabel = label_list[yaxis],)
00133 if plot_velocity:
00134 mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]),
00135 width = 0.001, scale = 1.)
00136 mpu.legend()
00137 else:
00138 from numpy import array
00139 from enthought.mayavi.api import Engine
00140 engine = Engine()
00141 engine.start()
00142 if len(engine.scenes) == 0:
00143 engine.new_scene()
00144
00145 z = pts[zaxis,:].A1.tolist()
00146 time_list = [t-traj.time_list[0] for t in traj.time_list]
00147 mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
00148 mlab.axes()
00149 mlab.xlabel(label_list[xaxis])
00150 mlab.ylabel(label_list[yaxis])
00151 mlab.zlabel(label_list[zaxis])
00152 mlab.colorbar(title='Time')
00153
00154
00155 axes = engine.scenes[0].children[0].children[0].children[1]
00156 axes.axes.position = array([ 0., 0.])
00157 axes.axes.label_format = '%-#6.2g'
00158 axes.title_text_property.font_size=4
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168 def compute_radial_tangential_forces(f_list,p_list,cx,cy):
00169 f_rad_l,f_tan_l, f_res_l = [], [], []
00170 for f,p in zip(f_list,p_list):
00171 rad_vec = np.array([p[0]-cx,p[1]-cy])
00172 rad_vec = rad_vec/np.linalg.norm(rad_vec)
00173 tan_vec = (np.matrix([[0,-1],[1,0]]) * np.matrix(rad_vec).T).A1
00174 f_vec = np.array([f[0],f[1]])
00175
00176 f_tan_mag = np.dot(f_vec, tan_vec)
00177 f_rad_mag = np.dot(f_vec, rad_vec)
00178
00179
00180 f_rad_mag = abs(f_rad_mag)
00181 f_tan_mag = abs(f_tan_mag)
00182
00183 f_rad_l.append(f_rad_mag)
00184 f_tan_l.append(f_tan_mag)
00185 f_res_l.append(abs(f[2]))
00186
00187 return f_rad_l, f_tan_l, f_res_l
00188
00189
00190
00191 def fit_circle_priors(rad_guess, x_guess, y_guess, pts, sigma_r,
00192 sigma_xy, sigma_pts, verbose=True):
00193 global x_prior, y_prior
00194 x_prior = x_guess
00195 y_prior = y_guess
00196 def error_function(params):
00197 center = np.matrix((params[0],params[1])).T
00198 rad = params[2]
00199 err_pts = ut.norm(pts-center).A1 - rad
00200 lik = np.dot(err_pts, err_pts) / (sigma_pts * sigma_pts)
00201 pri = ((rad - rad_guess)**2) / (sigma_r * sigma_r)
00202
00203
00204 return (lik + pri)
00205
00206 params_1 = [x_prior, y_prior, rad_guess]
00207 r = so.fmin_bfgs(error_function, params_1, full_output=1,
00208 disp = verbose, gtol=1e-5)
00209 opt_params_1,fopt_1 = r[0],r[1]
00210
00211 y_prior = y_guess + 2*rad_guess
00212 params_2 = [x_prior, y_prior, rad_guess]
00213 r = so.fmin_bfgs(error_function, params_2, full_output=1,
00214 disp = verbose, gtol=1e-5)
00215 opt_params_2,fopt_2 = r[0],r[1]
00216
00217 if fopt_2<fopt_1:
00218 return opt_params_2[2],opt_params_2[0],opt_params_2[1]
00219 else:
00220 return opt_params_1[2],opt_params_1[0],opt_params_1[1]
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233 def fit_circle(rad_guess, x_guess, y_guess, pts, method, verbose=True,
00234 rad_fix = False):
00235 def error_function(params):
00236 center = np.matrix((params[0],params[1])).T
00237 if rad_fix:
00238 rad = rad_guess
00239 else:
00240 rad = params[2]
00241
00242 err = ut.norm(pts-center).A1 - rad
00243 res = np.dot(err,err)
00244
00245
00246 return res
00247
00248 params_1 = [x_guess,y_guess]
00249 if not rad_fix:
00250 params_1.append(rad_guess)
00251 if method == 'fmin':
00252 r = so.fmin(error_function,params_1,xtol=0.0002,ftol=0.000001,full_output=1,disp=verbose)
00253 opt_params_1,fopt_1 = r[0],r[1]
00254 elif method == 'fmin_bfgs':
00255 r = so.fmin_bfgs(error_function, params_1, full_output=1,
00256 disp = verbose, gtol=1e-5)
00257 opt_params_1,fopt_1 = r[0],r[1]
00258 else:
00259 raise RuntimeError('unknown method: '+method)
00260
00261 params_2 = [x_guess,y_guess+2*rad_guess]
00262 if not rad_fix:
00263 params_2.append(rad_guess)
00264 if method == 'fmin':
00265 r = so.fmin(error_function,params_2,xtol=0.0002,ftol=0.000001,full_output=1,disp=verbose)
00266 opt_params_2,fopt_2 = r[0],r[1]
00267 elif method == 'fmin_bfgs':
00268 r = so.fmin_bfgs(error_function, params_2, full_output=1,
00269 disp = verbose, gtol=1e-5)
00270 opt_params_2,fopt_2 = r[0],r[1]
00271 else:
00272 raise RuntimeError('unknown method: '+method)
00273
00274 if fopt_2<fopt_1:
00275 if rad_fix:
00276 return rad_guess,opt_params_2[0],opt_params_2[1]
00277 else:
00278 return opt_params_2[2],opt_params_2[0],opt_params_2[1]
00279 else:
00280 if rad_fix:
00281 return rad_guess,opt_params_1[0],opt_params_1[1]
00282 else:
00283 return opt_params_1[2],opt_params_1[0],opt_params_1[1]
00284
00285
00286
00287
00288
00289
00290
00291
00292 def account_segway_motion(cart_traj, force_traj, st):
00293 ct = CartesianTajectory()
00294 ft = ForceTrajectory()
00295 for i in range(len(cart_traj.p_list)):
00296 x,y,a = st.x_list[i], st.y_list[i], st.a_list[i]
00297 p_tl = np.matrix(cart_traj.p_list[i]).T
00298 p_ts = smc.tsTtl(p_tl, x, y, a)
00299 p = p_ts
00300 ct.p_list.append(p.A1.tolist())
00301
00302
00303 f_tl = np.matrix(force_traj.f_list[i]).T
00304 f_ts = smc.tsRtl(f_tl, a)
00305 f = f_ts
00306 ft.f_list.append(f.A1.tolist())
00307
00308
00309
00310
00311 if cart_traj.v_list != []:
00312 v_tl = np.matrix(cart_traj.v_list[i]).T
00313 v_ts = smc.tsRtl(v_tl, a)
00314 ct.v_list.append(v_ts.A1.tolist())
00315
00316 ct.time_list = copy.copy(cart_traj.time_list)
00317 ft.time_list = copy.copy(force_traj.time_list)
00318 return ct, ft
00319
00320
00321
00322
00323 def account_zenithering(cart_traj, z_l):
00324 ct = CartesianTajectory()
00325 h_start = z_l[0]
00326 for i in range(len(cart_traj.p_list)):
00327 h = z_l[i]
00328 p = cart_traj.p_list[i]
00329 p[2] += h - h_start
00330 ct.p_list.append(p)
00331
00332
00333
00334
00335 if cart_traj.v_list != []:
00336 ct.v_list.append(cart_traj.v_list[i])
00337
00338 ct.time_list = copy.copy(cart_traj.time_list)
00339 return ct
00340
00341
00342
00343
00344
00345 def filter_cartesian_trajectory(ct):
00346 pts_list = ct.p_list
00347 ee_start_pos = pts_list[0]
00348 l = [pts_list[0]]
00349
00350 for i, p in enumerate(pts_list[1:]):
00351 l.append(p)
00352 pts_2d = (np.matrix(l).T)[0:2,:]
00353 st_pt = pts_2d[:,0]
00354 end_pt = pts_2d[:,-1]
00355 dist_moved = np.linalg.norm(st_pt-end_pt)
00356
00357 if dist_moved < 0.03:
00358 reject_idx = i
00359
00360 pts_2d = pts_2d[:,reject_idx:]
00361 return pts_2d, reject_idx
00362
00363
00364
00365
00366
00367
00368 def filter_trajectory_force(ct, ft):
00369 vel_list = copy.copy(ct.v_list)
00370 pts_list = copy.copy(ct.p_list)
00371 time_list = copy.copy(ct.time_list)
00372 ft_list = copy.copy(ft.f_list)
00373 f_mag_list = ut.norm(np.matrix(ft.f_list).T).A1.tolist()
00374
00375 if len(pts_list) != len(f_mag_list):
00376 print 'arm_trajectories.filter_trajectory_force: force and end effector lists are not of the same length.'
00377 print 'Exiting ...'
00378 sys.exit()
00379
00380 n_pts = len(pts_list)
00381 i = n_pts - 1
00382 hook_slip_off_threshold = 1.5
00383 while i > 0:
00384 if f_mag_list[i] < hook_slip_off_threshold:
00385 pts_list.pop()
00386 time_list.pop()
00387 ft_list.pop()
00388 if vel_list != []:
00389 vel_list.pop()
00390 else:
00391 break
00392 i -= 1
00393
00394 ct2 = CartesianTajectory()
00395 ct2.time_list = time_list
00396 ct2.p_list = pts_list
00397 ct2.v_list = vel_list
00398
00399 ft2 = ForceTrajectory()
00400 ft2.time_list = copy.copy(time_list)
00401 ft2.f_list = ft_list
00402 return ct2, ft2
00403
00404
00405 if __name__ == '__main__':
00406 import matplotlib_util.util as mpu
00407
00408 p = optparse.OptionParser()
00409 p.add_option('-f', action='store', type='string', dest='fname',
00410 help='pkl file to use.', default='')
00411 p.add_option('--xy', action='store_true', dest='xy',
00412 help='plot the x and y coordinates of the end effector.')
00413 p.add_option('--yz', action='store_true', dest='yz',
00414 help='plot the y and z coordinates of the end effector.')
00415 p.add_option('--xz', action='store_true', dest='xz',
00416 help='plot the x and z coordinates of the end effector.')
00417 p.add_option('--plot_ellipses', action='store_true', dest='plot_ellipses',
00418 help='plot the stiffness ellipse in the x-y plane')
00419 p.add_option('--pfc', action='store_true', dest='pfc',
00420 help='plot the radial and tangential components of the force.')
00421 p.add_option('--pff', action='store_true', dest='pff',
00422 help='plot the force field corresponding to a stiffness ellipse.')
00423 p.add_option('--pev', action='store_true', dest='pev',
00424 help='plot the stiffness ellipses for different combinations of the rel stiffnesses.')
00425 p.add_option('--plot_forces', action='store_true', dest='plot_forces',
00426 help='plot the force in the x-y plane')
00427 p.add_option('--plot_forces_error', action='store_true', dest='plot_forces_error',
00428 help='plot the error between the computed and measured (ATI) forces in the x-y plane')
00429 p.add_option('--xyz', action='store_true', dest='xyz',
00430 help='plot in 3d the coordinates of the end effector.')
00431 p.add_option('-r', action='store', type='float', dest='rad',
00432 help='radius of the joint.', default=None)
00433 p.add_option('--noshow', action='store_true', dest='noshow',
00434 help='do not display the figure (use while saving figures to disk)')
00435 p.add_option('--exptplot', action='store_true', dest='exptplot',
00436 help='put all the graphs of an experiment as subplots.')
00437 p.add_option('--sturm', action='store_true', dest='sturm',
00438 help='make log files to send to sturm')
00439 p.add_option('--icra_presentation_plot', action='store_true',
00440 dest='icra_presentation_plot',
00441 help='plot explaining CEP update.')
00442
00443 opt, args = p.parse_args()
00444 fname = opt.fname
00445 xy_flag = opt.xy
00446 yz_flag = opt.yz
00447 xz_flag = opt.xz
00448 plot_forces_flag = opt.plot_forces
00449 plot_ellipses_flag = opt.plot_ellipses
00450 plot_forces_error_flag = opt.plot_forces_error
00451 plot_force_components_flag = opt.pfc
00452 plot_force_field_flag = opt.pff
00453 xyz_flag = opt.xyz
00454 rad = opt.rad
00455 show_fig = not(opt.noshow)
00456 plot_ellipses_vary_flag = opt.pev
00457 expt_plot = opt.exptplot
00458 sturm_output = opt.sturm
00459
00460
00461 if plot_ellipses_vary_flag:
00462 show_fig=False
00463 i = 0
00464 ratio_list1 = [0.1,0.3,0.5,0.7,0.9]
00465 ratio_list2 = [0.1,0.3,0.5,0.7,0.9]
00466 ratio_list3 = [0.1,0.3,0.5,0.7,0.9]
00467
00468
00469
00470
00471
00472
00473
00474 inv_mean_list,std_list = [],[]
00475 x_l,y_l,z_l = [],[],[]
00476 s0 = 0.2
00477
00478 for s1 in ratio_list1:
00479 for s2 in ratio_list2:
00480 for s3 in ratio_list3:
00481 i += 1
00482 s_list = [s0,s1,s2,s3,0.8]
00483
00484 print '################## s_list:', s_list
00485 m,s = plot_stiff_ellipse_map(s_list,i)
00486 inv_mean_list.append(1./m)
00487 std_list.append(s)
00488 x_l.append(s1)
00489 y_l.append(s2)
00490 z_l.append(s3)
00491
00492 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},
00493 'stiff_dict_'+ut.formatted_time()+'.pkl')
00494 d3m.plot_points(np.matrix([x_l,y_l,z_l]),scalar_list=inv_mean_list,mode='sphere')
00495 mlab.axes()
00496 d3m.show()
00497
00498 sys.exit()
00499
00500 if fname=='':
00501 print 'please specify a pkl file (-f option)'
00502 print 'Exiting...'
00503 sys.exit()
00504
00505 d = ut.load_pickle(fname)
00506 actual_cartesian_tl = joint_to_cartesian(d['actual'], d['arm'])
00507 actual_cartesian, _ = account_segway_motion(actual_cartesian_tl,d['force'], d['segway'])
00508 if d.has_key('zenither_list'):
00509 actual_cartesian = account_zenithering(actual_cartesian,
00510 d['zenither_list'])
00511
00512 eq_cartesian_tl = joint_to_cartesian(d['eq_pt'], d['arm'])
00513 eq_cartesian, _ = account_segway_motion(eq_cartesian_tl, d['force'], d['segway'])
00514 if d.has_key('zenither_list'):
00515 eq_cartesian = account_zenithering(eq_cartesian, d['zenither_list'])
00516
00517 cartesian_force_clean, _ = filter_trajectory_force(actual_cartesian,
00518 d['force'])
00519 pts_2d, reject_idx = filter_cartesian_trajectory(cartesian_force_clean)
00520
00521 if rad != None:
00522
00523
00524
00525 pts_list = actual_cartesian.p_list
00526 eq_pts_list = eq_cartesian.p_list
00527 ee_start_pos = pts_list[0]
00528 x_guess = ee_start_pos[0]
00529 y_guess = ee_start_pos[1] - rad
00530 print 'before call to fit_rotary_joint'
00531 force_list = d['force'].f_list
00532
00533 if sturm_output:
00534 str_parts = fname.split('.')
00535 sturm_file_name = str_parts[0]+'_sturm.log'
00536 print 'Sturm file name:', sturm_file_name
00537 sturm_file = open(sturm_file_name,'w')
00538 sturm_pts = cartesian_force_clean.p_list
00539 print 'len(sturm_pts):', len(sturm_pts)
00540 print 'len(pts_list):', len(pts_list)
00541
00542 for i,p in enumerate(sturm_pts[1:]):
00543 sturm_file.write(" ".join(map(str,p)))
00544 sturm_file.write('\n')
00545
00546 sturm_file.write('\n')
00547 sturm_file.close()
00548
00549 rad_guess = rad
00550 rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
00551 method='fmin_bfgs',verbose=False)
00552 print 'rad, cx, cy:', rad, cx, cy
00553 c_ts = np.matrix([cx, cy, 0.]).T
00554 start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
00555 pts_2d[0,0]-cx) - math.pi/2)
00556 end_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,-1]-cy,
00557 pts_2d[0,-1]-cx) - math.pi/2)
00558 mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
00559 label='Actual\_opt', color='r')
00560
00561
00562 if opt.icra_presentation_plot:
00563 mpu.set_figure_size(30,20)
00564 rad = 1.0
00565 x_guess = pts_2d[0,0]
00566 y_guess = pts_2d[1,0] - rad
00567
00568 rad_guess = rad
00569 rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
00570 method='fmin_bfgs',verbose=False)
00571 print 'Estimated rad, cx, cy:', rad, cx, cy
00572
00573 start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
00574 pts_2d[0,0]-cx) - math.pi/2)
00575 end_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,-1]-cy,
00576 pts_2d[0,-1]-cx) - math.pi/2)
00577
00578 subsample_ratio = 1
00579 pts_2d_s = pts_2d[:,::subsample_ratio]
00580
00581 cep_force_clean, force_new = filter_trajectory_force(eq_cartesian,
00582 d['force'])
00583 cep_2d = np.matrix(cep_force_clean.p_list).T[0:2,reject_idx:]
00584
00585
00586 mpu.figure()
00587 mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b',
00588 label = '\huge{End Effector Trajectory}', axis = 'equal', alpha = 1.0,
00589 scatter_size=7, linewidth=0, marker='x',
00590 marker_edge_width = 1.5)
00591
00592 cep_2d_s = cep_2d[:,::subsample_ratio]
00593 mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
00594 label = '\huge{Equilibrium Point Trajectory}', axis = 'equal', alpha = 1.0,
00595 scatter_size=10, linewidth=0, marker='+',
00596 marker_edge_width = 1.5)
00597
00598 mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
00599 label='\huge{Estimated Kinematics}', color='r',
00600 alpha=0.7)
00601 mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
00602 interval=end_angle-start_angle, color='r',
00603 alpha=0.7)
00604 mpu.legend()
00605 fig_name = 'epc'
00606 fig_number = 1
00607 mpu.savefig(fig_name+'%d'%fig_number)
00608 fig_number += 1
00609
00610
00611
00612 zoom_location = 10
00613 pts_2d_zoom = pts_2d[:,:zoom_location]
00614 cep_2d_zoom = cep_2d[:,:zoom_location]
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635 mpu.figure()
00636 mpu.plot_yx(pts_2d_zoom[1,:].A1, pts_2d_zoom[0,:].A1, color='b',
00637 label = '\huge{End Effector Trajectory}', axis = 'equal', alpha = 1.0,
00638 scatter_size=7, linewidth=0, marker='x',
00639 marker_edge_width = 1.5)
00640 mpu.plot_yx(cep_2d_zoom[1,:].A1, cep_2d_zoom[0,:].A1, color='g',
00641 label = '\huge{Equilibrium Point Trajectory}', axis = 'equal', alpha = 1.0,
00642 scatter_size=10, linewidth=0, marker='+',
00643 marker_edge_width = 1.5)
00644 mpu.pl.xlim(0.28, 0.47)
00645 mpu.legend()
00646
00647 mpu.savefig(fig_name+'%d'%fig_number)
00648 fig_number += 1
00649
00650 rad, cx, cy = fit_circle(1.0,x_guess,y_guess,pts_2d_zoom,
00651 method='fmin_bfgs',verbose=False)
00652 print 'Estimated rad, cx, cy:', rad, cx, cy
00653 start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
00654 pts_2d[0,0]-cx) - math.pi/2)
00655 end_angle = tr.angle_within_mod180(math.atan2(pts_2d_zoom[1,-1]-cy,
00656 pts_2d_zoom[0,-1]-cx) - math.pi/2)
00657 mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
00658 label='\huge{Estimated Kinematics}', color='r',
00659 alpha=0.7)
00660 mpu.pl.xlim(0.28, 0.47)
00661 mpu.legend()
00662
00663 mpu.savefig(fig_name+'%d'%fig_number)
00664 fig_number += 1
00665
00666 current_pos = pts_2d_zoom[:,-1]
00667 radial_vec = current_pos - np.matrix([cx,cy]).T
00668 radial_vec = radial_vec / np.linalg.norm(radial_vec)
00669 tangential_vec = np.matrix([[0,-1],[1,0]]) * radial_vec
00670 mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
00671 [pts_2d_zoom[0,-1]],
00672 radial_vec, scale=10., width = 0.002)
00673 rad_text_loc = pts_2d_zoom[:,-1] + np.matrix([0.001,0.01]).T
00674 mpu.pl.text(rad_text_loc[0,0], rad_text_loc[1,0], '$\hat v_{rad}$', fontsize = 30)
00675 mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
00676 [pts_2d_zoom[0,-1]],
00677 tangential_vec, scale=10., width = 0.002)
00678
00679 tan_text_loc = pts_2d_zoom[:,-1] + np.matrix([-0.012, -0.011]).T
00680 mpu.pl.text(tan_text_loc[0,0], tan_text_loc[1,0], s = '$\hat v_{tan}$', fontsize = 30)
00681 mpu.pl.xlim(0.28, 0.47)
00682 mpu.legend()
00683
00684 mpu.savefig(fig_name+'%d'%fig_number)
00685 fig_number += 1
00686
00687 wrist_color = '#A0A000'
00688 wrist_force = -np.matrix(force_new.f_list[zoom_location]).T
00689 frad = (wrist_force[0:2,:].T * radial_vec)[0,0] * radial_vec
00690 mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
00691 [pts_2d_zoom[0,-1]],
00692 wrist_force, scale=50., width = 0.002,
00693 color=wrist_color)
00694
00695 wf_text = rad_text_loc + np.matrix([-0.06,0.015]).T
00696 mpu.pl.text(wf_text[0,0], wf_text[1,0], color=wrist_color,
00697 fontsize = 25, s = 'Total Force')
00698
00699 mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
00700 [pts_2d_zoom[0,-1]],
00701 frad, scale=50., width = 0.002,
00702 color=wrist_color)
00703 frad_text = rad_text_loc + np.matrix([0.,0.015]).T
00704 mpu.pl.text(frad_text[0,0], frad_text[1,0], color=wrist_color, s = '$\hat F_{rad}$', fontsize = 30)
00705
00706 mpu.pl.xlim(0.28, 0.47)
00707 mpu.legend()
00708
00709 mpu.savefig(fig_name+'%d'%fig_number)
00710 fig_number += 1
00711
00712 frad = (wrist_force[0:2,:].T * radial_vec)[0,0]
00713 hook_force_motion = -(frad - 5) * radial_vec * 0.001
00714 tangential_motion = 0.01 * tangential_vec
00715 total_cep_motion = hook_force_motion + tangential_motion
00716
00717 mpu.plot_quiver_yxv([cep_2d_zoom[1,-1]],
00718 [cep_2d_zoom[0,-1]],
00719 hook_force_motion, scale=0.1, width = 0.002)
00720 hw_text = cep_2d_zoom[:,-1] + np.matrix([-0.002,-0.012]).T
00721 mpu.pl.text(hw_text[0,0], hw_text[1,0], color='k', fontsize=20,
00722 s = '$h[t]$')
00723 mpu.pl.xlim(0.28, 0.47)
00724 mpu.legend()
00725
00726 mpu.savefig(fig_name+'%d'%fig_number)
00727 fig_number += 1
00728
00729 mpu.plot_quiver_yxv([cep_2d_zoom[1,-1]],
00730 [cep_2d_zoom[0,-1]],
00731 tangential_motion, scale=0.1, width = 0.002)
00732 mw_text = cep_2d_zoom[:,-1] + np.matrix([-0.018,0.001]).T
00733 mpu.pl.text(mw_text[0,0], mw_text[1,0], color='k', fontsize=20,
00734 s = '$m[t]$')
00735 mpu.pl.xlim(0.28, 0.47)
00736 mpu.legend()
00737
00738 mpu.savefig(fig_name+'%d'%fig_number)
00739 fig_number += 1
00740
00741 mpu.plot_quiver_yxv([cep_2d_zoom[1,-1]],
00742 [cep_2d_zoom[0,-1]],
00743 total_cep_motion, scale=0.1, width = 0.002)
00744 cep_text = cep_2d_zoom[:,-1] + np.matrix([-0.058,-0.023]).T
00745 mpu.pl.text(cep_text[0,0], cep_text[1,0], color='k', fontsize=20,
00746 s = '$x_{eq}[t]$ = &x_{eq}[t-1] + m[t] + h[t]$')
00747 mpu.pl.xlim(0.28, 0.47)
00748 mpu.legend()
00749
00750 mpu.savefig(fig_name+'%d'%fig_number)
00751 fig_number += 1
00752
00753 new_cep = cep_2d_zoom[:,-1] + total_cep_motion
00754 mpu.plot_yx(new_cep[1,:].A1, new_cep[0,:].A1, color='g',
00755 axis = 'equal', alpha = 1.0,
00756 scatter_size=10, linewidth=0, marker='+',
00757 marker_edge_width = 1.5)
00758 mpu.pl.xlim(0.28, 0.47)
00759 mpu.legend()
00760
00761 mpu.savefig(fig_name+'%d'%fig_number)
00762 fig_number += 1
00763
00764 mpu.show()
00765
00766 if xy_flag:
00767 st_pt = pts_2d[:,0]
00768 end_pt = pts_2d[:,-1]
00769
00770 if expt_plot:
00771 pl.subplot(233)
00772
00773 plot_cartesian(actual_cartesian, xaxis=0, yaxis=1, color='b',
00774 label='FK', plot_velocity=False)
00775 plot_cartesian(eq_cartesian, xaxis=0,yaxis=1,color='g',label='Eq Point')
00776
00777 elif yz_flag:
00778 plot_cartesian(actual_cartesian,xaxis=1,yaxis=2,color='b',label='FK')
00779 plot_cartesian(eq_cartesian, xaxis=1,yaxis=2,color='g',label='Eq Point')
00780 elif xz_flag:
00781 plot_cartesian(actual_cartesian,xaxis=0,yaxis=2,color='b',label='FK')
00782 plot_cartesian(eq_cartesian, xaxis=0,yaxis=2,color='g',label='Eq Point')
00783
00784
00785 if plot_forces_flag or plot_forces_error_flag or plot_ellipses_flag or plot_force_components_flag or plot_force_field_flag:
00786
00787
00788
00789
00790
00791 if plot_forces_flag:
00792 plot_forces_quiver(actual_cartesian,d['force'],color='k')
00793
00794
00795
00796 if plot_ellipses_flag:
00797
00798 if expt_plot:
00799 subplotnum=234
00800 else:
00801 pl.figure()
00802 subplotnum=111
00803 plot_stiff_ellipses(k_cart_list,eq_cartesian,subplotnum=subplotnum)
00804
00805 if plot_forces_error_flag:
00806 plot_error_forces(d['force'].f_list,ftraj_jinv.f_list)
00807
00808
00809 if plot_force_components_flag:
00810 p_list = actual_cartesian.p_list
00811
00812
00813 frad_list,ftan_list,_ = compute_radial_tangential_forces(d['force'].f_list,p_list,cx,cy)
00814
00815 if expt_plot:
00816 pl.subplot(235)
00817 else:
00818 pl.figure()
00819
00820 time_list = d['force'].time_list
00821 time_list = [t-time_list[0] for t in time_list]
00822 x_coord_list = np.matrix(p_list)[:,0].A1.tolist()
00823 mpu.plot_yx(frad_list,x_coord_list,scatter_size=50,color=time_list,cb_label='time',axis=None)
00824 pl.xlabel('x coord of end effector (m)')
00825 pl.ylabel('magnitude of radial force (N)')
00826 pl.title(d['info'])
00827 if expt_plot:
00828 pl.subplot(236)
00829 else:
00830 pl.figure()
00831 mpu.plot_yx(ftan_list,x_coord_list,scatter_size=50,color=time_list,cb_label='time',axis=None)
00832 pl.xlabel('x coord of end effector (m)')
00833 pl.ylabel('magnitude of tangential force (N)')
00834 pl.title(d['info'])
00835
00836 if plot_force_field_flag:
00837 plot_stiffness_field(k_cart_list[0],plottitle='start')
00838 plot_stiffness_field(k_cart_list[-1],plottitle='end')
00839
00840
00841 str_parts = fname.split('.')
00842 if d.has_key('strategy'):
00843 addon = ''
00844 if opt.xy:
00845 addon = '_xy'
00846 if opt.xz:
00847 addon = '_xz'
00848 fig_name = str_parts[0]+'_'+d['strategy']+addon+'.png'
00849 else:
00850 fig_name = str_parts[0]+'_res.png'
00851
00852 if expt_plot:
00853 f = pl.gcf()
00854 curr_size = f.get_size_inches()
00855 f.set_size_inches(curr_size[0]*2,curr_size[1]*2)
00856 f.savefig(fig_name)
00857
00858 if show_fig:
00859 pl.show()
00860 else:
00861 print '################################'
00862 print 'show_fig is FALSE'
00863 if not(expt_plot):
00864 pl.savefig(fig_name)
00865
00866 if xyz_flag:
00867 plot_cartesian(traj, xaxis=0,yaxis=1,zaxis=2)
00868 mlab.show()
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995
00996
00997
00998
00999
01000
01001
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123
01124
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137
01138
01139
01140
01141
01142
01143
01144