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')
