plotting and more trajectory optimization