Implemented forward kinematics. Added kinematics tests.