Function joints_to_vector6 

pub fn joints_to_vector6(j: Joints) -> Vector6<f64>

Converts Joints ([f64; 6]) to a Vector6<f64>