pub fn joints_to_vector6(j: Joints) -> Vector6<f64>
Converts Joints ([f64; 6]) to a Vector6<f64>
Joints ([f64; 6])
Vector6<f64>