Merge .inc files into .h files (#181)

This fixes clangd's header parsing and makes the function
implementations easier to find.

.styleguide

@@ -1,7 +1,6 @@
 cppHeaderFileInclude {
   \.h$
   \.hpp$
-  \.inc$
   expected$
 }
 

include/.styleguide

@@ -1,7 +1,6 @@
 cppHeaderFileInclude {
   \.h$
   \.hpp$
-  \.inc$
   expected$
 }
 

src/.styleguide

@@ -1,7 +1,6 @@
 cppHeaderFileInclude {
   \.h$
   \.hpp$
-  \.inc$
   expected$
 }
 

src/optimization/HolonomicTrajoptUtil.h

@@ -2,12 +2,13 @@
 
 #pragma once
 
-#include <memory>
-#include <vector>
-
 #include "optimization/TrajoptUtil.h"
-#include "trajopt/obstacle/Obstacle.h"
-#include "trajopt/solution/HolonomicSolution.h"
+#include "trajopt/constraint/HeadingConstraint.h"
+#include "trajopt/constraint/LinePointConstraint.h"
+#include "trajopt/constraint/PointAtConstraint.h"
+#include "trajopt/constraint/PointLineConstraint.h"
+#include "trajopt/constraint/PointPointConstraint.h"
+#include "trajopt/constraint/TranslationConstraint.h"
 
 namespace trajopt {
 
@@ -17,7 +18,56 @@ void ApplyHolonomicConstraint(Opti& opti, const Expr& x, const Expr& y,
                               const Expr& theta, const Expr& vx, const Expr& vy,
                               const Expr& omega, const Expr& ax, const Expr& ay,
                               const Expr& alpha,
-                              const HolonomicConstraint& constraint);
-}  // namespace trajopt
+                              const HolonomicConstraint& constraint) {
+  if (std::holds_alternative<HolonomicVelocityConstraint>(constraint)) {
+    const auto& velocityHolonomicConstraint =
+        std::get<HolonomicVelocityConstraint>(constraint);
+    ApplySet2dConstraint(opti, vx, vy,
+                         velocityHolonomicConstraint.velocityBound);
+  } else if (std::holds_alternative<AngularVelocityConstraint>(constraint)) {
+    const auto& angularVelocityConstraint =
+        std::get<AngularVelocityConstraint>(constraint);
+    ApplyIntervalSet1dConstraint(
+        opti, omega, angularVelocityConstraint.angularVelocityBound);
+  } else if (std::holds_alternative<TranslationConstraint>(constraint)) {
+    ApplyConstraint(opti, x, y, theta,
+                    std::get<TranslationConstraint>(constraint));
+  } else if (std::holds_alternative<HeadingConstraint>(constraint)) {
+    ApplyConstraint(opti, x, y, theta, std::get<HeadingConstraint>(constraint));
+  } else if (std::holds_alternative<LinePointConstraint>(constraint)) {
+    ApplyConstraint(opti, x, y, theta,
+                    std::get<LinePointConstraint>(constraint));
+  } else if (std::holds_alternative<PointAtConstraint>(constraint)) {
+    auto pointAtConstraint = std::get<PointAtConstraint>(constraint);
+    double fieldPointX = pointAtConstraint.fieldPointX;
+    double fieldPointY = pointAtConstraint.fieldPointY;
+    double headingTolerance = pointAtConstraint.headingTolerance;
+    /**
+     * dx,dy = desired heading
+     * ux,uy = unit vector of desired heading
+     * hx,hy = heading
+     * dot = dot product of ux,uy and hx,hy
+     *
+     * constrain dot to cos(1.0), which is colinear
+     * and cos(thetaTolerance)
+     */
+    auto dx = fieldPointX - x;
+    auto dy = fieldPointY - y;
+    auto ux = dx / hypot(dx, dy);  // NOLINT
+    auto uy = dy / hypot(dx, dy);  // NOLINT
+    auto hx = cos(theta);          // NOLINT
+    auto hy = sin(theta);          // NOLINT
+    auto dot = hx * ux + hy * uy;
 
-#include "optimization/HolonomicTrajoptUtil.inc"
+    ApplyIntervalSet1dConstraint(
+        opti, dot, IntervalSet1d(std::cos(headingTolerance), 1.0));
+  } else if (std::holds_alternative<PointLineConstraint>(constraint)) {
+    ApplyConstraint(opti, x, y, theta,
+                    std::get<PointLineConstraint>(constraint));
+  } else if (std::holds_alternative<PointPointConstraint>(constraint)) {
+    ApplyConstraint(opti, x, y, theta,
+                    std::get<PointPointConstraint>(constraint));
+  }  // TODO: Investigate a way to condense the code above
+}
+
+}  // namespace trajopt

