Indigo fix default collision padding

If you have "/robot_description" as your robot_description_ the padding feature does not work.

Also added some debug output.