Add back missing constructor

SleipnirGroup · Feb 18, 2024 · bc9af85 · bc9af85
1 parent 1d9cfee
commit bc9af85
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/src/trajectory/HolonomicTrajectory.cpp b/src/trajectory/HolonomicTrajectory.cpp
@@ -12,6 +12,18 @@ HolonomicTrajectory::HolonomicTrajectory(
     std::vector<HolonomicTrajectorySample> samples)
     : samples(std::move(samples)) {}
 
+HolonomicTrajectory::HolonomicTrajectory(const HolonomicSolution& solution) {
+  double ts = 0.0;
+  for (size_t samp = 0; samp < solution.x.size(); samp++) {
+    if (samp != 0) {
+      ts += solution.dt[samp - 1];
+    }
+    samples.emplace_back(ts, solution.x[samp], solution.y[samp],
+                         solution.theta[samp], solution.vx[samp],
+                         solution.vy[samp], solution.omega[samp]);
+  }
+}
+
 HolonomicTrajectory HolonomicTrajectory::FromSolution(
     const HolonomicSolution& solution) {
   std::vector<HolonomicTrajectorySample> samples;