Move aos/controls to frc971/control_loops
Also put what was aos/controls/control_loops.fbs in y2012/control_loops
because that's the only user.
Change-Id: I8f402b0708103077e135a41e55ef5e4f23681d87
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
new file mode 100644
index 0000000..f472a46
--- /dev/null
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -0,0 +1,152 @@
+#include "Eigen/Dense"
+
+#include <random>
+
+#include "frc971/control_loops/quaternion_utils.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace controls {
+namespace testing {
+
+// Tests that small perturbations around a couple quaternions averaged out
+// return the original quaternion.
+TEST(DownEstimatorTest, QuaternionMean) {
+ Eigen::Matrix<double, 4, 7> vectors;
+ vectors.col(0) << 0, 0, 0, 1;
+ for (int i = 0; i < 3; ++i) {
+ Eigen::Matrix<double, 4, 1> perturbation;
+ perturbation.setZero();
+ perturbation(i, 0) = 0.1;
+
+ vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
+ vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
+ }
+
+ for (int i = 0; i < 7; ++i) {
+ vectors.col(i).normalize();
+ }
+
+ Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
+
+ for (int i = 0; i < 4; ++i) {
+ EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
+ }
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a real rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a solution with negative
+// coefficients.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ -Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
+
+ EXPECT_NEAR(
+ 0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a real rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() * M_PI * 0.5);
+
+ EXPECT_NEAR(0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(
+ M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
+// that is too large in magnitude.
+TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
+ const double kMaxAngle = 2.0;
+ const Eigen::Vector3d rotation_vector =
+ Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
+ const Eigen::Matrix<double, 3, 1> clipped_vector =
+ ToRotationVectorFromQuaternion(
+ ToQuaternionFromRotationVector(rotation_vector, kMaxAngle));
+
+ EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
+// works for random rotations.
+TEST(DownEstimatorTest, RandomQuaternions) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
+
+ for (int i = 0; i < 1000; ++i) {
+ Eigen::Matrix<double, 3, 1> axis;
+ axis << random_scalar(generator), random_scalar(generator),
+ random_scalar(generator);
+ EXPECT_GE(axis.norm(), 1e-6);
+ axis.normalize();
+
+ const double angle = random_scalar(generator) * M_PI;
+
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(axis * angle);
+
+ Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
+
+ EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
+ EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
+
+ EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
+
+ const Eigen::Matrix<double, 3, 1> recalculated_axis =
+ ToRotationVectorFromQuaternion(quaternion);
+
+ EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
+
+ EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
+ }
+}
+
+} // namespace testing
+} // namespace controls
+} // namespace frc971