Robot Dynamics and Control Final Project