Add PointAt constraint (#427)

src-tauri/Cargo.toml

@@ -18,7 +18,7 @@ tauri-build = { version = "1.5.1", features = [] }
 tauri = { version = "1.6.0", features = [ "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 = "4b238aa71a1d92a8e4e9913dfc25a6ee81d57cb8", features = ["sleipnir"] }
+trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "c1a764608604837a1601034cddca44e068099dbd", features = ["sleipnir"] }
 open = "3"
 
 [features]

src-tauri/src/main.rs

@@ -222,6 +222,12 @@ enum Constraints {
     StraightLine {
         scope: ChoreoConstraintScope,
     },
+    PointAt {
+        scope: ChoreoConstraintScope,
+        x: f64,
+        y: f64,
+        tolerance: f64,
+    },
 }
 
 #[derive(Debug, serde::Serialize, serde::Deserialize)]
@@ -372,7 +378,24 @@ async fn generate_trajectory(
                         )
                     }
                 }
-            } // add more cases here to impl each constraint.
+            }
+            Constraints::PointAt {
+                scope,
+                x,
+                y,
+                tolerance,
+            } => match scope {
+                ChoreoConstraintScope::Waypoint(idx) => {
+                    path_builder.wpt_point_at(fix_scope(idx[0], &rm), *x, *y, *tolerance)
+                }
+                ChoreoConstraintScope::Segment(idx) => path_builder.sgmt_point_at(
+                    fix_scope(idx[0], &rm),
+                    fix_scope(idx[1], &rm),
+                    *x,
+                    *y,
+                    *tolerance,
+                ),
+            }, // add more cases here to impl each constraint.
         }
     }
     let half_wheel_base = config.wheelbase / 2.0;

src/document/ConstraintStore.tsx

@@ -2,6 +2,7 @@ import {
   Dangerous,
   Explore,
   KeyboardDoubleArrowRight,
+  NearMe,
   PriorityHigh,
   StopCircleOutlined,
   SyncDisabledOutlined,
@@ -129,6 +130,32 @@ export const constraints = {
     properties: {},
     wptScope: false,
     sgmtScope: true
+  },
+  PointAt: {
+    name: "Point At",
+    shortName: "Point At",
+    description: "Face a specified point",
+    icon: <NearMe />,
+    properties: {
+      x: {
+        name: "X",
+        description: "The x coordinate of the point the robot should face",
+        units: "m"
+      },
+      y: {
+        name: "Y",
+        description: "The y coordinate of the point the robot should face",
+        units: "m"
+      },
+      tolerance: {
+        name: "Heading Tolerance",
+        description:
+          "The allowable heading range relative to the direction to the point. Keep less than Pi.",
+        units: "rad"
+      }
+    },
+    wptScope: true,
+    sgmtScope: true
   }
 };
 const WaypointUUIDScope = types.model("WaypointScope", {