Added debug output to simplifySolution to match other simplifySolution function

Similar to https://github.com/ompl/ompl/pull/22/files#diff-2b469443df4022778dc4055f80020d41L165