#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <Eigen/Geometry>
#include <tesseract_urdf/safety_controller.h>
#include "tesseract_urdf_common_unit.h"