Trajectory optimization

shape