Removed the dependencies on KDL for rotation transformation. All rotations use Eigen library from now.