tf_conversions

PoseMath

PoseMath is a utility that makes it easy to work with PyKDL.Frame‘s. It can work with poses from a variety of sources: tf.Transformer.lookupTransform(), opencv and ROS messages. It has utility functions to convert between these types and the PyKDL.Frame pose representation.

>>> from geometry_msgs.msg import Pose
>>> import tf_conversions.posemath as pm
>>> import PyKDL
>>>
>>> msg = Pose()
>>> msg.position.x = 7.0
>>> msg.orientation.w = 1.0
>>>
>>> frame = PyKDL.Frame(PyKDL.Rotation.RPY(2, 0, 1), PyKDL.Vector(1,2,4))
>>>
>>> res = pm.toTf(pm.fromMsg(msg) * frame)
>>> print res
((8.0, 2.0, 4.0), (0.73846026260412856, 0.40342268011133486, 0.25903472399992566, 0.4741598817790379))
tf_conversions.posemath.fromTf(tf)
Parameters:tf (tuple (translation, quaternion)) – tf.Transformer transform
Returns:New PyKDL.Frame object

Convert a pose returned by tf.Transformer.lookupTransform() to a PyKDL.Frame.

>>> import rospy
>>> import tf
>>> import geometry_msgs.msg
>>> t = tf.Transformer(True, rospy.Duration(10.0))
>>> m = geometry_msgs.msg.TransformStamped()
>>> m.header.frame_id = 'THISFRAME'
>>> m.child_frame_id = 'CHILD'
>>> m.transform.translation.x = 668.5
>>> m.transform.rotation.w = 1.0
>>> t.setTransform(m)
>>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))
((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
>>> import tf_conversions.posemath as pm
>>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)))
>>> print(pm.toMsg(p * p))
position: 
  x: 1337.0
  y: 0.0
  z: 0.0
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
tf_conversions.posemath.fromMsg(p)
Parameters:p (geometry_msgs.msg.Pose) – input pose
Returns:New PyKDL.Frame object

Convert a pose represented as a ROS Pose message to a PyKDL.Frame.

tf_conversions.posemath.toMsg(f)
Parameters:f (PyKDL.Frame) – input pose

Return a ROS Pose message for the Frame f.

tf_conversions.posemath.fromMatrix(m)
Parameters:m (numpy.array()) – input 4x4 matrix
Returns:New PyKDL.Frame object

Convert a pose represented as a 4x4 numpy array to a PyKDL.Frame.

tf_conversions.posemath.toMatrix(f)
Parameters:f (PyKDL.Frame) – input pose

Return a numpy 4x4 array for the Frame F.

tf_conversions.posemath.fromCameraParams(cv, rvec, tvec)
Parameters:
  • cv – OpenCV module
  • rvec (3x1 CvMat) – A Rodrigues rotation vector - see Rodrigues2()
  • tvec (3x1 CvMat) – A translation vector
Returns:

New PyKDL.Frame object

For use with FindExtrinsicCameraParams2():

import cv
import tf_conversions.posemath as pm
...
rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec)
pose = pm.fromCameraParams(cv, rvec, tvec)

Indices and tables