geometry.py
Go to the documentation of this file.
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))


moveit_python
Author(s): Michael Ferguson
autogenerated on Fri Aug 28 2015 11:32:16