[wpimath] Merge .inc files into headers

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -6,46 +6,8 @@
 
 #include <wpi/json.h>
 
-#include "units/length.h"
-#include "units/math.h"
-
 using namespace frc;
 
-Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
-  auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
-  m_x = rectangular.X();
-  m_y = rectangular.Y();
-  m_z = rectangular.Z();
-}
-
-Translation3d::Translation3d(const Eigen::Vector3d& vector)
-    : m_x{units::meter_t{vector.x()}},
-      m_y{units::meter_t{vector.y()}},
-      m_z{units::meter_t{vector.z()}} {}
-
-units::meter_t Translation3d::Distance(const Translation3d& other) const {
-  return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
-                           units::math::pow<2>(other.m_y - m_y) +
-                           units::math::pow<2>(other.m_z - m_z));
-}
-
-units::meter_t Translation3d::Norm() const {
-  return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
-}
-
-Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
-  Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
-  auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
-  return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
-                       units::meter_t{qprime.Z()}};
-}
-
-bool Translation3d::operator==(const Translation3d& other) const {
-  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
-         units::math::abs(m_y - other.m_y) < 1E-9_m &&
-         units::math::abs(m_z - other.m_z) < 1E-9_m;
-}
-
 void frc::to_json(wpi::json& json, const Translation3d& translation) {
   json = wpi::json{{"x", translation.X().value()},
                    {"y", translation.Y().value()},

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -4,12 +4,22 @@
 
 #pragma once
 
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <unsupported/Eigen/MatrixFunctions>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
+#include "frc/DARE.h"
 #include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 
@@ -50,7 +60,8 @@ class LinearQuadraticRegulator {
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
                            const StateArray& Qelems, const InputArray& Relems,
-                           units::second_t dt);
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -69,7 +80,9 @@ class LinearQuadraticRegulator {
   LinearQuadraticRegulator(const Matrixd<States, States>& A,
                            const Matrixd<States, Inputs>& B,
                            const StateArray& Qelems, const InputArray& Relems,
-                           units::second_t dt);
+                           units::second_t dt)
+      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                                 MakeCostMatrix(Relems), dt) {}
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -85,7 +98,30 @@ class LinearQuadraticRegulator {
                            const Matrixd<States, Inputs>& B,
                            const Matrixd<States, States>& Q,
                            const Matrixd<Inputs, Inputs>& R,
-                           units::second_t dt);
+                           units::second_t dt) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    if (!IsStabilizable<States, Inputs>(discA, discB)) {
+      std::string msg = fmt::format(
+          "The system passed to the LQR is unstabilizable!\n\nA =\n{}\nB "
+          "=\n{}\n",
+          discA, discB);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_K = (discB.transpose() * S * discB + R)
+              .llt()
+              .solve(discB.transpose() * S * discA);
+
+    Reset();
+  }
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -103,7 +139,20 @@ class LinearQuadraticRegulator {
                            const Matrixd<States, States>& Q,
                            const Matrixd<Inputs, Inputs>& R,
                            const Matrixd<States, Inputs>& N,
-                           units::second_t dt);
+                           units::second_t dt) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
+
+    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+    m_K = (discB.transpose() * S * discB + R)
+              .llt()
+              .solve(discB.transpose() * S * discA + N.transpose());
+
+    Reset();
+  }
 
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
   LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -166,15 +215,21 @@ class LinearQuadraticRegulator {
    *
    * @param x The current state x.
    */
-  InputVector Calculate(const StateVector& x);
+  InputVector Calculate(const StateVector& x) {
+    m_u = m_K * (m_r - x);
+    return m_u;
+  }
 
   /**
    * Returns the next output of the controller.
    *
    * @param x     The current state x.
    * @param nextR The next reference vector r.
    */
-  InputVector Calculate(const StateVector& x, const StateVector& nextR);
+  InputVector Calculate(const StateVector& x, const StateVector& nextR) {
+    m_r = nextR;
+    return Calculate(x);
+  }
 
   /**
    * Adjusts LQR controller gain to compensate for a pure time delay in the
@@ -194,7 +249,13 @@ class LinearQuadraticRegulator {
    */
   template <int Outputs>
   void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
-                         units::second_t dt, units::second_t inputDelay);
+                         units::second_t dt, units::second_t inputDelay) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+  }
 
  private:
   // Current reference
@@ -215,5 +276,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
     LinearQuadraticRegulator<2, 2>;
 
 }  // namespace frc
-
-#include "LinearQuadraticRegulator.inc"

wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h

@@ -4,8 +4,10 @@
 
 #pragma once
 
+#include <wpi/ProtoHelper.h>
 #include <wpi/protobuf/Protobuf.h>
 
+#include "controller.pb.h"
 #include "frc/controller/SimpleMotorFeedforward.h"
 #include "units/length.h"
 
@@ -14,11 +16,35 @@
 
 template <class Distance>
 struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
-  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static google::protobuf::Message* New(google::protobuf::Arena* arena) {
+    return wpi::CreateMessage<wpi::proto::ProtobufSimpleMotorFeedforward>(
+        arena);
+  }
+
   static frc::SimpleMotorFeedforward<Distance> Unpack(
-      const google::protobuf::Message& msg);
+      const google::protobuf::Message& msg) {
+    auto m =
+        static_cast<const wpi::proto::ProtobufSimpleMotorFeedforward*>(&msg);
+    return {units::volt_t{m->ks()},
+            units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+                m->kv()},
+            units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+                m->ka()},
+            units::second_t{m->dt()}};
+  }
+
   static void Pack(google::protobuf::Message* msg,
-                   const frc::SimpleMotorFeedforward<Distance>& value);
+                   const frc::SimpleMotorFeedforward<Distance>& value) {
+    auto m = static_cast<wpi::proto::ProtobufSimpleMotorFeedforward*>(msg);
+    m->set_ks(value.GetKs().value());
+    m->set_kv(
+        units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+            value.GetKv()}
+            .value());
+    m->set_ka(
+        units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+            value.GetKa()}
+            .value());
+    m->set_dt(units::second_t{value.GetDt()}.value());
+  }
 };
-
-#include "frc/controller/proto/SimpleMotorFeedforwardProto.inc"