Rosenunstable

Lots of bug fixes and created better profile extraction:

- create faster FindLowestProfileFast and replace all old FindLowestProfile
- create ExtractOpenRAVETrajectoryFromProfiles and FindEarliestProfileIntersection for accurate and fast profile extraction
- fix slope computation at singularity point!
- add smaller integration step when near singularity point
- speed up AddSwitchPoint2 with binary search
- fix bug in remove discontinuous switch points
- increase step thresh near singularity point