fixed Eigen-KDL 3D Vector conversion functions

Fixed errors in the Eigen-KDL + KDL-Eigen vector conversion functions.