def generate_score(self):
# for item in data:
# print Bmat_to_pos_quat(item)
num = np.ones([len(self.goals), 1])
reference_options = ['head']
reference = np.zeros([len(self.goals), 1])
print 'Starting to convert data!'
run_data = DataReader(subject=self.subject, model=self.model, task=self.task)
run_data.receive_input_data(self.goals, num, reference_options, reference)
run_data.generate_output_goals()
run_data.generate_score(viz_rviz=True, visualize=self.visualize, plot=False)
def load_goals(self, task, model):
print 'Reading in raw data from the task.'
read_task_data = DataReader_Task(task, model)
raw_data, raw_num, raw_reference, self.raw_reference_options = read_task_data.reset_goals()
read_data = DataReader(subject=subject, data_start=data_start, reference_options=self.raw_reference_options,
data_finish=data_finish, model=model, task=task)
# raw_data = read_data.get_raw_data()
print 'Raw data is ready!'
self.goal_data = read_data.generate_output_goals(test_goals=raw_data, test_number=raw_num, test_reference=raw_reference)
# print 'Setting up openrave'
# self.setup_openrave()
# print 'I will now pick base locations to evaluate. They will share the same reachability score, but will have' \
# ' differing manipulability scores.'
# print 'before sorting:'
# for i in xrange(10):
# print self.scores[i]
self.scores = np.array(sorted(self.scores, key=lambda t: (t[1][1], t[1][2]), reverse=True))
# print 'after sorting:'
# for i in xrange(10):
# print self.scores[i]
self.best_base = self.scores[0]
if self.best_base[1][1] == 0:
print 'There are no base locations with reachable goals. Something went wrong in the scoring or the setup'
print 'The best base location is: \n', self.best_base
visualize_best = True
if visualize_best:
self.visualize_base_config(self.best_base, self.goal_data, self.raw_reference_options)
def __init__(self, visualize=False, subject='any_subject', task='yogurt', model='chair', tf_listener=None):
self.model = model
self.task = task
self.subject = subject
baselink_B_liftlink = createBMatrix([-0.05, 0.0, 0.8897], [0, 0, 0, 1])
goals = [[0.301033944729, 0.461276517595, 0.196885866571,
0.553557277528, 0.336724229346, -0.075691681684, 0.757932650828],
[0.377839595079, 0.11569018662, 0.0419789999723,
0.66106069088, 0.337429642677, -0.519856214523, 0.422953367233],
[0.2741387011303321, 0.005522571699560719, -0.011919598309888757,
-0.023580897114171894, 0.7483633417869068, 0.662774596931439, 0.011228696415565394],
[0.13608632401364894, 0.003540318703608347, 0.00607600258150498,
-0.015224467044577382, 0.7345761465214938, 0.6783020152473445, -0.008513323454022942]]
liftlink_B_goal = createBMatrix([0.5309877259429142, 0.4976163448816489, 0.16719537682372823],
[0.7765742993649133, -0.37100605554316285, -0.27784851903166524,
0.42671660945891])
data = np.array([baselink_B_liftlink*createBMatrix(goals[0][0:3], goals[0][3:]), # In reference to base link
baselink_B_liftlink*createBMatrix(goals[1][0:3], goals[1][3:]), # In reference to base link
createBMatrix(goals[2][0:3], goals[2][3:]),
createBMatrix(goals[3][0:3], goals[3][3:])]) # This one is in reference to the head
for item in data:
print Bmat_to_pos_quat(item)
# For my information, these are the [xyz] and quaternion [x,y,z,w] for the PoseStamped messages for the goal
# positions. The first two have parent tf /base_link. The third has parent link /head
# (array([ 0.48098773, 0.49761634, 0.91837238]), array([ 0.7765743 , -0.37100606, -0.27784852, 0.42671661]))
# (array([ 0.4598544 , 0.8806009 , 0.65371782]), array([ 0.45253993, 0.53399713, -0.17283745, 0.69295158]))
# (array([ 0.2741387 , 0.05522572, -0.0119196 ]), array([-0.0235809 , 0.74836334, 0.6627746 , 0.0112287 ]))
num = np.ones([len(data), 1])
reference_options = ['head', 'base_link']
reference = np.array([[1], [1], [0], [0]])
print 'Starting to convert data!'
runData = DataReader(subject=self.subject, model=self.model, task=self.task)
runData.receive_input_data(data, num, reference_options, reference)
runData.generate_output_goals()
runData.generate_score(viz_rviz=True, visualize=False, plot=False)
def generate_score(self):
# for item in data:
# print Bmat_to_pos_quat(item)
num = np.ones([len(self.goals), 1])
reference_options = ['head']
reference = np.zeros([len(self.goals), 1])
print 'Starting to convert data!'
run_data = DataReader(subject=self.subject,
model=self.model,
task=self.task)
run_data.receive_input_data(self.goals, num, reference_options,
reference)
run_data.generate_output_goals()
run_data.generate_score(viz_rviz=True,
visualize=self.visualize,
plot=False)
def load_goals(self, task, model):
print 'Reading in raw data from the task.'
read_task_data = DataReader_Task(task, model)
raw_data, raw_num, raw_reference, self.raw_reference_options = read_task_data.reset_goals(
)
read_data = DataReader(subject=subject,
data_start=data_start,
reference_options=self.raw_reference_options,
data_finish=data_finish,
model=model,
task=task)
# raw_data = read_data.get_raw_data()
print 'Raw data is ready!'
self.goal_data = read_data.generate_output_goals(
test_goals=raw_data,
test_number=raw_num,
test_reference=raw_reference)
# print 'Setting up openrave'
# self.setup_openrave()
# print 'I will now pick base locations to evaluate. They will share the same reachability score, but will have' \
# ' differing manipulability scores.'
# print 'before sorting:'
# for i in xrange(10):
# print self.scores[i]
self.scores = np.array(
sorted(self.scores, key=lambda t: (t[1][1], t[1][2]),
reverse=True))
# print 'after sorting:'
# for i in xrange(10):
# print self.scores[i]
self.best_base = self.scores[0]
if self.best_base[1][1] == 0:
print 'There are no base locations with reachable goals. Something went wrong in the scoring or the setup'
print 'The best base location is: \n', self.best_base
visualize_best = True
if visualize_best:
self.visualize_base_config(self.best_base, self.goal_data,
self.raw_reference_options)
def __init__(self,
visualize=False,
subject='any_subject',
task='yogurt',
model='chair',
tf_listener=None):
self.model = model
self.task = task
self.subject = subject
baselink_B_liftlink = createBMatrix([-0.05, 0.0, 0.8897], [0, 0, 0, 1])
goals = [[
0.301033944729, 0.461276517595, 0.196885866571, 0.553557277528,
0.336724229346, -0.075691681684, 0.757932650828
],
[
0.377839595079, 0.11569018662, 0.0419789999723,
0.66106069088, 0.337429642677, -0.519856214523,
0.422953367233
],
[
0.2741387011303321, 0.005522571699560719,
-0.011919598309888757, -0.023580897114171894,
0.7483633417869068, 0.662774596931439,
0.011228696415565394
],
[
0.13608632401364894, 0.003540318703608347,
0.00607600258150498, -0.015224467044577382,
0.7345761465214938, 0.6783020152473445,
-0.008513323454022942
]]
liftlink_B_goal = createBMatrix(
[0.5309877259429142, 0.4976163448816489, 0.16719537682372823], [
0.7765742993649133, -0.37100605554316285, -0.27784851903166524,
0.42671660945891
])
data = np.array([
baselink_B_liftlink * createBMatrix(
goals[0][0:3], goals[0][3:]), # In reference to base link
baselink_B_liftlink * createBMatrix(
goals[1][0:3], goals[1][3:]), # In reference to base link
createBMatrix(goals[2][0:3], goals[2][3:]),
createBMatrix(goals[3][0:3], goals[3][3:])
]) # This one is in reference to the head
for item in data:
print Bmat_to_pos_quat(item)
# For my information, these are the [xyz] and quaternion [x,y,z,w] for the PoseStamped messages for the goal
# positions. The first two have parent tf /base_link. The third has parent link /head
# (array([ 0.48098773, 0.49761634, 0.91837238]), array([ 0.7765743 , -0.37100606, -0.27784852, 0.42671661]))
# (array([ 0.4598544 , 0.8806009 , 0.65371782]), array([ 0.45253993, 0.53399713, -0.17283745, 0.69295158]))
# (array([ 0.2741387 , 0.05522572, -0.0119196 ]), array([-0.0235809 , 0.74836334, 0.6627746 , 0.0112287 ]))
num = np.ones([len(data), 1])
reference_options = ['head', 'base_link']
reference = np.array([[1], [1], [0], [0]])
print 'Starting to convert data!'
runData = DataReader(subject=self.subject,
model=self.model,
task=self.task)
runData.receive_input_data(data, num, reference_options, reference)
runData.generate_output_goals()
runData.generate_score(viz_rviz=True, visualize=False, plot=False)
def __init__(self, visualize_best=False, train_subj=6, test_subj=6):
output_raw_scores = False
# compare = True
self.visualize_best = visualize_best
self.tf_listener = tf.TransformListener()
self.train_subj = train_subj
self.test_subj = test_subj
print 'I will use data that was trained on subject ', self.train_subj
print 'I will test on data from subject ', self.test_subj
self.task = 'shaving' # options are: bathing, brushing, feeding, shaving, scratching_upper_arm/forearm/thigh/chest/knee
self.model = 'chair' # options are: 'chair', 'bed', 'autobed'
pos_clust = 2
ori_clust = 2
self.mc_simulation_number = None
self.visualize = False
data_start = 0
data_finish = 'end ' # 2000 # 4000 #'end'
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('hrl_base_selection')
print 'Loading scores.'
self.loaded_scores = self.load_task(self.task, self.model,
self.train_subj)
if self.loaded_scores is None:
print 'The scores do not exist. Must generate scores! This may take a long time...'
self.generate_scores(data_start, data_finish, pos_clust, ori_clust)
print 'Scores generated. I will now continue.'
print 'Now loading the scores I just generated'
self.loaded_scores = self.load_task(self.task, self.model,
self.train_subj)
if self.loaded_scores is None:
print 'The scores still do not exist. This is bad. Fixes needed in code.'
return
headx = 0
heady = 0
self.scores = self.loaded_scores[headx, heady]
if output_raw_scores:
self.output_scores()
subject = ''.join(['sub', str(self.test_subj), '_shaver'])
print 'Reading in raw data from the task.'
read_task_data = DataReader_Task(self.task, self.model)
raw_data, raw_num, raw_reference, self.raw_reference_options = read_task_data.reset_goals(
)
read_data = DataReader(subject=subject,
data_start=data_start,
reference_options=self.raw_reference_options,
data_finish=data_finish,
model=self.model,
task=self.task,
tf_listener=self.tf_listener)
# raw_data = read_data.get_raw_data()
print 'Raw data is ready!'
self.goal_data = read_data.generate_output_goals(
test_goals=raw_data,
test_number=raw_num,
test_reference=raw_reference)
# print 'Setting up openrave'
# self.setup_openrave()
# print 'I will now pick base locations to evaluate. They will share the same reachability score, but will have' \
# ' differing manipulability scores.'
# print 'before sorting:'
# for i in xrange(10):
# print self.scores[i]
self.scores = np.array(
sorted(self.scores, key=lambda t: (t[1][1], t[1][2]),
reverse=True))
# print 'after sorting:'
# for i in xrange(10):
# print self.scores[i]
self.best_base = self.scores[0]
if self.best_base[1][1] == 0:
print 'There are no base locations with reachable goals. Something went wrong in the scoring or the setup'
print 'The best base location is: \n', self.best_base
if visualize_best:
read_data.pub_rviz()
self.visualize_base_config(self.best_base, self.goal_data,
self.raw_reference_options)
def __init__(self, visualize_best=False, train_subj=6, test_subj=6):
output_raw_scores = False
# compare = True
self.visualize_best = visualize_best
self.tf_listener = tf.TransformListener()
self.train_subj = train_subj
self.test_subj = test_subj
print 'I will use data that was trained on subject ', self.train_subj
print 'I will test on data from subject ', self.test_subj
self.task = 'shaving' # options are: bathing, brushing, feeding, shaving, scratching_upper_arm/forearm/thigh/chest/knee
self.model = 'chair' # options are: 'chair', 'bed', 'autobed'
pos_clust = 2
ori_clust = 2
self.mc_simulation_number = None
self.visualize = False
data_start = 0
data_finish = 'end ' # 2000 # 4000 #'end'
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('hrl_base_selection')
print 'Loading scores.'
self.loaded_scores = self.load_task(self.task, self.model, self.train_subj)
if self.loaded_scores is None:
print 'The scores do not exist. Must generate scores! This may take a long time...'
self.generate_scores(data_start, data_finish, pos_clust, ori_clust)
print 'Scores generated. I will now continue.'
print 'Now loading the scores I just generated'
self.loaded_scores = self.load_task(self.task, self.model, self.train_subj)
if self.loaded_scores is None:
print 'The scores still do not exist. This is bad. Fixes needed in code.'
return
headx = 0
heady = 0
self.scores = self.loaded_scores[headx, heady]
if output_raw_scores:
self.output_scores()
subject = ''.join(['sub', str(self.test_subj), '_shaver'])
print 'Reading in raw data from the task.'
read_task_data = DataReader_Task(self.task, self.model)
raw_data, raw_num, raw_reference, self.raw_reference_options = read_task_data.reset_goals()
read_data = DataReader(subject=subject, data_start=data_start, reference_options=self.raw_reference_options,
data_finish=data_finish, model=self.model, task=self.task, tf_listener=self.tf_listener)
# raw_data = read_data.get_raw_data()
print 'Raw data is ready!'
self.goal_data = read_data.generate_output_goals(test_goals=raw_data, test_number=raw_num, test_reference=raw_reference)
# print 'Setting up openrave'
# self.setup_openrave()
# print 'I will now pick base locations to evaluate. They will share the same reachability score, but will have' \
# ' differing manipulability scores.'
# print 'before sorting:'
# for i in xrange(10):
# print self.scores[i]
self.scores = np.array(sorted(self.scores, key=lambda t: (t[1][1], t[1][2]), reverse=True))
# print 'after sorting:'
# for i in xrange(10):
# print self.scores[i]
self.best_base = self.scores[0]
if self.best_base[1][1] == 0:
print 'There are no base locations with reachable goals. Something went wrong in the scoring or the setup'
print 'The best base location is: \n', self.best_base
if visualize_best:
read_data.pub_rviz()
self.visualize_base_config(self.best_base, self.goal_data, self.raw_reference_options)
