from utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot
from utils.transformations import euler_from_quaternion
import rospy
import roslib
roslib.load_manifest("tf")
import tf
import numpy as np
class Globals:
listener = None
if rospy.get_name() == "/unnamed":
rospy.init_node("transform_kinect_frames")
Globals.listener = tf.TransformListener()
rospy.sleep(1)
ONIfromCL = trans_rot_to_hmat(*Globals.listener.lookupTransform("/camera_rgb_optical_frame","camera_link", rospy.Time(0)))
GAZfromONI = trans_rot_to_hmat([0, -.15, -.24], [0, 0, 0, 1])
HEADfromGAZ = trans_rot_to_hmat(*Globals.listener.lookupTransform("/head_plate_frame", "/wide_stereo_gazebo_l_stereo_camera_optical_frame",rospy.Time(0)))
HEADfromCL = np.dot(np.dot(HEADfromGAZ, GAZfromONI), ONIfromCL)
trans_final, quat_final = hmat_to_trans_rot(HEADfromCL)
eul_final = euler_from_quaternion(quat_final)
print "translation", trans_final
print "rotation", eul_final
print "%.4f %.4f %.4f %.4f %.4f %.4f %.4f"%(tuple(trans_final) + tuple(quat_final))
def transform_demo_with_fingertips(reg, demo, left=True, right=True, cloud_xyz=False):
"""
reg: NonrigidRegistration object
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz
(todo: replace 'cloud_xyz' with 'object_points')
"""
warped_demo = dict([(key,np.asarray(value)) for (key, value) in demo.items()])
if left:
_, ori_warped = reg.transform_frames(demo["l_gripper_xyzs"], quats2mats(demo["l_gripper_quats"]))
l_xyzs1 = reg.transform_points(demo["l_gripper_xyzs1"])
l_xyzs2 = reg.transform_points(demo["l_gripper_xyzs2"])
tool_xyzs = []
tool_quats = []
tool_angles = []
for (pos1, pos2, ori) in zip(l_xyzs1, l_xyzs2, ori_warped):
hmat, ang = calc_hand_pose(pos1, pos2, ori)
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
warped_demo["l_gripper_xyzs"] = tool_xyzs
warped_demo["l_gripper_xyzs1"] = l_xyzs1
warped_demo["l_gripper_xyzs2"] = l_xyzs2
warped_demo["l_gripper_quats"] = tool_quats
warped_demo["l_gripper_angles"] = tool_angles
if right:
_, ori_warped = reg.transform_frames(demo["r_gripper_xyzs"], quats2mats(demo["r_gripper_quats"]))
r_xyzs1 = reg.transform_points(demo["r_gripper_xyzs1"])
r_xyzs2 = reg.transform_points(demo["r_gripper_xyzs2"])
tool_xyzs = []
tool_quats = []
tool_angles = []
for (pos1, pos2, ori) in zip(r_xyzs1, r_xyzs2, ori_warped):
hmat, ang = calc_hand_pose(pos1, pos2, ori)
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
warped_demo["r_gripper_xyzs"] = tool_xyzs
warped_demo["r_gripper_xyzs1"] = r_xyzs1
warped_demo["r_gripper_xyzs2"] = r_xyzs2
warped_demo["r_gripper_quats"] = tool_quats
warped_demo["r_gripper_angles"] = tool_angles
if cloud_xyz:
old_cloud_xyz_pts = demo["cloud_xyz"].reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(demo["cloud_xyz"].shape)
return warped_demo
def hmat_to_ros_transform(hmat):
(x,y,z), (xx,yy,zz,ww) = conversions.hmat_to_trans_rot(hmat)
return gm.Transform(translation = gm.Vector3(x,y,z),
rotation = gm.Quaternion(xx,yy,zz,ww))
prev_positions = np.ones(len(msg.position))*100
prev_t = -np.inf
first_message = False
if t - prev_t > MIN_TIME and np.any(np.abs(np.array(msg.position) - prev_positions) > MIN_JOINT_CHANGE):
#print "adding data"
rr.update(msg)
joints.append(np.array(msg.position))
times.append(t)
if args.arms_used == "r" or args.arms_used == "b":
hmat = r_tool_link.GetTransform()
xyz, quat = conversions.hmat_to_trans_rot(hmat)
r_gripper_angles.append(r_gripper_joint.GetValues()[0])
r_gripper_xyzs.append(xyz)
r_gripper_xyzs1.append(r1_link.GetTransform()[:3,3])
r_gripper_xyzs2.append(r2_link.GetTransform()[:3,3])
r_gripper_quats.append(quat)
if args.arms_used == "l" or args.arms_used == "b":
hmat = l_tool_link.GetTransform()
xyz, quat = conversions.hmat_to_trans_rot(hmat)
l_gripper_angles.append(l_gripper_joint.GetValues()[0])
l_gripper_xyzs.append(xyz)
l_gripper_xyzs1.append(l1_link.GetTransform()[:3,3])
l_gripper_xyzs2.append(l2_link.GetTransform()[:3,3])
l_gripper_quats.append(quat)
prev_t = t
def lookupTransform(self,to_frame, from_frame, *args):
if not to_frame.startswith('/'): to_frame = '/'+to_frame
if not from_frame.startswith('/'): to_frame = '/'+from_frame
hmat = self.transformer.lookup_transform_mat(to_frame, from_frame)
return conversions.hmat_to_trans_rot(hmat)
