Merge branch 'main' into diffydrive

src-tauri/src/main.rs

@@ -11,7 +11,7 @@ use tauri::{
     api::{dialog::blocking::FileDialogBuilder, file},
     Manager,
 };
-use trajoptlib::{HolonomicTrajectory, Pose2d, SwerveDrivetrain, SwerveModule, SwervePathBuilder};
+use trajoptlib::{HolonomicTrajectory, Pose2d, SwerveDrivetrain, SwervePathBuilder, Translation2d};
 
 #[derive(Clone, serde::Serialize, Debug)]
 struct OpenFileEventPayload<'a> {
@@ -410,34 +410,25 @@ async fn generate_trajectory(
     let drivetrain = SwerveDrivetrain {
         mass: config.mass,
         moi: config.rotationalInertia,
+        wheel_radius: config.wheelRadius,
+        wheel_max_angular_velocity: config.wheelMaxVelocity,
+        wheel_max_torque: config.wheelMaxTorque,
         modules: vec![
-            SwerveModule {
+            Translation2d {
                 x: half_wheel_base,
                 y: half_track_width,
-                wheel_radius: config.wheelRadius,
-                wheel_max_angular_velocity: config.wheelMaxVelocity,
-                wheel_max_torque: config.wheelMaxTorque,
             },
-            SwerveModule {
+            Translation2d {
                 x: half_wheel_base,
                 y: -half_track_width,
-                wheel_radius: config.wheelRadius,
-                wheel_max_angular_velocity: config.wheelMaxVelocity,
-                wheel_max_torque: config.wheelMaxTorque,
             },
-            SwerveModule {
+            Translation2d {
                 x: -half_wheel_base,
                 y: half_track_width,
-                wheel_radius: config.wheelRadius,
-                wheel_max_angular_velocity: config.wheelMaxVelocity,
-                wheel_max_torque: config.wheelMaxTorque,
             },
-            SwerveModule {
+            Translation2d {
                 x: -half_wheel_base,
                 y: -half_track_width,
-                wheel_radius: config.wheelRadius,
-                wheel_max_angular_velocity: config.wheelMaxVelocity,
-                wheel_max_torque: config.wheelMaxTorque,
             },
         ],
     };

trajoptlib/examples/Swerve/src/Main.cpp

@@ -17,10 +17,10 @@ int main() {
   trajopt::SwerveDrivetrain swerveDrivetrain{
       .mass = 45,
       .moi = 6,
-      .modules = {{{+0.6, +0.6}, 0.04, 70, 2},
-                  {{+0.6, -0.6}, 0.04, 70, 2},
-                  {{-0.6, +0.6}, 0.04, 70, 2},
-                  {{-0.6, -0.6}, 0.04, 70, 2}}};
+      .wheelRadius = 0.04,
+      .wheelMaxAngularVelocity = 70,
+      .wheelMaxTorque = 2,
+      .modules = {{+0.6, +0.6}, {+0.6, -0.6}, {-0.6, +0.6}, {-0.6, -0.6}}};
 
   trajopt::LinearVelocityMaxMagnitudeConstraint zeroLinearVelocity{0.0};
   trajopt::AngularVelocityMaxMagnitudeConstraint zeroAngularVelocity{0.0};

trajoptlib/examples/swerve.rs

@@ -1,38 +1,17 @@
-use trajoptlib::{SwerveDrivetrain, SwerveModule, SwervePathBuilder};
+use trajoptlib::{SwerveDrivetrain, SwervePathBuilder, Translation2d};
 
 fn main() {
     let drivetrain = SwerveDrivetrain {
         mass: 45.0,
         moi: 6.0,
+        wheel_radius: 0.04,
+        wheel_max_angular_velocity: 70.0,
+        wheel_max_torque: 2.0,
         modules: vec![
-            SwerveModule {
-                x: 0.6,
-                y: 0.6,
-                wheel_radius: 0.04,
-                wheel_max_angular_velocity: 70.0,
-                wheel_max_torque: 2.0,
-            },
-            SwerveModule {
-                x: 0.6,
-                y: -0.6,
-                wheel_radius: 0.04,
-                wheel_max_angular_velocity: 70.0,
-                wheel_max_torque: 2.0,
-            },
-            SwerveModule {
-                x: -0.6,
-                y: 0.6,
-                wheel_radius: 0.04,
-                wheel_max_angular_velocity: 70.0,
-                wheel_max_torque: 2.0,
-            },
-            SwerveModule {
-                x: -0.6,
-                y: -0.6,
-                wheel_radius: 0.04,
-                wheel_max_angular_velocity: 70.0,
-                wheel_max_torque: 2.0,
-            },
+            Translation2d { x: 0.6, y: 0.6 },
+            Translation2d { x: 0.6, y: -0.6 },
+            Translation2d { x: -0.6, y: 0.6 },
+            Translation2d { x: -0.6, y: -0.6 },
         ],
     };
 

trajoptlib/include/trajopt/drivetrain/SwerveDrivetrain.hpp

@@ -4,7 +4,7 @@
 
 #include <vector>
 
-#include "trajopt/drivetrain/SwerveModule.hpp"
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
@@ -16,13 +16,25 @@ namespace trajopt {
  * be four. The order the swerve modules are listed does not matter.
  */
 struct TRAJOPT_DLLEXPORT SwerveDrivetrain {
-  /// the mass of the robot (kg)
+  /// The mass of the robot (kg).
   double mass;
-  /// the moment of inertia of the robot about the origin (kg−m²)
+
+  /// The moment of inertia of the robot about the origin (kg−m²).
   double moi;
-  /// The list of swerve modules that make the robot move, usually one in each
+
+  /// Radius of wheel (meters).
+  double wheelRadius;
+
+  /// Maximum angular velocity of wheel (rad/s).
+  double wheelMaxAngularVelocity;
+
+  /// Maximum torque (N−m) applied to wheel.
+  double wheelMaxTorque;
+
+  /// Translation (meters) of each swerve module from the origin of the robot
+  /// coordinate system to the center of the module. There's usually one in each
   /// corner.
-  std::vector<SwerveModule> modules;
+  std::vector<Translation2d> modules;
 };
 
 }  // namespace trajopt

trajoptlib/src/RustFFI.cpp

@@ -17,7 +17,6 @@
 #include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/PointAtConstraint.hpp"
 #include "trajopt/drivetrain/DifferentialDrivetrain.hpp"
-#include "trajopt/drivetrain/SwerveModule.hpp"
 #include "trajopt/path/DifferentialPathBuilder.hpp"
 #include "trajopt/trajectory/DifferentialTrajcectory.hpp"
 #include "trajopt/trajectory/DifferentialTrajectorySample.hpp"
@@ -29,17 +28,15 @@
 namespace trajopt::rsffi {
 
 void SwervePathBuilder::set_drivetrain(const SwerveDrivetrain& drivetrain) {
-  std::vector<trajopt::SwerveModule> cppModules;
+  std::vector<trajopt::Translation2d> cppModules;
   for (const auto& module : drivetrain.modules) {
-    cppModules.push_back(
-        trajopt::SwerveModule{{module.x, module.y},
-                              module.wheel_radius,
-                              module.wheel_max_angular_velocity,
-                              module.wheel_max_torque});
+    cppModules.emplace_back(module.x, module.y);
   }
 
   path_builder.SetDrivetrain(trajopt::SwerveDrivetrain{
-      drivetrain.mass, drivetrain.moi, std::move(cppModules)});
+      drivetrain.mass, drivetrain.moi, drivetrain.wheel_radius,
+      drivetrain.wheel_max_angular_velocity, drivetrain.wheel_max_torque,
+      std::move(cppModules)});
 }
 
 void SwervePathBuilder::set_control_interval_counts(

trajoptlib/src/SwerveTrajectoryGenerator.cpp

@@ -87,27 +87,25 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
 
   double minWidth = INFINITY;
   for (size_t i = 1; i < path.drivetrain.modules.size(); i++) {
-    if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() -
-                 path.drivetrain.modules.at(i).translation.X()) != 0) {
-      minWidth = std::min(
-          minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.X() -
-                             path.drivetrain.modules.at(i).translation.X()));
+    if (std::abs(path.drivetrain.modules.at(i - 1).X() -
+                 path.drivetrain.modules.at(i).X()) != 0) {
+      minWidth =
+          std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).X() -
+                                      path.drivetrain.modules.at(i).X()));
     }
-    if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() -
-                 path.drivetrain.modules.at(i).translation.Y()) != 0) {
-      minWidth = std::min(
-          minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.Y() -
-                             path.drivetrain.modules.at(i).translation.Y()));
+    if (std::abs(path.drivetrain.modules.at(i - 1).Y() -
+                 path.drivetrain.modules.at(i).Y()) != 0) {
+      minWidth =
+          std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).Y() -
+                                      path.drivetrain.modules.at(i).Y()));
     }
   }
 
   for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
     dt.emplace_back(problem.DecisionVariable());
