Add differential drive Rust example

trajoptlib/examples/differential.rs

@@ -0,0 +1,36 @@
+use trajoptlib::{DifferentialDriverail, DifferentialDrivetrain, DifferentialPathBuilder};
+
+fn main() {
+    let drivetrain = DifferentialDrivetrain {
+        mass: 45.0,
+        moi: 6.0,
+        trackwidth: 6.0,
+        left: DifferentialDriverail {
+            wheel_radius: 0.08,
+            wheel_max_angular_velocity: 70.0,
+            wheel_max_torque: 5.0,
+        },
+        right: DifferentialDriverail {
+            wheel_radius: 0.08,
+            wheel_max_angular_velocity: 70.0,
+            wheel_max_torque: 5.0,
+        },
+    };
+
+    let mut path = DifferentialPathBuilder::new();
+
+    path.add_progress_callback(|traj, handle| println!("{:?}: handle {}", traj, handle));
+    path.set_drivetrain(&drivetrain);
+    path.set_bumpers(1.3, 1.3);
+
+    path.pose_wpt(0, 0.0, 0.0, 0.0);
+    path.pose_wpt(1, 1.0, 0.0, 0.0);
+
+    path.wpt_angular_velocity_max_magnitude(0, 0.0);
+    path.wpt_angular_velocity_max_magnitude(1, 0.0);
+    path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2);
+
+    path.set_control_interval_counts(vec![40]);
+
+    println!("{:?}", path.generate(true, 0));
+}

trajoptlib/examples/swerve.rs

@@ -37,15 +37,19 @@ fn main() {
     };
 
     let mut path = SwervePathBuilder::new();
+
     path.add_progress_callback(|traj, handle| println!("{:?}: handle {}", traj, handle));
     path.set_drivetrain(&drivetrain);
     path.set_bumpers(1.3, 1.3);
+
     path.pose_wpt(0, 0.0, 0.0, 0.0);
     path.pose_wpt(1, 1.0, 0.0, 0.0);
+
     path.wpt_angular_velocity_max_magnitude(0, 0.0);
     path.wpt_angular_velocity_max_magnitude(1, 0.0);
     path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2);
+
     path.set_control_interval_counts(vec![40]);
-    println!("setup complete");
+
     println!("{:?}", path.generate(true, 0));
 }

trajoptlib/include/trajopt/path/DifferentialPathBuilder.hpp

@@ -2,13 +2,23 @@
 
 #pragma once
 
+#include <stdint.h>
+
+#include <functional>
+
 #include "trajopt/drivetrain/DifferentialDrivetrain.hpp"
 #include "trajopt/path/Path.hpp"
 #include "trajopt/path/PathBuilder.hpp"
 #include "trajopt/solution/DifferentialSolution.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
+
+/**
+ * Builds a swerve path using information about how the robot
+ * must travel through a series of waypoints. This path can be converted
+ * to a trajectory using DifferentialTrajectoryGenerator.
+ */
 class TRAJOPT_DLLEXPORT DifferentialPathBuilder
     : public PathBuilder<DifferentialPath> {
  public:
@@ -26,5 +36,18 @@ class TRAJOPT_DLLEXPORT DifferentialPathBuilder
    * @return the initial guess, as a solution
    */
   DifferentialSolution CalculateInitialGuess() const;
+
+  /**
+   * Add a callback to retrieve the state of the solver as a
+   * DifferentialSolution.
+   * This callback will run on every iteration of the solver.
+   * The callback's first parameter is the SwerveSolution based on the solver's
+   * state at that iteration. The second parameter is the handle passed into
+   * Generate().
+   * @param callback the callback
+   */
+  void AddIntermediateCallback(
+      const std::function<void(DifferentialSolution&, int64_t)> callback);
 };
+
 }  // namespace trajopt

trajoptlib/include/trajopt/path/SwervePathBuilder.hpp

@@ -4,16 +4,13 @@
 
 #include <stdint.h>
 
-#include <cassert>
-#include <cstddef>
 #include <functional>
-#include <vector>
 
 #include "trajopt/drivetrain/SwerveDrivetrain.hpp"
-#include "trajopt/geometry/Pose2.hpp"
 #include "trajopt/path/Path.hpp"
 #include "trajopt/path/PathBuilder.hpp"
 #include "trajopt/solution/SwerveSolution.hpp"
