Added 2 Degree of Freedom Code