# Kinematics model

This is a really simple model.
It has some inverse kinematics and forwards kinematics.

## Inverse model

The setpoint on the board is given as a x,y position.
We have to convert this point in to angles of the SCARA joints.
We have the following setup:
![Geometrics of the scara](geometrics.pdf)

We can resolve this via the *law of cosines* and *pythagoras theorem*.
We can find the length of **C** with pythagoras.
Then with the use of a tangent we can find the angle **phi**.
Then filling in the *law of cosines* with **C** calculated and **A** and **B** as arm lengths, we can calculate the angles **a**, **b** and **c**.
As a check we sum these angles and they should add up to the value of Pi.

Then we add **phi** and **b** to get the corner of the first joint relative to the world frame.
And **c** - Pi + **phi** + **b** gives the angle of the second joint relative to the world frame.


## Forward model
The forward model is as follows.
``` 
joint1 = A_length*[cos(angle1);sin(angle1)];
joint2 = B_length*[cos(angle2);sin(angle2)];
end_effector = joint1 + joint2;
```
This is then drawn as two arms in a 3D viewer.
![3D view of forward model](3D.png)
It follows the outside of the square.