fix convert_trajectory code
add variable weighting matrix option to acado
add (somewhat untested) arrival cost support to ocprt