Modified CommandProcessor class to return set points in setJointAngle function
Merge branch 'PID' of https://github.com/gcc-robotics/prometheus into PID