fixed bugs with ExtractOpenRAVETrajectoryFromProfiles
TOPP fixes
added FindEarliestProfileIntersection function, many fixes in ExtractOpenRAVETrajectoryFromProfiles for degenerate cases