Fix bug when singular switchpoints are close to beginning or end of trajectory