Function dwa_control 

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