Merge zero and max magnitude constraints (#464)

* Merge angular velocity zero and max magnitude constraints * Merge velocity zero and max magnitude constraints * Implement schema upgrade * Fix JS formatting
SleipnirGroup · Jun 18, 2024 · 089fd1d · 089fd1d
1 parent 295aabf
commit 089fd1d
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 51 deletions.
diff --git a/src-tauri/src/main.rs b/src-tauri/src/main.rs
@@ -210,9 +210,6 @@ enum Constraints {
         scope: ChoreoConstraintScope,
         direction: f64,
     },
-    WptZeroVelocity {
-        scope: ChoreoConstraintScope,
-    },
     StopPoint {
         scope: ChoreoConstraintScope,
     },
@@ -224,9 +221,6 @@ enum Constraints {
         scope: ChoreoConstraintScope,
         angular_velocity: f64,
     },
-    ZeroAngularVelocity {
-        scope: ChoreoConstraintScope,
-    },
     StraightLine {
         scope: ChoreoConstraintScope,
     },
@@ -344,11 +338,6 @@ async fn generate_trajectory(
                     path_builder.wpt_linear_velocity_direction(fix_scope(idx[0], &rm), *direction);
                 }
             }
-            Constraints::WptZeroVelocity { scope } => {
-                if let ChoreoConstraintScope::Waypoint(idx) = scope {
-                    path_builder.wpt_linear_velocity_max_magnitude(fix_scope(idx[0], &rm), 0.0f64);
-                }
-            }
             Constraints::StopPoint { scope } => {
                 if let ChoreoConstraintScope::Waypoint(idx) = scope {
                     path_builder.wpt_linear_velocity_max_magnitude(fix_scope(idx[0], &rm), 0.0f64);
@@ -369,24 +358,35 @@ async fn generate_trajectory(
                 scope,
                 angular_velocity,
             } => match scope {
-                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::ZeroAngularVelocity { scope } => match scope {
                 ChoreoConstraintScope::Waypoint(idx) => {
-                    path_builder.wpt_angular_velocity(fix_scope(idx[0], &rm), 0.0)
+                    // 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::Segment(idx) => path_builder.sgmt_angular_velocity(
-                    fix_scope(idx[0], &rm),
-                    fix_scope(idx[1], &rm),
-                    0.0,
-                ),
             },
             Constraints::StraightLine { scope } => {
                 if let ChoreoConstraintScope::Segment(idx) = scope {

diff --git a/src/document/ConstraintStore.tsx b/src/document/ConstraintStore.tsx
@@ -1,11 +1,9 @@
 import {
-  Dangerous,
   Explore,
   KeyboardDoubleArrowRight,
   NearMe,
   PriorityHigh,
   StopCircleOutlined,
-  SyncDisabledOutlined,
   SyncOutlined,
   Timeline
 } from "@mui/icons-material";
@@ -34,16 +32,12 @@ import { IHolonomicPathStore } from "./HolonomicPathStore";
       size_t fromIdx, const std::vector<InitialGuessPoint>& sgmtPoseGuess);
     void WptVelocityDirection(size_t idx, double angle);
     void WptVelocityMagnitude(size_t idx, double v);
-    void WptZeroVelocity(size_t idx);
     void WptVelocityPolar(size_t idx, double vr, double vtheta);
-    void WptZeroAngularVelocity(size_t idx);
     void SgmtVelocityDirection(size_t fromIdx, size_t toIdx, double angle,
                              bool includeWpts = true)
     // maximum
     void SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, double v,
                              bool includeWpts = true);
-     void SgmtZeroAngularVelocity(size_t fromIdx, size_t toIdx,
-                               bool includeWpts = true);
  */
 export type ConstraintPropertyDefinition = {
   name: string;
@@ -81,15 +75,6 @@ export const constraints = {
     wptScope: true,
     sgmtScope: false
   },
-  WptZeroVelocity: {
-    name: "Waypoint Zero Velocity",
-    shortName: "Wpt 0 Velo",
-    description: "Zero velocity at waypoint",
-    icon: <Dangerous></Dangerous>,
-    properties: {},
-    wptScope: true,
-    sgmtScope: false
-  },
   StopPoint: {
     name: "Stop Point",
     shortName: "Stop Point",
@@ -129,15 +114,6 @@ export const constraints = {
     wptScope: true,
     sgmtScope: true
   },
-  ZeroAngularVelocity: {
-    name: "Zero Angular Velocity",
-    shortName: "0 Ang Velo",
-    description: "Zero angular velocity throughout scope",
-    icon: <SyncDisabledOutlined></SyncDisabledOutlined>,
-    properties: {},
-    wptScope: true,
-    sgmtScope: true
-  },
   StraightLine: {
     name: "Straight Line",
     shortName: "Straight Line",

diff --git a/src/document/DocumentSpecTypes.ts b/src/document/DocumentSpecTypes.ts
@@ -291,6 +291,21 @@ export const VERSIONS = {
         }
       }
 
+      // Replace zero velocity and zero angular velocity constraints with max
+      // magnitude constraints
+      for (const entry of Object.keys(updated.paths)) {
+        const path = updated.paths[entry];
+        for (const constraint of path.constraints) {
+          if (constraint.type === "WptZeroVelocity") {
+            constraint.type = "MaxVelocity";
+            constraint.velocity = 0.0;
+          } else if (constraint.type === "ZeroAngularVelocity") {
+            constraint.type = "MaxAngularVelocity";
+            constraint.angular_velocity = 0.0;
+          }
+        }
+      }
+
       return updated;
     },
     schema: v0_3_1_Schema