This small tutorial is going to show how to use KDL for calculate the inverse and forward kinematics of a robotic kinematic chain. Note: This tutorial uses python code, but the code looks very similar than the correspoding one in C++ (hint: See the example code on the orocos-KDL page )
UPDATE! (07 september, 2008): You can download roboview from here 
Creating a kinematic chain
The kinematic chain has the following structure:
Chain Segment0 Joint, Frame Segment1 ... ...
A chain is composed of Segments which at the same time are composed of one Joint and one Frame each. A Frame is just the relative position and orientation of the end of the current link(segment) in reference to the frame of the last segment. The kinematic chain can also be constructed using DH parameters. For building the following chain:
One can use the following python code:
from PyKDL import * print "Creating Robotic Chain" chain=Chain() joint0=Joint(Joint.RotZ) frame0=Frame(Vector(0.2,0.3,0)) segment0=Segment(joint0,frame0) chain.addSegment(segment0) #Inertia zero (Don't want to mess with dynamics yet) joint1=joint0 #Iqual joint frame1=Frame(Vector(0.4,0,0)) segment1=Segment(joint1,frame1) chain.addSegment(segment1) joint2=joint1 #Iqual joint frame2=Frame(Vector(0.1,0.1,0)) segment2=Segment(joint2,frame2) chain.addSegment(segment2)
Let say we want to calculate what is the final position and orientation of the chain end-effector if we give it the following angles:
q0=30 q1=30 q2=-90
print "Forward kinematics" jointAngles=JntArray(3) jointAngles=0.5236 jointAngles=0.5236 jointAngles=-1.5708 fk=ChainFkSolverPos_recursive(chain) finalFrame=Frame() fk.JntToCart(jointAngles,finalFrame) print "Rotational Matrix of the final Frame: " print finalFrame.M print "End-effector position: ",finalFrame.p
The execution result:
Forward kinematics Rotational Matrix of the final Frame: [ 0.866025, 0.500001, 0; -0.500001, 0.866025, 0; 0, 0, 1] End-effector position: [ 0.359806, 0.742821, 0]
The resulting visualization using roboview is:
We want to position the robot end-effector on the following position and orientation with respect to the global frame:
x=0.4 y=0.4 z=0 Rotation Matrix=Identity (aligned with the global frame)
print "Inverse Kinematics" q_init=jointAngles #initial angles vik=ChainIkSolverVel_pinv(chain) ik=ChainIkSolverPos_NR(chain,fk,vik) desiredFrame=Frame(Vector(0.4,0.4,0)) print "Desired Position: ", desiredFrame.p q_out=JntArray(3) ik.CartToJnt(q_init,desiredFrame,q_out) print "Output angles in rads: ",q_out
The execution result:
Inverse Kinematics Desired Position: [ 0.4, 0.4, 0] Output angles in rads: [ 0.860978 -0.979266 0.118288]
The resulting visualization:
roboview.py a KDL interactive kinematic chain viewer
First tell roboview about the kinematic chain, for this, you have to edit the file robot.py. Search for the Robot class, modify the member segments to your needs.
In this example the segments member should look like this:
class Robot(object): segments = [ Segment(Joint(Joint.RotZ),Frame(Vector(0.2,0.3,0))), Segment(Joint(Joint.RotZ),Frame(Vector(0.4,0,0))), Segment(Joint(Joint.RotZ),Frame(Vector(0.1,0.1,0))) ] ... ...
Start the application:
Connect a yarp write to roboview:
yarp write /posOut /roboview/in
Give some angles(rads):
memeruiz@narsil:~$ yarp write /posOut /roboview/in Port /posOut listening at tcp://192.168.1.127:10302 yarp: Sending output from /posOut to /roboview/in using tcp Added output connection from "/posOut" to "/roboview/in" 0.5236 0.5236 -1.5708
The graphical representation should show the new configuration.