From 089fd1d34585cb788b3c0038db71167b7b9ff70b Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 17 Jun 2024 17:08:51 -0700 Subject: [PATCH] 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 --- src-tauri/src/main.rs | 54 +++++++++++++++---------------- src/document/ConstraintStore.tsx | 24 -------------- src/document/DocumentSpecTypes.ts | 15 +++++++++ 3 files changed, 42 insertions(+), 51 deletions(-) diff --git a/src-tauri/src/main.rs b/src-tauri/src/main.rs index 893cc796b1..7f9924676c 100644 --- 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 index 89f8f303c6..e9f919f616 100644 --- 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& 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: , - 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: , - properties: {}, - wptScope: true, - sgmtScope: true - }, StraightLine: { name: "Straight Line", shortName: "Straight Line", diff --git a/src/document/DocumentSpecTypes.ts b/src/document/DocumentSpecTypes.ts index a50a81c754..d9e58ff0a2 100644 --- 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