diff --git a/docs/document-settings/robot-configuration.md b/docs/document-settings/robot-configuration.md index a2b7cb41dc..17737513ed 100644 --- a/docs/document-settings/robot-configuration.md +++ b/docs/document-settings/robot-configuration.md @@ -38,7 +38,7 @@ These values should be determined by consulting the motor's documentation. !!! tip "Choosing a Motor Max Speed" - A reasonable choice of Motor Max Speed is ~80% of the free speed of the drive motor(s). Although your motors have more speed available, this headroom helps ensure that your robot is able to close any errors and return to the planned trajectory. Use the [Motor Calculator](#motor-calculator) to help select an appropriate value. + A reasonable choice of Motor Max Speed is ~80% of the free speed of the drive motor(s). Although your motors have more speed available, this headroom helps ensure that your robot is able to close any errors and return to the planned trajectory. - **Motor Max Torque** $[N * m]$: The maximum torque applied by each drive motor diff --git a/docs/media/field_options.png b/docs/media/field_options.png index 919f4fe098..bca75b56c0 100644 Binary files a/docs/media/field_options.png and b/docs/media/field_options.png differ diff --git a/docs/media/focus.png b/docs/media/focus.png index fe0f2cedbf..eae2969f38 100644 Binary files a/docs/media/focus.png and b/docs/media/focus.png differ diff --git a/docs/media/project_info.png b/docs/media/project_info.png index 52ab63cf4c..7e426d9101 100644 Binary files a/docs/media/project_info.png and b/docs/media/project_info.png differ diff --git a/docs/media/project_not_saved.png b/docs/media/project_not_saved.png index 7fc4e6726e..c4eaaac79c 100644 Binary files a/docs/media/project_not_saved.png and b/docs/media/project_not_saved.png differ diff --git a/docs/media/samples-mini-waypoints.png b/docs/media/samples-mini-waypoints.png new file mode 100644 index 0000000000..e493a199d2 Binary files /dev/null and b/docs/media/samples-mini-waypoints.png differ diff --git a/docs/media/samples.png b/docs/media/samples.png index fba8f0bd30..1e6a4745da 100644 Binary files a/docs/media/samples.png and b/docs/media/samples.png differ diff --git a/docs/media/sidebar.png b/docs/media/sidebar.png index 1e4769e8c6..300dd1ab3a 100644 Binary files a/docs/media/sidebar.png and b/docs/media/sidebar.png differ diff --git a/docs/media/view_options_panel.png b/docs/media/view_options_panel.png index 88bc90fe17..d56e855847 100644 Binary files a/docs/media/view_options_panel.png and b/docs/media/view_options_panel.png differ diff --git a/docs/media/waypoints.png b/docs/media/waypoints.png index 7b74fd4825..dd428df12c 100644 Binary files a/docs/media/waypoints.png and b/docs/media/waypoints.png differ diff --git a/docs/usage/saving.md b/docs/usage/saving.md index 9f850e0bfe..c831949b14 100644 --- a/docs/usage/saving.md +++ b/docs/usage/saving.md @@ -3,7 +3,7 @@ Once trajectories are created by Choreo, they must be saved within your robot project. Choreo has 2 different kinds of files: a .chor file which stores general configs for your project, -and multiple .traj files which store individual trajectories. Choreo generates and updates .traj files +and multiple .traj files which store individual path information. Each .traj file correlates to one path in Choreo. Choreo generates and updates .traj files in the same directory (or folder) that your .chor file is stored in. ## Saving your .chor file @@ -33,29 +33,23 @@ To access the sidebar, click the hamburger icon in the upper left corner. General Info: +In the sidebar, there are four actions related to saving your project, along with your project's current save location. -In the sidebar, there are five actions related to saving your project, along with your project's current save location. +## Open Project -## Open File +This opens the system’s file select dialog to select the robot’s `.chor` file. -This opens the system’s file select dialog to select the robot’s `.chor` file. This .chor file should be under the folders `src/main/deploy/choreo` -in your robot project. +## Save Project/Save Project As -## Save File +This opens the system’s file save dialog to select where to save the robot’s `.chor` file. -This opens the system’s file save dialog to select where to save the robot’s `.chor` file. Choreo strongly recommends saving this file in your `deploy/choreo` folder within your project. +## New Project -## New File +Creates a new unsaved project. -Creates a new file in memory, essentially clearing all trajectories. If you have unsaved changes, Choreo will ask before clearing them. +## Export Diagnostic Report -## Export Trajectory - -Exports the trajectory as a file individually to the path you select in the UI. This is not tied with the robot project structure, so you can place the trajectory file anywhere you like. - -## Save All Trajectories - -This saves all the trajectories into the folder structure described below. +This exports a .zip file with your project `.chor` and `.traj` files, as well as some logs. Choreo support may ask for this report when trying to help you. ## Project Details @@ -63,11 +57,11 @@ If you have saved your choreo file correctly, you should see the following: ![Project Info](../media/project_info.png) -Below "Project saved at", you can see the directory in which the saved `.chor` file lives. You can also copy this path or open it in your system's file explorer. +Below "Project saved at", you can see the directory in which the project files live. You can also copy this path or open it in your system's file explorer. !!! warning - If you get the below, it means you have not saved the file yet + If you get the below, it means you have not saved the file yet. ![Project not saved](../media/project_not_saved.png){: style="height:60px;"} @@ -75,8 +69,8 @@ Below "Project saved at", you can see the directory in which the saved `.chor` f For C++ and Java teams: -- Choreo file lives at: `~/Development/FRC/Roboto/src/main/deploy/choreo/ChoreoProject.chor` -- Trajectories (`.traj`) live in: `~/Development/FRC/Roboto/src/main/deploy/choreo/...` +- Choreo file lives at: `~/Development/FRC/Robot/src/main/deploy/choreo/ChoreoProject.chor` +- Trajectories (`.traj`) live in: `~/Development/FRC/Robot/src/main/deploy/choreo/...` For Python teams: diff --git a/docs/usage/view-options-panel.md b/docs/usage/view-options-panel.md index b06fed2996..a8adfe9ae7 100644 --- a/docs/usage/view-options-panel.md +++ b/docs/usage/view-options-panel.md @@ -52,11 +52,11 @@ Split trajectories on stop points are shown in different colors. In other words, ### Field -Shows the field svg overlay. +Shows the field background layer. ### Grid -Shows the grid overlay +Shows a 1 meter grid layer. ### Trajectory @@ -65,14 +65,15 @@ Shows the grid overlay ### Samples ![samples](../media/samples.png) +![Mini waypoints](../media/samples-mini-waypoints.png) + +Shows a dot at every sample along the path, and mini waypoints at the locations of the waypoints that generated the trajectory. ### Waypoints ![waypoints](../media/waypoints.png) -### Obstacles - -Shows the obstacles in the path. +Shows or hides the editable waypoints. ### Focus diff --git a/src-core/src/file_management/mod.rs b/src-core/src/file_management/mod.rs index 99a6a817fe..ba67b26caf 100644 --- a/src-core/src/file_management/mod.rs +++ b/src-core/src/file_management/mod.rs @@ -26,7 +26,8 @@ type DeployPath = Arc>; type TrajectoryFileName = String; mod diagnostics; -mod formatter; + +pub mod formatter; pub mod upgrader; pub use diagnostics::{create_diagnostic_file, get_log_lines}; diff --git a/src-core/src/spec/trajectory.rs b/src-core/src/spec/trajectory.rs index e7a5ce3d50..8e1797988c 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -3,7 +3,7 @@ use trajoptlib::{DifferentialTrajectorySample, SwerveTrajectorySample}; use super::{upgraders::upgrade_traj_file, Expr, SnapshottableType}; -#[derive(Serialize, Deserialize, Clone, Copy, Debug)] +#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq)] #[serde(rename_all = "camelCase")] /// A waypoint parameter. pub struct Waypoint { @@ -53,7 +53,7 @@ impl Waypoint { } /// A waypoint identifier. -#[derive(Serialize, Deserialize, Clone, Copy, Debug)] +#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq, Eq)] #[serde(rename_all = "camelCase")] pub enum WaypointID { /// The first waypoint. @@ -106,7 +106,7 @@ pub enum ConstraintScope { } /// A constraint on the robot's motion. -#[derive(Serialize, Deserialize, Clone, Copy, Debug)] +#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq)] #[serde(tag = "type", content = "props")] pub enum ConstraintData { /// A constraint on the maximum velocity. @@ -211,7 +211,7 @@ impl ConstraintData { } /// A constraint on the robot's motion and where it applies. -#[derive(Debug, Clone, Copy, Serialize, Deserialize)] +#[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq)] pub struct Constraint { /// The waypoint the constraint starts at. pub from: WaypointID, @@ -237,7 +237,7 @@ impl Constraint { } /// A constraint on the robot's motion and where it applies. -#[derive(Debug, Clone, Copy, Serialize, Deserialize)] +#[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq)] pub struct ConstraintIDX { /// The index of the waypoint the constraint starts at. pub from: usize, @@ -252,7 +252,7 @@ pub struct ConstraintIDX { /// A sample of the robot's state at a point in time during the trajectory. #[allow(missing_docs)] -#[derive(Debug, Clone, Copy, Serialize, Deserialize)] +#[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq)] #[serde(untagged)] pub enum Sample { /// A sample for a swerve drive. @@ -363,7 +363,7 @@ pub enum DriveType { } /// The parameters used for generating a trajectory. -#[derive(Serialize, Deserialize, Clone, Debug)] +#[derive(Serialize, Deserialize, Clone, Debug, PartialEq)] #[serde(rename_all = "camelCase")] pub struct Parameters { /// The waypoints the robot will pass through or use for initial guess. @@ -441,6 +441,15 @@ impl TrajectoryFile { let val = upgrade_traj_file(serde_json::from_str(content)?)?; serde_json::from_value(val).map_err(Into::into) } + + pub fn up_to_date(&self) -> bool { + // Can't use is_some_and due to its move semantics. + if let Some(snap) = &self.snapshot { + snap == &self.params.snapshot() + } else { + false + } + } } #[derive(Debug, Clone, Serialize, Deserialize)] @@ -482,3 +491,54 @@ pub enum PplibCommand { commands: Vec, }, } + +#[cfg(test)] +mod tests { + use crate::spec::TRAJ_SCHEMA_VERSION; + + use super::*; + fn test_trajectory() -> TrajectoryFile { + let parameters = Parameters:: { + waypoints: vec![Waypoint:: { + x: Expr::fill_in_value(0.0, "m"), + y: Expr::fill_in_value(0.0, "m"), + heading: Expr::fill_in_value(0.0, "rad"), + intervals: 20, + split: false, + fix_translation: false, + fix_heading: false, + override_intervals: false, + is_initial_guess: false, + }], + constraints: vec![], + target_dt: Expr::fill_in_value(0.05, "s"), + }; + TrajectoryFile { + name: "Test".to_string(), + version: TRAJ_SCHEMA_VERSION, + snapshot: Some(parameters.snapshot()), + params: parameters, + trajectory: Trajectory { + sample_type: Some(DriveType::Swerve), + waypoints: Vec::new(), + samples: Vec::new(), + splits: Vec::new(), + }, + events: Vec::new(), + } + } + #[test] + fn snapshot_equality() { + assert!(test_trajectory().up_to_date()); + } + + #[test] + fn snapshot_equality_through_serde() -> serde_json::Result<()> { + use crate::file_management::formatter; + let trajectory = test_trajectory(); + let serde_trajectory = formatter::to_string_pretty(&trajectory)?; + let deser_trajectory = TrajectoryFile::from_content(serde_trajectory.as_str()); + assert!(deser_trajectory.is_ok_and(|t| t.up_to_date())); + Ok(()) + } +} diff --git a/src-tauri/src/api.rs b/src-tauri/src/api.rs index 7a20823e53..39424311b7 100644 --- a/src-tauri/src/api.rs +++ b/src-tauri/src/api.rs @@ -150,6 +150,11 @@ pub async fn delete_trajectory( debug_result!(file_management::delete_trajectory_file(&resources, trajectory).await); } +#[tauri::command] +pub async fn trajectory_up_to_date(trajectory: TrajectoryFile) -> bool { + trajectory.up_to_date() +} + #[tauri::command] pub async fn set_deploy_root(app_handle: tauri::AppHandle, dir: String) { let resources = app_handle.state::(); diff --git a/src-tauri/src/tauri.rs b/src-tauri/src/tauri.rs index a5b9441a69..517ce89b67 100644 --- a/src-tauri/src/tauri.rs +++ b/src-tauri/src/tauri.rs @@ -207,6 +207,7 @@ pub fn run_tauri(project: Option) { open_project_dialog, read_trajectory, rename_trajectory, + trajectory_up_to_date, set_deploy_root, get_deploy_root, requested_file, diff --git a/src/components/config/robotconfig/PathGradient.tsx b/src/components/config/robotconfig/PathGradient.tsx index 9df7e380a8..992d31f8f4 100644 --- a/src/components/config/robotconfig/PathGradient.tsx +++ b/src/components/config/robotconfig/PathGradient.tsx @@ -8,14 +8,24 @@ import { IDocumentStore } from "../../../document/DocumentModel"; * Represents a path gradient. */ export type PathGradientArgs = { - point: S; - prev: S; - next: S; - arr: S[]; - total: number; - count: number; - sect: number; - idxInSect: number; + /** + * Array of all trajectory samples. + */ + samples: S[]; + + /** + * Index of current trajectory sample within sample array. + */ + index: number; + + /** + * Section number, where a section is a region between stop points. + */ + section: number; + + /** + * Document model. + */ documentModel: IDocumentStore; }; export type PathGradient = { @@ -26,17 +36,16 @@ export type PathGradient = { name: string; localizedDescription: string; + /** * The localized/user-facing description of the path gradient. */ description: string; + /** - * A function that calculates the gradient value for a given point in a saved trajectory sample. + * A function that calculates the gradient value for a given point based on a + * trajectory sample. * - * @param point - The saved trajectory sample point. - * @param i - The index of the point in the array. - * @param arr - The array of saved trajectory samples. - * @param documentModel - The document model. * @returns The gradient value as a string. */ function: (args: PathGradientArgs) => string; @@ -47,159 +56,130 @@ export type PathGradient = { */ class PathGradientFunctions { /** - * Returns the "select-yellow" color for the given point in the trajectory. + * Returns the "select-yellow" color for the given trajectory sample. + * * This is the default color used when no gradient is applied. * NOTE: This function is not used and is included only for completeness. * - * @param point - The current point in the trajectory. - * @param i - The index of the current point in the trajectory. - * @param arr - The array of all points in the trajectory. - * @param documentModel - The document model object. * @returns The "select-yellow" color. */ - static none(args: PathGradientArgs): string { + static none(_args: PathGradientArgs): string { return "var(--select-yellow)"; } /** - * Calculates the color gradient for a given point on a saved trajectory based on its velocity. - * Faster robot velocity is shown as green. + * Produces a color gradient for linear velocity. + * + * Lower velocities are red and higher velocities are green. * - * @param point - The point on the saved trajectory. - * @param i - The index of the point in the array. - * @param arr - The array of saved trajectory samples. - * @param documentModel - The document model. - * @returns The color gradient in HSL format. + * @returns The color value in HSL format. */ - static velocity({ point, documentModel }: PathGradientArgs): string { - // calculates the magnitude of the velocity vector, then divides it by the theoretical floor speed - // then it scales the ratio [0, 1]: red to green[0, 100] + static linearVelocity({ + samples, + index, + documentModel + }: PathGradientArgs): string { + const sample = samples[index]; + + // Linear velocity magnitude + let v = 0; + if (sample.vl !== undefined) { + v = Math.abs(sample.vl + sample.vr) / 2; + } else { + v = Math.hypot(sample.vx, sample.vy); + } + const floorSpeed = documentModel.robotConfig.wheelMaxVelocity * documentModel.robotConfig.radius.value; - const t = Math.hypot(point.vx, point.vy) / floorSpeed; - return `hsl(${100 * t}, 100%, 50%)`; + + // Divide by floor speed to scale linear velocity to [0, 1], then scale to + // red-green hue [0, 100] + return `hsl(${100 * (v / floorSpeed)}, 100%, 50%)`; } /** - * Calculates the progress color for a given point in a trajectory. - * The color is determined based on the index of the point in the trajectory array. - * Faster robot velocity is shown as green. + * Produces a color gradient for trajectory progress. * - * @param point - The current point in the trajectory. - * @param i - The index of the current point in the array. - * @param arr - The array of trajectory points. - * @param documentModel - The document model. - * @returns The progress color in HSL format. + * 0% progress is green and 100% progress is red. + * + * @returns The color value in HSL format. */ - static progress({ count, total }: PathGradientArgs): string { - // this creates a ratio [0, 1] of the current point against the total points - // then scales it from red to greeen, [0, 100] - const t = 1 - count / total; - return `hsl(${100 * t}, 100%, 50%)`; + static progress({ samples, index }: PathGradientArgs): string { + // Scale progress to [0, 1], invert the range, then scale to red-green hue + // [0, 100] + return `hsl(${100 * (1 - index / samples.length)}, 100%, 50%)`; } /** - * Calculates the color gradient for the acceleration of a trajectory point. - * Faster robot acceleration is shown as green. + * Produces a color gradient for linear acceleration. * - * @param point - The trajectory point. - * @param i - The index of the trajectory point in the array. - * @param arr - The array of trajectory points. - * @param documentModel - The document model. - * @returns The color gradient for the acceleration. + * Lower accelerations are red and higher accelerations are green. + * + * @returns The color value in HSL format. */ - static acceleration({ - point, - next, - count, - total - }: PathGradientArgs): string { - let t = 0; - if (point.vx) { - if (count != 0 && count != total - 1) { - // first calculates the magnitude of the change in velocity vector over change in time - // between the current point and the next point. - // then, it is scaled/normalized for the HSL color value. - const A = point; - const B = next; - t = Math.hypot(B.vx - A.vx, B.vy - A.vy); - const dt = B.t - A.t; - t /= dt * 10; - } - - return `hsl(${100 * (1 - t)}, 100%, 50%)`; + static linearAcceleration({ samples, index }: PathGradientArgs): string { + const sample = samples[index]; + + // Linear acceleration magnitude + let acceleration = 0; + if (sample.vl !== undefined) { + acceleration = Math.abs(sample.al + sample.ar) / 2; } else { - return "var(--select-yellow)"; + acceleration = Math.hypot(sample.ax, sample.ay); } + + // Divide by 10 to scale linear acceleration to [0, 1], invert range, then + // scale to red-green hue [0, 100] + return `hsl(${100 * (1 - acceleration / 10)}, 100%, 50%)`; } /** - * Computes the intervalDt value for a given point in a trajectory. - * Shorter time difference between intervals is shown as green. + * Produces a color gradient for trajectory sample interval time difference + * (dt). + * + * Shorter dts are red and longer dts are green. * - * @param point - The current point in the trajectory. - * @param i - The index of the current point in the array. - * @param arr - The array of trajectory points. - * @param documentModel - The document model. - * @returns The computed intervalDt value. + * @returns The color value in HSL format. */ - static intervalDt({ - point, - next, - count, - total - }: PathGradientArgs): string { - let t = 0; - if (count == 0 || count == total - 1) { - t = 0; - } else { - const A = point; - const B = next; - const dt = B.t - A.t; - t = 1.5 - 10 * dt; - } - return `hsl(${100 * t}, 100%, 50%)`; + static intervalDt({ samples, index }: PathGradientArgs): string { + const dt = samples[index + 1].t - samples[index].t; + return `hsl(${100 * (1.5 - 10 * dt)}, 100%, 50%)`; } /** - * Calculates the color value for the angular velocity of a trajectory point. - * Faster robot angular velocity is shown as green. + * Produces a color gradient for angular velocity. + * + * Lower angular velocities are red and higher angular velocities are green. * - * @param point - The trajectory point. - * @param i - The index of the trajectory point in the array. - * @param arr - The array of trajectory points. - * @param documentModel - The document model. * @returns The color value in HSL format. */ - static angularVelocity({ point }: PathGradientArgs): string { - // the color value is normalized from red (0) to green (100) - // based on an artificial angular velocity max of 2 r/s - return `hsl(${Math.abs((point.omega ?? 0) * 100) / 2}, 100%, 50%)`; + static angularVelocity({ samples, index }: PathGradientArgs): string { + // Scale angular velocity magnitude to [0, 1] using artificial 2π rad/s max, + // then normalize to red-green hue [0, 100] + return `hsl(${100 * (Math.abs(samples[index].omega) / (2 * Math.PI))}, 100%, 50%)`; } /** - * Returns a different HSL color for each split trajectory by stop point + * Produces a different color for each split trajectory delimited by stop + * points. * - * @param point - Each point. - * @param i - The index of the point in the array. - * @param arr - The array of saved trajectory samples. - * @param documentModel - The document model. - * @returns The color gradient in HSL format. + * @returns The color value in HSL format. */ - static splitTrajectories({ arr, sect: i }: PathGradientArgs): string { - if (i == 0) { + static splitTrajectories({ section }: PathGradientArgs): string { + if (section == 0) { return "var(--select-yellow)"; } - // an absolute value sine function is used to generate a distinct color between [0, 1] - // then a scalar is used to scale the color between the full color range [0, 360] - return `hsl(${Math.abs(Math.sin(i) * 360)}, 100%, 50%)`; + // Generate distinct color within [0, 1] with abs(sin(x)), then scale to + // full hue range [0, 360] + return `hsl(${360 * Math.abs(Math.sin(section))}, 100%, 50%)`; } } /** * Represents the available path gradients. + * * This links a gradient's user-facing description to its corresponding function.' */ export const PathGradients: Record = { @@ -209,11 +189,11 @@ export const PathGradients: Record = { description: "No path gradient applied.", function: PathGradientFunctions.none }, - Velocity: { - name: "Velocity", - localizedDescription: "Velocity", - description: "Faster robot velocity is shown as green.", - function: PathGradientFunctions.velocity + LinearVelocity: { + name: "LinearVelocity", + localizedDescription: "Linear Velocity", + description: "Faster robot linear velocity is shown as green.", + function: PathGradientFunctions.linearVelocity }, Progress: { name: "Progress", @@ -221,11 +201,11 @@ export const PathGradients: Record = { description: "Further progress through the path is shown as red.", function: PathGradientFunctions.progress }, - Acceleration: { - name: "Acceleration", - localizedDescription: "Acceleration", - description: "Faster robot acceleration is shown as green.", - function: PathGradientFunctions.acceleration + LinearAcceleration: { + name: "LinearAcceleration", + localizedDescription: "Linear Acceleration", + description: "Faster robot linear acceleration is shown as green.", + function: PathGradientFunctions.linearAcceleration }, IntervalDt: { name: "IntervalDt", diff --git a/src/components/field/svg/FieldGeneratedLines.tsx b/src/components/field/svg/FieldGeneratedLines.tsx index 495676c6d3..691e6cddf0 100644 --- a/src/components/field/svg/FieldGeneratedLines.tsx +++ b/src/components/field/svg/FieldGeneratedLines.tsx @@ -15,8 +15,8 @@ function FieldGeneratedLines() { // preserve the access of generationIterationNumber // to trigger rerenders when mutating the in-progress trajectory in place const _ = path.ui.generationIterationNumber; - trajectory.forEach((point) => { - generatedPathString += `${point.x},${point.y} `; + trajectory.forEach((sample) => { + generatedPathString += `${sample.x},${sample.y} `; }); const key = uiState.selectedPathGradient as keyof typeof PathGradients; const pathGradient = PathGradients[key]; @@ -38,35 +38,30 @@ function FieldGeneratedLines() { <> {trajectory.length > 1 && - trajectory.map((point, i, arr) => { - if (i == arr.length - 1) { + trajectory.map((sample, i, samples) => { + if (i == samples.length - 1) { return undefined; } - const point2 = arr[i + 1]; - const [sect, indexInSect] = path.trajectory.getIdxOfFullTrajectory( - i - ) ?? [0, 0]; + const nextSample = samples[i + 1]; + const args: PathGradientArgs = { - point: point, - prev: arr[i - 1], - next: arr[i + 1], - arr: path.ui.generating + samples: path.ui.generating ? path.ui.generationProgress : path.trajectory.samples, - total: arr.length, - count: i, - sect: path.ui.generating ? 0 : sect, - idxInSect: path.ui.generating ? 0 : indexInSect, + index: i, + section: path.ui.generating + ? 0 + : (path.trajectory.getIdxOfFullTrajectory(i) ?? [0, 0])[0], documentModel: doc }; // 0 t = red, 1 t = green return ( diff --git a/src/components/sidebar/PathSelector.tsx b/src/components/sidebar/PathSelector.tsx index 2384892cd8..e066d9d65e 100644 --- a/src/components/sidebar/PathSelector.tsx +++ b/src/components/sidebar/PathSelector.tsx @@ -1,4 +1,9 @@ -import { KeyboardArrowDown, Route, Settings } from "@mui/icons-material"; +import { + KeyboardArrowDown, + Route, + Settings, + ShapeLine +} from "@mui/icons-material"; import DeleteIcon from "@mui/icons-material/Delete"; import { CircularProgress, @@ -83,6 +88,7 @@ class PathSelectorOption extends Component { this.searchForName(""); const selected = this.props.uuid == doc.pathlist.activePathUUID; const name = this.getPath().name; + const upToDate = this.getPath().ui.upToDate; if (name != this.state.name && !this.state.renaming) { this.state.name = name; } @@ -105,13 +111,29 @@ class PathSelectorOption extends Component { }} variant="indeterminate" > - ) : ( + ) : upToDate ? ( + ) : ( + { + e.preventDefault(); + e.stopPropagation(); + doc.generatePath(this.getPath().uuid); + }} + > + + )} { doc.pathlist.addPath(trajectory.name, true, trajectory); }); diff --git a/src/document/DocumentModel.tsx b/src/document/DocumentModel.tsx index a3ad0e0aef..da56fa17d8 100644 --- a/src/document/DocumentModel.tsx +++ b/src/document/DocumentModel.tsx @@ -132,7 +132,6 @@ export const DocumentStore = types self.name = ser.name; self.variables.deserialize(ser.variables); self.robotConfig.deserialize(ser.config); - self.pathlist.paths.clear(); self.type = ser.type; }, setName(name: string) { diff --git a/src/document/PathListStore.ts b/src/document/PathListStore.ts index 54966cfae8..3fdc22654d 100644 --- a/src/document/PathListStore.ts +++ b/src/document/PathListStore.ts @@ -179,6 +179,10 @@ export const PathListStore = types })) .actions((self) => { return { + deleteAll() { + self.activePathUUID = self.defaultPath!.uuid; + self.paths.clear(); + }, deletePath(uuid: string) { if (self.paths.size === 1) { self.setActivePathUUID(self.defaultPath!.uuid); diff --git a/src/document/path/HolonomicPathStore.ts b/src/document/path/HolonomicPathStore.ts index 82317093b9..15ab1ae7dd 100644 --- a/src/document/path/HolonomicPathStore.ts +++ b/src/document/path/HolonomicPathStore.ts @@ -26,6 +26,7 @@ import { ChoreoPathStore } from "./ChoreoPathStore"; import { ChoreoTrajectoryStore } from "./ChoreoTrajectoryStore"; import { PathUIStore } from "./PathUIStore"; import { findUUIDIndex } from "./utils"; +import { Commands } from "../tauriCommands"; export function waypointIDToText( id: WaypointUUID | undefined, points: IHolonomicWaypointStore[] @@ -60,7 +61,6 @@ export const HolonomicPathStore = types name: "", uuid: types.identifier }) - .views((self) => { return { canGenerate(): boolean { @@ -162,6 +162,7 @@ export const HolonomicPathStore = types } }); self.setSnapshot(ser.snapshot); + self.ui.setUpToDate(true); }, deserialize(ser: Trajectory) { self.name = ser.name; @@ -191,7 +192,10 @@ export const HolonomicPathStore = types () => { return self.serialize; }, - (_value) => { + (ser) => { + Commands.trajectoryUpToDate(ser).then((upToDate) => + self.ui.setUpToDate(upToDate) + ); exporter(self.uuid); } ); diff --git a/src/document/path/PathUIStore.ts b/src/document/path/PathUIStore.ts index 4597f92c94..30edf67c6d 100644 --- a/src/document/path/PathUIStore.ts +++ b/src/document/path/PathUIStore.ts @@ -10,9 +10,15 @@ export const PathUIStore = types Array | Array >([]), generating: false, - generationIterationNumber: 0 + generationIterationNumber: 0, + upToDate: false }) .actions((self) => ({ + setUpToDate(upToDate: boolean) { + getEnv(self).withoutUndo(() => { + self.upToDate = upToDate; + }); + }, setVisibleWaypointsStart(start: number) { if (start <= self.visibleWaypointsEnd) { self.visibleWaypointsStart = start; diff --git a/src/document/tauriCommands.ts b/src/document/tauriCommands.ts index e71cfda41e..4bda1bb491 100644 --- a/src/document/tauriCommands.ts +++ b/src/document/tauriCommands.ts @@ -122,6 +122,13 @@ export const Commands = { deleteTrajectory: (trajectory: Trajectory) => invoke("delete_trajectory", { trajectory }), + /** + * Returns if the `Trajectory` parameters and snapshot are equivalent. + * @param trajectory The `Trajectory` to check + * @returns true if the parameters and snapshots are equivalent, false if not. + */ + trajectoryUpToDate: (trajectory: Trajectory) => + invoke("trajectory_up_to_date", { trajectory }), /** * If the application was opened via CLI and a file was specified, this will return the path of that file. * diff --git a/trajoptlib/CMakeLists.txt b/trajoptlib/CMakeLists.txt index e29de41af7..aa45d76675 100644 --- a/trajoptlib/CMakeLists.txt +++ b/trajoptlib/CMakeLists.txt @@ -102,9 +102,9 @@ set(BUILD_EXAMPLES OFF) fetchcontent_declare( Sleipnir - GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir.git - # main on 2024-09-18 - GIT_TAG 8bbce85252bc351c5aacb0de9f50fa31b8b9e1ae + GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir + # main on 2025-01-02 + GIT_TAG 5af8519f268a8075e245bb7cd411a81e1598f521 PATCH_COMMAND git apply ${CMAKE_CURRENT_SOURCE_DIR}/cmake/0001-Downgrade-to-C-20.patch UPDATE_DISCONNECTED 1 diff --git a/trajoptlib/cmake/0001-Downgrade-to-C-20.patch b/trajoptlib/cmake/0001-Downgrade-to-C-20.patch index dfe65fab7c..81a57ddc0e 100644 --- a/trajoptlib/cmake/0001-Downgrade-to-C-20.patch +++ b/trajoptlib/cmake/0001-Downgrade-to-C-20.patch @@ -35,7 +35,7 @@ index e9668b53d6a0317cf2367b212f5013cbb2aa7ad5..1db191b6492a5f10b657c42c7959a73e +fetchcontent_declare( + fmt + GIT_REPOSITORY https://github.com/fmtlib/fmt.git -+ GIT_TAG 11.0.2 ++ GIT_TAG 11.1.1 +) +fetchcontent_makeavailable(fmt) + @@ -48,8 +48,8 @@ diff --git a/cmake/modules/CompilerFlags.cmake b/cmake/modules/CompilerFlags.cma index 9f3fad1ce359897d6e87d5c37efae51d55bcf160..4331baf9394a7f27bde51906c9dd14b8543e4af4 100644 --- a/cmake/modules/CompilerFlags.cmake +++ b/cmake/modules/CompilerFlags.cmake -@@ -11,7 +11,7 @@ macro(compiler_flags target) - target_compile_options(${target} PRIVATE /wd4244 /wd4251 /WX) +@@ -22,7 +22,7 @@ macro(compiler_flags target) + ) endif() - target_compile_features(${target} PUBLIC cxx_std_23) diff --git a/trajoptlib/cmake/modules/CompilerFlags.cmake b/trajoptlib/cmake/modules/CompilerFlags.cmake index a248854d33..f148763433 100644 --- a/trajoptlib/cmake/modules/CompilerFlags.cmake +++ b/trajoptlib/cmake/modules/CompilerFlags.cmake @@ -11,6 +11,17 @@ macro(compiler_flags target) target_compile_options(${target} PRIVATE /wd4244 /wd4251 /WX) endif() + # Disable warning false positives in Eigen + if( + ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" + AND ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "12" + ) + target_compile_options( + ${target} + PRIVATE -Wno-array-bounds -Wno-stringop-overflow + ) + endif() + target_compile_features(${target} PUBLIC cxx_std_20) if(MSVC) target_compile_options(${target} PUBLIC /MP /Zf /utf-8 /bigobj) diff --git a/trajoptlib/include/trajopt/util/TrajoptUtil.hpp b/trajoptlib/include/trajopt/util/TrajoptUtil.hpp index ca30b1c209..990b9acf4f 100644 --- a/trajoptlib/include/trajopt/util/TrajoptUtil.hpp +++ b/trajoptlib/include/trajopt/util/TrajoptUtil.hpp @@ -52,15 +52,16 @@ inline std::vector Linspace(double start, double end, * @param minimumInput The minimum value expected from the input. * @param maximumInput The maximum value expected from the input. */ -constexpr double InputModulus(double input, double max, double min) { - double modulus = max - min; +constexpr double InputModulus(double input, double minimumInput, + double maximumInput) { + double modulus = maximumInput - minimumInput; // Wrap input if it's above the maximum input - int numMax = (input - min) / modulus; + int numMax = (input - minimumInput) / modulus; input -= numMax * modulus; // Wrap input if it's below the minimum input - int numMin = (input - max) / modulus; + int numMin = (input - maximumInput) / modulus; input -= numMin * modulus; return input; @@ -72,7 +73,7 @@ constexpr double InputModulus(double input, double max, double min) { * @param angle Angle to wrap in radians. */ constexpr double AngleModulus(double angle) { - return InputModulus(angle, std::numbers::pi, -std::numbers::pi); + return InputModulus(angle, -std::numbers::pi, std::numbers::pi); } /** diff --git a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp index 5ee8d1d7f0..cfb57845c5 100644 --- a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp +++ b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp @@ -168,7 +168,7 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator( const auto dist = std::hypot(dx, dy); const auto θ_0 = initialGuess.heading.at(sgmt_start); const auto θ_1 = initialGuess.heading.at(sgmt_end); - const auto dθ = std::abs(AngleModulus(θ_0 - θ_1)); + const auto dθ = std::abs(AngleModulus(θ_1 - θ_0)); auto maxLinearVel = maxDrivetrainVelocity; @@ -204,18 +204,21 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator( {θ.at(index + 1)}, {vl.at(index + 1)}, {vr.at(index + 1)}}; + slp::VariableMatrix u_k_1{{Fl.at(index + 1)}, {Fr.at(index + 1)}}; - // Dynamics constraints - RK4 - slp::VariableMatrix k1 = f(x_k, u_k); - slp::VariableMatrix k2 = f(x_k + dt * 0.5 * k1, u_k); - slp::VariableMatrix k3 = f(x_k + dt * 0.5 * k2, u_k); - slp::VariableMatrix k4 = f(x_k + dt * k3, u_k); - problem.SubjectTo(x_k_1 == - x_k + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4)); - - auto dx_dt = k1; - problem.SubjectTo(al.at(index) == dx_dt(3)); - problem.SubjectTo(ar.at(index) == dx_dt(4)); + // Dynamics constraints - direct collocation + // (https://mec560sbu.github.io/2016/09/30/direct_collocation/) + auto xdot_k = f(x_k, u_k); + auto xdot_k_1 = f(x_k_1, u_k_1); + auto xdot_c = -3 / (2 * dt) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1); + + auto x_c = 0.5 * (x_k + x_k_1) + dt / 8 * (xdot_k - xdot_k_1); + auto u_c = 0.5 * (u_k + u_k_1); + + problem.SubjectTo(xdot_c == f(x_c, u_c)); + + problem.SubjectTo(al.at(index) == xdot_k(3)); + problem.SubjectTo(ar.at(index) == xdot_k(4)); } } diff --git a/trajoptlib/src/SwerveTrajectoryGenerator.cpp b/trajoptlib/src/SwerveTrajectoryGenerator.cpp index 47175de51c..b19c7f6f80 100644 --- a/trajoptlib/src/SwerveTrajectoryGenerator.cpp +++ b/trajoptlib/src/SwerveTrajectoryGenerator.cpp @@ -152,7 +152,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( initialGuess.thetacos.at(sgmt_start)); const auto θ_1 = std::atan2(initialGuess.thetasin.at(sgmt_end), initialGuess.thetacos.at(sgmt_end)); - const auto dθ = std::abs(AngleModulus(θ_0 - θ_1)); + const auto dθ = std::abs(AngleModulus(θ_1 - θ_0)); auto maxLinearVel = maxDrivetrainVelocity;