## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Frame transformations (Python)
## multi-line description to be displayed in search 
## description = This tutorials gets you started with KDL Frames, Vectors, Rotations, etc
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= (BeginnerCategory)
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Data types ==

'''Creating a Frame, Vector and Rotation'''
{{{
#!python
import PyKDL

# create a vector
v = PyKDL.Vector(1,3,5)

# create a rotation from Roll Pitch, Yaw angles
r1 = PyKDL.Rotation.RPY(1.2, 3.4, 0)

# create a rotation from XYZ Euler angles
r2 = PyKDL.Rotation.EulerZYX(0, 1, 0)

# create a rotation from a rotation matrix
r3 = PyKDL.Rotation(1,0,0, 0,1,0, 0,0,1)

# create a frame from a vector and a rotation
f = PyKDL.Frame(r2, v)
}}}


'''Extracting information from a Frame, Vector and Rotation'''
{{{
#!python
import PyKDL

# frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                PyKDL.Vector(3,2,4))

# get the origin (a Vector) of the frame
origin = f.p

# get the x component of the origin
x = origin.x()
x = origin[0]

# get the rotation of the frame
rot = f.M

# get ZYX Euler angles from the rotation
[Rz, Ry, Rx] = rot.GetEulerZYX()

# get the RPY (fixed axis) from the rotation
[R, P, Y] = rot.GetRPY()
}}}


== Creating from ROS types ==
{{{
#!python
from tf_conversions import posemath

# you have a Pose message
pose = Pose()

# convert the pose into a kdl frame
f1 = posemath.fromMsg(pose)

# create another kdl frame
f2 = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                 PyKDL.Vector(3,2,4))

# Combine the two frames
f = f1 * f2

# and convert the result back to a pose message
pose = toMsg(f)
}}}

And it wouldn't be Python if you couldn't have done all this in a single line:
{{{
#!python
from tf_conversions import posemath

pose = posemath.toMsg(posemath.fromMsg(pose) * PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0), PyKDL.Vector(3,2,4)))
}}}



== Transformations ==
'''Transforming a point'''
{{{
#!python
import PyKDL

# define a frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                PyKDL.Vector(3,2,4))

# define a point
p = PyKDL.Vector(1, 0, 0)
print p

# transform this point with f
p = f * p
print p
}}}




## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE
## PythonCategory
## LearningCategory