def run_explore(filename):
global obj_init_frame
# 0. check if collected or not
dir_base = data_base + "/multi_pushes/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
if os.path.isfile(jsonfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0], center_world[1]] + [zup]
setCart(start_pos)
setAccRos(1, 1)
setZero()
wait_for_ft_calib()
#pause()
import Tkinter
import tkMessageBox
result = tkMessageBox.showinfo("Continue",
"Put the object at the starting place")
# 1.1 get object init pos as new center
box_pose_des_global = lookupTransformList(global_frame_id,
obj_frame_id, lr)
obj_init_frame = pose3d_to_pose2d(box_pose_des_global)
pushlen = 0.12
# 2. Push make the pushes fixed wrt the first frame.
# push: move to the startpose up , move down, push to endpose, move up
# 2.1 push([0, -0.12], [0, -0.12+pushlen])
push_center([0, -0.12], [0, -0.12 + pushlen])
# 2.2 push([0, 0.12], [0, 0.12-pushlen])
push_center([0, 0.12], [0, 0.12 - pushlen])
# 2.3 push([-0.12, -0.08], [-0.12+pushlen, -0.08+pushlen])
push_center([-0.08, -0.12], [-0.12 + pushlen, -0.08 + pushlen])
# 2.4 push([0.08, -0.12], [0.08-pushlen, -0.12+pushlen])
if shape_id == 'rect1':
push_center([0.08, -0.12], [0.08 - pushlen, -0.12 + pushlen])
else:
push_center([0.08, 0.12], [0.08 - pushlen, -0.12 - pushlen])
# 4.1 save contact_nm and contact_pt as json file
helper.make_sure_path_exists(dir_base)
with open(jsonfilename, 'w') as outfile:
json.dump(
{
'all_contact': all_contact,
'isreal': True,
'__title__': colname,
"surface_id": surface_id,
"shape_id": shape_id,
"probe_id": probe_id,
"muc": muc,
"probe_radius": probe_radius,
"offset": offset
},
outfile,
sort_keys=True,
indent=1)
# 4.2 save all_contact as mat file
sio.savemat(matfilename, {'all_contact': all_contact})
def main(argv):
# prepare the proxy, listener
global global_highvel, globalvel, z, zup, listener, obj_frame_id, obj_slot, pub
pub = rospy.Publisher('/robot_status', String, queue_size=10)
rospy.init_node('collect_motion_data')
listener = tf.TransformListener()
rospy.sleep(1)
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('-r', '--real', action="store_true", dest='real_exp',
help='Do the real experiment space',
default=False)
parser.add_option('', '--reptype', action="store", dest='reptype', type='string',
help='Do the real experiment space',
default='normal') # normal: no variation, speed, angle, position
parser.add_option('', '--nrep', action="store", dest='nrep', type='int',
help='Repeat how many times',
default=1)
parser.add_option('', '--slow', action="store_true", dest='slow',
help='Set slower global speed',
default=False)
parser.add_option('', '--probe', action="store", dest='probe_id',
help='The probe id e.g. probe1-5', default='probe5')
(opt, args) = parser.parse_args()
if opt.slow:
globalvel = global_slow_vel;
global_highvel = global_slow_vel;
probe_db = ProbeDB()
probe_length = probe_db.db[opt.probe_id]['length']
# parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
z = probe_length + ft_length + surface_thick + 0.009 #- (210-156)/1000.0 # the height above the table
z_recover = 0.012 + z # the height for recovery
zup = z + 0.04 # the prepare and end height
zup = z + 0.08 # the prepare and end height
probe_radius = probe_db.db[opt.probe_id]['radius']
# parameters about object
shape_id = opt.shape_id
shape_db = ShapeDB()
shape_type = shape_db.shape_db[shape_id]['shape_type']
shape = shape_db.shape_db[shape_id]['shape']
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
# space for the experiment
real_exp = opt.real_exp
rep_label = ''
dist_after_contact = 0.05
allowed_distance = 0.04
if real_exp:
if opt.nrep == 1:
accelerations = [0.1, 0.2, 0.5, 0.75, 1, 1.5, 2, 2.5]
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
if shape_type == 'poly':
if shape == 'hex':
side_params = np.linspace(0, 1, 6) # decrease the number of pushes
else:
side_params = np.linspace(0, 1, 11)
else:
side_params = np.linspace(0,1,40,endpoint=False)
angles = np.linspace(-pi/180.0*80.0, pi/180*80, 9)
nside = len(shape)
else:
# set the nominal parameters
side_params = [0.7] #[0.3]#[0.1]#[0.7]
angles = [0]#[0.6981317] #[-0.34906585] #[-0.6981317] ##[0]
speeds = [20]
accelerations = []
nside = 1
dist_after_contact = 0.15
#dist_after_contact = 0.10 #0.05 # hack for video
#shape = shape[0:1] # only do it on one side
# what dimension we want to explore
if opt.reptype == 'normal':
pass
elif opt.reptype == 'angle':
angles = [10, 20, 50]
elif opt.reptype == 'speed':
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
elif opt.reptype == 'position':
side_params = np.linspace(0.5, 1, 6)
allowed_distance = 0
nspeeds = len(speeds)
nacc = len(accelerations)
speeds = np.append(speeds, np.repeat(-1, nacc))
accelerations = np.append(np.repeat(0, nspeeds), accelerations)
else:
accelerations = [0, 0, 0.1, 1, 2.5]
speeds = [50, 400, -1, -1, -1]
angles = np.linspace(-pi/4, pi/4, 2)
if shape_type == 'poly':
side_params = np.linspace(0.1,0.9,1)
else:
side_params = np.linspace(0,1,1,endpoint=False)
# parameters about rosbag
if opt.nrep == 1:
dir_save_bagfile = os.environ['DATA_BASE'] + '/straight_push/%s/%s/' % (surface_id,shape_id)
else:
dir_save_bagfile = os.environ['DATA_BASE'] + '/straight_push_rep/%s/%s/%s/' % (surface_id,shape_id,opt.reptype)
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
helper.make_sure_path_exists(dir_save_bagfile)
print side_params
run_it(accelerations, speeds, shape, nside, side_params, angles, opt.nrep,
shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt)
def main(argv):
rospy.init_node('collect_friction_map_data')
parser = optparse.OptionParser()
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--cage', action="store_true", dest='tocage',
help='Do the cage process (placing the object inside the cage finger) or not. ', default=False)
(opt, args) = parser.parse_args()
shape_id = 'rect1'
ori = [0, 0, 1, 0]
center_world = [0.375, 0, 0]
z = 0.147-0.01158 + SurfaceDB().db[opt.surface_id]['thickness']
z_place = z+0.03
z_up = z_place+0.03
globalmaxacc = 100
topics = ['-a']
skip_when_exists = True
global_speed = 100
global_slow_speed = 20
setZone(0)
# scanning
resolution = 0.005
eps = 1e-6
max_x = 0.450
min_x = 0.250
max_y = 0.197
min_y = -0.233
center = [(max_x + min_x) /2, (max_y + min_y) /2 ]
range_x = np.linspace(max_x, min_x, 3)
acc = 0
vel = 20
degs = xrange(0, 360, 5)
rep = 0
nrep = 2
radius = 0.06
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/friction_scan_isotropic/%s/%s/' % (opt.surface_id,shape_id)
helper.make_sure_path_exists(dir_save_bagfile)
for rep in xrange(nrep):
if rep % 2 == 0:
degs_order = degs
else:
degs_order = reversed(degs)
for deg in degs_order:
th = np.deg2rad(deg)
start_pos = [np.cos(th)* radius + center[0], np.sin(th)* radius + center[1]]
end_pos = [np.cos(th+np.pi)* radius + center[0], np.sin(th+np.pi)* radius + center[1]]
bagfilename = 'record_surface=%s_shape=%s_a=%.0f_v=%.0f_deg=%d_rep=%03d.bag' % (opt.surface_id, shape_id, acc*1000, vel, deg, rep)
print bagfilename
bagfilepath = dir_save_bagfile+bagfilename
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
setSpeed(tcp=global_slow_speed, ori=1000)
setCart([start_pos[0], start_pos[1], z], ori)
setCart([start_pos[0], start_pos[1], z_place], ori)
setZero()
wait_for_ft_calib()
setCart([start_pos[0], start_pos[1], z], ori)
setSpeed(tcp=vel, ori=1000)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
setCart([end_pos[0], end_pos[1], z], ori)
helper.terminate_ros_node("/record")
rospy.sleep(1) # make sure record is terminated completely
def main(argv):
rospy.init_node('collect_friction_map_data')
parser = optparse.OptionParser()
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--cage', action="store_true", dest='tocage',
help='Do the cage process (placing the object inside the cage finger) or not. ', default=False)
(opt, args) = parser.parse_args()
shape_id = 'rect1'
ori = [0, 0, 1, 0]
center_world = [0.375, 0, 0]
z = 0.147-0.01158 + SurfaceDB().db[opt.surface_id]['thickness']
z_place = z+0.03
z_up = z_place+0.03
globalmaxacc = 100
topics = ['-a']
skip_when_exists = True
setZone(0)
# move to center and cage the object
if opt.tocage:
setSpeed(tcp=20, ori=1000)
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setCart([center_world[0], center_world[1], z_up], ori)
pause()
setCart([center_world[0], center_world[1], z_place], ori)
print 'place the object in the cage with some support and hold it'
pause()
setCart([center_world[0], center_world[1], z], ori)
print 'remove the support'
pause()
return
# scanning
resolution = 0.005
eps = 1e-6
max_x = 0.450
min_x = 0.250 - eps
max_y = 0.197
min_y = -0.233
range_x = np.arange(max_x, min_x, -resolution)
acc = 0
vel = 20
rep = 0
nrep = 1
setSpeed(tcp=vel, ori=1000)
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/friction_scan_fine/%s/%s/' % (opt.surface_id,shape_id)
helper.make_sure_path_exists(dir_save_bagfile)
for rep in xrange(nrep):
bagfilename = 'record_surface=%s_shape=%s_a=%.0f_v=%.0f_rep=%03d.bag' % (opt.surface_id, shape_id, acc*1000, vel, rep)
print bagfilename
bagfilepath = dir_save_bagfile+bagfilename
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
setCart([range_x[0], max_y, z], ori)
setCart([range_x[0], max_y, z_place], ori)
setZero()
wait_for_ft_calib()
setCart([range_x[0], max_y, z], ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
for ind, x in enumerate(range_x):
setCart([x, max_y, z], ori)
setCart([x, min_y, z], ori)
setCart([x, max_y, z], ori)
helper.terminate_ros_node("/record")
if rep == nrep -1:
rospy.sleep(1) # make sure record is terminated completely
def main(argv):
# prepare the proxy, listener
global globalvel, z, zup, z_ofset, listener, obj_frame_id, obj_slot, pub
pub = rospy.Publisher('/robot_status', String, queue_size=10)
rospy.init_node('collect_motion_data')
listener = tf.TransformListener()
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('-r', '--real', action="store_true", dest='real_exp',
help='Do the real experiment space',
default=False)
parser.add_option('', '--reptype', action="store", dest='reptype', type='string',
help='Do the real experiment space',
default='normal') # normal: no variation, speed, angle, position
parser.add_option('', '--nrep', action="store", dest='nrep', type='int',
help='Repeat how many times',
default=1)
parser.add_option('', '--slow', action="store_true", dest='slow',
help='Set slower global speed',
default=False)
parser.add_option('', '--probe', action="store", dest='probe_id',
help='The probe id e.g. probe1-5', default='probe5')
(opt, args) = parser.parse_args()
if opt.slow: globalvel = global_slow_vel
probe_db = ProbeDB()
probe_length = probe_db.db[opt.probe_id]['length']
# parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
z = probe_length + ft_length + surface_thick + 0.007 # the height above the table
z_recover = 0.012 + z # the height for recovery
zup = z + 0.04 # the prepare and end height
zup = z + 0.08 # the prepare and end height
probe_radius = probe_db.db[opt.probe_id]['radius']
# parameters about object
shape_id = opt.shape_id
shape_db = ShapeDB()
shape_type = shape_db.shape_db[shape_id]['shape_type']
shape = shape_db.shape_db[shape_id]['shape']
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
# space for the experiment
real_exp = opt.real_exp
rep_label = ''
dist_after_contact = 0.05
allowed_distance = 0.06
if real_exp:
if opt.nrep == 1:
accelerations = [0.1, 0.2, 0.5, 0.75, 1, 1.5, 2, 2.5]
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
if shape_type == 'poly':
if shape == 'hex':
side_params = np.linspace(0, 1, 6) # decrease the number of pushes
else:
side_params = np.linspace(0, 1, 11)
else:
side_params = np.linspace(0,1,40,endpoint=False)
angles = np.linspace(-pi/180.0*80.0, pi/180*80, 9)
nside = len(shape)
else:
# set the nominal parameters
side_params = [0.7]
angles = [0]
speeds = []
accelerations = [1]
nside = 1
dist_after_contact = 0.15
dist_after_contact = 0.05 # hack for video
#shape = shape[0:1] # only do it on one side
# what dimension we want to explore
if opt.reptype == 'normal':
pass
elif opt.reptype == 'angle':
angles = [10, 20, 50]
elif opt.reptype == 'speed':
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
elif opt.reptype == 'position':
side_params = np.linspace(0.5, 1, 6)
allowed_distance = 0
nspeeds = len(speeds)
nacc = len(accelerations)
speeds = np.append(speeds, np.repeat(-1, nacc))
accelerations = np.append(np.repeat(0, nspeeds), accelerations)
else:
accelerations = [0, 0, 0.1, 1, 2.5]
speeds = [50, 400, -1, -1, -1]
angles = np.linspace(-pi/4, pi/4, 2)
if shape_type == 'poly':
side_params = np.linspace(0.1,0.9,1)
else:
side_params = np.linspace(0,1,1,endpoint=False)
# parameters about rosbag
if opt.nrep == 1:
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/straight_push/%s/%s/' % (surface_id,shape_id)
else:
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/straight_push_rep/%s/%s/%s/' % (surface_id,shape_id,opt.reptype)
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
helper.make_sure_path_exists(dir_save_bagfile)
run_it(accelerations, speeds, shape, nside, side_params, angles, opt.nrep,
shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt)
def main(argv):
rospy.init_node('collect_friction_map_data')
parser = optparse.OptionParser()
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--cage', action="store_true", dest='tocage',
help='Do the cage process (placing the object inside the cage finger) or not. ', default=False)
(opt, args) = parser.parse_args()
shape_id = 'rect1'
ori = [0, 0, 1, 0]
center_world = [0.375, -0.05, 0]
z = 0.147-0.01158 + SurfaceDB().db[opt.surface_id]['thickness']
z_place = z+0.03
z_up = z_place+0.03
globalmaxacc = 100
topics = ['-a']
skip_when_exists = True
setZone(0)
# move to center and cage the object
if opt.tocage:
setSpeed(tcp=20, ori=1000)
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setCart([center_world[0], center_world[1], z_up], ori)
pause()
setCart([center_world[0], center_world[1], z_place], ori)
print 'place the object in the cage with some support and hold it'
pause()
setCart([center_world[0], center_world[1], z], ori)
print 'remove the support'
pause()
return
# scanning
max_x = 0.450
min_x = 0.250
max_y = 0.197
min_y = -0.233
range_x = np.linspace(max_x, min_x, 3)
acc = 0
vel = 20
rep = 0
nrep = 100
setSpeed(tcp=vel, ori=1000)
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/friction_scan/%s/%s/' % (opt.surface_id,shape_id)
helper.make_sure_path_exists(dir_save_bagfile)
for rep in xrange(nrep):
bagfilename = 'record_surface=%s_shape=%s_a=%.0f_v=%.0f_rep=%03d.bag' % (opt.surface_id, shape_id, acc*1000, vel, rep)
print bagfilename
bagfilepath = dir_save_bagfile+bagfilename
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
setCart([range_x[0], max_y, z], ori)
setCart([range_x[0], max_y, z_place], ori)
setZero()
wait_for_ft_calib()
#pause()
setCart([range_x[0], max_y, z], ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
for x in range_x:
setCart([x, max_y, z], ori)
setCart([x, min_y, z], ori)
setCart([x, max_y, z], ori)
helper.terminate_ros_node("/record")
if rep == nrep -1:
rospy.sleep(1) # make sure record is terminated completely
def main(argv):
rospy.init_node('collect_friction_map_data')
parser = optparse.OptionParser()
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--cage', action="store_true", dest='tocage',
help='Do the cage process (placing the object inside the cage finger) or not. ', default=False)
(opt, args) = parser.parse_args()
shape_id = 'rect1'
ori = [0, 0, 1, 0] # qw qx qy qz
center_world = [0.375, -0.05, 0]
z = 0.147-0.01158 + SurfaceDB().db[opt.surface_id]['thickness']
z_place = z+0.03
z_up = z_place+0.03
globalmaxacc = 100
topics = ['-a']
skip_when_exists = True
global_speed = 40
global_slow_speed = 20
setZone(0)
# scanning
resolution = 0.005
eps = 1e-6
max_x = 0.450
min_x = 0.250
max_y = 0.197
min_y = -0.233
center = [(max_x + min_x) /2, (max_y + min_y) /2 ]
range_x = np.linspace(max_x, min_x, 3)
acc = 0
vel = 20
#degs_default = reversed([0, 180]) #+ list(xrange(0, 360, 5))
degs_default = [180, 0]
rep = 0
radii = [0.05, 0.025 , 0.0125, 0]
rotdegs_default = np.linspace(-80, 80, 21)
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/friction_scan_limitsurface/%s/%s/' % (opt.surface_id,shape_id)
helper.make_sure_path_exists(dir_save_bagfile)
rep = 0
cnt = 0
cntlimit = 100
orispeed = 10
global_orispeed = 40
for radius in radii:
if radius == 0:
degs = [0]
else:
degs = degs_default
for deg in degs: # translation velocity direction
th = np.deg2rad(deg)
# import pdb; pdb.set_trace()
if radius == 0:
rotdegs = [80, -80]
orispeed = 10 # to prevent rotating very quick
elif deg in [0, 90, 180, 270]:
rotdegs = np.linspace(-88, 88, 45)
orispeed = 10
else:
rotdegs = rotdegs_default
orispeed = 10
for rotdeg in rotdegs: # rotation velocity direction
rotth = np.deg2rad(rotdeg)
start_ori = ik.helper.qwxyz_from_qxyzw(tfm.quaternion_from_matrix((np.dot(tfm.euler_matrix(0,np.pi,0), tfm.euler_matrix(0,0,rotth)))))
end_ori = ik.helper.qwxyz_from_qxyzw(tfm.quaternion_from_matrix((np.dot(tfm.euler_matrix(0,np.pi,0), tfm.euler_matrix(0,0,-rotth)))))
start_pos = [np.cos(th)* radius + center[0], np.sin(th)* radius + center[1]]
end_pos = [np.cos(th+np.pi)* radius + center[0], np.sin(th+np.pi)* radius + center[1]]
bagfilename = 'record_surface=%s_shape=%s_a=%.0f_v=%.0f_deg=%d_rotdeg=%d_radius=%.3f_rep=%03d.bag' % (opt.surface_id, shape_id, acc*1000, vel, deg, rotdeg, radius, rep)
print bagfilename
bagfilepath = dir_save_bagfile+bagfilename
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
setSpeed(tcp=global_speed, ori=global_orispeed) # go to starting position and prepare
setCart([start_pos[0], start_pos[1], z], start_ori)
setCart([start_pos[0], start_pos[1], z_place], start_ori)
setZero()
wait_for_ft_calib()
setCart([start_pos[0], start_pos[1], z], start_ori)
setSpeed(tcp=vel, ori=orispeed)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
setCart([end_pos[0], end_pos[1], z], end_ori)
helper.terminate_ros_node("/record")
cnt += 1
if cnt > cntlimit:
rospy.sleep(1) # make sure record is terminated completely
return
rospy.sleep(1) # make sure record is terminated completely
def main(argv):
rospy.init_node('collect_friction_map_data')
parser = optparse.OptionParser()
parser.add_option('',
'--surface',
action="store",
dest='surface_id',
help='The surface id e.g. plywood, abs',
default='plywood')
parser.add_option(
'',
'--cage',
action="store_true",
dest='tocage',
help=
'Do the cage process (placing the object inside the cage finger) or not. ',
default=False)
(opt, args) = parser.parse_args()
shape_id = 'rect1'
ori = [0, 0, 1, 0] # qw qx qy qz
center_world = [0.375, -0.05, 0]
z = 0.147 - 0.01158 + SurfaceDB().db[opt.surface_id]['thickness']
z_place = z + 0.03
z_up = z_place + 0.03
globalmaxacc = 100
topics = ['-a']
skip_when_exists = True
global_speed = 40
global_slow_speed = 20
setZone(0)
# scanning
resolution = 0.005
eps = 1e-6
max_x = 0.450
min_x = 0.250
max_y = 0.197
min_y = -0.233
center = [(max_x + min_x) / 2, (max_y + min_y) / 2]
range_x = np.linspace(max_x, min_x, 3)
acc = 0
vel = 20
#degs_default = reversed([0, 180]) #+ list(xrange(0, 360, 5))
degs_default = [180, 0]
rep = 0
radii = [0.05, 0.025, 0.0125, 0]
rotdegs_default = np.linspace(-80, 80, 21)
dir_save_bagfile = os.environ[
'PNPUSHDATA_BASE'] + '/friction_scan_limitsurface/%s/%s/' % (
opt.surface_id, shape_id)
helper.make_sure_path_exists(dir_save_bagfile)
rep = 0
cnt = 0
cntlimit = 100
orispeed = 10
global_orispeed = 40
for radius in radii:
if radius == 0:
degs = [0]
else:
degs = degs_default
for deg in degs: # translation velocity direction
th = np.deg2rad(deg)
# import pdb; pdb.set_trace()
if radius == 0:
rotdegs = [80, -80]
orispeed = 10 # to prevent rotating very quick
elif deg in [0, 90, 180, 270]:
rotdegs = np.linspace(-88, 88, 45)
orispeed = 10
else:
rotdegs = rotdegs_default
orispeed = 10
for rotdeg in rotdegs: # rotation velocity direction
rotth = np.deg2rad(rotdeg)
start_ori = ik.helper.qwxyz_from_qxyzw(
tfm.quaternion_from_matrix(
(np.dot(tfm.euler_matrix(0, np.pi, 0),
tfm.euler_matrix(0, 0, rotth)))))
end_ori = ik.helper.qwxyz_from_qxyzw(
tfm.quaternion_from_matrix(
(np.dot(tfm.euler_matrix(0, np.pi, 0),
tfm.euler_matrix(0, 0, -rotth)))))
start_pos = [
np.cos(th) * radius + center[0],
np.sin(th) * radius + center[1]
]
end_pos = [
np.cos(th + np.pi) * radius + center[0],
np.sin(th + np.pi) * radius + center[1]
]
bagfilename = 'record_surface=%s_shape=%s_a=%.0f_v=%.0f_deg=%d_rotdeg=%d_radius=%.3f_rep=%03d.bag' % (
opt.surface_id, shape_id, acc * 1000, vel, deg, rotdeg,
radius, rep)
print bagfilename
bagfilepath = dir_save_bagfile + bagfilename
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
setSpeed(
tcp=global_speed,
ori=global_orispeed) # go to starting position and prepare
setCart([start_pos[0], start_pos[1], z], start_ori)
setCart([start_pos[0], start_pos[1], z_place], start_ori)
setZero()
wait_for_ft_calib()
setCart([start_pos[0], start_pos[1], z], start_ori)
setSpeed(tcp=vel, ori=orispeed)
rosbag_proc = helper.start_ros_bag(bagfilename, topics,
dir_save_bagfile)
setCart([end_pos[0], end_pos[1], z], end_ori)
helper.terminate_ros_node("/record")
cnt += 1
if cnt > cntlimit:
rospy.sleep(1) # make sure record is terminated completely
return
rospy.sleep(1) # make sure record is terminated completely
def run_explore(filename):
# 0. check if collected or not
dir_base = data_base + "/contour_following/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
if os.path.isfile(jsonfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0] + explore_radius, center_world[1]] + [zup]
setCart(start_pos)
# 2. Rough probing
scan_contact_pts = []
# 2.1 populate start poses for small probing in opposite direction
start_configs = []
jmax = (nstart + 1) // 2
kmax = 2 if nstart > 1 else 1
dth = 2 * np.pi / nstart
for j in xrange(jmax):
for k in xrange(kmax):
i = j + k * (nstart // 2)
start_pos = [
center_world[0] + explore_radius * np.cos(i * dth),
center_world[1] + explore_radius * np.sin(i * dth)
]
direc = [-np.cos(i * dth), -np.sin(i * dth), 0]
start_configs.append([start_pos, direc])
print 'direc', direc
# 2.2 move toward center till contact, and record contact points
for i, (start_pos, direc) in enumerate(reversed(start_configs)):
setZero()
wait_for_ft_calib()
setCart(start_pos + [zup])
setCart(start_pos + [z])
curr_pos = start_pos + [z]
cnt = 0
while True:
curr_pos = np.array(curr_pos) + np.array(direc) * step_size
setCart(curr_pos)
# let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getFTmsglist()
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
print 'obj_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_pt = curr_pos - normal * probe_radius
scan_contact_pts.append(contact_pt.tolist())
if i < len(
start_configs
) - 1: # if the last one, stay in contact and do exploration from there.
setCart([curr_pos[0], curr_pos[1], zup])
break
#if too far and no contact break.
if cnt > explore_radius * 2 / step_size:
break
if len(scan_contact_pts) == 0:
print "Error: Cannot touch the object"
return
# 3. Contour following, use the normal to move along the block
setSpeed(tcp=global_slow_vel)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 3.1 move
direc = np.dot(tfm.euler_matrix(0, 0, 2),
normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos)
# 3.2 let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getAveragedFT()
if index % 50 == 0:
#print index #, '[ft explore]', ft
print index
else:
sys.stdout.write('.')
sys.stdout.flush()
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
# 3.3 visualize it.
vizBlock(box_pose_des_global, obj_mesh, global_frame_id)
# 3.4 record the data if in contact
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radius
contact_pts.append(contact_pt.tolist())
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(),
(contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z.
# Also the normal should point toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] +
(-normal).tolist()[0:2] + [0] +
box_pose_des_global[0:3] +
box_pose_des_global[6:7] +
box_pose_des_global[3:6] +
curr_pos.tolist())
index += 1
# 3.5 break if we collect enough
if len(contact_pts) > limit:
break
# 3.6 periodically zero the ft
if len(
contact_pts
) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=global_super_slow_vel)
setCart(curr_pos + normal * move_away_size)
rospy.sleep(1)
print 'offset ft:', getAveragedFT()
setZero()
wait_for_ft_calib()
setCart(curr_pos - normal * overshoot_size)
setSpeed(tcp=global_slow_vel)
# 4.1 save contact_nm and contact_pt as json file
helper.make_sure_path_exists(dir_base)
with open(jsonfilename, 'w') as outfile:
json.dump(
{
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts,
'isreal': True,
'__title__': colname,
"surface_id": surface_id,
"shape_id": shape_id,
"probe_id": probe_id,
"muc": muc,
"probe_radius": probe_radius,
"offset": offset
},
outfile,
sort_keys=True,
indent=1)
# 4.2 save all_contact as mat file
sio.savemat(matfilename, {
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts
})
# 5. move back to startPos
setSpeed(tcp=global_vel)
start_pos = [curr_pos[0], curr_pos[1]] + [zup]
setCart(start_pos)
recover(obj_slot, True)