00001
00002 import numpy as np
00003 import math
00004 from copy import deepcopy
00005
00006 import roslib; roslib.load_manifest('tf')
00007 import rospy
00008 from geometry_msgs.msg import PoseStamped, Point, Quaternion
00009 from tf import transformations as tft
00010
00011 def pose_relative_trans(pose, x=0., y=0., z=0.):
00012 """Return a pose translated relative to a given pose."""
00013 ps = deepcopy(pose)
00014 M_trans = tft.translation_matrix([x,y,z])
00015 q_ps = [ps.pose.orientation.x, ps.pose.orientation.y,
00016 ps.pose.orientation.z, ps.pose.orientation.w]
00017 M_rot = tft.quaternion_matrix(q_ps)
00018 trans = np.dot(M_rot,M_trans)
00019 ps.pose.position.x += trans[0][-1]
00020 ps.pose.position.y += trans[1][-1]
00021 ps.pose.position.z += trans[2][-1]
00022
00023 return ps
00024
00025 def pose_relative_rot(pose, r=0., p=0., y=0., degrees=True):
00026 """Return a pose rotated relative to a given pose."""
00027 ps = deepcopy(pose)
00028 if degrees:
00029 r = math.radians(r)
00030 p = math.radians(p)
00031 y = math.radians(y)
00032 des_rot_mat = tft.euler_matrix(r,p,y)
00033 q_ps = [ps.pose.orientation.x,
00034 ps.pose.orientation.y,
00035 ps.pose.orientation.z,
00036 ps.pose.orientation.w]
00037 state_rot_mat = tft.quaternion_matrix(q_ps)
00038 final_rot_mat = np.dot(state_rot_mat, des_rot_mat)
00039 ps.pose.orientation = Quaternion(
00040 *tft.quaternion_from_matrix(final_rot_mat))
00041 return ps
00042
00043 def aim_frame_to(target_pt, point_dir=(1,0,0)):
00044 goal_dir = np.array([target_pt.x, target_pt.y, target_pt.z])
00045 goal_norm = np.divide(goal_dir, np.linalg.norm(goal_dir))
00046 point_norm = np.divide(point_dir, np.linalg.norm(point_dir))
00047 axis = np.cross(point_norm, goal_norm)
00048 angle = np.arccos(np.vdot(goal_norm, point_norm))
00049 return tft.quaternion_about_axis(angle, axis)
00050
00051 def aim_pose_to(ps, pts, point_dir=(1,0,0)):
00052 if not (ps.header.frame_id.lstrip('/') == pts.header.frame_id.lstrip('/')):
00053 rospy.logerr("[Pose_Utils.aim_pose_to]:"+
00054 "Pose and point must be in same frame: %s, %s"
00055 %(ps.header.frame_id, pt2.header.frame_id))
00056 target_pt = np.array((pts.point.x, pts.point.y, pts.point.z))
00057 base_pt = np.array((ps.pose.position.x,
00058 ps.pose.position.y,
00059 ps.pose.position.z))
00060 base_quat = np.array((ps.pose.orientation.x, ps.pose.orientation.y,
00061 ps.pose.orientation.z, ps.pose.orientation.w))
00062
00063 b_to_t_vec = np.array((target_pt[0]-base_pt[0],
00064 target_pt[1]-base_pt[1],
00065 target_pt[2]-base_pt[2]))
00066 b_to_t_norm = np.divide(b_to_t_vec, np.linalg.norm(b_to_t_vec))
00067
00068 point_dir_hom = (point_dir[0], point_dir[1], point_dir[2], 1)
00069 base_rot_mat = tft.quaternion_matrix(base_quat)
00070 point_dir_hom = np.dot(point_dir_hom, base_rot_mat.T)
00071 point_dir = np.array((point_dir_hom[0]/point_dir_hom[3],
00072 point_dir_hom[1]/point_dir_hom[3],
00073 point_dir_hom[2]/point_dir_hom[3]))
00074 point_dir_norm = np.divide(point_dir, np.linalg.norm(point_dir))
00075 axis = np.cross(point_dir_norm, b_to_t_norm)
00076 angle = np.arccos(np.vdot(point_dir_norm, b_to_t_norm))
00077 quat = tft.quaternion_about_axis(angle, axis)
00078 new_quat = tft.quaternion_multiply(quat, base_quat)
00079 ps.pose.orientation = Quaternion(*new_quat)
00080
00081 def find_approach(pose, standoff=0., axis='x'):
00082 """Return a PoseStamped pointed down the z-axis of input pose."""
00083 ps = deepcopy(pose)
00084 if axis == 'x':
00085 ps = pose_relative_rot(ps, p=90)
00086 ps = pose_relative_trans(ps, -standoff)
00087 return ps
00088
00089 def calc_dist(ps1, ps2):
00090 """ Return the cartesian distance between the points of 2 poses."""
00091 p1 = ps1.pose.position
00092 p2 = ps2.pose.position
00093 return math.sqrt((p1.x-p2.x)**2 + (p1.y-p2.y)**2 + (p1.z-p2.z)**2)
00094
00095 class PoseUtilsTest():
00096 def __init__(self):
00097 rospy.Subscriber('pose_in', PoseStamped, self.cb)
00098 self.recd_pub = rospy.Publisher('pose_utils_test_received', PoseStamped)
00099 self.trans_pub = rospy.Publisher('pose_utils_test_trans', PoseStamped)
00100 self.rot_pub = rospy.Publisher('pose_utils_test_rot', PoseStamped)
00101 self.ps = PoseStamped()
00102 self.ps.header.frame_id = '/torso_lift_link'
00103 self.ps.header.stamp = rospy.Time.now()
00104 self.ps.pose.position.x = 5
00105 self.ps.pose.position.y = 2
00106 self.ps.pose.position.z = 3
00107
00108 def cb(self, ps):
00109 self.ps.pose.orientation = Quaternion(*tft.random_quaternion())
00110 print ps
00111 rospy.sleep(0.5)
00112 self.recd_pub.publish(ps)
00113
00114
00115
00116
00117
00118 ps_rot = pose_relative_rot(ps, 90, 30 , 45)
00119 self.rot_pub.publish(ps_rot)
00120 rospy.loginfo("Pose Utils Test: Pose Rotated: \n\r %s" %ps_rot)
00121
00122 if __name__=='__main__':
00123 rospy.init_node('pose_utils_test')
00124 put = PoseUtilsTest()
00125 while not rospy.is_shutdown():
00126 put.cb(put.ps)
00127 rospy.sleep(5)