00001 import roslib; roslib.load_manifest('modeling_forces')
00002 import os, sys
00003
00004
00005 import hrl_lib.util as ut
00006 import hrl_lib.transforms as tr
00007 import hrl_lib.data_process as dp
00008 import hrl_lib.data_display as dd
00009
00010 import numpy as np
00011 import pylab as pb
00012 import math
00013 import copy
00014 import pdb
00015 import smooth
00016 from scipy import interpolate
00017
00018 filter = dp.filter
00019 gradient = dp.gradient
00020 interpolate_1d = dp.interpolate_1d
00021 histogram_get_bin_numb = dp.histogram_get_bin_numb
00022 histogram = dp.histogram
00023 equalize_times = dp.equalize_times
00024 random_color = dd.random_color
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 def gradient(t, x):
00035 dx = x[:, 2:] - x[:, 0:-2]
00036 dt = t[0, 2:] - t[0, 0:-2]
00037 dx_dt = np.multiply(dx, 1/dt)
00038
00039
00040
00041 return dx_dt
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 def velocity(kin_info, smooth_window):
00062 mech_time_mat = np.matrix(kin_info['mech_time_arr'])
00063 tstep_size = .03333333333
00064 num_el = mech_time_mat.shape[1]
00065 uniform_time2 = np.cumsum(np.round((mech_time_mat[0,1:] - mech_time_mat[0,0:-1]) / tstep_size) * tstep_size)
00066 uniform_time2 = np.column_stack((np.matrix([0]), uniform_time2))
00067
00068 mech_intrinsic_poses = kin_info['disp_mech_coord_arr']
00069 if uniform_time2.shape[1] != mech_intrinsic_poses.shape[0]:
00070 pdb.set_trace()
00071
00072 vel = gradient(uniform_time2, np.matrix(mech_intrinsic_poses))
00073 return smooth.smooth(vel.A1, smooth_window, 'blackman')
00074
00075
00076
00077
00078 def kinematic_params(mech_x_l, time_list, smooth_window):
00079 mech_time_mat = np.matrix(time_list)
00080 tstep_size = .03333333333
00081 num_el = mech_time_mat.shape[1]
00082 uniform_time2 = np.cumsum(np.round((mech_time_mat[0,1:] - mech_time_mat[0,0:-1]) / tstep_size) * tstep_size)
00083 uniform_time2 = np.column_stack((np.matrix([0]), uniform_time2))
00084
00085 mech_x_mat = np.matrix(mech_x_l)
00086 if uniform_time2.shape[1] != mech_x_mat.shape[1]:
00087 pdb.set_trace()
00088
00089 mech_x_mat = np.matrix(smooth.smooth(mech_x_mat.A1, smooth_window,
00090 'blackman'))
00091 uniform_time2 = uniform_time2[:,smooth_window-1:-smooth_window+1]
00092
00093 vel = gradient(uniform_time2, mech_x_mat)
00094 uniform_time2 = uniform_time2[:,1:-1]
00095
00096 vel = np.matrix(smooth.smooth(vel.A1, smooth_window, 'blackman'))
00097 mech_x_mat = mech_x_mat[:,smooth_window-1:-smooth_window+1]
00098 uniform_time2 = uniform_time2[:,smooth_window-1:-smooth_window+1]
00099
00100 acc = gradient(uniform_time2, vel)
00101 uniform_time2 = uniform_time2[:,1:-1]
00102 vel = vel[:,1:-1]
00103 mech_x_mat = mech_x_mat[:,2:-2]
00104
00105 acc = np.matrix(smooth.smooth(acc.A1, smooth_window, 'blackman'))
00106 vel = vel[:,smooth_window-1:-smooth_window+1]
00107 mech_x_mat = mech_x_mat[:,smooth_window-1:-smooth_window+1]
00108 uniform_time2 = uniform_time2[:,smooth_window-1:-smooth_window+1]
00109
00110 return mech_x_mat.A1.tolist(), vel.A1.tolist(), acc.A1.tolist(), uniform_time2.A1.tolist()
00111
00112 def interpolate_1d(x, y, xquery):
00113 try:
00114 minx = np.min(x)
00115 minx_query = np.min(xquery)
00116
00117 maxx = np.max(x)
00118 maxx_querry = np.max(xquery)
00119
00120 if minx_query <= minx:
00121 x = np.concatenate((np.array([minx_query-.01]), x))
00122 y = np.concatenate((np.array([y[0]]), y))
00123
00124 if maxx <= maxx_querry:
00125 x = np.concatenate((x, np.array([maxx_querry+.01])))
00126 y = np.concatenate((y, np.array([y[-1]])))
00127
00128 f = interpolate.interp1d(x, y)
00129 return f(xquery)
00130 except ValueError, e:
00131 pdb.set_trace()
00132 print e
00133
00134 def calculate_derived_quantities(ler_result, smooth_window):
00135 exp_records = ler_result['records']
00136 mech_info = ler_result['mech_info']
00137 kin_info_list = ler_result['kin_info_list']
00138
00139
00140 for kin_info in kin_info_list:
00141 kin_info['velocity'] = velocity(kin_info, smooth_window)
00142
00143
00144 for segment_name in exp_records.keys():
00145 for exp_number, record in enumerate(exp_records[segment_name]):
00146 record['mech_coord_arr'] = interpolate_1d(kin_info_list[exp_number]['mech_time_arr'],
00147 kin_info_list[exp_number]['disp_mech_coord_arr'],
00148 record['ftimes'])
00149 record['mech_pose_vel_arr'] = interpolate_1d(kin_info_list[exp_number]['mech_time_arr'],
00150 kin_info_list[exp_number]['velocity'],
00151 record['ftimes'])
00152 record['force_norms'] = ut.norm(np.matrix(record['forces']).T[0:3,:]).A1
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186 def segment_as_dict(segments):
00187 a = {}
00188 for s in segments:
00189 s, e, name = s
00190 a[name] = [s,e]
00191 return a
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 def rotary_angles(mechanism_rotations, radius):
00216 directions_x = (np.row_stack(mechanism_rotations)[:,0]).T.reshape(len(mechanism_rotations), 3).T
00217 start_normal = directions_x[:,0]
00218 ra = np.arccos(start_normal.T * directions_x).A1
00219 if np.any(np.isnan(ra)):
00220 ra[np.where(np.isnan(ra))] = 0.0
00221
00222
00223 return ra.tolist()
00224
00225
00226
00227 def linear_distances(mechanism_positions):
00228 pos_mat = np.column_stack(mechanism_positions)
00229 ld = ut.norm(pos_mat - pos_mat[:,0]).A1.tolist()
00230 ld_arr = np.array(ld)
00231 if np.any(np.isnan(ld_arr)):
00232 pdb.set_trace()
00233 return ld
00234
00235
00236
00237
00238
00239
00240 def ft_to_camera(force_tool, hand_rot_matrix, number):
00241
00242 hc_rot_tool = tr.Rx(math.radians(90)) * tr.Ry(math.radians(180.)) * tr.Rz(math.radians(30.))
00243
00244 while number != 1:
00245 hc_rot_tool = tr.Ry(math.radians(90.)) * hc_rot_tool
00246 number = number-1
00247
00248 force_hc = hc_rot_tool * force_tool
00249 return hand_rot_matrix * force_hc
00250
00251
00252
00253
00254 def fts_to_camera(combined_dict):
00255 cd = combined_dict
00256 number = cd['hook_checker_number']
00257 hand_rot = cd['hand_rot_list']
00258 ft_mat = np.matrix(cd['ft_list']).T
00259 force_mat = ft_mat[0:3, :]
00260
00261 n_forces = force_mat.shape[1]
00262 force_cam_list = []
00263 for i in range(n_forces):
00264 force_cam_list.append(ft_to_camera(force_mat[:,i],
00265 hand_rot[i], number))
00266 force_cam = np.column_stack(force_cam_list)
00267 return force_cam
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278 def decompose_forces_and_recover_mechanism_state(d, type='rotary'):
00279 if not d.has_key('force_tan_list'):
00280 actual_cartesian = end_effector_poses_from_log(d)
00281 radius, center = find_circle_center_from_log(d, actual_cartesian)
00282 frad_list, ftan_list = at.compute_radial_tangential_forces(d['force'].f_list, actual_cartesian.p_list, center[0,0], center[1,0])
00283 if type == 'rotary':
00284 poses_list = rotary_mechanism_configurations_from_global_poses((np.matrix(actual_cartesian.p_list).T)[0:2,:] , center)
00285 elif type == 'prismatic':
00286 poses_list = linear_mechanism_configurations_from_global_poses((np.matrix(actual_cartesian.p_list).T)[0:2,:] , center)
00287 else:
00288 print 'decompose_forces_and_recover_mechanism_state: detected NEW dictionary!'
00289
00290 frad_list, ftan_list = d['force_rad_list'], d['force_tan_list']
00291 poses_list = d['mechanism_x']
00292
00293 return poses_list, frad_list, ftan_list
00294
00295
00296 def compute_segway_motion_mag(d):
00297 st = d['segway']
00298 x_list, y_list = st.x_list, st.y_list
00299 n1 = ut.norm(np.matrix([x_list, y_list]))
00300 n1 = n1 - np.min(n1)
00301 n1 = n1*10
00302 return n1.A1.tolist()
00303
00304 def linear_mechanism_configurations_from_global_poses(pts_2d, center):
00305 st_pt = pts_2d[:,0]
00306 poses_list = []
00307 for pos_idx in range(pts_2d.shape[1]):
00308 end_pt = pts_2d[:, pos_idx]
00309 end_displacement = np.linalg.norm(end_pt - st_pt)
00310 poses_list.append(end_displacement)
00311 return poses_list
00312
00313 def rotary_mechanism_configurations_from_global_poses(pts_2d, center):
00314 st_pt = pts_2d[:,0]
00315 cx = center[0,0]
00316 cy = center[1,0]
00317 start_angle = tr.angle_within_mod180(math.atan2(st_pt[1,0]-cy, st_pt[0,0]-cx) - math.radians(90))
00318 poses_list = []
00319 for pos_idx in range(pts_2d.shape[1]):
00320 end_pt = pts_2d[:, pos_idx]
00321 end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0]-cy, end_pt[0,0]-cx) - math.radians(90))
00322 poses_list.append(end_angle)
00323 return poses_list
00324
00325
00326
00327
00328
00329 def end_effector_poses_from_log(d):
00330 actual_cartesian_tl = at.joint_to_cartesian(d['actual'])
00331 actual_cartesian = at.account_segway_motion(actual_cartesian_tl, d['segway'])
00332 return actual_cartesian
00333
00334
00335
00336
00337
00338 def find_circle_center_from_log(d, actual_cartesian, plot=False):
00339 pts_list = actual_cartesian.p_list
00340 pts_2d = (np.matrix(pts_list).T)[0:2,:]
00341
00342 ee_start_pos = pts_list[0]
00343 rad_guess = 1.0
00344 x_guess = ee_start_pos[0]
00345 y_guess = ee_start_pos[1] - rad_guess
00346
00347 rad_opt, cx, cy = at.fit_circle(rad_guess, x_guess, y_guess, pts_2d, method='fmin_bfgs', verbose=False)
00348
00349 if plot:
00350 print 'rad_opt', rad_opt
00351 print 'cx', cx
00352 print 'cy', cy
00353 pb.figure(1)
00354 pb.plot(pts_2d[0,:].tolist()[0], pts_2d[1,:].tolist()[0], 'b-')
00355 pb.axis('equal')
00356 pb.plot([cx], [cy], 'ro')
00357 pb.show()
00358
00359 return rad_opt, np.matrix([cx, cy]).T
00360
00361
00362
00363 def bin(poses_list, ftan_list):
00364 poses_list = copy.copy(poses_list)
00365 bin_size = math.radians(2.)
00366 max_dist = max(poses_list)
00367 poses_array = np.arange(0., max_dist, bin_size)
00368 ftan_array = np.zeros(len(poses_array))
00369
00370 for i in range(len(poses_list)):
00371 p = poses_list[i]
00372 f = ftan_list[i]
00373 idx = int(p/bin_size)
00374 if abs(f) > abs(ftan_array[idx]):
00375 ftan_array[idx] = f
00376
00377 plist = poses_array.tolist()
00378
00379 return poses_array.tolist(), ftan_array.tolist()
00380
00381
00382 if __name__ == '__main__':
00383 pdb.set_trace()
00384 a = np.arange(0, 3, .1)
00385
00386 bins, ranges = histogram([a], [[a]], .2)
00387 pdb.set_trace()
00388 print 'done'
00389