I'll modify the code in this pull request so that position-controlled and velocity-controlled joints use PID controllers instead of calling SetAngle() or SetVelocity(). The PID parameters will be read from the ROS Parameter Server.