trajectory optimization algorithm