def callback(msg, ar_tags):
for i in range(0, len(msg.transforms)):
# YOUR CODE HERE
# The code should look at the transforms for each AR tag
# Then compute the rigid body transform between AR0 and AR1,
# AR0 and ARZ, AR1 and ARZ
# hint: use the functions you wrote in exp_quat_func
#note:callback gets called for every message, not every packet of 3.
#extract tag ID
tag = msg.transforms[i].child_frame_id
#extract rotation quaternion as an nd array
rotation_msg = msg.transforms[i].transform.rotation
rotation_quaternion = np.array(
[rotation_msg.x, rotation_msg.y, rotation_msg.z, rotation_msg.w])
#compute omega, theta, of the quaternion.
omega, theta = eqf.quaternion_to_exp(rotation_quaternion)
#extract translation as nd array
translation_msg = msg.transforms[i].transform.translation
translation = np.array(
[translation_msg.x, translation_msg.y, translation_msg.z])
#compute transform from camera to AR tag.
tag_transform = eqf.create_rbt(omega, theta, translation)
ar_tag_name = ar_tags[tag]
most_recent_transforms[ar_tag_name] = tag_transform
#insert tag_transform into the dictionary tags
print("Read tag " + str(tag))
print(tag)
if len(most_recent_transforms) is not 3:
print("ERROR: Tags is not the right length (3)")
else:
g0 = most_recent_transforms['ar0']
g1 = most_recent_transforms['ar1']
g2 = most_recent_transforms['ar2']
g01 = eqf.compute_gab(g0, g1)
g02 = eqf.compute_gab(g0, g2)
g12 = eqf.compute_gab(g1, g2)
print("g01 is ")
print(str(g01))
print("g02 is ")
print(str(g02))
print("g12 is ")
print(str(g12))
print(" ")
def callback(msg, ar_tags):
for i in range(0, len(msg.transforms)):
# YOUR CODE HERE
# The code should look at the transforms for each AR tag
# Then compute the rigid body transform between AR0 and AR1,
# AR0 and ARZ, AR1 and ARZ
# hint: use the functions you wrote in exp_quat_func
# note: you can change anything in this function to get it working
# note: you may want to save the last known position of the AR tag
#lkp[msg.transforms[i].child_frame_id] = 0 # position / orientation
id = msg.transforms[i].child_frame_id
if id not in ar_tags.values(): continue
seen.add(id)
quat = msg.transforms[i].transform.rotation
trans = msg.transforms[i].transform.translation
x = quat.x
y = quat.y
z = quat.z
w = quat.w
omega, theta = eqf.quaternion_to_exp(np.array([x,y,z,w]))
g=eqf.create_rbt(omega, theta, np.array([trans.x, trans.y, trans.z]))
lkp[msg.transforms[i].child_frame_id] = g # position / orientation
print seen
if len(lkp) == 3:
for j1 in lkp:
for j2 in lkp:
if j1 != j2:
print((j1,j2))
print(eqf.compute_gab(lkp[j1], lkp[j2]))
def callback(msg, ar_tags):
for i in range(0, len(msg.transforms)):
# YOUR CODE HERE
# The code should look at the transforms for each AR tag
# Then compute the rigid body transform between AR0 and AR1,
# AR0 and ARZ, AR1 and ARZ
# hint: use the functions you wrote in exp_quat_func
#note:callback gets called for every message, not every packet of 3.
#extract tag ID
tag = msg.transforms[i].child_frame_id
#extract rotation quaternion as an nd array
rotation_msg = msg.transforms[i].transform.rotation
rotation_quaternion = np.array([rotation_msg.x, rotation_msg.y, rotation_msg.z, rotation_msg.w])
#compute omega, theta, of the quaternion.
omega, theta = eqf.quaternion_to_exp(rotation_quaternion)
#extract translation as nd array
translation_msg = msg.transforms[i].transform.translation
translation = np.array([translation_msg.x, translation_msg.y, translation_msg.z])
#compute transform from camera to AR tag.
tag_transform = eqf.create_rbt(omega,theta,translation)
ar_tag_name = ar_tags[tag]
most_recent_transforms[ar_tag_name] = tag_transform
#insert tag_transform into the dictionary tags
print("Read tag " + str(tag))
print(tag)
if len(most_recent_transforms) is not 3:
print("ERROR: Tags is not the right length (3)")
else:
g0 = most_recent_transforms['ar0']
g1 = most_recent_transforms['ar1']
g2 = most_recent_transforms['ar2']
g01 = eqf.compute_gab(g0,g1)
g02 = eqf.compute_gab(g0,g2)
g12 = eqf.compute_gab(g1,g2)
print("g01 is ")
print(str(g01))
print("g02 is ")
print(str(g02))
print("g12 is ")
print(str(g12))
print(" ")
def callback(msg, ar_tags):
for i in range(0, len(msg.transforms)):
# YOUR CODE HERE
# The code should look at the transforms for each AR tag
# Then compute the rigid body transform between AR0 and AR1,
# AR0 and ARZ, AR1 and ARZ
# hint: use the functions you wrote in exp_quat_func
# note: you can change anything in this function to get it working
# note: you may want to save the last known position of the AR tag
lkp[msg.transforms[i].child_frame_id] = msg.transforms[i].transform # position / orientation
# print lkp
tag_name = lkp.keys()
marker1_pose = lkp['ar_marker_0']
marker1_rot = np.array([marker1_pose.rotation.x, marker1_pose.rotation.y,marker1_pose.rotation.z, marker1_pose.rotation.w])
marker1_trans = np.array([marker1_pose.translation.x, marker1_pose.translation.y,marker1_pose.translation.z])
marker1_exp = eqf.quaternion_to_exp(marker1_rot)
marker1_rbt = eqf.create_rbt(marker1_exp[0], marker1_exp[1], marker1_trans)
marker2_pose = lkp['ar_marker_1']
marker2_rot = np.array([marker2_pose.rotation.x, marker2_pose.rotation.y,marker2_pose.rotation.z, marker2_pose.rotation.w])
marker2_trans = np.array([marker2_pose.translation.x, marker2_pose.translation.y,marker2_pose.translation.z])
marker2_exp = eqf.quaternion_to_exp(marker2_rot)
marker2_rbt = eqf.create_rbt(marker2_exp[0], marker2_exp[1], marker2_trans)
marker3_pose = lkp['ar_marker_17']
marker3_rot = np.array([marker3_pose.rotation.x, marker3_pose.rotation.y,marker3_pose.rotation.z, marker3_pose.rotation.w])
marker3_trans = np.array([marker3_pose.translation.x, marker3_pose.translation.y,marker3_pose.translation.z])
marker3_exp = eqf.quaternion_to_exp(marker3_rot)
marker3_rbt = eqf.create_rbt(marker3_exp[0], marker3_exp[1], marker3_trans)
g01 = eqf.compute_gab(marker1_rbt,marker2_rbt)
g02 = eqf.compute_gab(marker1_rbt,marker3_rbt)
g12 = eqf.compute_gab(marker2_rbt,marker3_rbt)
print ('rot between tag 0 and 1')
print g01
print ('rot between tag 0 and 2')
print g02
print ('rot between tag 1 and 2')
print g12
def rel_rbt(a_t, a_r, b_t, b_r):
rbt_a = quat_to_rbt(a_t, a_r)
rbt_b = quat_to_rbt(b_t, b_r)
return eqf.compute_gab(rbt_a, rbt_b)
# twist = None
# trans = np.array([[1],[0],[0]])
# tnew = np.array([[0],[1],[0]])
# rot = np.array([[0],[0],[0],[1]])
# rw_cur = Transform(Vector3(trans[0], trans[1], trans[2]),Quaternion(rot[0], rot[1], rot[2], rot[3]))
# print rw_cur
# rw_des = Transform(Vector3(tnew[0], tnew[1], tnew[2]),Quaternion(rot[0], rot[1], rot[2], rot[3]))
# rw_cur = np.array([[1,0,0,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])
# rw_des = np.array([[1,0,0,1],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
o1, t1 = eqf.quaternion_to_exp(np.array([rw_cur.rotation.x, rw_cur.rotation.y, rw_cur.rotation.z, rw_cur.rotation.w]))
o2, t2 = eqf.quaternion_to_exp(np.array([rw_des.rotation.x, rw_des.rotation.y, rw_des.rotation.z, rw_des.rotation.w]))
print 'blah' + str([rw_des.rotation.x, rw_des.rotation.y, rw_des.rotation.z, rw_des.rotation.w])
# print 'o2' + str(o2)
rb_cur = eqf.create_rbt(o1, t1, np.array([rw_cur.translation.x, rw_cur.translation.y, rw_cur.translation.z]))
rb_des = eqf.create_rbt(o2, t2, np.array([rw_des.translation.x, rw_des.translation.y, rw_des.translation.z]))
homogeneous = eqf.compute_gab(rb_cur,rb_des)
print 'h**o'+str(homogeneous)
trans = homogeneous[0:3,3]
# (omega,theta) = eqf.find_omega_theta(homogeneous[0:2,0:2])
# try:
# (trans, rot) = listener.lookupTransform(rw_des, rw_cur, rospy.Time(0))
# except:
# print "not enough"
# continue
twist = [[0,0,0], [0,0,0]]
unfound = True
print 'trans'+str(trans)
if trans[0]**2+ trans[1]**2+ trans[2]**2 < .01:
pub.publish(Twist(Vector3(0,0,0), Vector3(0,0,0)))
# print 'twist'+str(twist[0][0]**2+ twist[0][1]**2+ twist[0][2]**2)
pub_state.publish(doneWith(True, rw_des.translation.x, rw_des.translation.y))
