Moving robot code to new directory, adding linalg logic

Moving robot code to new directory, adding linalg logic #72