def pixelToWorld(u, v):
# u, v are x, y coord in pixel frame respectively, z is the height of the camera above the table
# rospy.init_node('Sawyer_wrist_cam_tf_listener')
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('/base', '/right_gripper_base', rospy.Time(0))
# print('trans is', trans)
# print('rot is', rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if trans is None:
continue
else:
break
rate.sleep()
z = trans[2]
trans = np.array(trans)
trans.shape = (3, 1)
# print(trans)
hom_Mtrx_g_b = transformations.quaternion_matrix(rot)
hom_Mtrx_g_b[0][3] = trans[0]
hom_Mtrx_g_b[1][3] = trans[1]
hom_Mtrx_g_b[2][3] = trans[2]
# print("homogeneous transformation from /gripper_base to /base is:")
# print(hom_Mtrx_g_b)
hom_Mtrx_c_g = transformations.rotation_matrix(-math.pi / 2.0, [0, 0, 1], [0, 0, 0])
hom_Mtrx_c_g[0][3] = 0.1
# print("homogeneous transformation from /camera to /gripper_base is:")
# print(hom_Mtrx_c_g)
hom_Mtrx_c_b = np.dot(hom_Mtrx_g_b, hom_Mtrx_c_g)
# print("homogeneous transformation from /camera to /base is:")
# print(hom_Mtrx_c_b)
Mtrx_c_b = hom_Mtrx_c_b[:3, :4]
Mtrx_c_b = np.matrix(Mtrx_c_b)
# print("transformation from /camera to /gripper_base is:")
# print(Mtrx_c_b)
camMtrx = camCalib.getCamMatrx()
camMtrxInv = np.linalg.inv(camMtrx)
camMtrxInv = np.matrix(camMtrxInv)
# z = 0.5 # height of camera above the table
pixVec = np.matrix([[z * u], [z * v], [z * 1]]) # pixel vector augmented by 1
# testVec = camMtrxInv*z*pixVec
one = np.array([1])
one.shape = (1, 1)
camVec = np.concatenate((camMtrxInv * pixVec, one), axis=0)
worldVec = Mtrx_c_b * camVec
# print(camVec)
# print(Mtrx_c_b * camVec)
return worldVec
def pixelToWorld(u, v):
# u is x, v is y
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
# transform from '/gripper' (target) frame to '/base' (source) frame
(trans, rot) = listener.lookupTransform('/base',
'/right_gripper_base',
rospy.Time(0))
# print('trans is', trans)
# print('rot is', rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if trans is None:
continue
else:
break
rate.sleep()
z = trans[2]
trans = np.array(trans)
trans.shape = (3, 1)
# print(trans)
hom_Mtrx_g_b = transformations.quaternion_matrix(rot)
hom_Mtrx_g_b[0][3] = trans[0]
hom_Mtrx_g_b[1][3] = trans[1]
hom_Mtrx_g_b[2][3] = trans[2]
# print("homogeneous transformation from /gripper_base to /base is:")
# print(hom_Mtrx_g_b)
hom_Mtrx_c_g = transformations.rotation_matrix(-math.pi / 2.0, [0, 0, 1],
[0, 0, 0])
# TODO: change this value to address camera offset in x direction
hom_Mtrx_c_g[0][3] = 0.05
# TODO: change this value to address camera offset in y direction
hom_Mtrx_c_g[1][3] = 0.01
# TODO: change this value to address camera offset in z direction
hom_Mtrx_c_g[2][3] = 0.07
# print("homogeneous transformation from /camera to /gripper_base is:")
# print(hom_Mtrx_c_g)
hom_Mtrx_c_b = np.dot(hom_Mtrx_g_b, hom_Mtrx_c_g)
# print("homogeneous transformation from /camera to /base is:")
# print(hom_Mtrx_c_b)
Mtrx_c_b = hom_Mtrx_c_b[:3, :4]
Mtrx_c_b = np.matrix(Mtrx_c_b)
# print("transformation from /camera to /gripper_base is:")
# print(Mtrx_c_b)
camMtrx = camCalib.getCamMatrx()
camMtrxInv = np.linalg.inv(camMtrx)
camMtrxInv = np.matrix(camMtrxInv)
# z = 0.5 # height of camera above the table
pixVec = np.matrix([[z * u], [z * v],
[z * 1]]) # pixel vector augmented by 1
# testVec = camMtrxInv*z*pixVec
one = np.array([1])
one.shape = (1, 1)
camVec = np.concatenate((camMtrxInv * pixVec, one), axis=0)
worldVec = Mtrx_c_b * camVec
# print(camVec)
# print(Mtrx_c_b * camVec)
return worldVec, hom_Mtrx_c_b, rot
def pixelToWorld(u, v):
# u is x, v is y
## all the ## comment are from Zihan && Frank summer research project
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
# transform from '/gripper' (target) frame to '/base' (source) frame
(trans, rot) = listener.lookupTransform('/base', '/right_gripper_base', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if trans is None:
continue
else:
break
rate.sleep()
# print trans
# print rot
z = trans[2]
trans = np.array(trans)
trans.shape = (3, 1)
# print trans
hom_Mtrx_g_b = transformations.quaternion_matrix(rot) ## using quaternion matrix to get transformation matrix
## modifying the x-y-z position vector
hom_Mtrx_g_b[0][3] = trans[0]
hom_Mtrx_g_b[1][3] = trans[1]
hom_Mtrx_g_b[2][3] = trans[2]
# print hom_Mtrx_g_b
# homogeneous transformation from /camera to /gripper
hom_Mtrx_c_g = transformations.rotation_matrix(-math.pi / 2.0, [0, 0, 1], [0, 0, 0]) ## this is the key thing to check and verify
# hom_Mtrx_c_g = transformations.rotation_matrix(0, (0, 0, 1))
hom_Mtrx_c_g[0][3] = in_to_m(-3.2) ## this is a guess number from gripper to camera
# print hom_Mtrx_c_g
## this is just calculating the transformation matrix from base to camera
hom_Mtrx_c_b = np.dot(hom_Mtrx_g_b, hom_Mtrx_c_g)
# print hom_Mtrx_c_b
Mtrx_c_b = hom_Mtrx_c_b[:3, :4]
# print Mtrx_c_b
Mtrx_c_b = np.matrix(Mtrx_c_b)
# print Mtrx_c_b
camMtrx = camCalib.getCamMatrx()
# print camMtrx
camMtrxInv = np.linalg.inv(camMtrx)
# print camMtrxInv
camMtrxInv = np.matrix(camMtrxInv)
# print camMtrxInv
pixVec = np.matrix([[z * u], [z * v], [z * 1]]) # pixel vector augmented by 1
# print pixVec
# testVec = camMtrxInv*z*pixVec
one = np.array([1])
one.shape = (1, 1)
camVec = np.concatenate((camMtrxInv * pixVec, one), axis=0)
# print camVec
worldVec = Mtrx_c_b * camVec
return worldVec, hom_Mtrx_c_b, rot
