Function calc_control_and_trajectory 

pub fn calc_control_and_trajectory(
    x: Vector5<f64>,
    dw: Vector4<f64>,
    config: &Config,
    goal: (f64, f64),
    ob: &[(f64, f64)],
) -> ((f64, f64), Vec<Vector5<f64>>)