think I have the trajectory optimization working
I think I released the end angle constraint