Fixed a debug message
Changed code to use KDL::Tree based IK solver with joint limits, which actually works, as opposed to the chain version, commented out all of the chain based FK and IK code
Cleaned up code, also made to command all zero velocities when out of workspace
Added code to individually tune pid gains per joint.  Added code to scale all joint velocities to the max limit overshoot ratio, commented out right now