Added IK search to KDLRobotModel, a bunch of other small changes.