Upgrade TrajoptLib and fix angular velocity constraints (SleipnirGrou…

…p#506)
spacey-sooty · Jun 26, 2024 · 46b256b · 46b256b
1 parent bc735c7
commit 46b256b
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 32 deletions.
diff --git a/src-tauri/Cargo.lock b/src-tauri/Cargo.lock
diff --git 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
@@ -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 {