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)