Implement optimize$\_$trajectory, which will take in initial and terminal conditions, control limits$,$ dt$,$ and return the optimal sequence of

states $($i$.$e$.,$ an array of shape $(T+1,4$ timesteps $(T$ timesteps $+$ the initial state$)$ and $4$ states per timestep$)$

Generate a plot of your optimal trajectory. You are welcome to use the plot$\_$trajectory function or write your own version.