#include "debug.hh"
#include <cstring>
#include <boost/numeric/ublas/matrix.hpp>
#include "roboptim/core/util.hh"