Merge branch 'main' into embind

include/trajopt/trajectory/HolonomicTrajectory.h

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <utility>
 #include <vector>
 
 #include "trajopt/SymbolExports.h"
@@ -18,19 +19,32 @@ class TRAJOPT_DLLEXPORT HolonomicTrajectory {
   /// Trajectory samples.
   std::vector<HolonomicTrajectorySample> samples;
 
+  HolonomicTrajectory() = default;
+
   /**
    * Construct a HolonomicTrajectory from samples.
    *
    * @param samples The samples.
    */
-  explicit HolonomicTrajectory(std::vector<HolonomicTrajectorySample> samples);
+  explicit HolonomicTrajectory(std::vector<HolonomicTrajectorySample> samples)
+      : samples{std::move(samples)} {}
 
   /**
    * Construct a HolonomicTrajectory from a solution.
    *
    * @param solution The solution.
    */
-  explicit HolonomicTrajectory(const HolonomicSolution& solution);
+  explicit 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]);
+    }
+  }
 };
 
 }  // namespace trajopt

include/trajopt/trajectory/HolonomicTrajectorySample.h

@@ -12,25 +12,27 @@ namespace trajopt {
 class TRAJOPT_DLLEXPORT HolonomicTrajectorySample {
  public:
   /// The timestamp.
-  double timestamp;
+  double timestamp = 0.0;
 
   /// The x coordinate.
-  double x;
+  double x = 0.0;
 
   /// The y coordinate.
-  double y;
+  double y = 0.0;
 
   /// The heading.
-  double heading;
+  double heading = 0.0;
 
   /// The velocity's x component.
-  double velocityX;
+  double velocityX = 0.0;
 
   /// The velocity's y component.
-  double velocityY;
+  double velocityY = 0.0;
 
   /// The angular velocity.
-  double angularVelocity;
+  double angularVelocity = 0.0;
+
+  constexpr HolonomicTrajectorySample() = default;
 
   /**
    * Construct a HolonomicTrajectorySample.
@@ -43,9 +45,16 @@ class TRAJOPT_DLLEXPORT HolonomicTrajectorySample {
    * @param velocityY The velocity's y component.
    * @param angularVelocity The angular velocity.
    */
-  HolonomicTrajectorySample(double timestamp, double x, double y,
-                            double heading, double velocityX, double velocityY,
-                            double angularVelocity);
+  constexpr HolonomicTrajectorySample(double timestamp, double x, double y,
+                                      double heading, double velocityX,
+                                      double velocityY, double angularVelocity)
+      : timestamp{timestamp},
+        x{x},
+        y{y},
+        heading{heading},
+        velocityX{velocityX},
+        velocityY{velocityY},
+        angularVelocity{angularVelocity} {}
 };
 
 }  // namespace trajopt