#include <limits>
#include "body2dmanifold.h"
#include "se2.h"
#include <iostream>
#include <assert.h>