Make all classes functional

[gchenfc]

This issue was imported from github.gatech.edu.
The original issue was opened by @aescontrela3 on 2020-02-18T21:15:08Z and assigned to @yzhang3333 @aescontrela3 @smccormick32

All classes in GTDynamics should be made functional. Part of this change will involve the notion of "fixing links" after they have been created and "removing links" from the Robot after it has been instantiated. Everything should be implemented in the constructor and the class's member variables should not be changed from there on after. As such, all instance methods should be const.

Originally posted by @aescontrela3 in https://github.gatech.edu/borglab/GTDynamics/pull/6/review_comment/create