Fixed the KDLRobotModel. It wasn't initializing correctly. Added in a bunch of error checking and fixed bugs. Also added some nice debug output.