Add lua interpretor and fix End effector position reading
Connect mass matrix ports