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
