# 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 )

### 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)
#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)
joint2=joint1 #Iqual joint
frame2=Frame(Vector(0.1,0.1,0))
segment2=Segment(joint2,frame2)
```

### 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.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:

### 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
```

```  memeruiz@narsil:~\$ yarp write /posOut /roboview/in