commit f65b1a5d84ffa08311b78674ca592306b1ffdebc
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Tue Oct 08 23:37:35 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Wed Oct 16 20:36:35 2024 -0700
tree 4d9d1f9a204e0d678ed22618580b0e6b2056c192
parent 601b23a39b0a8e708ed6602ee305d57962af4435

Remove deceptive velocity physics codegen for C++

I had added some "velocity" physics which just stripped out non-velocity
related states, which doesn't make sense for what is *actually* implied
by the velocity phsyics we refer to (which *also* implies that we are
not doing a slip ratio calculation and instead assume that the torque
from the drive motors passes through perfectly to the ground).

Change-Id: I853192cf6af531a2516f545284e8fbecb4a09a41
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/swerve/generate_physics.cc

diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index af058f6..f0c2d11 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -336,7 +336,7 @@
     result_h.emplace_back(
         "inline constexpr size_t kNumVelocityStates = "
         "static_cast<size_t>(VelocityStates::kNumStates);");
-    result_h.emplace_back("struct Inputs {");
+    result_h.emplace_back("struct InputStates {");
     result_h.emplace_back("enum States {");
     result_h.emplace_back("  kIs0 = 0,");
     result_h.emplace_back("  kId0 = 1,");
@@ -351,7 +351,7 @@
     result_h.emplace_back("};");
     result_h.emplace_back(
         "inline constexpr size_t kNumInputs = "
-        "static_cast<size_t>(Inputs::kNumInputs);");
+        "static_cast<size_t>(InputStates::kNumInputs);");
     result_h.emplace_back("");
     result_h.emplace_back("// Returns the derivative of our state vector");
     result_h.emplace_back(
@@ -362,29 +362,6 @@
     result_h.emplace_back(
         "    Eigen::Ref<const Eigen::Matrix<double, kNumInputs, 1>> U);");
     result_h.emplace_back("");
-    result_h.emplace_back(
-        "Eigen::Matrix<double, kNumVelocityStates, 1> ToVelocityState(");
-    result_h.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
-        "1>> X);");
-    result_h.emplace_back("");
-    result_h.emplace_back(
-        "Eigen::Matrix<double, kNumFullDynamicsStates, 1> FromVelocityState(");
-    result_h.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> "
-        "X);");
-    result_h.emplace_back("");
-    result_h.emplace_back(
-        "inline Eigen::Matrix<double, kNumVelocityStates, 1> VelocityPhysics(");
-    result_h.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> "
-        "X,");
-    result_h.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumInputs, 1>> U) {");
-    result_h.emplace_back(
-        "  return ToVelocityState(SwervePhysics(FromVelocityState(X), U));");
-    result_h.emplace_back("}");
-    result_h.emplace_back("");
     result_h.emplace_back("}  // namespace frc971::control_loops::swerve");
     result_h.emplace_back("");
     result_h.emplace_back(absl::Substitute("#endif  // $0_", include_guard));
@@ -398,40 +375,6 @@
     result_cc.emplace_back("namespace frc971::control_loops::swerve {");
     result_cc.emplace_back("");
     result_cc.emplace_back(
-        "Eigen::Matrix<double, kNumVelocityStates, 1> ToVelocityState(");
-    result_cc.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
-        "1>> X) {");
-    result_cc.emplace_back(
-        "    Eigen::Matrix<double, kNumVelocityStates, 1> velocity;");
-    const std::vector<std::string_view> velocity_states = {
-        "kThetas0", "kOmegas0", "kThetas1", "kOmegas1", "kThetas2", "kOmegas2",
-        "kThetas3", "kOmegas3", "kTheta",   "kVx",      "kVy",      "kOmega"};
-    for (const std::string_view velocity_state : velocity_states) {
-      result_cc.emplace_back(absl::StrFormat(
-          "  velocity(VelocityStates::%s) = X(FullDynamicsStates::%s);",
-          velocity_state, velocity_state));
-    }
-    result_cc.emplace_back("  return velocity;");
-    result_cc.emplace_back("}");
-    result_cc.emplace_back("");
-    result_cc.emplace_back(
-        "Eigen::Matrix<double, kNumFullDynamicsStates, 1> FromVelocityState(");
-    result_cc.emplace_back(
-        "    Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> X) "
-        "{");
-    result_cc.emplace_back(
-        "    Eigen::Matrix<double, kNumFullDynamicsStates, 1> full;");
-    result_cc.emplace_back("    full.setZero();");
-    for (const std::string_view velocity_state : velocity_states) {
-      result_cc.emplace_back(absl::StrFormat(
-          "  full(FullDynamicsStates::%s) = X(VelocityStates::%s);",
-          velocity_state, velocity_state));
-    }
-    result_cc.emplace_back("  return full;");
-    result_cc.emplace_back("}");
-    result_cc.emplace_back("");
-    result_cc.emplace_back(
         "Eigen::Matrix<double, kNumFullDynamicsStates, 1> SwervePhysics(");
     result_cc.emplace_back(
         "    Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "