#include <iomanip>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <momentumopt/kinopt/KinematicsOptimizer.hpp>
#include <momentumopt/utilities/OrientationUtils.hpp>