#include <iostream>
#include <fstream>
#include <Mathematics/FootConstraintsAsLinearSystemForVelRef.hh>
#include <Debug.hh>