00001 # Copyright 2011-2014, Michael Ferguson 00002 # All rights reserved. 00003 # 00004 # Redistribution and use in source and binary forms, with or without 00005 # modification, are permitted provided that the following conditions 00006 # are met: 00007 # 00008 # * Redistributions of source code must retain the above copyright 00009 # notice, this list of conditions and the following disclaimer. 00010 # * Redistributions in binary form must reproduce the above 00011 # copyright notice, this list of conditions and the following 00012 # disclaimer in the documentation and/or other materials provided 00013 # with the distribution. 00014 # 00015 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00016 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00017 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00018 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00019 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00021 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00022 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00023 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00024 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00026 # POSSIBILITY OF SUCH DAMAGE. 00027 00028 ## @package moveit_python.geometry Helpers for performing geometric 00029 ## transformations on geometry_msgs messages 00030 00031 from geometry_msgs.msg import * 00032 from tf.transformations import * 00033 00034 ## @brief Get a translation matrix from a geometry_msgs/Point 00035 ## @param point geometry_msgs/Point to turn into matrix 00036 def matrix_from_point_msg(point): 00037 return translation_matrix((point.x, point.y, point.z)) 00038 00039 ## @brief Get a rotation matrix from a geometry_msgs/Quaternion 00040 ## @param quaternion geometry_msgs/Quaternion to turn into matrix 00041 def matrix_from_quaternion_msg(quaternion): 00042 q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] 00043 return quaternion_matrix(q) 00044 00045 ## @brief Get a transformation matrix from a geometry_msgs/Pose 00046 ## @param pose geometry_msgs/Pose to turn into matrix 00047 def matrix_from_pose_msg(pose): 00048 t = matrix_from_point_msg(pose.position) 00049 r = matrix_from_quaternion_msg(pose.orientation) 00050 return concatenate_matrices(t, r) 00051 00052 ## @brief Get a geometry_msgs/Point from a transformation matrix 00053 ## @param transformation The matrix to convert to a point 00054 def point_msg_from_matrix(transformation): 00055 msg = Point() 00056 msg.x = transformation[0][3] 00057 msg.y = transformation[1][3] 00058 msg.z = transformation[2][3] 00059 return msg 00060 00061 ## @brief Get a geometry_msgs/Quaternion from a transformation matrix 00062 ## @param transformation The matrix to convert to a quaternion 00063 def quaternion_msg_from_matrix(transformation): 00064 q = quaternion_from_matrix(transformation) 00065 msg = Quaternion() 00066 msg.x = q[0] 00067 msg.y = q[1] 00068 msg.z = q[2] 00069 msg.w = q[3] 00070 return msg 00071 00072 ## @brief Get a geometry_msgs/Pose from a transformation matrix 00073 ## @param transformation The matrix to convert to a pose 00074 def pose_msg_from_matrix(transformation): 00075 msg = Pose() 00076 msg.position = point_msg_from_matrix(transformation) 00077 msg.orientation = quaternion_msg_from_matrix(transformation) 00078 return msg 00079 00080 ## @brief Translate a geometry_msgs/Pose 00081 ## @param pose The pose to translate 00082 ## @param x The displacement in X coordinate axis 00083 ## @param y The displacement in Y coordinate axis 00084 ## @param z The displacement in Z coordinate axis 00085 def translate_pose_msg(pose, x, y, z): 00086 initial = matrix_from_pose_msg(pose) 00087 transform = translation_matrix((x,y,z)) 00088 return pose_msg_from_matrix(concatenate_matrices(initial, transform)) 00089 00090 ## @brief Rotate a geometry_msgs/Pose 00091 ## @param pose The pose to rotate 00092 ## @param r The roll 00093 ## @param p The pitch 00094 ## @param y The yaw 00095 def rotate_pose_msg_by_euler_angles(pose, r, p, y): 00096 initial = matrix_from_pose_msg(pose) 00097 transform = quaternion_matrix(quaternion_from_euler(r, p, y)) 00098 return pose_msg_from_matrix(concatenate_matrices(initial, transform)) 00099 00100 ## @brief Rotate a geometry_msgs/Pose 00101 ## @param pose The pose to rotate 00102 ## @param r The roll 00103 ## @param p The pitch 00104 ## @param y The yaw 00105 def rotate_pose_msg_about_origin(pose, r, p, y): 00106 initial = matrix_from_pose_msg(pose) 00107 transform = quaternion_matrix(quaternion_from_euler(r, p, y)) 00108 return pose_msg_from_matrix(concatenate_matrices(transform, initial))