Started to incorperate changes to control multiple joints, mostly using joint_state_controller as a template