Added basic forward an invers kinematics

I added the file with the forward and inverse kinematics to the 501
development project.