geometry.py
Go to the documentation of this file.
1 # Copyright 2011-2014, Michael Ferguson
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above
11 # copyright notice, this list of conditions and the following
12 # disclaimer in the documentation and/or other materials provided
13 # with the distribution.
14 #
15 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
18 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
21 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 ## @package moveit_python.geometry Helpers for performing geometric
29 ## transformations on geometry_msgs messages
30 
31 from geometry_msgs.msg import *
32 from tf.transformations import *
33 
34 ## @brief Get a translation matrix from a geometry_msgs/Point
35 ## @param point geometry_msgs/Point to turn into matrix
37  return translation_matrix((point.x, point.y, point.z))
38 
39 ## @brief Get a rotation matrix from a geometry_msgs/Quaternion
40 ## @param quaternion geometry_msgs/Quaternion to turn into matrix
42  q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
43  return quaternion_matrix(q)
44 
45 ## @brief Get a transformation matrix from a geometry_msgs/Pose
46 ## @param pose geometry_msgs/Pose to turn into matrix
48  t = matrix_from_point_msg(pose.position)
49  r = matrix_from_quaternion_msg(pose.orientation)
50  return concatenate_matrices(t, r)
51 
52 ## @brief Get a geometry_msgs/Point from a transformation matrix
53 ## @param transformation The matrix to convert to a point
54 def point_msg_from_matrix(transformation):
55  msg = Point()
56  msg.x = transformation[0][3]
57  msg.y = transformation[1][3]
58  msg.z = transformation[2][3]
59  return msg
60 
61 ## @brief Get a geometry_msgs/Quaternion from a transformation matrix
62 ## @param transformation The matrix to convert to a quaternion
63 def quaternion_msg_from_matrix(transformation):
64  q = quaternion_from_matrix(transformation)
65  msg = Quaternion()
66  msg.x = q[0]
67  msg.y = q[1]
68  msg.z = q[2]
69  msg.w = q[3]
70  return msg
71 
72 ## @brief Get a geometry_msgs/Pose from a transformation matrix
73 ## @param transformation The matrix to convert to a pose
74 def pose_msg_from_matrix(transformation):
75  msg = Pose()
76  msg.position = point_msg_from_matrix(transformation)
77  msg.orientation = quaternion_msg_from_matrix(transformation)
78  return msg
79 
80 ## @brief Translate a geometry_msgs/Pose
81 ## @param pose The pose to translate
82 ## @param x The displacement in X coordinate axis
83 ## @param y The displacement in Y coordinate axis
84 ## @param z The displacement in Z coordinate axis
85 def translate_pose_msg(pose, x, y, z):
86  initial = matrix_from_pose_msg(pose)
87  transform = translation_matrix((x,y,z))
88  return pose_msg_from_matrix(concatenate_matrices(initial, transform))
89 
90 ## @brief Rotate a geometry_msgs/Pose
91 ## @param pose The pose to rotate
92 ## @param r The roll
93 ## @param p The pitch
94 ## @param y The yaw
96  initial = matrix_from_pose_msg(pose)
97  transform = quaternion_matrix(quaternion_from_euler(r, p, y))
98  return pose_msg_from_matrix(concatenate_matrices(initial, transform))
99 
100 ## @brief Rotate a geometry_msgs/Pose
101 ## @param pose The pose to rotate
102 ## @param r The roll
103 ## @param p The pitch
104 ## @param y The yaw
105 def rotate_pose_msg_about_origin(pose, r, p, y):
106  initial = matrix_from_pose_msg(pose)
107  transform = quaternion_matrix(quaternion_from_euler(r, p, y))
108  return pose_msg_from_matrix(concatenate_matrices(transform, initial))
def matrix_from_quaternion_msg(quaternion)
Get a rotation matrix from a geometry_msgs/Quaternion.
Definition: geometry.py:41
def matrix_from_pose_msg(pose)
Get a transformation matrix from a geometry_msgs/Pose.
Definition: geometry.py:47
def rotate_pose_msg_by_euler_angles(pose, r, p, y)
Rotate a geometry_msgs/Pose.
Definition: geometry.py:95
def translate_pose_msg(pose, x, y, z)
Translate a geometry_msgs/Pose.
Definition: geometry.py:85
def rotate_pose_msg_about_origin(pose, r, p, y)
Rotate a geometry_msgs/Pose.
Definition: geometry.py:105
def matrix_from_point_msg(point)
Get a translation matrix from a geometry_msgs/Point.
Definition: geometry.py:36
def point_msg_from_matrix(transformation)
Get a geometry_msgs/Point from a transformation matrix.
Definition: geometry.py:54
def quaternion_msg_from_matrix(transformation)
Get a geometry_msgs/Quaternion from a transformation matrix.
Definition: geometry.py:63
def pose_msg_from_matrix(transformation)
Get a geometry_msgs/Pose from a transformation matrix.
Definition: geometry.py:74


moveit_python
Author(s): Michael Ferguson
autogenerated on Wed Jun 5 2019 21:51:17