def polyapprox(shape, s):
ss = shape[0]
accu = []
length = 0
for i in range(len(ss)):
accu.append(
norm(np.array(ss[(i + 1) % len(ss)]) - np.array(ss[i])) + length)
length = accu[-1]
targetlength = s * length
ind = 0
for i in range(len(ss)):
if accu[i] > targetlength:
ind = i
break
seglength = norm(np.array(ss[(ind + 1) % len(ss)]) - np.array(ss[ind]))
t = (targetlength - accu[ind]) / seglength
pos = np.array(ss[ind]) * (1 - t) + np.array(ss[(ind + 1) % len(ss)]) * t
pos = np.append(pos, [0])
tangent = np.array(ss[(ind + 1) % len(ss)]) - np.array(ss[ind])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
return (pos, normal)
def polyapprox_check_collision(shape, pos_start_probe_object, probe_radius):
ss = shape[0]
# find the closest point on the shape to pos_start_probe_object
min_dist = 10000
min_ind = 0
safety_margin = 0.005
for i in range(len(ss)):
dist = norm(np.array(ss[i]) - np.array(pos_start_probe_object[0:2]))
if dist < min_dist:
min_dist = dist
min_ind = i
tangent = np.array(ss[(min_ind+1) % len(ss)])-np.array(ss[min_ind])
normal = np.array([tangent[1], -tangent[0]]) # pointing out of shape
normal = normal / norm(normal) # normalize it
d = np.dot(normal, np.array(pos_start_probe_object[0:2]) - np.array(ss[min_ind]) )
if d < probe_radius + safety_margin:
return True
else:
return False
def polyapprox_check_collision(shape, pos_start_probe_object, probe_radius):
ss = shape[0]
# find the closest point on the shape to pos_start_probe_object
min_dist = 10000
min_ind = 0
safety_margin = 0.005
for i in range(len(ss)):
dist = norm(np.array(ss[i]) - np.array(pos_start_probe_object[0:2]))
if dist < min_dist:
min_dist = dist
min_ind = i
tangent = np.array(ss[(min_ind+1) % len(ss)])-np.array(ss[min_ind])
normal = np.array([tangent[1], -tangent[0]]) # pointing out of shape
normal = normal / norm(normal) # normalize it
d = np.dot(normal, np.array(pos_start_probe_object[0:2]) - np.array(ss[min_ind]) )
if d < probe_radius + safety_margin:
return True
else:
return False
def polyapprox(shape, s):
ss = shape[0]
accu = []
length = 0
for i in range(len(ss)):
accu.append(norm(np.array(ss[(i+1) % len(ss)])-np.array(ss[i])) + length)
length = accu[-1]
targetlength = s*length
ind = 0
for i in range(len(ss)):
if accu[i] > targetlength:
ind = i
break
seglength = norm(np.array(ss[(ind+1) % len(ss)])-np.array(ss[ind]))
t = (targetlength-accu[ind]) / seglength
pos = np.array(ss[ind]) * (1-t) + np.array(ss[(ind+1) % len(ss)]) * t
pos = np.append(pos, [0])
tangent = np.array(ss[(ind+1) % len(ss)])-np.array(ss[ind])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
return (pos, normal)
def run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt):
# hack to restart the script to prevent ros network issues.
global pub
limit = 100
cnt = 0
topics = ['-a']
# enumerate the possible trajectories
for cnt_acc, acc in enumerate(accelerations):
# enumerate the side we want to push
for i in range(nside):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
pos = np.array(shape[i]) *(1-s) + np.array(shape[(i+1) % len(shape)]) *(s)
pos = np.append(pos, [0])
tangent = np.array(shape[(i+1) % len(shape)]) - np.array(shape[i])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
elif shape_type == 'ellip':
(a,b) = shape[0][0], shape[0][1]
pos = [shape[0][0] * np.cos(s*2*np.pi), shape[0][1] * np.sin(s*2*np.pi), 0]
normal = [np.cos(s*2*np.pi)/a, np.sin(s*2*np.pi)/b, 0]
normal = normal / norm(normal) # normalize it
elif shape_type == 'polyapprox':
pos, normal = polyapprox(shape, s)
# enumerate the direction in which we want to push
for t in angles:
for rep in xrange(nrep):
if nrep > 1:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
else:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t)
bagfilepath = dir_save_bagfile+bagfilename
# if exists then skip it
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
# find the probe pos in contact in object frame
pos_probe_contact_object = pos + normal * probe_radius
# find the start point
direc = np.dot(tfm.euler_matrix(0,0,t) , normal.tolist() + [1])[0:3] # in the direction of moving out
pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact
if shape_type == 'polyapprox' and polyapprox_check_collision(shape, pos_start_probe_object, probe_radius):
print bagfilename, 'will be in collision', 'skip'
continue
# find the end point
pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact
# zero force torque sensor
rospy.sleep(0.1)
#setZero()
#wait_for_ft_calib()
# transform start and end to world frame
if hasRobot:
pos_start_probe_world = coordinateFrameTransformList(pos_start_probe_object, obj_frame_id, global_frame_id, listener)
pos_end_probe_world = coordinateFrameTransformList(pos_end_probe_object, obj_frame_id, global_frame_id, listener)
pos_contact_probe_world = coordinateFrameTransformList(pos_probe_contact_object, obj_frame_id, global_frame_id, listener)
pos_center_obj_world = coordinateFrameTransformList([0,0,0], obj_frame_id, global_frame_id, listener)
else:
pos_start_probe_world = pos_start_probe_object + center_world
pos_end_probe_world = pos_end_probe_object + center_world
pos_contact_probe_world = pos_probe_contact_object + center_world
pos_center_obj_world = center_world
# start bag recording
# move to startPos
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=global_highvel)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos,ori)
setAcc(acc=globalhighacc, deacc=globalacc)
setSpeed(tcp=global_highvel, ori=1000)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos,ori)
setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
pub.publish('move_to_mid_pos')
setCart(mid_pos,ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if acc == 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=speeds[cnt_acc], ori=1000)
else: # there is acceleration
setAcc(acc=acc, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
pub.publish('start_pushing')
setCart(end_pos,ori)
pub.publish('end_pushing')
# end bag recording
helper.terminate_ros_node("/record")
setSpeed(tcp=global_highvel, ori=1000)
setAcc(acc=globalhighacc, deacc=globalhighacc)
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos,ori)
distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world))
# recover
recover(obj_slot, distance_obj_center > allowed_distance)
#pause()
cnt += 1
if cnt > limit: return
def push(start_pos, end_pos):
global frame_work_robot_2d, zup, z
global contact_pts, contact_nms, all_contact
global step_size
direc = (npa(end_pos) - npa(start_pos))
dist = np.linalg.norm(direc)
direc = direc / dist * step_size
setSpeed(tcp=global_slow_vel)
setCart(list(start_pos) + [zup])
# move down, not supposed to make contact
setCart(list(start_pos) + [z])
nsteps = int(dist // step_size)
# calibrate the FT sensor
#setZero()
#wait_for_ft_calib()
for i in xrange(1, nsteps + 1):
# move
curr_pos = (direc * i + start_pos).tolist() + [z]
setCart(curr_pos)
# collect vicon object pose
box_pose_des_global = lookupTransformList(global_frame_id,
obj_frame_id, lr)
# collect ft measurement
rospy.sleep(sleepForFT)
ft = getAveragedFT()
# collect apriltag measurement
rospy.sleep(sleepForAP)
has_apriltag = False
object_apriltag = [0, 0, 0, 0, 0, 0, 0]
apriltag_detections_msg = ROS_Wait_For_Msg(
'/apriltags/detections', AprilTagDetections).getmsg()
for det in apriltag_detections_msg.detections:
if det.id == 0:
pose_webcam = transform_back([0.02, 0.02, 0.009, 0, 0, 0, 1],
pose2list(det.pose))
object_apriltag = poseTransform(pose_webcam, '/head_camera',
'/map', lr)
has_apriltag = True
break
# compute contact point and normal if in contact
if norm(ft[0:2]) > threshold:
incontact = True
# 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.8).tolist())
else:
incontact = False
normal = npa([0, 0, 0])
contact_pt = npa([0, 0, 0])
# record robot pos, object pose, contact point, contact normal, and ft measurement
# 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 + [incontact] +
object_apriltag[0:3] + object_apriltag[6:7] +
object_apriltag[3:6] +
[pose3d_to_pose2d(object_apriltag)[2]] +
[has_apriltag])
setCart((direc * nsteps + start_pos).tolist() + [zup])
def main(argv):
parser = optparse.OptionParser()
parser.add_option('',
'--avgcolorbar',
action="store",
type='float',
dest='avgcolorbar',
help='Color bar',
nargs=2,
default=(0, 1))
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error("Usage: plot_friction_map.py [bag_file_path.h5]")
return
hdf5_filepath = args[0]
figfname = hdf5_filepath.replace('.h5', '_fmap.png')
figfname_std = hdf5_filepath.replace('.h5', '_fmapstd.png')
ft_wrench = []
max_x = 0.450
min_x = 0.250
rangex = [0.45, 0.35, 0.25]
# 0.450, 0.350, 0.250
max_y = 0.197 - 0.1
min_y = -0.233 + 0.1
rangey = [0.07, -0.03, -0.13]
# -0.13, -0.03, 0.07,
f = h5py.File(hdf5_filepath, "r")
tip_array = f['tip_array'].value
ft_wrench = f['ft_wrench'].value
radius = 0.01
image_vals = [[[], [], []], [[], [], []], [[], [], []]]
image_avg = [[1, 2, 3], [0, 0, 0], [0, 0, 0]]
image_std = [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
N = 0.8374 * 9.81
scale = (len(ft_wrench) * 1.0 / len(tip_array))
for i in range(len(tip_array)):
ind = None
for idx in range(len(rangex)):
for idy in range(len(rangey)):
if helper.norm(
np.array(tip_array[i][1:3]) -
np.array([rangex[idx], rangey[idy]])) < radius:
ind = (idx, idy)
break
if ind is not None:
break
ft_i = int(i * scale)
if ind:
image_vals[ind[0]][ind[1]].append(np.fabs(
ft_wrench[ft_i][2])) # force_y
for idx in range(len(rangex)):
for idy in range(len(rangey)):
print idx, idy, len(image_vals[idx][idy])
image_avg[idx][idy] = sum(image_vals[idx][idy]) / len(
image_vals[idx][idy]) / N
image_std[idx][idy] = np.std(image_vals[idx][idy]) / N
print image_avg
print image_std
plt.rc('font', family='serif', size=30)
plt.imshow(image_avg,
extent=(rangey[0] + 0.05, rangey[2] - 0.05, rangex[2] - 0.05,
rangex[0] + 0.05),
interpolation='nearest',
cmap=cm.Greys,
vmin=opt.avgcolorbar[0],
vmax=opt.avgcolorbar[1])
plt.colorbar()
plt.savefig(figfname)
print figfname
plt.close()
plt.imshow(image_std,
extent=(rangey[0] + 0.05, rangey[2] - 0.05, rangex[2] - 0.05,
rangex[0] + 0.05),
interpolation='nearest',
cmap=cm.Greys,
vmin=0.01,
vmax=0.03)
plt.colorbar()
plt.savefig(figfname_std)
def main(argv):
parser = optparse.OptionParser()
parser.add_option('', '--fmt', action="store", dest='fmt',
help='Figure format e.g. png, pdf', default='png')
(opt, args) = parser.parse_args()
if len(args) < 2:
parser.error("Usage: plot_friction_change_over_time.py [file_pattern.h5]")
return
filepattern = args[0] # ./record_surface=plywood_shape=rect1_a=-1000_v=20_rep=%3d.h5
filenum = int(args[1]) # 100
figfname = filepattern.replace('.h5', '.%s' % opt.fmt)
max_x = 0.450
min_x = 0.250
rangex = [0.45, 0.35, 0.25]
max_y = 0.197-0.1
min_y = -0.233+0.1
rangey = [0.07, -0.03, -0.13]
target = [0.35, -0.03]
radius = 0.01
image_avgs_t = []
image_stds_t = []
N = 0.8374 * 9.81
nan = float('nan')
if not os.path.exists(filepattern.replace('.h5', '_fcot.h5')):
for f_ind in range(filenum):
hdf5_filepath = filepattern % f_ind
print 'processing', hdf5_filepath
if not os.path.exists(hdf5_filepath): # in case the data is bad
print 'not found'
image_avgs_t.append([[nan,nan,nan],[nan,nan,nan],[nan,nan,nan]])
image_stds_t.append([[nan,nan,nan],[nan,nan,nan],[nan,nan,nan]])
continue
f = h5py.File(hdf5_filepath, "r")
tip_array = f['tip_array'].value
ft_wrench = f['ft_wrench'].value
image_vals = [[[],[],[]],[[],[],[]],[[],[],[]]]
image_avgs = [[0,0,0],[0,0,0],[0,0,0]]
image_stds = [[0,0,0],[0,0,0],[0,0,0]]
scale = (len(ft_wrench)*1.0/len(tip_array))
for i in range(len(tip_array)):
ind = None
for idx in range(len(rangex)):
for idy in range(len(rangey)):
if helper.norm(np.array(tip_array[i][1:3]) - np.array([rangex[idx], rangey[idy]])) < radius:
ind = (idx,idy)
break
if ind is not None:
break
ft_i = int(i * scale)
if ind:
image_vals[ind[0]][ind[1]].append(np.fabs(ft_wrench[ft_i][2])) # force_y
for idx in range(len(rangex)):
for idy in range(len(rangey)):
if len(image_vals[idx][idy]) > 0:
image_avgs[idx][idy] = sum(image_vals[idx][idy]) / len(image_vals[idx][idy]) / N
image_stds[idx][idy] = np.std(image_vals[idx][idy]) / N
image_avgs_t.append(image_avgs)
image_stds_t.append(image_stds)
f.close()
with h5py.File(filepattern.replace('.h5', '_fcot.h5'), "w") as f:
f.create_dataset("image_avgs_t", data=image_avgs_t)
f.create_dataset("image_stds_t", data=image_stds_t)
else:
with h5py.File(filepattern.replace('.h5', '_fcot.h5'), "r") as f:
image_avgs_t = f['image_avgs_t'].value
image_stds_t = f['image_stds_t'].value
# for idx in range(len(rangex)):
# for idy in range(len(rangey)):
# tmp = []
# for t in range(filenum):
# tmp.append(image_avgs_t[t][idx][idy])
# plt.errorbar(range(1,filenum+1), tmp, fmt='-o', color='k')
rangexx = []
vals = []
print len(image_avgs_t)
for t in range(filenum):
tmp = 0
for idx in range(len(rangex)):
for idy in range(len(rangey)):
tmp += image_avgs_t[t][idx][idy] / 9.0
if not np.isnan(tmp):
vals.append(tmp)
rangexx.append(t+1)
plt.errorbar(rangexx, vals, fmt='-o', color='k')
plt.ylabel('Coefficient of friction')
plt.xlabel('Push time')
#plt.title('Change of frictional coefficient over time')
plt.savefig(figfname)
plt.show()
def main(argv):
parser = optparse.OptionParser()
parser.add_option('', '--avgcolorbar', action="store", type='float', dest='avgcolorbar',
help='Color bar', nargs=2, default=(0,1))
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error("Usage: plot_friction_map.py [bag_file_path.h5]")
return
hdf5_filepath = args[0]
figfname = hdf5_filepath.replace('.h5', '_fmap.png')
figfname_std = hdf5_filepath.replace('.h5', '_fmapstd.png')
ft_wrench = []
max_x = 0.450
min_x = 0.250
rangex = [0.45, 0.35, 0.25]
# 0.450, 0.350, 0.250
max_y = 0.197-0.1
min_y = -0.233+0.1
rangey = [0.07, -0.03, -0.13]
# -0.13, -0.03, 0.07,
f = h5py.File(hdf5_filepath, "r")
tip_array = f['tip_array'].value
ft_wrench = f['ft_wrench'].value
radius = 0.01
image_vals = [[[],[],[]],[[],[],[]],[[],[],[]]]
image_avg = [[1,2,3],[0,0,0],[0,0,0]]
image_std = [[0,0,0],[0,0,0],[0,0,0]]
N = 0.8374 * 9.81
scale = (len(ft_wrench)*1.0/len(tip_array))
for i in range(len(tip_array)):
ind = None
for idx in range(len(rangex)):
for idy in range(len(rangey)):
if helper.norm(np.array(tip_array[i][1:3]) - np.array([rangex[idx], rangey[idy]])) < radius:
ind = (idx,idy)
break
if ind is not None:
break
ft_i = int(i * scale)
if ind:
image_vals[ind[0]][ind[1]].append(np.fabs(ft_wrench[ft_i][2])) # force_y
for idx in range(len(rangex)):
for idy in range(len(rangey)):
print idx, idy, len(image_vals[idx][idy])
image_avg[idx][idy] = sum(image_vals[idx][idy]) / len(image_vals[idx][idy]) / N
image_std[idx][idy] = np.std(image_vals[idx][idy]) / N
print image_avg
print image_std
plt.rc('font', family='serif', size=30)
plt.imshow(image_avg, extent=(rangey[0]+0.05, rangey[2]-0.05, rangex[2]-0.05, rangex[0]+0.05),
interpolation='nearest', cmap=cm.Greys, vmin=opt.avgcolorbar[0], vmax=opt.avgcolorbar[1])
plt.colorbar()
plt.savefig(figfname)
print figfname
plt.close()
plt.imshow(image_std, extent=(rangey[0]+0.05, rangey[2]-0.05, rangex[2]-0.05, rangex[0]+0.05),
interpolation='nearest', cmap=cm.Greys, vmin=0.01, vmax=0.03)
plt.colorbar()
plt.savefig(figfname_std)
def run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt):
# hack to restart the script to prevent ros network issues.
global pub
limit = 100
cnt = 0
topics = ['-a']
# enumerate the possible trajectories
for cnt_acc, acc in enumerate(accelerations):
# enumerate the side we want to push
for i in range(nside):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
pos = np.array(shape[i]) *(1-s) + np.array(shape[(i+1) % len(shape)]) *(s)
pos = np.append(pos, [0])
tangent = np.array(shape[(i+1) % len(shape)]) - np.array(shape[i])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
elif shape_type == 'ellip':
(a,b) = shape[0][0], shape[0][1]
pos = [shape[0][0] * np.cos(s*2*np.pi), shape[0][1] * np.sin(s*2*np.pi), 0]
normal = [np.cos(s*2*np.pi)/a, np.sin(s*2*np.pi)/b, 0]
normal = normal / norm(normal) # normalize it
elif shape_type == 'polyapprox':
pos, normal = polyapprox(shape, s)
# enumerate the direction in which we want to push
for t in angles:
for rep in xrange(nrep):
if nrep > 1:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
else:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t)
bagfilepath = dir_save_bagfile+bagfilename
# if exists then skip it
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
# find the probe pos in contact in object frame
pos_probe_contact_object = pos + normal * probe_radius
# find the start point
direc = np.dot(tfm.euler_matrix(0,0,t) , normal.tolist() + [1])[0:3] # in the direction of moving out
pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact
if shape_type == 'polyapprox' and polyapprox_check_collision(shape, pos_start_probe_object, probe_radius):
print bagfilename, 'will be in collision', 'skip'
continue
# find the end point
pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact
# zero force torque sensor
rospy.sleep(0.1)
setZero()
wait_for_ft_calib()
# transform start and end to world frame
if hasRobot:
pos_start_probe_world = coordinateFrameTransform(pos_start_probe_object, obj_frame_id, global_frame_id, listener)
pos_end_probe_world = coordinateFrameTransform(pos_end_probe_object, obj_frame_id, global_frame_id, listener)
pos_contact_probe_world = coordinateFrameTransform(pos_probe_contact_object, obj_frame_id, global_frame_id, listener)
pos_center_obj_world = coordinateFrameTransform([0,0,0], obj_frame_id, global_frame_id, listener)
else:
pos_start_probe_world = pos_start_probe_object + center_world
pos_end_probe_world = pos_end_probe_object + center_world
pos_contact_probe_world = pos_probe_contact_object + center_world
pos_center_obj_world = center_world
# start bag recording
# move to startPos
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=global_highvel)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos,ori)
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos,ori)
setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
pub.publish('move_to_mid_pos')
setCart(mid_pos,ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if acc == 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=speeds[cnt_acc], ori=1000)
else: # there is acceleration
setAcc(acc=acc, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
pub.publish('start_pushing')
setCart(end_pos,ori)
pub.publish('end_pushing')
# end bag recording
helper.terminate_ros_node("/record")
setSpeed(tcp=global_highvel, ori=1000)
setAcc(acc=globalhighacc, deacc=globalhighacc)
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos,ori)
distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world))
# recover
recover(obj_slot, distance_obj_center > allowed_distance)
#pause()
cnt += 1
if cnt > limit: return
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)
def turn(a):
x = tfm.quaternion_multiply(a, ori_down)
return list(npa(x) / norm(x))