big refactoring at handling trajectory goals