Make all classes functional

This issue was imported from [github.gatech.edu](https://github.gatech.edu/borglab/GTDynamics/issues/8).
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_