#include "debug.hh"
#include <roboptim/trajectory/sys.hh>
#include <cmath>
#include <roboptim/trajectory/frontal-speed.hh>