# Inverse Kinematics

To get started:

```svn co http://svn.cs.brynmawr.edu/Myro/trunk/robonova/kinematics/
cd kinematics
```

To use the package in Python:

```% python
>>> from robot import *
```

```>>> from robot.twolink import *
```

This defines tl, a two-link arm with each link 1 unit long each.

If you set both links to angle zero:

```>>> T0 = fkine(tl, [0, 0])
```

That returns a transform describing the end effector. The arm is outstretched, both joints fully open, and the arm completely extended.

You could use the transform returned in the inverse kinematics function to figure out what angles your links should be:

```>>> ikine(tl, T0)
matrix([ [ 0.,  0.] ])
```

NOTE: Because our arm has fewer than 6 DOF, it will complain that we need to provide a mask (m) to ikine showing which dimensions we have. You can ignore this warning.

That transform places the end effector (the end of the two-link arm) at (2, 0, 0).

If we want to move the arm to (0, 2, 0) (raised arm) we can create a transform matrix:

```>>> T1 = mat([ [1, 0, 0, 0],
[0, 1, 0, 2],
[0, 0, 1, 0],
[0, 0, 0, 1] ])
```

where the 4th column represents x, y, z, 1. You can also call the transl() function to create the matrix for you:

```>>> transl(0, 2, 0)
matrix([ [ 1.,  0.,  0.,  0.],
[ 0.,  1.,  0.,  2.],
[ 0.,  0.,  1.,  0.],
[ 0.,  0.,  0.,  1.]])
```

We can apply the transform to the arm to see what the resulting angles for each link should be:

```>>> ikine(tl, T1)
matrix([ [ 2.089618  , -1.44197838] ])
```

How close is that solution? Apply the joint positions to the forward kinematics:

```>>> fkine(tl, matrix([ [ 2.089618  , -1.44197838] ]))
matrix([ [ 0.79751005, -0.60330566,  0.        ,  0.30165283],
[ 0.60330566,  0.79751005,  0.        ,  1.47170973],
[ 0.        ,  0.        ,  1.        ,  0.        ],
[ 0.        ,  0.        ,  0.        ,  1.        ] ])
```

So, that moves the endpoint to about (0.30, 1.47, 0).

Questions:

1. How do you construct a transform matrix for putting the end point at (1.41, 1.41, 0) (Hitler-salute)?