#include "RobotFrames.hpp"
#include <kdl_parser/kdl_parser.hpp>
#include <algorithm>
#include <base-logging/Logging.hpp>
#include <kdl/frames_io.hpp>