From 46b256b8fa73b080014da7c1c7c8e34f6a54ac43 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 25 Jun 2024 22:04:26 -0700
Subject: [PATCH] Upgrade TrajoptLib and fix angular velocity constraints
 (#506)

---
 src-tauri/Cargo.lock  |  2 +-
 src-tauri/Cargo.toml  |  2 +-
 src-tauri/src/main.rs | 39 +++++++++------------------------------
 3 files changed, 11 insertions(+), 32 deletions(-)

diff --git a/src-tauri/Cargo.lock b/src-tauri/Cargo.lock
index c4030d07d1..34fda75070 100644
--- a/src-tauri/Cargo.lock
+++ b/src-tauri/Cargo.lock
@@ -3212,7 +3212,7 @@ dependencies = [
 [[package]]
 name = "trajoptlib"
 version = "0.1.0"
-source = "git+https://github.com/SleipnirGroup/TrajoptLib.git?rev=edf60f9a1bce4b78f6db8c7362ea28e485693851#edf60f9a1bce4b78f6db8c7362ea28e485693851"
+source = "git+https://github.com/SleipnirGroup/TrajoptLib.git?rev=86603543a263aac6d5f7e86cd53384a6d3c6bea2#86603543a263aac6d5f7e86cd53384a6d3c6bea2"
 dependencies = [
  "cmake",
  "cxx",
diff --git a/src-tauri/Cargo.toml b/src-tauri/Cargo.toml
index e12937fb8e..2aa160c57f 100644
--- a/src-tauri/Cargo.toml
+++ b/src-tauri/Cargo.toml
@@ -18,7 +18,7 @@ tauri-build = { version = "1.5.2", features = [] }
 tauri = { version = "1.6.6", features = [ "os-all", "window-close", "window-set-title", "path-all", "dialog", "dialog-confirm", "dialog-save", "dialog-open", "dialog-ask", "fs-all", "shell-open", "devtools"] }
 serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
-trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "edf60f9a1bce4b78f6db8c7362ea28e485693851" }
+trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "86603543a263aac6d5f7e86cd53384a6d3c6bea2" }
 open = "3"
 lazy_static = "1.4.0"
 
diff --git a/src-tauri/src/main.rs b/src-tauri/src/main.rs
index 7f9924676c..8306832556 100644
--- a/src-tauri/src/main.rs
+++ b/src-tauri/src/main.rs
@@ -341,7 +341,7 @@ async fn generate_trajectory(
             Constraints::StopPoint { scope } => {
                 if let ChoreoConstraintScope::Waypoint(idx) = scope {
                     path_builder.wpt_linear_velocity_max_magnitude(fix_scope(idx[0], &rm), 0.0f64);
-                    path_builder.wpt_angular_velocity(fix_scope(idx[0], &rm), 0.0);
+                    path_builder.wpt_angular_velocity_max_magnitude(fix_scope(idx[0], &rm), 0.0f64);
                 }
             }
             Constraints::MaxVelocity { scope, velocity } => match scope {
@@ -358,35 +358,14 @@ async fn generate_trajectory(
                 scope,
                 angular_velocity,
             } => match scope {
-                ChoreoConstraintScope::Waypoint(idx) => {
-                    // If the angular velocity max magnitude is zero, use an
-                    // angular velocity equality constraint instead
-                    if *angular_velocity == 0.0f64 {
-                        path_builder.wpt_angular_velocity(fix_scope(idx[0], &rm), 0.0f64)
-                    } else {
-                        path_builder.wpt_angular_velocity_max_magnitude(
-                            fix_scope(idx[0], &rm),
-                            *angular_velocity,
-                        )
-                    }
-                }
-                ChoreoConstraintScope::Segment(idx) => {
-                    // If the angular velocity max magnitude is zero, use an
-                    // angular velocity equality constraint instead
-                    if *angular_velocity == 0.0f64 {
-                        path_builder.sgmt_angular_velocity(
-                            fix_scope(idx[0], &rm),
-                            fix_scope(idx[1], &rm),
-                            0.0f64,
-                        )
-                    } else {
-                        path_builder.sgmt_angular_velocity_max_magnitude(
-                            fix_scope(idx[0], &rm),
-                            fix_scope(idx[1], &rm),
-                            *angular_velocity,
-                        )
-                    }
-                }
+                ChoreoConstraintScope::Waypoint(idx) => path_builder
+                    .wpt_angular_velocity_max_magnitude(fix_scope(idx[0], &rm), *angular_velocity),
+                ChoreoConstraintScope::Segment(idx) => path_builder
+                    .sgmt_angular_velocity_max_magnitude(
+                        fix_scope(idx[0], &rm),
+                        fix_scope(idx[1], &rm),
+                        *angular_velocity,
+                    ),
             },
             Constraints::StraightLine { scope } => {
                 if let ChoreoConstraintScope::Segment(idx) = scope {