src/optimization/SwerveTrajoptUtil.h

@@ -5,25 +5,47 @@
 #include <utility>
 #include <vector>
 
-#include "optimization/HolonomicTrajoptUtil.h"
-#include "optimization/OptiSys.h"
 #include "optimization/TrajoptUtil.h"
 #include "trajopt/drivetrain/SwerveDrivetrain.h"
-#include "trajopt/path/Path.h"
 #include "trajopt/solution/SwerveSolution.h"
 
 namespace trajopt {
 
 template <typename Expr>
   requires ExprSys<Expr>
 std::pair<Expr, Expr> SolveNetForce(const std::vector<Expr>& Fx,
-                                    const std::vector<Expr>& Fy);
+                                    const std::vector<Expr>& Fy) {
+  Expr Fx_net = 0;
+  Expr Fy_net = 0;
+
+  for (auto& _Fx : Fx) {
+    Fx_net += _Fx;
+  }
+  for (auto& _Fy : Fy) {
+    Fy_net += _Fy;
+  }
+
+  return {Fx_net, Fy_net};
+}
 
 template <typename Expr>
   requires ExprSys<Expr>
 Expr SolveNetTorque(const Expr& theta, const std::vector<Expr>& Fx,
                     const std::vector<Expr>& Fy,
-                    const std::vector<SwerveModule>& swerveModules);
+                    const std::vector<SwerveModule>& swerveModules) {
+  Expr tau_net = 0;
+
+  for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) {
+    auto& swerveModule = swerveModules.at(moduleIdx);
+    auto [x_m, y_m] =
+        RotateConstantVector(swerveModule.x, swerveModule.y, theta);
+    auto& Fx_m = Fx.at(moduleIdx);
+    auto& Fy_m = Fy.at(moduleIdx);
+    tau_net += x_m * Fy_m - y_m * Fx_m;
+  }
+
+  return tau_net;
+}
 
 template <typename Expr, typename Opti>
   requires OptiSys<Expr, Opti>
