Finished cleaning ModelController
Minor changes on position control threshold and JointPlus struct