Return trajectory to caller
Return trajectory to caller, SBPL