ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# transformStamped to transformation matrix (python)

This may be trivial but I cannot find it anywhere, can someone help me? Goal: Find a transformStamped by a tf2 transform lookup, then convert this to a matrix so I can multiply it. Something like the below:

import tf2_ros
import tf

#(more code here, but not important to question)

tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
trans = tfBuffer.lookup_transform("gps", "base_link", rospy.Time.now(), rospy.Duration(0.1))
matrix = tf.transformations.translation_matrix(trans)
# (More code after this point as well.)


Again, this may be trivial but I cannot find the solution to this nor a question that answers my problem.

edit retag close merge delete

Sort by ยป oldest newest most voted

Although ros_numpy ( https://github.com/eric-wieser/ros_numpy ) is not a "native" way, it is a well established package that can do what you want.

from ros_numpy import numpify
matrix = numpify(msg)

more

I don't know of a native way to do this, but I wrote the following conversion functions a while ago that I've been using. A call to msg_to_se3 can return a 4x4 SE(3) matrix for a few different ROS message types.

import tf.transformations as tr

from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Transform
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Vector3

import numpy as np

def pose_to_pq(msg):
"""Convert a C{geometry_msgs/Pose} into position/quaternion np arrays

@param msg: ROS message to be converted
@return:
- p: position as a np.array
- q: quaternion as a numpy array (order = [x,y,z,w])
"""
p = np.array([msg.position.x, msg.position.y, msg.position.z])
q = np.array([msg.orientation.x, msg.orientation.y,
msg.orientation.z, msg.orientation.w])
return p, q

def pose_stamped_to_pq(msg):
"""Convert a C{geometry_msgs/PoseStamped} into position/quaternion np arrays

@param msg: ROS message to be converted
@return:
- p: position as a np.array
- q: quaternion as a numpy array (order = [x,y,z,w])
"""
return pose_to_pq(msg.pose)

def transform_to_pq(msg):
"""Convert a C{geometry_msgs/Transform} into position/quaternion np arrays

@param msg: ROS message to be converted
@return:
- p: position as a np.array
- q: quaternion as a numpy array (order = [x,y,z,w])
"""
p = np.array([msg.translation.x, msg.translation.y, msg.translation.z])
q = np.array([msg.rotation.x, msg.rotation.y,
msg.rotation.z, msg.rotation.w])
return p, q

def transform_stamped_to_pq(msg):
"""Convert a C{geometry_msgs/TransformStamped} into position/quaternion np arrays

@param msg: ROS message to be converted
@return:
- p: position as a np.array
- q: quaternion as a numpy array (order = [x,y,z,w])
"""
return transform_to_pq(msg.transform)

def msg_to_se3(msg):
"""Conversion from geometric ROS messages into SE(3)

@param msg: Message to transform. Acceptable types - C{geometry_msgs/Pose}, C{geometry_msgs/PoseStamped},
C{geometry_msgs/Transform}, or C{geometry_msgs/TransformStamped}
@return: a 4x4 SE(3) matrix as a numpy array
@note: Throws TypeError if we receive an incorrect type.
"""
if isinstance(msg, Pose):
p, q = pose_to_pq(msg)
elif isinstance(msg, PoseStamped):
p, q = pose_stamped_to_pq(msg)
elif isinstance(msg, Transform):
p, q = transform_to_pq(msg)
elif isinstance(msg, TransformStamped):
p, q = transform_stamped_to_pq(msg)
else:
raise TypeError("Invalid type for conversion to SE(3)")
norm = np.linalg.norm(q)
if np.abs(norm - 1.0) > 1e-3:
raise ValueError(
"Received un-normalized quaternion (q = {0:s} ||q|| = {1:3.6f})".format(
str(q), np.linalg.norm(q)))
elif np.abs(norm - 1.0) > 1e-6:
q = q / norm
g = tr.quaternion_matrix(q)
g[0:3, -1] = p
return g

more