Fix issue #60

This PR should fix the segmentation fault reported in #60, and also addresses a separate scenario in which the current trajectory could not be defined for the current control cycle, but for the next one onwards.