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