+#include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
 

trajoptlib/src/RustFFI.cpp

@@ -416,6 +416,32 @@ DifferentialTrajectory DifferentialPathBuilder::generate(bool diagnostics,
   }
 }
 
+/**
+ * Add a callback that will be called on each iteration of the solver.
+ *
+ * @param callback: a `fn` (not a closure) to be executed. The callback's
+ * first parameter will be a `trajopt::DifferentialTrajectory`, and the second
+ * parameter will be an `i64` equal to the handle passed in `generate()`
+ *
+ * This function can be called multiple times to add multiple callbacks.
+ */
+void DifferentialPathBuilder::add_progress_callback(
+    rust::Fn<void(DifferentialTrajectory, int64_t)> callback) {
+  path_builder.AddIntermediateCallback(
+      [=](trajopt::DifferentialSolution& solution, int64_t handle) {
+        trajopt::DifferentialTrajectory cppTrajectory{solution};
+
+        rust::Vec<DifferentialTrajectorySample> rustSamples;
+        for (const auto& cppSample : cppTrajectory.samples) {
+          rustSamples.push_back(DifferentialTrajectorySample{
+              cppSample.timestamp, cppSample.x, cppSample.y, cppSample.heading,
+              cppSample.velocityL, cppSample.velocityR});
+        }
+
+        callback(DifferentialTrajectory{rustSamples}, handle);
+      });
+}
+
 std::unique_ptr<DifferentialPathBuilder> differential_path_builder_new() {
   return std::make_unique<DifferentialPathBuilder>();
 }

trajoptlib/src/RustFFI.hpp

@@ -120,6 +120,9 @@ class DifferentialPathBuilder {
   DifferentialTrajectory generate(bool diagnostics = false,
                                   int64_t handle = 0) const;
 
+  void add_progress_callback(
+      rust::Fn<void(DifferentialTrajectory, int64_t)> callback);
+
  private:
   trajopt::DifferentialPathBuilder path_builder;
 };

trajoptlib/src/lib.rs

@@ -309,6 +309,11 @@ mod ffi {
             uuid: i64,
         ) -> Result<DifferentialTrajectory>;
 
+        fn add_progress_callback(
+            self: Pin<&mut DifferentialPathBuilder>,
+            callback: fn(DifferentialTrajectory, i64),
+        );
+
         fn differential_path_builder_new() -> UniquePtr<DifferentialPathBuilder>;
 
         fn cancel_all();
@@ -816,6 +821,23 @@ impl DifferentialPathBuilder {
             Err(msg) => Err(msg.what().to_string()),
         }
     }
+
+    ///
+    /// Add a callback that will be called on each iteration of the solver.
+    ///
+    /// * callback: a `fn` (not a closure) to be executed. The callback's first
+    ///       parameter will be a `trajopt::HolonomicTrajectory`, and the second
+    ///       parameter will be an `i64` equal to the handle passed in
+    ///       `generate()`
+    ///
+    /// This function can be called multiple times to add multiple callbacks.
+    ///
+    pub fn add_progress_callback(&mut self, callback: fn(DifferentialTrajectory, i64)) {
+        crate::ffi::DifferentialPathBuilder::add_progress_callback(
+            self.path_builder.pin_mut(),
+            callback,
+        );
+    }
 }
 
 impl Default for DifferentialPathBuilder {

trajoptlib/src/path/DifferentialPathBuilder.cpp

@@ -4,8 +4,6 @@
 
 #include <utility>
 
-#include "trajopt/drivetrain/DifferentialDrivetrain.hpp"
-#include "trajopt/path/Path.hpp"
 #include "trajopt/solution/DifferentialSolution.hpp"
 #include "trajopt/util/GenerateLinearInitialGuess.hpp"
 
@@ -20,4 +18,9 @@ DifferentialSolution DifferentialPathBuilder::CalculateInitialGuess() const {
       initialGuessPoints, controlIntervalCounts);
 }
 
+void DifferentialPathBuilder::AddIntermediateCallback(
+    const std::function<void(DifferentialSolution&, int64_t)> callback) {
+  path.callbacks.push_back(callback);
+}
+
 }  // namespace trajopt