working on trajectory optimization