KDL-simple

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 [1])

UPDATE! (07 september, 2008): You can download roboview from here [2]

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)

Forward kinematics

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

The code:

 print "Forward kinematics"
 jointAngles=JntArray(3)
 jointAngles[0]=0.5236
 jointAngles[1]=0.5236
 jointAngles[2]=-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:

Inverse kinematics

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)

The code:

 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:

 ./roboview.py 

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.