updated working simple humanoid controller.  not yet tested on gazebo, just with bag files