def plot_goals(self): fig = plt.figure() #fig = plt.gcf() fig.set_size_inches(14,11,forward=True) ax = fig.add_subplot(111, projection='3d') X = self.clustered_goal_data[:,0,3] Y = self.clustered_goal_data[:,1,3] Z = self.clustered_goal_data[:,2,3] #print X,len(X),Y,len(Y),Z,len(Z) c = 'b' surf = ax.scatter(X, Y, Z,s=40, c=c,alpha=.5) #surf = ax.scatter(X, Y,s=40, c=c,alpha=.6) ax.set_xlabel('X Axis') ax.set_ylabel('Y Axis') ax.set_zlabel('Z Axis') ax.set_title(''.join(['Plot of goals from ',self.subject,'. Data: (',str(self.data_start),' - ',str(self.data_finish),')'])) #fig.colorbar(surf, shrink=0.5, aspect=5) rospack = rospkg.RosPack() pkg_path = rospack.get_path('hrl_base_selection') ax.set_xlim(-.2,.2) ax.set_ylim(-.2,.2) ax.set_zlim(-.2,.2) #plt.savefig(''.join([pkg_path, '/images/goals_plot_',self.model,'_',self.subject,'_numbers_',str(self.data_start),'_',str(self.data_finish),'.png']), bbox_inches='tight') plt.ion() plt.show() ut.get_keystroke('Hit a key to proceed next')
def plot_goals(self): fig = plt.figure() #fig = plt.gcf() fig.set_size_inches(14, 11, forward=True) ax = fig.add_subplot(111, projection='3d') X = self.clustered_goal_data[:, 0, 3] Y = self.clustered_goal_data[:, 1, 3] Z = self.clustered_goal_data[:, 2, 3] #print X,len(X),Y,len(Y),Z,len(Z) c = 'b' surf = ax.scatter(X, Y, Z, s=40, c=c, alpha=.5) #surf = ax.scatter(X, Y,s=40, c=c,alpha=.6) ax.set_xlabel('X Axis') ax.set_ylabel('Y Axis') ax.set_zlabel('Z Axis') ax.set_title(''.join([ 'Plot of goals from ', self.subject, '. Data: (', str(self.data_start), ' - ', str(self.data_finish), ')' ])) #fig.colorbar(surf, shrink=0.5, aspect=5) rospack = rospkg.RosPack() pkg_path = rospack.get_path('hrl_base_selection') ax.set_xlim(-.2, .2) ax.set_ylim(-.2, .2) ax.set_zlim(-.2, .2) #plt.savefig(''.join([pkg_path, '/images/goals_plot_',self.model,'_',self.subject,'_numbers_',str(self.data_start),'_',str(self.data_finish),'.png']), bbox_inches='tight') plt.ion() plt.show() ut.get_keystroke('Hit a key to proceed next')
def q_image_axis_angle(self, X): print "Number of data: ", X.shape[0] angle_array = np.zeros((X.shape[0],1)) direc_array = np.zeros((X.shape[0],3)) for i in xrange(len(X)): angle, direc = qt.quat_to_angle_and_axis(X[i,:]) angle_array[i,0] = angle direc_array[i,:] = direc # Normalize angles angle_array = (angle_array)/np.pi*180.0 # matplot setup fig = plt.figure(figsize=(12,12)) plt.rc('text', usetex=True) plt.rc('font', family='serif') ax = fig.add_subplot(111, projection='3d') # Plot a sphere r = 0.999 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ax.plot_surface(x*r, y*r, z*r, rstride=1, cstride=1, color='c', alpha = 0.4, linewidth = 0) # Plot quaternions cmap = plt.cm.hsv sc = ax.scatter(direc_array[:,0],direc_array[:,1],direc_array[:,2],c=angle_array,cmap=cmap,vmin=-180.0, vmax=180.0,s=100) #edgecolor='none' cbar = plt.colorbar(sc, ticks=np.arange(-180,180+30,30)) ax.set_aspect("equal") ax.set_xlim([-1.0,1.0]) ax.set_ylim([-1.0,1.0]) ax.set_zlim([-1.0,1.0]) font_dict={'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20,80) plt.ion() plt.show() #ax.mouse_init() ut.get_keystroke('Hit a key to proceed next') return
def scoopAndFeed(self): runScooping = True while runScooping: ## Scooping ----------------------------------- print "Initializing left arm for scooping" print self.armReachAction("leftArmInitScooping") print self.armReachAction("chooseManualBowlPos") ut.get_keystroke('Hit a key to proceed next') print 'Initializing scooping' print self.armReachAction('initArmScooping') time.sleep(2) self.log.log_start() print "Running scooping!" print self.armReachAction("runScooping") # time.sleep(1) self.log.close_log_file() ## Feeding ----------------------------------- print "Initializing left arm for feeding" print self.armReachAction("leftArmInitFeeding") time.sleep(2.0) print self.armReachAction("chooseManualHeadPos") print 'Initializing feeding' print self.armReachAction('initArmFeeding') time.sleep(2.0) self.log.log_start(secondDetector=True) ## ut.get_keystroke('Hit a key to proceed next') print "Running feeding!" print self.armReachAction("runFeeding") # time.sleep(1) self.log.close_log_file(secondDetector=True) runScoopingAns = raw_input("Run process again? [y/n] ") while runScoopingAns != 'y' and runScoopingAns != 'n': print "Please enter 'y' or 'n' ! " runScoopingAns = raw_input("Run process again? [y/n] ") runScooping = runScoopingAns == 'y' print "Finished scooping and feeding trials!" print "Exiting program!" sys.exit()
tag_id = 9 tag_side_length = 0.053 #0.033 pos_thres = 0.2 max_idx = 18 save_file = '/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/params/ar_tag/bowl_offsetframe.pkl' atd = arTagDetector(tag_id, tag_side_length, pos_thres) if opt.bRenew == False: if atd.getCalibration(save_file) == False: opt.bRenew=True rate = rospy.Rate(10) # 25Hz, nominally. while not rospy.is_shutdown(): if opt.bVirtual: atd.pubVirtualBowlcenPose() continue ## ret = input("Is bowl tag fine? ") if atd.bowl_calib == False and opt.bRenew == True: ret = ut.get_keystroke('Is bowl tag fine? (y: yes, n: no)') if ret == 'y': atd.setCalibration(save_file) rate.sleep()
tag_id = 10 #9 tag_side_length = 0.053 #0.033 pos_thres = 0.2 max_idx = 18 save_file = '/home/akapusta/git/hrl-assistive/hrl_base_selection/params/ar_tag/mouth_offsetframe.pkl' atd = arTagDetector(tag_id, tag_side_length, pos_thres) if opt.bRenew == False: if atd.getCalibration(save_file) == False: opt.bRenew=True rate = rospy.Rate(10) # 25Hz, nominally. while not rospy.is_shutdown(): if opt.bVirtual: atd.pubVirtualMouthPose() continue ## ret = input("Is head tag fine? ") if atd.head_calib == False and opt.bRenew == True: ret = ut.get_keystroke('Is head tag fine? (y: yes, n: no)') if ret == 'y': atd.setCalibration(save_file) rate.sleep()
opt, args = p.parse_args() total_tags = 1 tag_id = 9 tag_side_length = 0.053 #0.033 pos_thres = 0.2 max_idx = 18 save_file = '/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/params/ar_tag/bowl_offsetframe.pkl' atd = arTagDetector(tag_id, tag_side_length, pos_thres) if opt.bRenew == False: if atd.getCalibration(save_file) == False: opt.bRenew=True rate = rospy.Rate(10) # 25Hz, nominally. while not rospy.is_shutdown(): ## ret = input("Is bowl tag fine? ") if atd.bowl_calib == False and opt.bRenew == True: ret = ut.get_keystroke('Is bowl tag fine? (y: yes, n: no)') if ret == 'y': atd.setCalibration(save_file) rate.sleep()
def q_image_axis_cluster(self, X, Y): print "Number of data: ", X.shape[0] angle_array = np.zeros((X.shape[0],1)) direc_array = np.zeros((X.shape[0],3)) for i in xrange(len(X)): angle, direc = qt.quat_to_angle_and_axis(X[i,:]) angle_array[i,0] = angle direc_array[i,:] = direc ## # Normalize angles angle_array = (angle_array)/np.pi*180.0 # Normalize labels max_label = float(np.max(Y)) fY = np.zeros((len(Y),1)) if max_label != 0: for i in xrange(len(Y)): fY[i] = float(Y[i])/max_label # matplot setup fig = plt.figure(figsize=(24,12)) plt.rc('text', usetex=True) plt.rc('font', family='serif') #-------------- matplot 1 -------------- ax = fig.add_subplot(121, projection='3d') font_dict={'fontsize': 45, 'family': 'serif'} ax.set_title("QuTEM distribution", fontdict=font_dict) # Plot a sphere r = 1.0 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ax.plot_surface(x*r, y*r, z*r, rstride=1, cstride=1, color='c', alpha = 0.4, linewidth = 0) # Plot quaternions cmap = plt.cm.hsv sc = ax.scatter(direc_array[:,0],direc_array[:,1],direc_array[:,2],c=angle_array,cmap=cmap,vmin=-180.0, vmax=180.0,s=100) #edgecolor='none' cbar = plt.colorbar(sc, ticks=np.arange(-180,180+30,30)) ## cbar.set_clim(-180.0, 180.0) ax.set_aspect("equal") ax.set_xlim([-1.0,1.0]) ax.set_ylim([-1.0,1.0]) ax.set_zlim([-1.0,1.0]) font_dict={'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20,40) #-------------- matplot 2 -------------- ax = fig.add_subplot(122, projection='3d') font_dict={'fontsize': 45, 'family': 'serif'} ax.set_title("Clustering", fontdict=font_dict) # Plot a sphere r = 0.92 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ## ax.plot_surface(x*r, y*r, z*r, rstride=1, cstride=1, color='c', alpha = 1.0, linewidth = 0) # Plot quaternions cmap = plt.cm.jet sc = ax.scatter(direc_array[:,0],direc_array[:,1],direc_array[:,2],c=Y,vmin=0,vmax=abs(Y).max(),s=100) ## plt.colorbar(sc) ax.set_aspect("equal") ax.set_xlim([-1.0,1.0]) ax.set_ylim([-1.0,1.0]) ax.set_zlim([-1.0,1.0]) font_dict={'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20,40) plt.ion() plt.show() #ax.mouse_init() ut.get_keystroke('Hit a key to proceed next') return
def q_image_axis_cluster(self, X, Y): print "Number of data: ", X.shape[0] angle_array = np.zeros((X.shape[0], 1)) direc_array = np.zeros((X.shape[0], 3)) for i in xrange(len(X)): angle, direc = qt.quat_to_angle_and_axis(X[i, :]) angle_array[i, 0] = angle direc_array[i, :] = direc ## # Normalize angles angle_array = (angle_array) / np.pi * 180.0 # Normalize labels max_label = float(np.max(Y)) fY = np.zeros((len(Y), 1)) if max_label != 0: for i in xrange(len(Y)): fY[i] = float(Y[i]) / max_label # matplot setup fig = plt.figure(figsize=(24, 12)) plt.rc('text', usetex=True) plt.rc('font', family='serif') #-------------- matplot 1 -------------- ax = fig.add_subplot(121, projection='3d') font_dict = {'fontsize': 45, 'family': 'serif'} ax.set_title("QuTEM distribution", fontdict=font_dict) # Plot a sphere r = 1.0 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ax.plot_surface(x * r, y * r, z * r, rstride=1, cstride=1, color='c', alpha=0.4, linewidth=0) # Plot quaternions cmap = plt.cm.hsv sc = ax.scatter(direc_array[:, 0], direc_array[:, 1], direc_array[:, 2], c=angle_array, cmap=cmap, vmin=-180.0, vmax=180.0, s=100) #edgecolor='none' cbar = plt.colorbar(sc, ticks=np.arange(-180, 180 + 30, 30)) ## cbar.set_clim(-180.0, 180.0) ax.set_aspect("equal") ax.set_xlim([-1.0, 1.0]) ax.set_ylim([-1.0, 1.0]) ax.set_zlim([-1.0, 1.0]) font_dict = {'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20, 40) #-------------- matplot 2 -------------- ax = fig.add_subplot(122, projection='3d') font_dict = {'fontsize': 45, 'family': 'serif'} ax.set_title("Clustering", fontdict=font_dict) # Plot a sphere r = 0.92 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ## ax.plot_surface(x*r, y*r, z*r, rstride=1, cstride=1, color='c', alpha = 1.0, linewidth = 0) # Plot quaternions cmap = plt.cm.jet sc = ax.scatter(direc_array[:, 0], direc_array[:, 1], direc_array[:, 2], c=Y, vmin=0, vmax=abs(Y).max(), s=100) ## plt.colorbar(sc) ax.set_aspect("equal") ax.set_xlim([-1.0, 1.0]) ax.set_ylim([-1.0, 1.0]) ax.set_zlim([-1.0, 1.0]) font_dict = {'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20, 40) plt.ion() plt.show() #ax.mouse_init() ut.get_keystroke('Hit a key to proceed next') return
def q_image_axis_angle(self, X): print "Number of data: ", X.shape[0] angle_array = np.zeros((X.shape[0], 1)) direc_array = np.zeros((X.shape[0], 3)) for i in xrange(len(X)): angle, direc = qt.quat_to_angle_and_axis(X[i, :]) angle_array[i, 0] = angle direc_array[i, :] = direc # Normalize angles angle_array = (angle_array) / np.pi * 180.0 # matplot setup fig = plt.figure(figsize=(12, 12)) plt.rc('text', usetex=True) plt.rc('font', family='serif') ax = fig.add_subplot(111, projection='3d') # Plot a sphere r = 0.999 u = np.linspace(0, 2 * np.pi, 120) v = np.linspace(0, np.pi, 60) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) ax.plot_surface(x * r, y * r, z * r, rstride=1, cstride=1, color='c', alpha=0.4, linewidth=0) # Plot quaternions cmap = plt.cm.hsv sc = ax.scatter(direc_array[:, 0], direc_array[:, 1], direc_array[:, 2], c=angle_array, cmap=cmap, vmin=-180.0, vmax=180.0, s=100) #edgecolor='none' cbar = plt.colorbar(sc, ticks=np.arange(-180, 180 + 30, 30)) ax.set_aspect("equal") ax.set_xlim([-1.0, 1.0]) ax.set_ylim([-1.0, 1.0]) ax.set_zlim([-1.0, 1.0]) font_dict = {'fontsize': 30, 'family': 'serif'} ax.set_xlabel('x', fontdict=font_dict) ax.set_ylabel('y', fontdict=font_dict) ax.set_zlabel('z', fontdict=font_dict) ax.view_init(20, 80) plt.ion() plt.show() #ax.mouse_init() ut.get_keystroke('Hit a key to proceed next') return
## Scooping ----------------------------------- print "Initializing left arm for scooping" print armReachActionLeft("initScooping") print armReachActionRight("initScooping") #ut.get_keystroke('Hit a key to proceed next') print armReachActionLeft("getBowlPos") print armReachActionLeft('lookAtBowl') print "Running scooping!" print armReachActionLeft("runScooping") ## Feeding ----------------------------------- print "Initializing left arm for feeding" print armReachActionRight("initFeeding") print armReachActionLeft("initFeeding") print "Detect ar tag on the head" print armReachActionLeft('lookAtMouth') print armReachActionLeft("getHeadPos") ut.get_keystroke('Hit a key to proceed next') print "Running feeding!" print armReachActionLeft("runFeeding1") print armReachActionLeft("runFeeding2") ## t1 = datetime.datetime.now() ## t2 = datetime.datetime.now() ## t = t2-t1 ## print "time delay: ", t.seconds
print armReachActionLeft("initScooping") print armReachActionRight("initScooping") #ut.get_keystroke('Hit a key to proceed next') print armReachActionLeft("getBowlPos") print armReachActionLeft('lookAtBowl') print "Running scooping!" print armReachActionLeft("runScooping") ## Feeding ----------------------------------- print "Initializing left arm for feeding" print armReachActionRight("initFeeding") print armReachActionLeft("initFeeding") print "Detect ar tag on the head" print armReachActionLeft('lookAtMouth') print armReachActionLeft("getHeadPos") ut.get_keystroke('Hit a key to proceed next') print "Running feeding!" print armReachActionLeft("runFeeding1") print armReachActionLeft("runFeeding2") ## t1 = datetime.datetime.now() ## t2 = datetime.datetime.now() ## t = t2-t1 ## print "time delay: ", t.seconds
calib_data[idxs] = 0. idxs = np.where(calib_data > meka_taxel_saturate_threshold)[0] calib_data[idxs] = saturated_taxel_force_value return calib_data def zero_sensor_cb(msg): scn.compute_bias(rdc, 10) if __name__ == '__main__': rospy.init_node('forearm_raw_data_subscriber') zero_sensor_subscriber = rospy.Subscriber('zero_sensor', Empty, zero_sensor_cb) scn = SkinCalibration_Naive() scn.precompute_taxel_location_and_normal() rdc = RawDataClient('taxels/raw_data') scn.compute_bias(rdc, 1000) while not rospy.is_shutdown(): d = rdc.get_raw_data(True) scn.publish_taxel_array(d) if False: ut.get_keystroke('Hit a key to get biased data') d = rdc.get_biased_data(False, 5) print 'd:', d
if __name__ == '__main__': rospy.init_node('forearm_raw_data_subscriber') zero_sensor_subscriber = rospy.Subscriber('zero_sensor', Empty, zero_sensor_cb) scn = SkinCalibration_Naive() scn.precompute_taxel_location_and_normal() rdc = RawDataClient('taxels/raw_data') scn.compute_bias(rdc, 1000) while not rospy.is_shutdown(): d = rdc.get_raw_data(True) scn.publish_taxel_array(d) if False: ut.get_keystroke('Hit a key to get biased data') d = rdc.get_biased_data(False, 5) print 'd:', d