void leatherman::getRPY(const Eigen::Matrix3d &m, double &roll, double &pitch, double &yaw) { Eigen::Vector3d v = m.eulerAngles(0,1,2); roll = v(0); pitch = v(1); yaw = v(2); }