00001
00002 import roslib; roslib.load_manifest('hrl_pr2_kinematics')
00003 import rospy
00004
00005 from equilibrium_point_control.msg import MechanismKinematicsRot
00006 from equilibrium_point_control.msg import MechanismKinematicsJac
00007
00008 import epc
00009 from threading import RLock
00010
00011
00012
00013
00014
00015
00016
00017
00018 def rot_mat_from_angles(hook, surface):
00019 rot_mat = tr.Rz(hook) * tr.Rx(surface)
00020 return rot_mat
00021
00022
00023
00024 class Door_EPC(epc.EPC):
00025 def __init__(self, robot):
00026 epc.EPC.__init__(self, robot)
00027
00028 self.mech_kinematics_lock = RLock()
00029 self.fit_circle_lock = RLock()
00030
00031 rospy.Subscriber('mechanism_kinematics_rot',
00032 MechanismKinematicsRot,
00033 self.mechanism_kinematics_rot_cb)
00034 rospy.Subscriber('mechanism_kinematics_jac',
00035 MechanismKinematicsJac,
00036 self.mechanism_kinematics_jac_cb)
00037 self.mech_traj_pub = rospy.Publisher('mechanism_trajectory', Point32)
00038 self.eq_pt_not_moving_counter = 0
00039
00040
00041 def log_state(self, arm):
00042 t_now = rospy.get_time()
00043 q_now = self.robot.get_joint_angles(arm)
00044
00045 self.pull_trajectory.q_list.append(q_now)
00046 self.pull_trajectory.time_list.append(t_now)
00047
00048 self.eq_pt_trajectory.q_list.append(self.q_guess)
00049 self.eq_pt_trajectory.time_list.append(t_now)
00050
00051 wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
00052 self.force_trajectory.f_list.append(wrist_force.A1.tolist())
00053 self.force_trajectory.time_list.append(t_now)
00054 return ''
00055
00056
00057
00058
00059
00060
00061
00062 def update_eq_point(self, arm, motion_vec, step_size, rot_mat):
00063 self.eq_pt_cartesian = self.eq_pt_cartesian_ts
00064 next_pt = self.eq_pt_cartesian + step_size * motion_vec
00065 q_eq = self.robot.IK(arm, next_pt, rot_mat, self.q_guess)
00066 self.eq_pt_cartesian = next_pt
00067 self.eq_pt_cartesian_ts = self.eq_pt_cartesian
00068 self.q_guess = q_eq
00069 return q_eq
00070
00071 def common_stopping_conditions(self):
00072 stop = ''
00073 if self.q_guess == None:
00074 stop = 'IK fail'
00075
00076 wrist_force = self.robot.get_wrist_force('right_arm',base_frame=True)
00077 mag = np.linalg.norm(wrist_force)
00078 print 'force magnitude:', mag
00079 if mag > self.eq_force_threshold:
00080 stop = 'force exceed'
00081
00082 if mag < 1.2 and self.hooked_location_moved:
00083 if (self.prev_force_mag - mag) > 30.:
00084 stop = 'slip: force step decrease and below thresold.'
00085
00086 else:
00087 self.slip_count += 1
00088 else:
00089 self.slip_count = 0
00090
00091 if self.slip_count == 10:
00092 stop = 'slip: force below threshold for too long.'
00093 return stop
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 def eqpt_gen_control_radial_force(self, arm, rot_mat,
00104 h_force_possible, v_force_possible, cep_vel):
00105 self.log_state(arm)
00106 step_size = 0.1 * cep_vel
00107 q_eq = self.update_eq_point(arm, self.eq_motion_vec, step_size,
00108 rot_mat)
00109 stop = self.common_stopping_conditions()
00110
00111 wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
00112 mag = np.linalg.norm(wrist_force)
00113
00114 curr_pos = self.robot.FK(arm,self.pull_trajectory.q_list[-1])
00115 if len(self.pull_trajectory.q_list)>1:
00116 start_pos = np.matrix(self.cartesian_pts_list[0]).T
00117 else:
00118 start_pos = curr_pos
00119
00120
00121 self.mech_traj_pub.publish(Point32(curr_pos[0,0],
00122 curr_pos[1,0], curr_pos[2,0]))
00123
00124 if self.use_jacobian:
00125 self.mech_kinematics_lock.acquire()
00126
00127 self.cartesian_pts_list.append(curr_pos.A1.tolist())
00128 tangential_vec_ts = self.tangential_vec_ts
00129 force_vec_ts = self.constraint_vec_1_ts + self.constraint_vec_2_ts
00130
00131
00132
00133 if h_force_possible:
00134 force_vec_ts[2,0] = 0.
00135 if v_force_possible:
00136 force_vec_ts[1,0] = 0.
00137 force_vec_ts = force_vec_ts / np.linalg.norm(force_vec_ts)
00138 if force_vec_ts[2,0] < 0.:
00139 force_vec_ts = -force_vec_ts
00140 if force_vec_ts[1,0] < 0.:
00141 force_vec_ts = -force_vec_ts
00142
00143 self.mech_kinematics_lock.release()
00144
00145 force_vec = force_vec_ts
00146 tangential_vec = tangential_vec_ts
00147
00148 if self.use_rotation_center:
00149 self.fit_circle_lock.acquire()
00150 self.cartesian_pts_list.append(curr_pos.A1.tolist())
00151 rad = self.rad
00152 cx_start, cy_start = self.cx_start, self.cy_start
00153 cz_start = self.cz_start
00154 self.fit_circle_lock.release()
00155 cx, cy = cx_start, cy_start
00156 cz = cz_start
00157
00158 radial_vec = curr_pos_tl-np.matrix([cx,cy,cz]).T
00159 radial_vec = radial_vec/np.linalg.norm(radial_vec)
00160 if cy_start<start_pos[1,0]:
00161 tan_x,tan_y = -radial_vec[1,0],radial_vec[0,0]
00162 else:
00163 tan_x,tan_y = radial_vec[1,0],-radial_vec[0,0]
00164
00165 if tan_x > 0. and (start_pos[0,0]-curr_pos[0,0]) < 0.09:
00166 tan_x = -tan_x
00167 tan_y = -tan_y
00168
00169 if cy_start > start_pos[1,0]:
00170 radial_vec = -radial_vec
00171
00172 rv = radial_vec
00173 force_vec = np.matrix([rv[0,0], rv[1,0], 0.]).T
00174 tangential_vec = np.matrix([tan_x, tan_y, 0.]).T
00175
00176 tangential_vec_ts = tangential_vec
00177 radial_vec_ts = radial_vec
00178 force_vec_ts = force_vec
00179
00180 f_vec = -1*np.array([wrist_force[0,0], wrist_force[1,0],
00181 wrist_force[2,0]])
00182 f_rad_mag = np.dot(f_vec, force_vec.A1)
00183 err = f_rad_mag-5.
00184 if err>0.:
00185 kp = -0.1
00186 else:
00187 kp = -0.2
00188 radial_motion_mag = kp * err
00189
00190 if self.use_rotation_center:
00191 if h_force_possible == False and parallel_to_floor:
00192 radial_motion_mag = 0.
00193 if v_force_possible == False and parallel_to_floor == False:
00194 radial_motion_mag = 0.
00195
00196 radial_motion_vec = force_vec * radial_motion_mag
00197 self.eq_motion_vec = copy.copy(tangential_vec)
00198 self.eq_motion_vec += radial_motion_vec
00199
00200 self.prev_force_mag = mag
00201
00202 if self.init_tangent_vector == None:
00203 self.init_tangent_vector = copy.copy(tangential_vec_ts)
00204 c = np.dot(tangential_vec_ts.A1, self.init_tangent_vector.A1)
00205 ang = np.arccos(c)
00206
00207 dist_moved = np.dot((curr_pos - start_pos).A1, self.tangential_vec_ts.A1)
00208 ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
00209 if abs(dist_moved) > 0.09 and self.hooked_location_moved == False:
00210
00211 self.hooked_location_moved = True
00212 self.eq_force_threshold = ut.bound(mag+30.,20.,80.)
00213 self.ftan_threshold = self.ftan_threshold + max(10., 1.5*ftan)
00214 if self.hooked_location_moved:
00215 if abs(tangential_vec_ts[2,0]) < 0.2 and ftan > self.ftan_threshold:
00216 stop = 'ftan threshold exceed: %f'%ftan
00217 else:
00218 self.ftan_threshold = max(self.ftan_threshold, ftan)
00219
00220
00221 if self.hooked_location_moved and ang > math.radians(90.):
00222 print 'Angle:', math.degrees(ang)
00223 self.open_ang_exceed_count += 1
00224 if self.open_ang_exceed_count > 2:
00225 stop = 'opened mechanism through large angle: %.1f'%(math.degrees(ang))
00226 else:
00227 self.open_ang_exceed_count = 0
00228
00229 self.mech_time_list.append(time.time())
00230 self.mech_x_list.append(ang)
00231 self.f_rad_list.append(f_rad_mag)
00232 self.f_tan_list.append(np.dot(f_vec, tangential_vec.A1))
00233 self.tan_vec_list.append(tangential_vec_ts.A1.tolist())
00234 self.rad_vec_list.append(force_vec_ts.A1.tolist())
00235
00236 return stop, q_eq
00237
00238 def pull(self, arm, hook_angle, force_threshold, ea,
00239 kinematics_estimation = 'rotation_center', pull_left = False):
00240
00241 self.init_tangent_vector = None
00242 self.open_ang_exceed_count = 0.
00243 if kinematics_estimation == 'rotation_center':
00244 self.use_rotation_center = True
00245 else:
00246 self.use_rotation_center = False
00247
00248 if kinematics_estimation == 'jacobian':
00249 self.use_jacobian = True
00250 else:
00251 self.use_jacobian = False
00252
00253
00254 rot_mat = rot_mat_from_angles(hook_angle, surface_angle)
00255
00256 time_dict = {}
00257 time_dict['before_pull'] = time.time()
00258
00259 self.pull_trajectory = at.JointTrajectory()
00260 self.eq_pt_trajectory = at.JointTrajectory()
00261 self.force_trajectory = at.ForceTrajectory()
00262
00263 self.robot.step()
00264 start_config = self.robot.get_joint_angles(arm)
00265
00266 self.eq_force_threshold = force_threshold
00267 self.ftan_threshold = 2.
00268 self.hooked_location_moved = False
00269 self.prev_force_mag = np.linalg.norm(self.robot.get_wrist_force(arm))
00270 self.eq_motion_vec = np.matrix([-1.,0.,0.]).T
00271 self.slip_count = 0
00272
00273 self.eq_pt_cartesian = self.robot.FK(arm, ea)
00274 self.eq_pt_cartesian_ts = self.robot.FK(arm, ea)
00275 self.start_pos = copy.copy(self.eq_pt_cartesian)
00276 self.q_guess = ea
00277
00278 if not pull_left:
00279 self.tangential_vec_ts = np.matrix([-1.,0.,0.]).T
00280 self.constraint_vec_2_ts = np.matrix([0.,0.,1.]).T
00281 self.constraint_vec_1_ts = np.matrix([0.,1.,0.]).T
00282 else:
00283 self.tangential_vec_ts = np.matrix([0.,1.,0.]).T
00284 self.constraint_vec_2_ts = np.matrix([0.,0.,1.]).T
00285 self.constraint_vec_1_ts = np.matrix([1.,0.,0.]).T
00286
00287 self.mech_time_list = []
00288 self.mech_x_list = []
00289 self.f_rad_list = []
00290 self.f_tan_list = []
00291 self.tan_vec_list = []
00292 self.rad_vec_list = []
00293
00294 self.cartesian_pts_list = []
00295 ee_pos = self.robot.end_effector_pos(arm)
00296
00297 if self.use_rotation_center:
00298
00299
00300
00301 self.cx_start = ee_pos[0,0]
00302 self.cy_start = ee_pos[1,0]-1.0
00303 self.cz_start = ee_pos[2,0]
00304 self.rad = 5.0
00305
00306 h_force_possible = abs(hook_angle) < math.radians(30.)
00307 v_force_possible = abs(hook_angle) > math.radians(60.)
00308 arg_list = [arm, rot_mat, h_force_possible, v_force_possible, cep_vel]
00309 result, jep = self.epc_motion(self.eqpt_gen_control_radial_force,
00310 0.1, arm, arg_list, self.log_state)
00311
00312 print 'EPC motion result:', result
00313 print 'Original force threshold:', force_threshold
00314 print 'Adapted force threshold:', self.eq_force_threshold
00315 print 'Adapted ftan threshold:', self.ftan_threshold
00316
00317 time_dict['after_pull'] = time.time()
00318
00319 d = {'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory,
00320 'force': self.force_trajectory, 'torque': self.jt_torque_trajectory,
00321 'info': info_string, 'force_threshold': force_threshold,
00322 'eq_force_threshold': self.eq_force_threshold, 'hook_angle':hook_angle,
00323 'result':result, 'time_dict':time_dict,
00324 'cep_vel': cep_vel,
00325 'ftan_threshold': self.ftan_threshold}
00326
00327 self.robot.step()
00328
00329 ut.save_pickle(d,'pull_trajectories_'+d['info']+'_'+ut.formatted_time()+'.pkl')
00330
00331 dd = {'mechanism_x': self.mech_x_list,
00332 'mechanism_time': self.mech_time_list,
00333 'force_rad_list': self.f_rad_list,
00334 'force_tan_list': self.f_tan_list,
00335 'tan_vec_list': self.tan_vec_list,
00336 'rad_vec_list': self.rad_vec_list
00337 }
00338 ut.save_pickle(dd,'mechanism_trajectories_robot_'+d['info']+'_'+ut.formatted_time()+'.pkl')
00339
00340
00341
00342
00343
00344
00345
00346
00347 def search_and_hook(self, arm, hook_angle, hook_loc, angle,
00348 hooking_force_threshold = 5.):
00349
00350 rot_mat = rot_mat_from_angles(hook_angle, angle)
00351 if arm == 'right_arm':
00352 hook_dir = np.matrix([0.,1.,0.]).T
00353 elif arm == 'left_arm':
00354 hook_dir = np.matrix([0.,-1.,0.]).T
00355 else:
00356 raise RuntimeError('Unknown arm: %s', arm)
00357 start_loc = hook_loc + rot_mat.T * hook_dir * -0.03
00358
00359
00360 normal_tl = tr.Rz(-angle) * np.matrix([1.0,0.,0.]).T
00361
00362 pt1 = start_loc - normal_tl * 0.1
00363 pt1[2,0] -= 0.02
00364 self.robot.go_cartesian(arm, pt1, rot_mat, speed=0.2)
00365
00366 vec = normal_tl * 0.2
00367 s, jep = self.firenze.move_till_hit(arm, vec=vec, force_threshold=2.0,
00368 rot=rot_mat, speed=0.07)
00369
00370 self.eq_pt_cartesian = self.firenze.FK(arm, jep)
00371 self.eq_pt_cartesian_ts = self.firenze.FK(arm, jep)
00372 self.start_pos = copy.copy(self.eq_pt_cartesian)
00373 self.q_guess = jep
00374 move_dir = rot_mat.T * hook_dir
00375
00376 arg_list = [arm, move_dir, rot_mat, hooking_force_threshold]
00377 s, jep = self.compliant_motion(self.equi_generator_surface_follow, 0.05,
00378 arm, arg_list)
00379 return s, jep
00380
00381
00382 if __name__ == '__main__':
00383
00384 print 'Hello World'
00385
00386