[wpimath] Add core State-space classes (#2614)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>

wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp

@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
+#include "units/time.h"
+
+namespace frc {
+
+Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
+                                     const Eigen::Matrix<double, 1, 1>& u) {
+  Eigen::Matrix<double, 2, 1> result;
+
+  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
+           (frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
+
+  return result;
+}
+
+Eigen::Matrix<double, 2, 1> StateDynamics(
+    const Eigen::Matrix<double, 2, 1>& x) {
+  Eigen::Matrix<double, 2, 1> result;
+
+  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
+
+  return result;
+}
+
+TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
+  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
+                                            const Eigen::Matrix<double, 1, 1>&)>
+      modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
+
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
+      modelDynamics, units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
+}
+
+TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
+  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
+      modelDynamics = [](auto& x) { return StateDynamics(x); };
+
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0, 1;
+
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
+      modelDynamics, B, units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(LinearPlantInversionFeedforwardTest, Calculate) {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 1, 0, 0, 1;
+
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0, 1;
+
+  frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
+                                                         units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp

@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(2);
+
+    // Carriage mass
+    constexpr auto m = 5_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.0181864_m;
+
+    // Gear ratio
+    constexpr double G = 40.0 / 40.0;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{
+      plant, {0.02, 0.4}, {12.0}, 0.00505_s};
+
+  EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6);
+  EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6);
+}
+
+TEST(LinearQuadraticRegulatorTest, ArmGains) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(2);
+
+    // Carriage mass
+    constexpr auto m = 4_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.4_m;
+
+    // Gear ratio
+    constexpr double G = 100.0;
+
+    return frc::LinearSystemId::SingleJointedArmSystem(
+        motors, 1.0 / 3.0 * m * r * r, G);
+  }();
+
+  LinearQuadraticRegulator<2, 1> controller{
+      plant, {0.01745, 0.08726}, {12.0}, 0.00505_s};
+
+  EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1);
+  EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1);
+}
+
+TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(4);
+
+    // Carriage mass
+    constexpr auto m = 8_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.75_in;
+
+    // Gear ratio
+    constexpr double G = 14.67;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s};
+
+  EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1);
+  EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
+}
+
+}  // namespace frc