def get_firm_hook(self, hook_angle): rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90)) # move in the +x until contact. vec = np.matrix([0.08,0.,0.]).T self.firenze.move_till_hit('right_arm',vec=vec,force_threshold=2.0,rot=rot_mat, speed=0.05) # now move in direction of hook. vec = tr.rotX(-hook_angle) * np.matrix([0.,0.05,0.]).T self.firenze.move_till_hit('right_arm',vec=vec,force_threshold=5.0,rot=rot_mat, speed=0.05,bias_FT=False) self.firenze.set_arm_settings(self.settings_r,None) self.firenze.step()
def get_firm_hook(self, hook_angle): rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry( math.radians(-90)) # move in the +x until contact. vec = np.matrix([0.08, 0., 0.]).T self.firenze.move_till_hit('right_arm', vec=vec, force_threshold=2.0, rot=rot_mat, speed=0.05) # now move in direction of hook. vec = tr.rotX(-hook_angle) * np.matrix([0., 0.05, 0.]).T self.firenze.move_till_hit('right_arm', vec=vec, force_threshold=5.0, rot=rot_mat, speed=0.05, bias_FT=False) self.firenze.set_arm_settings(self.settings_r, None) self.firenze.step()
def pull(self,hook_angle,force_threshold,use_utm=False,use_camera=False,strategy='line_neg_x', pull_loc=None, info_string=''): ''' force_threshold - max force at which to stop pulling. hook_angle - radians(0, -90, 90 etc.) 0 - horizontal, -pi/2 hook points up, +pi/2 hook points down use_utm - to take 3D scans or not. use_camera - to take pictures from the camera or not. strategy - 'line_neg_x': move eq point along -x axis. 'piecewise_linear': try and estimate circle and move along it. 'control_radial_force': try and keep the radial force constant 'control_radial_dist' pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into gravity comp and user can show the location. info_string - string saved with key 'info' in the pkl. ''' if use_utm: self.firenze.step() armconfig1 = self.firenze.get_joint_angles('right_arm') plist1,slist1 = self.scan_3d() if use_camera: cam_plist1, cam_imlist1 = self.image_region() else: cam_plist1,cam_imlist1 = None,None rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90)) if pull_loc == None: self.pose_arm(hook_angle) pull_loc = self.firenze.end_effector_pos('right_arm') ut.save_pickle(pull_loc,'pull_loc_'+info_string+'_'+ut.formatted_time()+'.pkl') else: pt1 = copy.copy(pull_loc) pt1[0,0] = pt1[0,0]-0.1 print 'pt1:', pt1.A1 print 'pull_loc:', pull_loc.A1 self.firenze.go_cartesian('right_arm',pt1,rot_mat,speed=0.2) self.firenze.go_cartesian('right_arm',pull_loc,rot_mat,speed=0.07) print 'press ENTER to pull' k=m3t.get_keystroke() if k != '\r': return time_dict = {} time_dict['before_hook'] = time.time() print 'first getting a good hook' self.get_firm_hook(hook_angle) time.sleep(0.5) time_dict['before_pull'] = time.time() print 'pull begins' stiffness_scale = self.settings_r.stiffness_scale vec = tr.rotX(-hook_angle) * np.matrix([0.,0.05/stiffness_scale,0.]).T self.keep_hook_vec = vec self.hook_maintain_dist_plane = np.dot(vec.A1,np.array([0.,1.,0.])) self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec q_eq = self.firenze.IK('right_arm',self.eq_pt_cartesian,rot_mat) self.firenze.go_jointangles('right_arm',q_eq,speed=math.radians(30)) self.q_guess = q_eq # self.q_guess = self.firenze.get_joint_angles('right_arm') self.pull_trajectory = at.JointTrajectory() self.jt_torque_trajectory = at.JointTrajectory() self.eq_pt_trajectory = at.JointTrajectory() self.force_trajectory = at.ForceTrajectory() self.firenze.step() start_config = self.firenze.get_joint_angles('right_arm') self.eq_IK_rot_mat = rot_mat # for equi pt generators. self.eq_force_threshold = force_threshold self.hooked_location_moved = False # flag to indicate when the hooking location started moving. self.prev_force_mag = np.linalg.norm(self.firenze.get_wrist_force('right_arm')) self.eq_motion_vec = np.matrix([-1.,0.,0.]).T self.slip_count = 0 if strategy == 'line_neg_x': result = self.compliant_motion(self.equi_pt_generator_line,0.025) elif strategy == 'control_radial_force': self.cartesian_pts_list = [] self.piecewise_force_threshold = force_threshold self.rad_guess = 1.0 self.cx = 0.6 self.cy = -self.rad_guess self.start() # start the circle estimation thread result = self.compliant_motion(self.equi_pt_generator_control_radial_force,0.025) else: raise RuntimeError('unknown pull strategy: ', strategy) if result == 'slip: force step decrease' or result == 'danger of self collision': self.firenze.motors_off() print 'powered off the motors.' print 'Compliant motion result:', result print 'Original force threshold:', force_threshold print 'Adapted force threshold:', self.eq_force_threshold time_dict['after_pull'] = time.time() d = {'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory, 'force': self.force_trajectory, 'torque': self.jt_torque_trajectory, 'stiffness': self.firenze.arm_settings['right_arm'], 'info': info_string, 'force_threshold': force_threshold, 'eq_force_threshold': self.eq_force_threshold, 'hook_angle':hook_angle, 'result':result,'strategy':strategy,'time_dict':time_dict} self.firenze.step() armconfig2 = self.firenze.get_joint_angles('right_arm') if use_utm: plist2,slist2 = self.scan_3d() d['start_config']=start_config d['armconfig1']=armconfig1 d['armconfig2']=armconfig2 d['l1'],d['l2']=0.,-0.055 d['scanlist1'],d['poslist1']=slist1,plist1 d['scanlist2'],d['poslist2']=slist2,plist2 d['cam_plist1']=cam_plist1 d['cam_imlist1']=cam_imlist1 ut.save_pickle(d,'pull_trajectories_'+d['info']+'_'+ut.formatted_time()+'.pkl')
def pull(self, hook_angle, force_threshold, use_utm=False, use_camera=False, strategy='line_neg_x', pull_loc=None, info_string=''): ''' force_threshold - max force at which to stop pulling. hook_angle - radians(0, -90, 90 etc.) 0 - horizontal, -pi/2 hook points up, +pi/2 hook points down use_utm - to take 3D scans or not. use_camera - to take pictures from the camera or not. strategy - 'line_neg_x': move eq point along -x axis. 'piecewise_linear': try and estimate circle and move along it. 'control_radial_force': try and keep the radial force constant 'control_radial_dist' pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into gravity comp and user can show the location. info_string - string saved with key 'info' in the pkl. ''' if use_utm: self.firenze.step() armconfig1 = self.firenze.get_joint_angles('right_arm') plist1, slist1 = self.scan_3d() if use_camera: cam_plist1, cam_imlist1 = self.image_region() else: cam_plist1, cam_imlist1 = None, None rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry( math.radians(-90)) if pull_loc == None: self.pose_arm(hook_angle) pull_loc = self.firenze.end_effector_pos('right_arm') ut.save_pickle( pull_loc, 'pull_loc_' + info_string + '_' + ut.formatted_time() + '.pkl') else: pt1 = copy.copy(pull_loc) pt1[0, 0] = pt1[0, 0] - 0.1 print 'pt1:', pt1.A1 print 'pull_loc:', pull_loc.A1 self.firenze.go_cartesian('right_arm', pt1, rot_mat, speed=0.2) self.firenze.go_cartesian('right_arm', pull_loc, rot_mat, speed=0.07) print 'press ENTER to pull' k = m3t.get_keystroke() if k != '\r': return time_dict = {} time_dict['before_hook'] = time.time() print 'first getting a good hook' self.get_firm_hook(hook_angle) time.sleep(0.5) time_dict['before_pull'] = time.time() print 'pull begins' stiffness_scale = self.settings_r.stiffness_scale vec = tr.rotX(-hook_angle) * np.matrix( [0., 0.05 / stiffness_scale, 0.]).T self.keep_hook_vec = vec self.hook_maintain_dist_plane = np.dot(vec.A1, np.array([0., 1., 0.])) self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec q_eq = self.firenze.IK('right_arm', self.eq_pt_cartesian, rot_mat) self.firenze.go_jointangles('right_arm', q_eq, speed=math.radians(30)) self.q_guess = q_eq # self.q_guess = self.firenze.get_joint_angles('right_arm') self.pull_trajectory = at.JointTrajectory() self.jt_torque_trajectory = at.JointTrajectory() self.eq_pt_trajectory = at.JointTrajectory() self.force_trajectory = at.ForceTrajectory() self.firenze.step() start_config = self.firenze.get_joint_angles('right_arm') self.eq_IK_rot_mat = rot_mat # for equi pt generators. self.eq_force_threshold = force_threshold self.hooked_location_moved = False # flag to indicate when the hooking location started moving. self.prev_force_mag = np.linalg.norm( self.firenze.get_wrist_force('right_arm')) self.eq_motion_vec = np.matrix([-1., 0., 0.]).T self.slip_count = 0 if strategy == 'line_neg_x': result = self.compliant_motion(self.equi_pt_generator_line, 0.025) elif strategy == 'control_radial_force': self.cartesian_pts_list = [] self.piecewise_force_threshold = force_threshold self.rad_guess = 1.0 self.cx = 0.6 self.cy = -self.rad_guess self.start() # start the circle estimation thread result = self.compliant_motion( self.equi_pt_generator_control_radial_force, 0.025) else: raise RuntimeError('unknown pull strategy: ', strategy) if result == 'slip: force step decrease' or result == 'danger of self collision': self.firenze.motors_off() print 'powered off the motors.' print 'Compliant motion result:', result print 'Original force threshold:', force_threshold print 'Adapted force threshold:', self.eq_force_threshold time_dict['after_pull'] = time.time() d = { 'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory, 'force': self.force_trajectory, 'torque': self.jt_torque_trajectory, 'stiffness': self.firenze.arm_settings['right_arm'], 'info': info_string, 'force_threshold': force_threshold, 'eq_force_threshold': self.eq_force_threshold, 'hook_angle': hook_angle, 'result': result, 'strategy': strategy, 'time_dict': time_dict } self.firenze.step() armconfig2 = self.firenze.get_joint_angles('right_arm') if use_utm: plist2, slist2 = self.scan_3d() d['start_config'] = start_config d['armconfig1'] = armconfig1 d['armconfig2'] = armconfig2 d['l1'], d['l2'] = 0., -0.055 d['scanlist1'], d['poslist1'] = slist1, plist1 d['scanlist2'], d['poslist2'] = slist2, plist2 d['cam_plist1'] = cam_plist1 d['cam_imlist1'] = cam_imlist1 ut.save_pickle( d, 'pull_trajectories_' + d['info'] + '_' + ut.formatted_time() + '.pkl')