fixed Eigen-KDL 3D Vector conversion functions