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