arm_trajectories.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 # 
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 # 
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 # Author: Advait Jain
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 #import util as ut
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 #import shapely.geometry as sg
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 = [] # time in seconds
00055         self.q_list = [] #each element is a list of 7 joint angles.
00056         self.qdot_list = [] #each element is a list of 7 joint angles.
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 = [] # time in seconds
00064         self.p_list = [] #each element is a list of 3 coordinates
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 = [] # time in seconds
00072         self.f_list = [] #each element is a list of 3 coordinates
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     #return np.matrix(pts).T
00089     return ct
00090 
00091 
00092 def plot_forces_quiver(pos_traj,force_traj,color='k'):
00093     import arm_trajectories as at
00094     #if traj.__class__ == at.JointTrajectory:
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 #    pl.quiver(x,y,u,v,width=0.002,color=color)
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     #if traj.__class__ == at.JointTrajectory:
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 ## compute the force that the arm would apply given the stiffness matrix
00157 # @param  q_actual_traj - Joint Trajectory (actual angles.)
00158 # @param  q_eq_traj - Joint Trajectory (equilibrium point angles.)
00159 # @param torque_traj - JointTrajectory (torques measured at the joints.)
00160 # @param rel_stiffness_list - list of 5 elements (stiffness numbers for the joints.)
00161 # @return lots of things, look at the code.
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 #    stiffness_settings = [0.15,0.7,0.8,0.8,0.8]
00171 #    dia = np.array(stiffness_settings) * np.array(stiff_list_Nm_rad)
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))    # calculating stiff matrix using Jacobian for eq pt.
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         #force = (-1*pseudo_inv_jac*tau_full)[0:3]
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 ## return two lists containing the radial and tangential components of the forces.
00213 # @param f_list - list of forces. (each force is a list of 2 or 3 floats)
00214 # @param p_list - list of positions. (each position is a list of 2 or 3 floats)
00215 # @param cx - x coord of the center of the circle.
00216 # @param cy - y coord of the center of the circle.
00217 # @return list of magnitude of radial component, list of magnitude tangential component.
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         #err_rel_x.append(ut.bound(abs(m[0]-c[0])/abs(m[0])*100,100,0))
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         #err_rel_y.append(ut.bound(abs(m[1]-c[1])/abs(m[1])*100,100,0))
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     #pl.legend(loc='best')
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 #    pl.figure()
00294 #    pl.plot(zero,c='k',linewidth=0.5,label='_nolegend_')
00295 #    pl.plot(err_y,c='b',linewidth=1,label='error')
00296 #    pl.plot(err_rel_y,c='r',linewidth=1,label='relative error %')
00297 #    pl.scatter(x_idx,err_y,c='b',s=10,label='_nolegend_', linewidths=0)
00298 #    pl.plot(mag_y,c='g',linewidth=1,label='magnitude')
00299 #    pl.scatter(x_idx,mag_y,c='g',s=10,label='_nolegend_', linewidths=0)
00300 #
00301 #    pl.xlabel('measurement number')
00302 #    pl.ylabel('y component of force (N)')
00303 #    pl.legend(loc='best')
00304 #    pl.axis('equal')
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 #        print 'k_c:', k_c
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     #fig = pl.figure()
00335     #ax = fig.add_subplot(111, aspect='equal')
00336     ax = pl.subplot(subplotnum, aspect='equal')
00337 
00338     for e in ells:
00339         ax.add_artist(e)
00340         #e.set_clip_box(ax.bbox)
00341         #e.set_alpha(1.)
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 #    plot_cartesian(pos_traj,xaxis=0,yaxis=1,color='b',label='Eq Point',
00347 #                   linewidth=1.5,scatter_size=0)
00348 #    plot_cartesian(d['actual'],xaxis=0,yaxis=1,color='b',label='FK',
00349 #                   linewidth=1.5,scatter_size=0)
00350 #    plot_cartesian(d['eq_pt'], xaxis=0,yaxis=1,color='g',label='Eq Point',
00351 #                   linewidth=1.5,scatter_size=0)
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 # plot the force field in the xy plane for the stiffness matrix k_cart.
00360 ## @param k_cart: 3x3 cartesian space stiffness matrix.
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 #    mpu.plot_circle(0,0,1.0,0.,math.radians(360))
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     #for z in np.arange(-0.1,-0.36,-0.05):
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 #    hook_3dprint_angle = math.radians(20-2.54)
00440 #    rot_mat = tr.Rz(math.radians(-90.)-hook_3dprint_angle)*tr.Ry(math.radians(-90))
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 # search along z coord and make a histogram of the areas
00473 def compute_workspace_z():
00474     n_points_list,area_list,z_list = [],[],[]
00475     #for z in np.arange(-0.1,-0.36,-0.02):
00476     #for z in np.arange(-0.05,-0.35,-0.01):
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 #        hull,n_points = compute_workspace(z,plot=False)
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 #    pl.legend(loc='best')
00500     pl.xlabel('Z coordinate (m)')
00501     pl.ylabel('# points')
00502     pl.savefig('hist_points.png')
00503 
00504 
00505 
00506 
00507 ## find the x and y coord of the center of the circle of given radius that
00508 # best matches the data.
00509 # @param rad - radius of the circle (not optimized)
00510 # @param x_guess - guess for x coord of center
00511 # @param y_guess - guess for y coord of center.
00512 # @param pts - 2xN np matrix of points.
00513 # @return x,y  (x and y coord of the center of the circle)
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         #print 'pts.shape', pts.shape
00518         #print 'center.shape', center.shape
00519         #print 'ut.norm(pts-center).shape',ut.norm(pts-center).shape
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 ## find the x and y coord of the center of the circle and the radius that
00538 # best matches the data.
00539 # @param rad_guess - guess for the radius of the circle
00540 # @param x_guess - guess for x coord of center
00541 # @param y_guess - guess for y coord of center.
00542 # @param pts - 2xN np matrix of points.
00543 # @param method - optimization method. ('fmin' or 'fmin_bfgs')
00544 # @param verbose - passed onto the scipy optimize functions. whether to print out the convergence info.
00545 # @return r,x,y  (radius, x and y coord of the center of the circle)
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         #print 'pts.shape', pts.shape
00551         #print 'center.shape', center.shape
00552         #print 'ut.norm(pts-center).shape',ut.norm(pts-center).shape
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 #        hull = compute_workspace(z=-0.22,plot=True)
00644 #        pl.show()
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] # coarse search
00650         ratio_list2 = [0.1,0.3,0.5,0.7,0.9] # coarse search
00651         ratio_list3 = [0.1,0.3,0.5,0.7,0.9] # coarse search
00652 #        ratio_list1 = [0.7,0.8,0.9,1.0]
00653 #        ratio_list2 = [0.7,0.8,0.9,1.0]
00654 #        ratio_list3 = [0.3,0.4,0.5,0.6,0.7]
00655 #        ratio_list1 = [1.0,2.,3.0]
00656 #        ratio_list2 = [1.,2.,3.]
00657 #        ratio_list3 = [0.3,0.4,0.5,0.6,0.7]
00658 
00659         inv_mean_list,std_list = [],[]
00660         x_l,y_l,z_l = [],[],[]
00661         s0 = 0.2
00662         #s0 = 0.4
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                     #s_list = [s1,s2,s3,s0,0.8]
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         #rad = 0.39 # lab cabinet recessed.
00700         #rad = 0.42 # kitchen cabinet
00701         #rad = 0.80 # lab glass door
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         # transform points so that the mechanism is in a fixed position.
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 #        if rad<0.6:
00806 #            mpu.plot_radii(cx,cy,rad,start_angle,end_angle,interval=math.radians(100),color='r')
00807 #        pl.title(d['info'])
00808         leg = pl.legend(loc='best')#,handletextsep=0.020,handlelen=0.003,labelspacing=0.003)
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             #plot_forces_quiver(actual_cartesian,ftraj_stiff,color='y')
00838 
00839         if plot_ellipses_flag:
00840             #plot_stiff_ellipses(k_cart_list,actual_cartesian)
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             #plot_error_forces(d['force'].f_list,ftraj_stiff.f_list)
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 


2009_humanoids_epc_pull
Author(s): Advait Jain, Charles C. Kemp (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 12:05:07