Fixed bug in visualizing collision spheres
Very small changes (print statements).
Added small function to robot model, and todo in kdl_robot_model