Fixed optimizeTrajectory() bug --
replaced isPathValid() check with interpolated segmentValid() check.
Added pathValid() checker based on segmentValid().
Replaced planning_scene::isPathValid() calls with pathValid().
Fixed problem with incrementing lookup index in fitPlan()
Added handling for gradient descent failure in demo()