pose_utils.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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     #print ps
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         #trans_pose = pose_relative_trans(ps, 0.5, 0.5, 0.2)
00115         #self.trans_pub.publish(trans_pose)
00116         #rospy.loginfo("Pose Utils Test: Pose Translated: \n\r %s" %trans_pose)
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)


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34