#include <cmath>
#include <gtest/gtest.h>
#include "drake/systems/plants/constraint/dynamic_constraint.h"
#include "drake/util/eigen_matrix_compare.h"