Fixed a bunch of bugs but end effector link is ill defined.