@@ -33,7 +55,38 @@ void ApplyKinematicsConstraints(
     const std::vector<Expr>& vy, const std::vector<Expr>& omega,
     const std::vector<Expr>& ax, const std::vector<Expr>& ay,
     const std::vector<Expr>& alpha, const std::vector<Expr>& dt,
-    const std::vector<size_t>& N);
+    const std::vector<size_t>& N) {
+  size_t wptCnt = N.size() + 1;
+
+  for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) {
+    size_t N_sgmt = N.at(wptIdx - 1);
+    auto dt_sgmt = dt.at(wptIdx - 1);
+    for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) {
+      size_t idx = GetIdx(N, wptIdx, sampIdx);
+      auto x_n = x.at(idx);
+      auto x_n_1 = x.at(idx - 1);
+      auto y_n = y.at(idx);
+      auto y_n_1 = y.at(idx - 1);
+      auto theta_n = theta.at(idx);
+      auto theta_n_1 = theta.at(idx - 1);
+      auto vx_n = vx.at(idx);
+      auto vx_n_1 = vx.at(idx - 1);
+      auto vy_n = vy.at(idx);
+      auto vy_n_1 = vy.at(idx - 1);
+      auto omega_n = omega.at(idx);
+      auto omega_n_1 = omega.at(idx - 1);
+      auto ax_n = ax.at(idx);
+      auto ay_n = ay.at(idx);
+      auto alpha_n = alpha.at(idx);
+      opti.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n);
+      opti.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n);
+      opti.SubjectTo(theta_n_1 + omega_n * dt_sgmt == theta_n);
+      opti.SubjectTo(vx_n_1 + ax_n * dt_sgmt == vx_n);
+      opti.SubjectTo(vy_n_1 + ay_n * dt_sgmt == vy_n);
+      opti.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n);
+    }
+  }
+}
 
 /**
  * Applies the drivetrain-specific constraints to the optimizer. These
@@ -68,15 +121,50 @@ template <typename Expr, typename Opti>
 void ApplyDynamicsConstraints(Opti& opti, const Expr& ax, const Expr& ay,
                               const Expr& alpha, const Expr& Fx_net,
                               const Expr& Fy_net, const Expr& tau_net,
-                              double mass, double moi);
+                              double mass, double moi) {
+  opti.SubjectTo(Fx_net == mass * ax);
+  opti.SubjectTo(Fy_net == mass * ay);
+  opti.SubjectTo(tau_net == moi * alpha);
+}
 
 template <typename Expr, typename Opti>
   requires OptiSys<Expr, Opti>
 void ApplyPowerConstraints(Opti& opti, const Expr& theta, const Expr& vx,
                            const Expr& vy, const Expr& omega,
                            const std::vector<Expr>& Fx,
                            const std::vector<Expr>& Fy,
-                           const SwerveDrivetrain& swerveDrivetrain);
+                           const SwerveDrivetrain& swerveDrivetrain) {
+  auto [vx_prime, vy_prime] = RotateVector(vx, vy, -theta);
+
+  size_t moduleCount = swerveDrivetrain.modules.size();
+
+  std::vector<Expr> vx_m;
+  std::vector<Expr> vy_m;
+  vx_m.reserve(moduleCount);
+  vy_m.reserve(moduleCount);
+
+  for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) {
+    auto x_m = swerveDrivetrain.modules.at(moduleIdx).x;
+    auto y_m = swerveDrivetrain.modules.at(moduleIdx).y;
+    vx_m.emplace_back(vx_prime - y_m * omega);
+    vy_m.emplace_back(vy_prime + x_m * omega);
+  }
+
+  for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) {
+    auto& _module = swerveDrivetrain.modules.at(moduleIdx);
+    double maxWheelVelocity =
+        _module.wheelRadius * _module.wheelMaxAngularVelocity;
+    double maxForce = _module.wheelMaxTorque / _module.wheelRadius;
+    auto _vx_m = vx_m.at(moduleIdx);
+    auto _vy_m = vy_m.at(moduleIdx);
+    auto Fx_m = Fx.at(moduleIdx);
+    auto Fy_m = Fy.at(moduleIdx);
+    opti.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <=
+                   maxWheelVelocity * maxWheelVelocity);
+
+    opti.SubjectTo(Fx_m * Fx_m + Fy_m * Fy_m <= maxForce * maxForce);
+  }
+}
 
 template <typename Expr, typename Opti>
   requires OptiSys<Expr, Opti>
@@ -87,7 +175,27 @@ SwerveSolution ConstructSwerveSolution(
     const std::vector<Expr>& ax, const std::vector<Expr>& ay,
     const std::vector<Expr>& alpha, const std::vector<std::vector<Expr>>& Fx,
     const std::vector<std::vector<Expr>>& Fy, const std::vector<Expr>& dt,
-    const std::vector<size_t>& N);
-}  // namespace trajopt
+    const std::vector<size_t>& N) {
+  std::vector<double> dtPerSamp;
+  for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) {
+    size_t N_sgmt = N.at(sgmtIdx);
+    Expr dt_sgmt = dt.at(sgmtIdx);
+    double dt_val = opti.SolutionValue(dt_sgmt);
+    for (size_t i = 0; i < N_sgmt; ++i) {
+      dtPerSamp.push_back(dt_val);
+    }
+  }
+  return SwerveSolution{
+      {{dtPerSamp, RowSolutionValue(opti, x), RowSolutionValue(opti, y),
+        RowSolutionValue(opti, theta)},
+       RowSolutionValue(opti, vx),
+       RowSolutionValue(opti, vy),
+       RowSolutionValue(opti, omega),
+       RowSolutionValue(opti, ax),
+       RowSolutionValue(opti, ay),
+       RowSolutionValue(opti, alpha)},
+      MatrixSolutionValue(opti, Fx),
+      MatrixSolutionValue(opti, Fy)};
+}
 
-#include "optimization/SwerveTrajoptUtil.inc"
+}  // namespace trajopt