def normals_cb(normals_cloud):
print "normals_cb got called."
d = {}
t0 = time.time()
pts = ros_pts_to_np(normals_cloud.pts)
t1 = time.time()
print "time to go from ROS point cloud to np matrx:", t1 - t0
d["pts"] = pts
if normals_cloud.chan[0].name != "nx":
print "################################################################################"
print "synthetic_point_clouds.normals_cloud: DANGER DANGER normals_cloud.chan[0] is NOT nx, it is:", normals_cloud.chan[
0
].name
print "Exiting..."
print "################################################################################"
sys.exit()
normals_list = []
for i in range(3):
normals_list.append(normals_cloud.chan[i].vals)
d["normals"] = np.matrix(normals_list)
d["curvature"] = normals_cloud.chan[3].vals
print "d['pts'].shape:", d["pts"].shape
print "d['normals'].shape:", d["normals"].shape
ut.save_pickle(d, "normals_cloud_" + ut.formatted_time() + ".pkl")
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0., 0., 0., 0., 0.],
control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True, left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True, right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,
ut.formatted_time() + '_good_configs_list.pkl')
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True,left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True,right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def normals_cb(normals_cloud):
print 'normals_cb got called.'
d = {}
t0 = time.time()
pts = ros_pts_to_np(normals_cloud.pts)
t1 = time.time()
print 'time to go from ROS point cloud to np matrx:', t1 - t0
d['pts'] = pts
if normals_cloud.chan[0].name != 'nx':
print '################################################################################'
print 'synthetic_point_clouds.normals_cloud: DANGER DANGER normals_cloud.chan[0] is NOT nx, it is:', normals_cloud.chan[
0].name
print 'Exiting...'
print '################################################################################'
sys.exit()
normals_list = []
for i in range(3):
normals_list.append(normals_cloud.chan[i].vals)
d['normals'] = np.matrix(normals_list)
d['curvature'] = normals_cloud.chan[3].vals
print 'd[\'pts\'].shape:', d['pts'].shape
print 'd[\'normals\'].shape:', d['normals'].shape
ut.save_pickle(d, 'normals_cloud_' + ut.formatted_time() + '.pkl')
def record_initial(firenze):
equilibrium_pos_list = []
for i in range(50):
equilibrium_pos_list.append(firenze.end_effector_pos('right_arm'))
eq_pos = np.column_stack(equilibrium_pos_list).mean(1)
ut.save_pickle(eq_pos,'eq_pos_'+ut.formatted_time()+'.pkl')
firenze.bias_wrist_ft('right_arm')
def record_initial(firenze):
equilibrium_pos_list = []
for i in range(50):
equilibrium_pos_list.append(firenze.end_effector_pos('right_arm'))
eq_pos = np.column_stack(equilibrium_pos_list).mean(1)
ut.save_pickle(eq_pos, 'eq_pos_' + ut.formatted_time() + '.pkl')
firenze.bias_wrist_ft('right_arm')
def create_dict(fname):
firenze = cak.CodyArmKinematics('r')
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK(gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dic = {'cart_pts_mat':m, 'good_configs_list':good_configs_list}
ut.save_pickle(dic,ut.formatted_time()+'_goodconf_dict.pkl')
def pull(self, arm, force_threshold, cep_vel, mechanism_type=''):
self.mechanism_type = mechanism_type
self.stopping_string = ''
self.eq_pt_not_moving_counter = 0
self.init_log()
self.init_tangent_vector = None
self.open_ang_exceed_count = 0.
self.eq_force_threshold = force_threshold
self.ftan_threshold = 2.
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(self.robot.get_wrist_force(arm))
self.slip_count = 0
self.started_pulling_on_handle = False
self.started_pulling_on_handle_count = 0
ee_pos, _ = self.robot.get_ee_jtt(arm)
self.cx_start = ee_pos[0, 0]
self.rad = 10.0
self.cy_start = ee_pos[1, 0] - self.rad
self.cz_start = ee_pos[2, 0]
cep, _ = self.robot.get_cep_jtt(arm)
arg_list = [arm, cep, cep_vel]
result, _ = self.epc_motion(
self.cep_gen_control_radial_force,
0.1,
arm,
arg_list,
self.log_state,
#0.01, arm, arg_list,
control_function=self.robot.set_cep_jtt)
print 'EPC motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
print 'Adapted ftan threshold:', self.ftan_threshold
d = {
'f_list': self.f_list,
'ee_list': self.ee_list,
'cep_list': self.cep_list,
'ftan_list': self.ft.tangential_force,
'config_list': self.ft.configuration,
'frad_list': self.ft.radial_force,
'f_list_ati': self.f_list_ati,
'f_list_estimate': self.f_list_estimate,
'f_list_torques': self.f_list_torques
}
ut.save_pickle(d, 'pr2_pull_' + ut.formatted_time() + '.pkl')
def create_dict(fname):
firenze = ar.M3HrlRobot(connect=False)
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK('right_arm', gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dict = {'cart_pts_mat': m, 'good_configs_list': good_configs_list}
ut.save_pickle(dict, ut.formatted_time() + '_goodconf_dict.pkl')
def save(self):
dict = {'datasets': self.datasets,'version': 0.1}
#for now: make a backup first:
database_filename = self.path+'/'+self.filename
backup_filename = self.path+'/'+self.filename+'_backup_'+ut.formatted_time()
print 'Backing up old database to ' + backup_filename
shutil.copy(database_filename, backup_filename)
print "Saving: "+database_filename
ut.save_pickle(dict,database_filename)
def create_dict(fname):
firenze = ar.M3HrlRobot(connect=False)
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK('right_arm',gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dict = {'cart_pts_mat':m, 'good_configs_list':good_configs_list}
ut.save_pickle(dict,ut.formatted_time()+'_goodconf_dict.pkl')
def create_workspace_dict():
dd = {}
ha_list = [math.radians(d) for d in [0.,90.,-90.]]
for ha in ha_list:
d = {}
for z in np.arange(-0.1,-0.55,-0.01):
print 'z:',z
pts2d = compute_workspace(z,hook_angle=ha)
d[z] = pts2d
dd[ha] = {}
dd[ha]['pts'] = d
ut.save_pickle(dd,'workspace_dict_'+ut.formatted_time()+'.pkl')
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm', base_frame=True))
ut.save_pickle({'pos_list':pos_list,'force_list':force_list},'stiffness_'+ut.formatted_time()+'.pkl')
firenze.stop()
def slot_import_image(self):
fileName = QtGui.QFileDialog.getOpenFileName(self,"Open Image", self.path, "Image Files (*.png)")
print "Import image into new dataset:" + fileName
name = ut.formatted_time()
new_dataset = scan_dataset.scan_dataset()
new_dataset.id = name
new_dataset.image_filename = 'data/'+name+'_image.png'
shutil.copy(fileName,self.path+'/'+new_dataset.image_filename)
self.scans_database.add_dataset(new_dataset)
#proceed to new dataset:
while True == self.slot_next_dataset():
pass
def pull(self, arm, force_threshold, cep_vel, mechanism_type=''):
self.mechanism_type = mechanism_type
self.stopping_string = ''
self.eq_pt_not_moving_counter = 0
self.init_log()
self.init_tangent_vector = None
self.open_ang_exceed_count = 0.
self.eq_force_threshold = force_threshold
self.ftan_threshold = 2.
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(self.robot.get_wrist_force(arm))
self.slip_count = 0
self.started_pulling_on_handle = False
self.started_pulling_on_handle_count = 0
ee_pos, _ = self.robot.get_ee_jtt(arm)
self.cx_start = ee_pos[0,0]
self.rad = 10.0
self.cy_start = ee_pos[1,0]-self.rad
self.cz_start = ee_pos[2,0]
cep, _ = self.robot.get_cep_jtt(arm)
arg_list = [arm, cep, cep_vel]
result, _ = self.epc_motion(self.cep_gen_control_radial_force,
0.1, arm, arg_list, self.log_state,
#0.01, arm, arg_list,
control_function = self.robot.set_cep_jtt)
print 'EPC motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
print 'Adapted ftan threshold:', self.ftan_threshold
d = {
'f_list': self.f_list, 'ee_list': self.ee_list,
'cep_list': self.cep_list, 'ftan_list': self.ft.tangential_force,
'config_list': self.ft.configuration, 'frad_list': self.ft.radial_force,
'f_list_ati': self.f_list_ati,
'f_list_estimate': self.f_list_estimate,
'f_list_torques': self.f_list_torques
}
ut.save_pickle(d,'pr2_pull_'+ut.formatted_time()+'.pkl')
def slot_import_image(self):
fileName = QtGui.QFileDialog.getOpenFileName(self, "Open Image",
self.path,
"Image Files (*.png)")
print "Import image into new dataset:" + fileName
name = ut.formatted_time()
new_dataset = scan_dataset.scan_dataset()
new_dataset.id = name
new_dataset.image_filename = 'data/' + name + '_image.png'
shutil.copy(fileName, self.path + '/' + new_dataset.image_filename)
self.scans_database.add_dataset(new_dataset)
#proceed to new dataset:
while True == self.slot_next_dataset():
pass
def test_IK(arm, rot_mat):
''' try out the IK at a number of different cartesian
points in the workspace, with the given rotation
matrix for the end effector.
'''
print 'press ENTER to start.'
k=m3t.get_keystroke()
while k=='\r':
p = firenze.end_effector_pos(arm)
firenze.go_cartesian(arm,p,rot_mat,speed=0.1)
firenze.step()
print 'press ENTER to save joint angles.'
k=m3t.get_keystroke()
if k == '\r':
firenze.step()
q = firenze.get_joint_angles(arm)
ut.save_pickle(q,'arm_config_'+ut.formatted_time()+'.pkl')
print 'press ENTER for next IK test. something else to exit.'
k=m3t.get_keystroke()
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm',
base_frame=True))
ut.save_pickle({
'pos_list': pos_list,
'force_list': force_list
}, 'stiffness_' + ut.formatted_time() + '.pkl')
firenze.stop()
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
import cody_arm_client as cac
if use_left_arm:
arm = 'l'
else:
arm = 'r'
firenze = cac.CodyArmClient(arm)
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def select_location(c, thok, angle):
thok.servo.move_angle(angle)
cvim = c.get_frame()
cvim = c.get_frame()
cvim = c.get_frame()
im_angle = thok.servo.read_angle()
tilt_angles = (math.radians(-20) + angle, math.radians(30) + angle)
pos_list, scan_list = thok.scan(tilt_angles, speed=math.radians(10))
m = p3d.generate_pointcloud(pos_list, scan_list, math.radians(-60),
math.radians(60), 0.0, -0.055)
pts = mcf.utmcam0Tglobal(mcf.globalTthok0(m), im_angle)
cam_params = cc.camera_parameters['mekabotUTM']
fx = cam_params['focal_length_x_in_pixels']
fy = cam_params['focal_length_y_in_pixels']
cx, cy = cam_params['optical_center_x_in_pixels'], cam_params[
'optical_center_y_in_pixels']
cam_proj_mat = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
cvim, pts2d = cul.project_points_in_image(cvim, pts, cam_proj_mat)
cp = cul.get_click_location(cvim)
print 'Clicked location:', cp
if cp == None:
return None
idx = cul.get_index(pts2d.T, cp)
pt3d = pts[:, idx]
pt_interest = mcf.globalTutmcam0(pt3d, im_angle)
hl_thok0 = mcf.thok0Tglobal(pt_interest)
l1, l2 = 0.0, -0.055
d = {}
d['pt'] = hl_thok0
d['pos_list'], d['scan_list'] = pos_list, scan_list
d['l1'], d['l2'] = l1, l2
d['img'] = uto.cv2np(cvim)
ut.save_pickle(d, 'hook_plane_scan_' + ut.formatted_time() + '.pkl')
return pt_interest
def slot_take_scan(self):
#save database, let scanner add dataset, reload it then
self.slot_save()
if False == self.scanner:
self.scanner = scanner.scanner(self.config)
if False == self.processor:
self.processor = processor.processor(self.config)
name = ut.formatted_time()
self.scanner.capture_and_save(name)
#self.processor.load_raw_data(name)
#self.processor.load_metadata(name)
#self.processor.process_raw_data()
#self.processor.save_mapped_image(name)
#self.processor.display_all_data()
print 'scan ' + name + ' taken'
self.scans_database.load(self.path, 'database.pkl')
#proceed to new scan:
while True == self.slot_next_dataset():
pass
def slot_take_scan(self):
#save database, let scanner add dataset, reload it then
self.slot_save()
if False == self.scanner:
self.scanner = scanner.scanner(self.config)
if False == self.processor:
self.processor = processor.processor(self.config)
name = ut.formatted_time()
self.scanner.capture_and_save(name)
#self.processor.load_raw_data(name)
#self.processor.load_metadata(name)
#self.processor.process_raw_data()
#self.processor.save_mapped_image(name)
#self.processor.display_all_data()
print 'scan ' + name + ' taken'
self.scans_database.load(self.path,'database.pkl')
#proceed to new scan:
while True == self.slot_next_dataset():
pass
max_dist = opt.max_dist
if pts_pkl_fname != None:
pts = ut.load_pickle(pts_pkl_fname)
elif dict_pkl_fname != None:
import tilting_hokuyo.processing_3d as p3d
dict = ut.load_pickle(dict_pkl_fname)
pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'],
math.radians(-max_pan_angle),
math.radians(max_pan_angle),
dict['l1'],dict['l2'],
min_tilt=math.radians(-90),max_tilt=math.radians(90))
else:
print 'Specify either a pts pkl or a dict pkl (-c or -f)'
print 'Exiting...'
sys.exit()
dist_mat = ut.norm(pts)
idxs = np.where(dist_mat<max_dist)[1]
print 'pts.shape', pts.shape
pts = pts[:,idxs.A1]
print 'pts.shape', pts.shape
if save_cloud_flag:
ut.save_pickle(pts,'numpy_pc_'+ut.formatted_time()+'.pkl')
plot_points(pts)
mlab.show()
logging_time = 5.
while (t_now - t_start) < logging_time:
ft, t_msg = client.read(fresh=True, with_time_stamp=True)
t_now = time.time()
if ft != None:
l.append(ft.A1.tolist())
time_list.append(t_msg)
time.sleep(1/1000.0)
print 'saving the pickle'
d = {}
d['ft_list'] = l
d['time_list'] = time_list
fname = 'ft_log_'+ut.formatted_time()+'.pkl'
ut.save_pickle(d, fname)
if opt.plot:
import matplotlib_util.util as mpu
if opt.fname == '' and (not opt.all):
raise RuntimeError('need a pkl name (-f or --fname) or --all')
if opt.all:
fname_list = glob.glob('ft_log*.pkl')
else:
fname_list = [opt.fname]
for fname in fname_list:
logging_time = 5.
while (t_now - t_start) < logging_time:
ft, t_msg = client.read(fresh=True, with_time_stamp=True)
t_now = time.time()
if ft != None:
l.append(ft.A1.tolist())
time_list.append(t_msg)
time.sleep(1 / 1000.0)
print 'saving the pickle'
d = {}
d['ft_list'] = l
d['time_list'] = time_list
fname = 'ft_log_' + ut.formatted_time() + '.pkl'
ut.save_pickle(d, fname)
if opt.plot:
import matplotlib_util.util as mpu
if opt.fname == '' and (not opt.all):
raise RuntimeError('need a pkl name (-f or --fname) or --all')
if opt.all:
fname_list = glob.glob('ft_log*.pkl')
else:
fname_list = [opt.fname]
for fname in fname_list:
d = ut.load_pickle(fname)
def scan(self, range, speed, save_scan=False, avg=1, _max_retries=0):
''' range - (start,end) in radians
speed - scan speed in radians/sec
save_scan - save a dict of pos_list,scan_list,l1,l2
avg - average scans from the hokuyo.
returns pos_list,scan_list. list of angles and HokuyoScans
'''
ramp_up_angle = math.radians(5)
if abs(range[0])+ramp_up_angle > math.radians(95) or \
abs(range[1])+ramp_up_angle > math.radians(95):
print 'tilt_hokuyo_servo.scan:bad angles- ', math.degrees(
range[0]), math.degrees(range[1])
min_angle = min(range[0], range[1])
max_angle = max(range[0], range[1])
# if max_angle>math.radians(60.5):
# print 'tilt_hokuyo_servo.scan: maximum angle is too high, will graze bottom plate of mount. angle:', math.degrees(max_angle)
# sys.exit()
self.servo.move_angle(range[0] + np.sign(range[0]) * ramp_up_angle)
# time.sleep(0.05)
# while(self.servo.is_moving()):
# continue
self.servo.move_angle(range[1] + np.sign(range[1]) * ramp_up_angle,
speed,
blocking=False)
#self.servo.move_angle(range[1], speed)
time.sleep(0.05)
t1 = time.time()
pos_list = []
scan_list = []
while self.servo.is_moving():
pos = self.servo.read_angle()
#print 'h6', pos
if pos < min_angle or pos > max_angle:
continue
pos_list.append(pos)
plane_scan = self.hokuyo.get_scan(avoid_duplicate=True,
remove_graze=True,
avg=avg)
scan_list.append(plane_scan)
t2 = time.time()
self.servo.move_angle(0)
if save_scan:
date_name = ut.formatted_time()
dict = {
'pos_list': pos_list,
'scan_list': scan_list,
'l1': self.l1,
'l2': self.l2
}
ut.save_pickle(dict, date_name + '_dict.pkl')
runtime = t2 - t1
expected_number_scans = 19.0 * runtime * (1.0 / avg)
scan_threshold = expected_number_scans - expected_number_scans * .2
if len(scan_list) < scan_threshold:
print 'tilt_hokuyo_servo.scan: WARNING! Expected at least %d scans but got only %d scans.' % (
expected_number_scans, len(scan_list))
print 'tilt_hokuyo_servo.scan: trying again.. retries:', _max_retries
if _max_retries > 0:
return self.scan(range,
speed,
save_scan,
avg,
_max_retries=_max_retries - 1)
else:
print 'tilt_hokuyo_servo.scan: returning anyway'
print 'tilt_hokuyo_servo.scan: got %d scans over range %f with speed %f.' % (
len(scan_list), (max_angle - min_angle), speed)
return pos_list, scan_list
def save_frame(self):
cvim = self.cam.get_frame()
cvSaveImage('im_'+ut.formatted_time()+'.png',cvim)
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')
log_images = True
t0 = time.time()
ft_pub.publish() # send trigger to the ft logger.
print "started logging"
im = cam.get_frame_debayered() # undistorting slows down frame rate
if log_images:
time_list.append(time.time())
im_list.append(cvCloneImage(im))
print "frame rate:", len(time_list) / (t1 - t0)
print "before saving the pkl"
d = {}
t_string = ut.formatted_time()
video_name = "mechanism_video_" + t_string + ".avi"
vwr = cvCreateVideoWriter(video_name, CV_FOURCC("I", "4", "2", "0"), 30, cvGetSize(im_list[0]), True)
t0 = time.time()
im_name_list = []
time_stamp = ut.formatted_time()
for im in im_list:
cvWriteFrame(vwr, im)
time.sleep(0.01) # Important to keep force torque server
# from restarting
t1 = time.time()
print "disk writing rate:", len(time_list) / (t1 - t0)
d["time_list"] = time_list
d["video_name"] = video_name
pygame.display.flip()
events = pygame.event.get()
for e in events:
if e.type==pygame.locals.QUIT:
loopFlag=False
if e.type==pygame.locals.KEYDOWN:
if e.key == 27: # Esc
loopFlag=False
if widget_clicked == False:
if e.type==pygame.locals.MOUSEMOTION:
if e.buttons[0] == 1:
# left button
org_x += e.rel[0]
org_y += e.rel[1]
if e.buttons[2] == 1:
# right button
MAX_DIST *= (1+e.rel[1]*0.01)
MAX_DIST = max(MAX_DIST,0.1)
# Try to keep the specified framerate
clk.tick(fps)
if test_show_change_flag:
ut.save_pickle(n_ch_pts_list,ut.formatted_time()+'_ch_pts_list.pkl')
max_dist = opt.max_dist
if pts_pkl_fname != None:
pts = ut.load_pickle(pts_pkl_fname)
elif dict_pkl_fname != None:
import tilting_hokuyo.processing_3d as p3d
dict = ut.load_pickle(dict_pkl_fname)
pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'],
math.radians(-max_pan_angle),
math.radians(max_pan_angle),
dict['l1'],dict['l2'],
min_tilt=math.radians(-90),max_tilt=math.radians(90))
else:
print 'Specify either a pts pkl or a dict pkl (-c or -f)'
print 'Exiting...'
sys.exit()
dist_mat = ut.norm(pts)
idxs = np.where(dist_mat<max_dist)[1]
print 'pts.shape', pts.shape
pts = pts[:,idxs.A1]
print 'pts.shape', pts.shape
if save_cloud_flag:
ut.save_pickle(pts,'numpy_pc_'+ut.formatted_time()+'.pkl')
plot_points(pts)
mlab.show()
def generate_pointcloud(pos_list,
scan_list,
min_angle,
max_angle,
l1,
l2,
save_scan=False,
max_dist=np.Inf,
min_dist=-np.Inf,
min_tilt=-np.Inf,
max_tilt=np.Inf,
get_intensities=False,
reject_zero_ten=True):
''' pos_list - list of motor positions (in steps)
scan_list - list of UrgScan objects at the corres positions.
l1,l2 - parameterizing the transformation (doc/ folder)
save_scan - pickle 3xN numpy matrix of points.
max_dist - ignore points at distance > max_dist
min_dist - ignore points at distance < max_dist
min_tilt, max_tilt - min and max tilt angles (radians)
+ve tilts the hokuyo down.
get_intensites - additionally return intensity values
returns 3xN numpy matrix of 3d coords of the points, returns (3xN, 1xN) including the intensity values if get_intensites=True
'''
t0 = time.time()
allpts = []
allintensities = []
pos_arr = np.array(pos_list)
scan_arr = np.array(scan_list)
idxs = np.where(np.multiply(pos_arr <= max_tilt, pos_arr >= min_tilt))
pos_list = pos_arr[idxs].tolist()
scan_list = scan_arr[idxs].tolist()
n_scans = len(scan_list)
if n_scans > 1:
scan_list = copy.deepcopy(scan_list)
# remove graze in across scans.
ranges_mat = []
for s in scan_list:
ranges_mat.append(s.ranges.T)
ranges_mat = np.column_stack(ranges_mat)
start_index = hp.angle_to_index(scan_list[0], min_angle)
end_index = hp.angle_to_index(scan_list[0], max_angle)
for r in ranges_mat[start_index:end_index + 1]:
hp.remove_graze_effect(r,
np.matrix(pos_list),
skip=1,
graze_angle_threshold=math.radians(169.))
for i, s in enumerate(scan_list):
s.ranges = ranges_mat[:, i].T
for p, s in zip(pos_list, scan_list):
mapxydata = hp.get_xy_map(
s,
min_angle,
max_angle,
max_dist=max_dist,
min_dist=min_dist,
reject_zero_ten=reject_zero_ten,
sigmoid_hack=True,
get_intensities=get_intensities) # pts is 2xN
if get_intensities == True:
pts, intensities = mapxydata
allintensities.append(intensities)
else:
pts = mapxydata
pts = np.row_stack((pts, np.zeros(pts.shape[1]))) # pts is now 3xN
pts = tr.Ry(-p) * (pts + np.matrix((l1, 0, -l2)).T)
allpts.append(pts)
allpts = np.column_stack(allpts)
if save_scan:
date_name = ut.formatted_time()
ut.save_pickle(allpts, date_name + '_cloud.pkl')
t1 = time.time()
# print 'Time to generate point cloud:', t1-t0
# allpts = tr.rotZ(math.radians(5))*allpts
if get_intensities == True:
allintensities = np.column_stack(allintensities)
return allpts, allintensities
else:
return allpts
def pull(self, arm, hook_angle, surface_angle, force_threshold, jep, use_utm=False,
use_camera=False, strategy='line_neg_x', info_string='',
cep_vel = 0.1, kinematics_estimation='rotation_center',
pull_left = False):
self.init_tangent_vector = None
self.open_ang_exceed_count = 0.
if kinematics_estimation == 'rotation_center':
self.use_rotation_center = True
else:
self.use_rotation_center = False
if kinematics_estimation == 'jacobian':
self.use_jacobian = True
else:
self.use_jacobian = False
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)*tr.Ry(math.radians(-90))
rot_mat = rot_mat_from_angles(hook_angle, surface_angle)
if self.move_segway_flag:
self.segway_pose.set_origin()
self.segway_command_node.set_max_velocity(0.15,0.1,math.radians(15))
time_dict = {}
time_dict['before_pull'] = time.time()
stiffness_scale = self.firenze.arm_settings[arm].stiffness_scale
self.pull_trajectory = at.JointTrajectory()
self.jt_torque_trajectory = at.JointTrajectory()
self.eq_pt_trajectory = at.JointTrajectory()
self.force_trajectory = at.ForceTrajectory()
self.torque_trajectory = at.ForceTrajectory()
self.firenze.step()
start_config = self.firenze.get_joint_angles(arm)
self.eq_force_threshold = force_threshold
self.ftan_threshold = 2.
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(arm))
self.eq_motion_vec = np.matrix([-1.,0.,0.]).T # might want to change this to account for the surface_angle.
self.slip_count = 0
self.eq_pt_cartesian = self.firenze.FK(arm, jep)
self.eq_pt_cartesian_ts = self.firenze.FK(arm, jep)
self.start_pos = copy.copy(self.eq_pt_cartesian)
self.q_guess = jep
if strategy == 'control_radial_force':
if not pull_left:
self.tangential_vec_ts = np.matrix([-1.,0.,0.]).T
self.constraint_vec_2_ts = np.matrix([0.,0.,1.]).T
self.constraint_vec_1_ts = np.matrix([0.,1.,0.]).T
else:
self.tangential_vec_ts = np.matrix([0.,1.,0.]).T
self.constraint_vec_2_ts = np.matrix([0.,0.,1.]).T
self.constraint_vec_1_ts = np.matrix([1.,0.,0.]).T
self.mech_time_list = []
self.mech_x_list = []
self.f_rad_list = []
self.f_tan_list = []
self.tan_vec_list = []
self.rad_vec_list = []
self.cartesian_pts_list = []
ee_pos = self.firenze.end_effector_pos(arm)
if self.use_rotation_center:
# this might have to change depending on left and right
# arm? or maybe not since the right arm can open both
# doors.
self.cx_start = ee_pos[0,0]
self.cy_start = ee_pos[1,0]-1.0
self.cz_start = ee_pos[2,0]
self.rad = 5.0
z = ee_pos[2,0]
print 'ee_pos[2,0]:',z
self.wkd = self.workspace_dict[hook_angle]
k = self.wkd['pts'].keys()
k_idx = np.argmin(np.abs(np.array(k)-z))
self.wrkspc_pts = self.wkd['pts'][k[k_idx]]
self.bndry = self.wkd['bndry'][k[k_idx]]
#self.bndry = smc.compute_boundary(self.wrkspc_pts)
# thread.start_new_thread(self.circle_estimator,())
if self.move_segway_flag:
thread.start_new_thread(self.segway_mover,())
self.segway_command_node.set_max_velocity(0.1,0.1,math.radians(2.5))
h_force_possible = abs(hook_angle) < math.radians(30.)
v_force_possible = abs(hook_angle) > math.radians(60.)
arg_list = [arm, rot_mat, h_force_possible, v_force_possible, cep_vel]
result, jep = self.compliant_motion(self.equi_pt_generator_control_radial_force,
0.1, arm, arg_list, self.log_state)
if self.move_segway_flag:
self.segway_command_node.stop()
self.z.estop()
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
print 'Adapted ftan threshold:', self.ftan_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,
'torque_ft': self.torque_trajectory,
'stiffness': self.firenze.arm_settings[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,
'segway':self.segway_trajectory, 'wrkspc':self.wrkspc_pts,
'bndry':self.bndry, 'cep_vel': cep_vel,
'zenither_list': self.zenither_list, 'ftan_threshold': self.ftan_threshold}
self.firenze.step()
if use_utm:
armconfig2 = self.firenze.get_joint_angles(arm)
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')
dd = {'mechanism_x': self.mech_x_list,
'mechanism_time': self.mech_time_list,
'force_rad_list': self.f_rad_list,
'force_tan_list': self.f_tan_list,
'tan_vec_list': self.tan_vec_list,
'rad_vec_list': self.rad_vec_list
}
ut.save_pickle(dd,'mechanism_trajectories_robot_'+d['info']+'_'+ut.formatted_time()+'.pkl')
measured_force_list.append(ft.A1.tolist())
time_list.append(time.time())
im = cam.get_frame()
im = cam.get_frame()
nm = '%05d.png' % i
cvSaveImage(nm, im)
i += 1
# for obj in ['hand', 'mechanism']:
# pose_d[obj]['pos_list'].append(current_pose_d[obj]['pos'])
# pose_d[obj]['rot_list'].append(current_pose_d[obj]['rot'])
f = get_number('Enter the spring scale reading', dtype=float)
spring_scale_list.append(f)
elif str == 'e':
print 'Exiting ...'
break
ft_dict = {}
ft_dict['ft_list'] = measured_force_list
ft_dict['time_list'] = time_list
fname = 'ft_log_' + ut.formatted_time() + '.pkl'
ut.save_pickle(ft_dict, fname)
# pose_d['hand']['time_list'] = time_list
# pose_d['mechanism']['time_list'] = time_list
# ut.save_pickle(pose_d, 'poses_dict.pkl')
ut.save_pickle(spring_scale_list,
'spring_scale_' + ut.formatted_time() + '.pkl')
def generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2,save_scan=False,
max_dist=np.Inf, min_dist=-np.Inf,min_tilt=-np.Inf,max_tilt=np.Inf, get_intensities=False, reject_zero_ten=True):
''' pos_list - list of motor positions (in steps)
scan_list - list of UrgScan objects at the corres positions.
l1,l2 - parameterizing the transformation (doc/ folder)
save_scan - pickle 3xN numpy matrix of points.
max_dist - ignore points at distance > max_dist
min_dist - ignore points at distance < max_dist
min_tilt, max_tilt - min and max tilt angles (radians)
+ve tilts the hokuyo down.
get_intensites - additionally return intensity values
returns 3xN numpy matrix of 3d coords of the points, returns (3xN, 1xN) including the intensity values if get_intensites=True
'''
t0 = time.time()
allpts = []
allintensities = []
pos_arr = np.array(pos_list)
scan_arr = np.array(scan_list)
idxs = np.where(np.multiply(pos_arr<=max_tilt,pos_arr>=min_tilt))
pos_list = pos_arr[idxs].tolist()
scan_list = scan_arr[idxs].tolist()
n_scans = len(scan_list)
if n_scans>1:
scan_list = copy.deepcopy(scan_list)
# remove graze in across scans.
ranges_mat = []
for s in scan_list:
ranges_mat.append(s.ranges.T)
ranges_mat = np.column_stack(ranges_mat)
start_index = hp.angle_to_index(scan_list[0],min_angle)
end_index = hp.angle_to_index(scan_list[0],max_angle)
for r in ranges_mat[start_index:end_index+1]:
hp.remove_graze_effect(r,np.matrix(pos_list),skip=1,graze_angle_threshold=math.radians(169.))
for i,s in enumerate(scan_list):
s.ranges = ranges_mat[:,i].T
for p,s in zip(pos_list,scan_list):
mapxydata = hp.get_xy_map(s,min_angle,max_angle,max_dist=max_dist,min_dist=min_dist,reject_zero_ten=reject_zero_ten,sigmoid_hack=True,get_intensities=get_intensities) # pts is 2xN
if get_intensities == True:
pts, intensities = mapxydata
allintensities.append(intensities)
else:
pts = mapxydata
pts = np.row_stack((pts,np.zeros(pts.shape[1]))) # pts is now 3xN
pts = tr.Ry(-p)*(pts+np.matrix((l1,0,-l2)).T)
allpts.append(pts)
allpts = np.column_stack(allpts)
if save_scan:
date_name = ut.formatted_time()
ut.save_pickle(allpts,date_name+'_cloud.pkl')
t1 = time.time()
# print 'Time to generate point cloud:', t1-t0
# allpts = tr.rotZ(math.radians(5))*allpts
if get_intensities == True:
allintensities = np.column_stack(allintensities)
return allpts, allintensities
else:
return allpts
print '################## s_list:', s_list
m, s = plot_stiff_ellipse_map(s_list, i)
inv_mean_list.append(1. / m)
std_list.append(s)
x_l.append(s1)
y_l.append(s2)
z_l.append(s3)
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
}, 'stiff_dict_' + ut.formatted_time() + '.pkl')
d3m.plot_points(np.matrix([x_l, y_l, z_l]),
scalar_list=inv_mean_list,
mode='sphere')
mlab.axes()
d3m.show()
sys.exit()
if fname == '':
print 'please specify a pkl file (-f option)'
print 'Exiting...'
sys.exit()
d = ut.load_pickle(fname)
actual_cartesian_tl = joint_to_cartesian(d['actual'], d['arm'])
for s2 in ratio_list2:
for s3 in ratio_list3:
i += 1
s_list = [s0, s1, s2, s3, 0.8]
# s_list = [s1,s2,s3,s0,0.8]
print "################## s_list:", s_list
m, s = plot_stiff_ellipse_map(s_list, i)
inv_mean_list.append(1.0 / m)
std_list.append(s)
x_l.append(s1)
y_l.append(s2)
z_l.append(s3)
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},
"stiff_dict_" + ut.formatted_time() + ".pkl",
)
d3m.plot_points(np.matrix([x_l, y_l, z_l]), scalar_list=inv_mean_list, mode="sphere")
mlab.axes()
d3m.show()
sys.exit()
if fname == "":
print "please specify a pkl file (-f option)"
print "Exiting..."
sys.exit()
d = ut.load_pickle(fname)
actual_cartesian = joint_to_cartesian(d["actual"])
eq_cartesian = joint_to_cartesian(d["eq_pt"])
raise RuntimeError('Unsupported. Advait Jan 02, 2010.')
if opt.use_jacobian:
kinematics_estimation = 'jacobian'
else:
kinematics_estimation = 'rotation_center'
t_pull = time.time()
cmg.pull(arm, math.radians(ha), residual_angle, ft, jep,
strategy = 'control_radial_force',
info_string=info_string, cep_vel = 0.10,
kinematics_estimation=kinematics_estimation,
pull_left = opt.sliding_left)
t_end = time.time()
pose_dict['t0'] = t_begin
pose_dict['t1'] = t_pull
pose_dict['t2'] = t_end
ut.save_pickle(pose_dict,'pose_dict_'+ut.formatted_time()+'.pkl')
print 'hit a key to end everything'
k=m3t.get_keystroke()
cmg.stop()
except: # catch all exceptions, stop and re-raise them
print 'Hello, Mr. Exception'
cmg.stop()
raise
def scan(self, range, speed, save_scan=False,avg=1, _max_retries=0):
''' range - (start,end) in radians
speed - scan speed in radians/sec
save_scan - save a dict of pos_list,scan_list,l1,l2
avg - average scans from the hokuyo.
returns pos_list,scan_list. list of angles and HokuyoScans
'''
ramp_up_angle = math.radians(5)
if abs(range[0])+ramp_up_angle > math.radians(95) or \
abs(range[1])+ramp_up_angle > math.radians(95):
print 'tilt_hokuyo_servo.scan:bad angles- ',math.degrees(range[0]),math.degrees(range[1])
min_angle = min(range[0],range[1])
max_angle = max(range[0],range[1])
# if max_angle>math.radians(60.5):
# print 'tilt_hokuyo_servo.scan: maximum angle is too high, will graze bottom plate of mount. angle:', math.degrees(max_angle)
# sys.exit()
self.servo.move_angle(range[0]+np.sign(range[0])*ramp_up_angle)
# time.sleep(0.05)
# while(self.servo.is_moving()):
# continue
self.servo.move_angle(range[1]+np.sign(range[1])*ramp_up_angle,speed,blocking=False)
#self.servo.move_angle(range[1], speed)
time.sleep(0.05)
t1 = time.time()
pos_list = []
scan_list = []
while self.servo.is_moving():
pos = self.servo.read_angle()
#print 'h6', pos
if pos < min_angle or pos > max_angle:
continue
pos_list.append(pos)
plane_scan = self.hokuyo.get_scan(avoid_duplicate=True,remove_graze=True,avg=avg)
scan_list.append(plane_scan)
t2 = time.time()
self.servo.move_angle(0)
if save_scan:
date_name = ut.formatted_time()
dict = {'pos_list': pos_list,'scan_list': scan_list,
'l1': self.l1, 'l2': self.l2}
ut.save_pickle(dict,date_name+'_dict.pkl')
runtime = t2 - t1
expected_number_scans = 19.0 * runtime * (1.0/avg)
scan_threshold = expected_number_scans - expected_number_scans*.2
if len(scan_list) < scan_threshold:
print 'tilt_hokuyo_servo.scan: WARNING! Expected at least %d scans but got only %d scans.' % (expected_number_scans, len(scan_list))
print 'tilt_hokuyo_servo.scan: trying again.. retries:', _max_retries
if _max_retries > 0:
return self.scan(range, speed, save_scan, avg, _max_retries = _max_retries-1)
else:
print 'tilt_hokuyo_servo.scan: returning anyway'
print 'tilt_hokuyo_servo.scan: got %d scans over range %f with speed %f.' % (len(scan_list), (max_angle - min_angle), speed)
return pos_list,scan_list
log_images = True
t0 = time.time()
ft_pub.publish() # send trigger to the ft logger.
print 'started logging'
im = cam.get_frame_debayered() # undistorting slows down frame rate
if log_images:
time_list.append(time.time())
im_list.append(cvCloneImage(im))
print 'frame rate:', len(time_list)/(t1-t0)
print 'before saving the pkl'
d = {}
t_string = ut.formatted_time()
video_name = 'mechanism_video_'+t_string+'.avi'
vwr = cvCreateVideoWriter(video_name, CV_FOURCC('I','4','2','0'),
30, cvGetSize(im_list[0]), True)
t0 = time.time()
im_name_list = []
time_stamp = ut.formatted_time()
for im in im_list:
cvWriteFrame(vwr, im)
time.sleep(.01) #Important to keep force torque server
#from restarting
t1 = time.time()
print 'disk writing rate:', len(time_list)/(t1-t0)
d['time_list'] = time_list
# epc.robot.go_cep_jtt(arm, p1)
# epc.move_till_hit(arm)
raw_input('Hit ENTER to close')
pr2_arms.close_gripper(arm)
raw_input('Hit ENTER to search_and_hook')
p1 = np.matrix([0.8, -0.22, -0.05]).T
epc.search_and_hook(arm, p1)
epc.pull_back_cartesian_control(arm, 0.3)
d = {
'f_list': epc.f_list, 'ee_list': epc.ee_list,
'cep_list': epc.cep_list
}
ut.save_pickle(d, 'pr2_pull_'+ut.formatted_time()+'.pkl')
# if False:
# ea = [0, 0, 0, 0, 0, 0, 0]
# ea = epc.robot.get_joint_angles(arm)
# rospy.logout('Going to starting position')
# epc.robot.set_jointangles(arm, ea, duration=4.0)
# raw_input('Hit ENTER to pull')
# epc.pull_back(arm, ea, tr.Rx(0), 0.2)
#
# if False:
# p = np.matrix([0.9, -0.3, -0.15]).T
# rot = tr.Rx(0.)
# rot = tr.Rx(math.radians(90.))
test_find_closest_object_point(srf, scan)
if test_graze_effect_flag:
widget_clicked |= tune_graze_effect_update(sl, srf, scan)
if test_find_lines_flag:
test_find_lines()
pygame.display.flip()
events = pygame.event.get()
for e in events:
if e.type == pygame.locals.QUIT:
loopFlag = False
if e.type == pygame.locals.KEYDOWN:
if e.key == 27: # Esc
loopFlag = False
if widget_clicked == False:
if e.type == pygame.locals.MOUSEMOTION:
if e.buttons[0] == 1:
# left button
org_x += e.rel[0]
org_y += e.rel[1]
if e.buttons[2] == 1:
# right button
MAX_DIST *= (1 + e.rel[1] * 0.01)
MAX_DIST = max(MAX_DIST, 0.1)
# Try to keep the specified framerate
clk.tick(fps)
if test_show_change_flag:
ut.save_pickle(n_ch_pts_list, ut.formatted_time() + '_ch_pts_list.pkl')
# psyco.full()
# print "Psyco loaded"
#except ImportError:
# pass
#from labeling import label_object, scan_dataset, scans_database
#database = scans_database.scans_database()
#database.load('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling','database.pkl')
#dataset = scan_dataset.scan_dataset()
#print dataset
cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
sc = scanner.scanner(cfg)
pc = Processor.Processor(cfg)
name = ut.formatted_time()
name='2009Oct17_122644'
#sc.capture_and_save(name)
pc.load_data(name)
#pc.load_raw_data('pill_table/2009Sep12_142422')
pc.process_raw_data()
pc.save_mapped_image(name)
pc.display_all_data()
print 'done'
for s1 in ratio_list1:
for s2 in ratio_list2:
for s3 in ratio_list3:
i += 1
s_list = [s0,s1,s2,s3,0.8]
#s_list = [s1,s2,s3,s0,0.8]
print '################## s_list:', s_list
m,s = plot_stiff_ellipse_map(s_list,i)
inv_mean_list.append(1./m)
std_list.append(s)
x_l.append(s1)
y_l.append(s2)
z_l.append(s3)
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},
'stiff_dict_'+ut.formatted_time()+'.pkl')
d3m.plot_points(np.matrix([x_l,y_l,z_l]),scalar_list=inv_mean_list,mode='sphere')
mlab.axes()
d3m.show()
sys.exit()
if fname=='':
print 'please specify a pkl file (-f option)'
print 'Exiting...'
sys.exit()
d = ut.load_pickle(fname)
actual_cartesian = joint_to_cartesian(d['actual'])
eq_cartesian = joint_to_cartesian(d['eq_pt'])
im = cam.get_frame()
nm = '%05d.png'%i
cvSaveImage(nm, im)
i += 1
# for obj in ['hand', 'mechanism']:
# pose_d[obj]['pos_list'].append(current_pose_d[obj]['pos'])
# pose_d[obj]['rot_list'].append(current_pose_d[obj]['rot'])
f = get_number('Enter the spring scale reading', dtype = float)
spring_scale_list.append(f)
elif str == 'e':
print 'Exiting ...'
break
ft_dict = {}
ft_dict['ft_list'] = measured_force_list
ft_dict['time_list'] = time_list
fname = 'ft_log_'+ut.formatted_time()+'.pkl'
ut.save_pickle(ft_dict, fname)
# pose_d['hand']['time_list'] = time_list
# pose_d['mechanism']['time_list'] = time_list
# ut.save_pickle(pose_d, 'poses_dict.pkl')
ut.save_pickle(spring_scale_list,
'spring_scale_'+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')