-    for (auto module : path.drivetrain.modules) {
-      problem.SubjectTo(dt.at(sgmtIndex) * module.wheelRadius *
-                            module.wheelMaxAngularVelocity <=
-                        minWidth);
-    }
+    problem.SubjectTo(dt.at(sgmtIndex) * path.drivetrain.wheelRadius *
+                          path.drivetrain.wheelMaxAngularVelocity <=
+                      minWidth);
   }
 
   // Minimize total time
@@ -163,13 +161,16 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
     auto Fy_net = std::accumulate(Fy.at(index).begin(), Fy.at(index).end(),
                                   sleipnir::Variable{0.0});
 
+    const auto& wheelRadius = path.drivetrain.wheelRadius;
+    const auto& wheelMaxAngularVelocity =
+        path.drivetrain.wheelMaxAngularVelocity;
+    const auto& wheelMaxTorque = path.drivetrain.wheelMaxTorque;
+
     // Solve for net torque
     sleipnir::Variable tau_net = 0.0;
     for (size_t moduleIndex = 0; moduleIndex < path.drivetrain.modules.size();
          ++moduleIndex) {
-      const auto& [translation, wheelRadius, wheelMaxAngularVelocity,
-                   wheelMaxTorque] = path.drivetrain.modules.at(moduleIndex);
-
+      const auto& translation = path.drivetrain.modules.at(moduleIndex);
       auto r = translation.RotateBy(theta);
       Translation2v F{Fx.at(index).at(moduleIndex),
                       Fy.at(index).at(moduleIndex)};
@@ -181,8 +182,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
     auto vWrtRobot = v.RotateBy(-theta);
     for (size_t moduleIndex = 0; moduleIndex < path.drivetrain.modules.size();
          ++moduleIndex) {
-      const auto& [translation, wheelRadius, wheelMaxAngularVelocity,
-                   wheelMaxTorque] = path.drivetrain.modules.at(moduleIndex);
+      const auto& translation = path.drivetrain.modules.at(moduleIndex);
 
       Translation2v vWheelWrtRobot{
           vWrtRobot.X() - translation.Y() * omega.at(index),

trajoptlib/src/lib.rs

@@ -1,19 +1,19 @@
 #[cxx::bridge(namespace = "trajopt::rsffi")]
 mod ffi {
     #[derive(Debug, Deserialize, Serialize)]
-    struct SwerveModule {
+    struct Translation2d {
         x: f64,
         y: f64,
-        wheel_radius: f64,
-        wheel_max_angular_velocity: f64,
-        wheel_max_torque: f64,
     }
 
     #[derive(Debug, Deserialize, Serialize)]
     struct SwerveDrivetrain {
         mass: f64,
         moi: f64,
-        modules: Vec<SwerveModule>,
+        wheel_radius: f64,
+        wheel_max_angular_velocity: f64,
+        wheel_max_torque: f64,
+        modules: Vec<Translation2d>,
     }
 
     #[derive(Debug, Deserialize, Serialize)]
@@ -851,4 +851,4 @@ pub use ffi::HolonomicTrajectory;
 pub use ffi::HolonomicTrajectorySample;
 pub use ffi::Pose2d;
 pub use ffi::SwerveDrivetrain;
-pub use ffi::SwerveModule;
+pub use ffi::Translation2d;

trajoptlib/test/src/ObstacleTest.cpp

@@ -11,12 +11,13 @@ TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") {
 
   using namespace trajopt;
 
-  SwerveDrivetrain swerveDrivetrain{.mass = 45,
-                                    .moi = 6,
-                                    .modules = {{{+0.6, +0.6}, 0.04, 70, 2},
-                                                {{+0.6, -0.6}, 0.04, 70, 2},
-                                                {{-0.6, +0.6}, 0.04, 70, 2},
-                                                {{-0.6, -0.6}, 0.04, 70, 2}}};
+  SwerveDrivetrain swerveDrivetrain{
+      .mass = 45,
+      .moi = 6,
+      .wheelRadius = 0.04,
+      .wheelMaxAngularVelocity = 70,
+      .wheelMaxTorque = 2,
+      .modules = {{+0.6, +0.6}, {+0.6, -0.6}, {-0.6, +0.6}, {-0.6, -0.6}}};
 
   trajopt::SwervePathBuilder path;
   path.PoseWpt(0, 0.0, 0.0, 0.0);