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))
| 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
| 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.
| Parameters: | f (PyKDL.Frame) – input pose |
|---|
Return a ROS Pose message for the Frame f.
| 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.
| Parameters: | f (PyKDL.Frame) – input pose |
|---|
Return a numpy 4x4 array for the Frame F.
| Parameters: |
|
|---|---|
| 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)