From bd4592b49261fecf408dc34ae7a3c47cf323e150 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sun, 6 Oct 2024 20:57:11 +0800 Subject: [PATCH 01/12] [trajoptlib] Expose lane constraint to Rust Signed-off-by: Jade Turner --- trajoptlib/src/lib.rs | 143 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 142 insertions(+), 1 deletion(-) diff --git a/trajoptlib/src/lib.rs b/trajoptlib/src/lib.rs index 5c5a28f18..3307a1e47 100644 --- a/trajoptlib/src/lib.rs +++ b/trajoptlib/src/lib.rs @@ -151,6 +151,15 @@ mod ffi { field_points_x: Vec, field_points_y: Vec, ); + fn wpt_keep_in_lane( + self: Pin<&mut SwervePathBuilder>, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn wpt_keep_out_circle( self: Pin<&mut SwervePathBuilder>, index: usize, @@ -207,7 +216,17 @@ mod ffi { field_points_x: Vec, field_points_y: Vec, ); - + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + self: Pin<&mut SwervePathBuilder>, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn sgmt_keep_out_circle( self: Pin<&mut SwervePathBuilder>, from_index: usize, @@ -314,6 +333,15 @@ mod ffi { field_points_x: Vec, field_points_y: Vec, ); + fn wpt_keep_in_lane( + self: Pin<&mut DifferentialPathBuilder>, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn wpt_keep_out_circle( self: Pin<&mut DifferentialPathBuilder>, index: usize, @@ -361,6 +389,17 @@ mod ffi { field_points_x: Vec, field_points_y: Vec, ); + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + self: Pin<&mut DifferentialPathBuilder>, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn sgmt_keep_out_circle( self: Pin<&mut DifferentialPathBuilder>, @@ -424,6 +463,15 @@ pub trait PathBuilder: Any { field_points_x: Vec, field_points_y: Vec, ); + fn wpt_keep_in_lane( + &mut self, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn wpt_keep_out_circle( &mut self, index: usize, @@ -466,6 +514,17 @@ pub trait PathBuilder: Any { field_points_x: Vec, field_points_y: Vec, ); + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + &mut self, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); fn sgmt_keep_out_circle( &mut self, @@ -683,6 +742,25 @@ impl PathBuilder for SwervePathBuilder { field_points_y, ); } + fn wpt_keep_in_lane( + &mut self, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::SwervePathBuilder::wpt_keep_in_lane( + self.path_builder.pin_mut(), + index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ); + } fn wpt_keep_out_circle( &mut self, @@ -784,6 +862,28 @@ impl PathBuilder for SwervePathBuilder { field_points_y, ); } + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + &mut self, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::SwervePathBuilder::sgmt_keep_in_lane( + self.path_builder.pin_mut(), + from_index, + to_index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ) + } fn sgmt_keep_out_circle( &mut self, @@ -1011,6 +1111,25 @@ impl PathBuilder for DifferentialPathBuilder { field_points_y, ); } + fn wpt_keep_in_lane( + &mut self, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::DifferentialPathBuilder::wpt_keep_in_lane( + self.path_builder.pin_mut(), + index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ); + } fn wpt_keep_out_circle( &mut self, @@ -1112,6 +1231,28 @@ impl PathBuilder for DifferentialPathBuilder { field_points_y, ); } + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + &mut self, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::DifferentialPathBuilder::sgmt_keep_in_lane( + self.path_builder.pin_mut(), + from_index, + to_index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ) + } fn sgmt_keep_out_circle( &mut self, From 447765a7f0ea0e9efe8f810c126137ad7dbb5fd7 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sun, 6 Oct 2024 22:15:36 +0800 Subject: [PATCH 02/12] start work on UI --- .../generation/transformers/constraints.rs | 107 +++++++-- src-core/src/spec/trajectory.rs | 30 +++ .../FieldConstraintDisplayLayer.tsx | 8 + .../constraintDisplay/KeepInLaneOverlay.tsx | 219 ++++++++++++++++++ .../KeepInRectangleOverlay.tsx | 2 +- src/document/ConstraintDefinitions.tsx | 69 ++++++ 6 files changed, 419 insertions(+), 16 deletions(-) create mode 100644 src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index aecf8f0ab..776a8abc6 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -2,7 +2,10 @@ use trajoptlib::PathBuilder; use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope}; -use super::{DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, SwerveGenerationTransformer}; +use super::{ + DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, + SwerveGenerationTransformer, +}; fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { let mut to_subtract: usize = 0; @@ -16,7 +19,7 @@ fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { pub struct ConstraintSetter { guess_points: Vec, - constraint_idx: Vec> + constraint_idx: Vec>, } impl ConstraintSetter { @@ -26,7 +29,9 @@ impl ConstraintSetter { let mut constraint_idx = Vec::>::new(); let num_wpts = ctx.params.waypoints.len(); - ctx.params.waypoints.iter() + ctx.params + .waypoints + .iter() .enumerate() .filter(|(_, w)| w.is_initial_guess && !w.fix_heading && !w.fix_translation) .for_each(|(idx, _)| guess_points.push(idx)); @@ -64,7 +69,7 @@ impl ConstraintSetter { from: fixed_from, to: fixed_to, data: constraint.data, - enabled: constraint.enabled + enabled: constraint.enabled, }); } } @@ -73,7 +78,7 @@ impl ConstraintSetter { FeatureLockedTransformer::always(Self { guess_points, - constraint_idx + constraint_idx, }) } } @@ -95,7 +100,7 @@ impl SwerveGenerationTransformer for ConstraintSetter { flip, } => match to_opt { None => builder.wpt_point_at(from, x, y, tolerance, flip), - Some(to) => builder.sgmt_point_at(from, to, x, y, tolerance, flip) + Some(to) => builder.sgmt_point_at(from, to, x, y, tolerance, flip), }, ConstraintData::MaxVelocity { max } => match to_opt { None => builder.wpt_linear_velocity_max_magnitude(from, max), @@ -114,7 +119,7 @@ impl SwerveGenerationTransformer for ConstraintSetter { builder.wpt_linear_velocity_max_magnitude(from, 0.0f64); builder.wpt_angular_velocity_max_magnitude(from, 0.0f64); } - }, + } ConstraintData::KeepInCircle { x, y, r } => match to_opt { None => builder.wpt_keep_in_circle(from, x, y, r), Some(to) => builder.sgmt_keep_in_circle(from, to, x, y, r), @@ -127,11 +132,45 @@ impl SwerveGenerationTransformer for ConstraintSetter { Some(to) => builder.sgmt_keep_in_polygon(from, to, xs, ys), } } - ConstraintData::KeepOutCircle { x, y, r } => { + ConstraintData::KeepInLane { + below_start_x, + below_start_y, + below_end_x, + below_end_y, + above_start_x, + above_start_y, + above_end_x, + above_end_y, + } => { + let center_line_start_x = (below_start_x + above_start_x) / 2.0; + let center_line_start_y = (below_start_y + above_start_y) / 2.0; + let center_line_end_x = (below_end_x + above_end_x) / 2.0; + let center_line_end_y = (below_end_y + above_end_y) / 2.0; + let tolerance = 1.0; // TODO how + match to_opt { - None => builder.wpt_keep_out_circle(from, x, y, r), - Some(to) => builder.sgmt_keep_out_circle(from, to, x, y, r), + None => builder.wpt_keep_in_lane( + from, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ), + Some(to) => builder.sgmt_keep_in_lane( + from, + to, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ), } + } + ConstraintData::KeepOutCircle { x, y, r } => match to_opt { + None => builder.wpt_keep_out_circle(from, x, y, r), + Some(to) => builder.sgmt_keep_out_circle(from, to, x, y, r), }, }; } @@ -152,7 +191,11 @@ impl DifferentialGenerationTransformer for ConstraintSetter { y, tolerance, flip, - } => if to_opt.is_none() { builder.wpt_point_at(from, x, y, tolerance, flip) }, + } => { + if to_opt.is_none() { + builder.wpt_point_at(from, x, y, tolerance, flip) + } + } ConstraintData::MaxVelocity { max } => match to_opt { None => builder.wpt_linear_velocity_max_magnitude(from, max), Some(to) => builder.sgmt_linear_velocity_max_magnitude(from, to, max), @@ -170,7 +213,7 @@ impl DifferentialGenerationTransformer for ConstraintSetter { builder.wpt_linear_velocity_max_magnitude(from, 0.0f64); builder.wpt_angular_velocity_max_magnitude(from, 0.0f64); } - }, + } ConstraintData::KeepInCircle { x, y, r } => match to_opt { None => builder.wpt_keep_in_circle(from, x, y, r), Some(to) => builder.sgmt_keep_in_circle(from, to, x, y, r), @@ -183,11 +226,45 @@ impl DifferentialGenerationTransformer for ConstraintSetter { Some(to) => builder.sgmt_keep_in_polygon(from, to, xs, ys), } } - ConstraintData::KeepOutCircle { x, y, r } => { + ConstraintData::KeepInLane { + below_start_x, + below_start_y, + below_end_x, + below_end_y, + above_start_x, + above_start_y, + above_end_x, + above_end_y, + } => { + let center_line_start_x = (below_start_x + above_start_x) / 2.0; + let center_line_start_y = (below_start_y + above_start_y) / 2.0; + let center_line_end_x = (below_end_x + above_end_x) / 2.0; + let center_line_end_y = (below_end_y + above_end_y) / 2.0; + let tolerance = 1.0; // TODO how + match to_opt { - None => builder.wpt_keep_out_circle(from, x, y, r), - Some(to) => builder.sgmt_keep_out_circle(from, to, x, y, r), + None => builder.wpt_keep_in_lane( + from, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ), + Some(to) => builder.sgmt_keep_in_lane( + from, + to, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ), } + } + ConstraintData::KeepOutCircle { x, y, r } => match to_opt { + None => builder.wpt_keep_out_circle(from, x, y, r), + Some(to) => builder.sgmt_keep_out_circle(from, to, x, y, r), }, }; } diff --git a/src-core/src/spec/trajectory.rs b/src-core/src/spec/trajectory.rs index 63f7f67ea..674a70a3c 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -147,6 +147,17 @@ pub enum ConstraintData { KeepInCircle { x: T, y: T, r: T }, /// A constraint to contain the bumpers within a rectangular region of the field KeepInRectangle { x: T, y: T, w: T, h: T }, + /// A constraint to contain the bumpers within two line + KeepInLane { + below_start_x: T, + below_start_y: T, + below_end_x: T, + below_end_y: T, + above_start_x: T, + above_start_y: T, + above_end_x: T, + above_end_y: T, + }, /// A constraint to contain the bumpers outside a circlular region of the field KeepOutCircle { x: T, y: T, r: T }, } @@ -195,6 +206,25 @@ impl ConstraintData { w: w.snapshot(), h: h.snapshot(), }, + ConstraintData::KeepInLane { + below_start_x, + below_start_y, + below_end_x, + below_end_y, + above_start_x, + above_start_y, + above_end_x, + above_end_y, + } => ConstraintData::KeepInLane { + below_start_x: below_start_x.snapshot(), + below_start_y: below_start_y.snapshot(), + below_end_x: below_end_x.snapshot(), + below_end_y: below_end_y.snapshot(), + above_start_x: above_start_x.snapshot(), + above_start_y: above_start_y.snapshot(), + above_end_x: above_end_x.snapshot(), + above_end_y: above_end_y.snapshot(), + }, ConstraintData::KeepOutCircle { x, y, r } => ConstraintData::KeepOutCircle { x: x.snapshot(), y: y.snapshot(), diff --git a/src/components/field/svg/constraintDisplay/FieldConstraintDisplayLayer.tsx b/src/components/field/svg/constraintDisplay/FieldConstraintDisplayLayer.tsx index ca5b9e9be..5898f5b2f 100644 --- a/src/components/field/svg/constraintDisplay/FieldConstraintDisplayLayer.tsx +++ b/src/components/field/svg/constraintDisplay/FieldConstraintDisplayLayer.tsx @@ -7,6 +7,7 @@ import KeepInCircleOverlay from "./KeepInCircleOverlay"; import KeepInRectangleOverlay from "./KeepInRectangleOverlay"; import PointAtOverlay from "./PointAtOverlay"; import KeepOutCircleOverlay from "./KeepOutCircleOverlay"; +import KeepInLaneOverlay from "./KeepInLaneOverlay"; const overlays = { PointAt: ( @@ -42,6 +43,13 @@ const overlays = { lineColor={lineColor} > ), + KeepInLane: (constraint: IConstraintStoreKeyed<"KeepInLane">) => ( + + ), KeepOutCircle: ( constraint: IConstraintStoreKeyed<"KeepOutCircle">, lineColor: string diff --git a/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx b/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx new file mode 100644 index 000000000..ad8765f86 --- /dev/null +++ b/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx @@ -0,0 +1,219 @@ +import React, { Component } from "react"; +import { IConstraintDataStore } from "../../../../document/ConstraintDataStore"; +import { + ConstraintKey, + DataMap +} from "../../../../document/ConstraintDefinitions"; +import { IHolonomicWaypointStore } from "../../../../document/HolonomicWaypointStore"; +import * as d3 from "d3"; +import { observer } from "mobx-react"; +import { doc } from "../../../../document/DocumentManager"; + +const STROKE = 0.1; +const DOT = 0.1; + +type Props = { + data: IConstraintDataStore; + start?: IHolonomicWaypointStore; + end?: IHolonomicWaypointStore; +}; +class KeepInLaneOverlay extends Component, object> { + rootRef: React.RefObject = React.createRef(); + componentDidMount() { + if (this.rootRef.current) { + // Theres probably a better way to do this + // TODO doesn't work :( + const dragHandleDragAbove = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select(`dragTarget-keepInLaneAbove`).call( + dragHandleDragAbove + ); + + const dragHandleDragBelow = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select(`dragTarget-keepInLaneBelow`).call( + dragHandleDragBelow + ); + + const dragHandleDragBelowStart = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `dragTarget-keepInLaneBelowStart` + ).call(dragHandleDragBelowStart); + + const dragHandleDragBelowEnd = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `dragTarget-keepInLaneBelowEnd` + ).call(dragHandleDragBelowEnd); + + const dragHandleDragAboveStart = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `dragTarget-keepInLaneAboveStart` + ).call(dragHandleDragAboveStart); + + const dragHandleDragAboveEnd = d3 + .drag() + .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `dragTarget-keepInLaneAboveEnd` + ).call(dragHandleDragAboveEnd); + } + } + + dragPointTranslate(event: any, xOffset: boolean, yOffset: boolean) { + const data = this.props.data; + console.log(xOffset, yOffset); + data.below_start_x.set( + data.serialize.props.below_start_x.val + + event.dBelowStartX * (xOffset ? 0.0 : 1.0) + ); + data.below_start_y.set( + data.serialize.props.below_start_y.val + + event.dBelowStartY * (yOffset ? 0.0 : 1.0) + ); + data.below_end_x.set( + data.serialize.props.below_end_x.val + + event.dBelowEndX * (xOffset ? 0.0 : 1.0) + ); + data.below_end_y.set( + data.serialize.props.below_end_y.val + + event.dBelowEndY * (yOffset ? 0.0 : 1.0) + ); + data.above_start_x.set( + data.serialize.props.above_start_x.val + + event.dAboveStartX * (xOffset ? 0.0 : 1.0) + ); + data.above_start_y.set( + data.serialize.props.above_start_y.val + + event.dAboveStartY * (yOffset ? 0.0 : 1.0) + ); + data.above_end_x.set( + data.serialize.props.above_end_x.val + + event.dAboveEndX * (xOffset ? 0.0 : 1.0) + ); + data.above_end_y.set( + data.serialize.props.above_end_y.val + + event.dAboveEndY * (yOffset ? 0.0 : 1.0) + ); + } + + render() { + const data = this.props.data.serialize as DataMap["KeepInLane"]; + const belowStartX = data.props.below_start_x.val; + const belowStartY = data.props.below_start_y.val; + const belowEndX = data.props.below_end_x.val; + const belowEndY = data.props.below_end_y.val; + const aboveStartX = data.props.above_start_x.val; + const aboveStartY = data.props.above_start_y.val; + const aboveEndX = data.props.above_end_x.val; + const aboveEndY = data.props.above_end_y.val; + return ( + + {/* Lines */} + + + {/* Points */} + + + + + + ); + } +} +export default observer(KeepInLaneOverlay); diff --git a/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx b/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx index a0b93e8ea..8666c821a 100644 --- a/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx +++ b/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx @@ -117,7 +117,7 @@ class KeepInRectangleOverlay extends Component< const data = this.props.data; data.x.set(data.serialize.props.x.val + event.dx); - data.y.set(this.props.data.serialize.props.y.val + event.dy); + data.y.set(data.serialize.props.y.val + event.dy); } fixWidthHeight() { diff --git a/src/document/ConstraintDefinitions.tsx b/src/document/ConstraintDefinitions.tsx index c3c9277bf..eafd7cda5 100644 --- a/src/document/ConstraintDefinitions.tsx +++ b/src/document/ConstraintDefinitions.tsx @@ -68,6 +68,16 @@ export type ConstraintDataTypeMap = { w: Expr; h: Expr; }; + KeepInLane: { + below_start_x: Expr; + below_start_y: Expr; + below_end_x: Expr; + below_end_y: Expr; + above_start_x: Expr; + above_start_y: Expr; + above_end_x: Expr; + above_end_y: Expr; + }; KeepOutCircle: { x: Expr; y: Expr; @@ -245,6 +255,65 @@ export const ConstraintDefinitions: defs = { wptScope: true, sgmtScope: true } satisfies ConstraintDefinition<"KeepInRectangle">, + KeepInLane: { + type: "KeepInLane" as const, + name: "Keep In Lane", + shortName: "Keep In Lane", + description: "Keep the robot's bumpers within a lane", + icon: , // TODO + properties: { + below_start_x: { + name: "Below Start X", + description: "The start of the blow lines X value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + below_start_y: { + name: "Below Start Y", + description: "The start of the blow lines Y value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + below_end_x: { + name: "Below End X", + description: "The end of the blow lines X value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + below_end_y: { + name: "Below End Y", + description: "The end of the blow lines Y value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + above_start_x: { + name: "Above Start X", + description: "The above of the blow lines X value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + above_start_y: { + name: "Above Start Y", + description: "The above of the blow lines Y value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + above_end_x: { + name: "Above End X", + description: "The above of the blow lines X value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + }, + above_end_y: { + name: "Above End Y", + description: "The above of the blow lines Y value", + dimension: Dimensions.Length, + defaultVal: { exp: "0 m", val: 1 } + } + }, + wptScope: true, + sgmtScope: true + } satisfies ConstraintDefinition<"KeepInLane">, KeepOutCircle: { type: "KeepOutCircle" as const, name: "Keep Out Circle", From db26845d38d2fdd3cb57ac7e6a721ff9a2b453f7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 6 Oct 2024 18:17:11 -0700 Subject: [PATCH 03/12] Update src-core/src/generation/transformers/constraints.rs --- src-core/src/generation/transformers/constraints.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 776a8abc6..cf495a8bc 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -240,7 +240,7 @@ impl DifferentialGenerationTransformer for ConstraintSetter { let center_line_start_y = (below_start_y + above_start_y) / 2.0; let center_line_end_x = (below_end_x + above_end_x) / 2.0; let center_line_end_y = (below_end_y + above_end_y) / 2.0; - let tolerance = 1.0; // TODO how + let tolerance = (above_start_x - below_start_x).hypot(above_start_y - below_start_y) / 2.0; match to_opt { None => builder.wpt_keep_in_lane( From b958a25be2e51eb1aaa58674bb6d24c0da305686 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 6 Oct 2024 18:17:18 -0700 Subject: [PATCH 04/12] Update src-core/src/generation/transformers/constraints.rs --- src-core/src/generation/transformers/constraints.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index cf495a8bc..15fda9ae1 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -146,7 +146,7 @@ impl SwerveGenerationTransformer for ConstraintSetter { let center_line_start_y = (below_start_y + above_start_y) / 2.0; let center_line_end_x = (below_end_x + above_end_x) / 2.0; let center_line_end_y = (below_end_y + above_end_y) / 2.0; - let tolerance = 1.0; // TODO how + let tolerance = (above_start_x - below_start_x).hypot(above_start_y - below_start_y) / 2.0; match to_opt { None => builder.wpt_keep_in_lane( From 726695c60a607b38519c776750ac743fe6565cdc Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 10:56:30 -0700 Subject: [PATCH 05/12] Change constraint to just tolerance param; fix UI overlay --- .../generation/transformers/constraints.rs | 110 ++++----- src-core/src/spec/trajectory.rs | 32 +-- src/assets/KeepInLane.tsx | 23 ++ .../constraintDisplay/KeepInLaneOverlay.tsx | 230 ++++++------------ src/document/ConstraintDefinitions.tsx | 67 +---- .../trajopt/constraint/LaneConstraint.hpp | 4 +- .../constraint/PointLineRegionConstraint.hpp | 16 +- trajoptlib/src/RustFFI.cpp | 62 ++--- 8 files changed, 191 insertions(+), 353 deletions(-) create mode 100644 src/assets/KeepInLane.tsx diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 6cf29a032..33d92fd72 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -1,4 +1,4 @@ -use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope}; +use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope, Waypoint}; use super::{ DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, @@ -18,20 +18,29 @@ fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { pub struct ConstraintSetter { guess_points: Vec, constraint_idx: Vec>, + /// A vector of remaining waypoints matching the indexing scheme of constraint_idx + waypoint_idx: Vec> } impl ConstraintSetter { fn initialize(context: &GenerationContext) -> FeatureLockedTransformer { let mut guess_points: Vec = Vec::new(); let mut constraint_idx = Vec::>::new(); + let mut waypoint_idx = Vec::>::new(); let num_wpts = context.params.waypoints.len(); context.params .waypoints .iter() .enumerate() - .filter(|(_, w)| w.is_initial_guess && !w.fix_heading && !w.fix_translation) - .for_each(|(idx, _)| guess_points.push(idx)); + .for_each(|(idx, w)| { + if w.is_initial_guess && !w.fix_heading && !w.fix_translation { + // filtered out, save the index + guess_points.push(idx); + } else { + waypoint_idx.push(w.clone()); + } + }); for constraint in context.params.get_enabled_constraints() { let from = constraint.from.get_idx(num_wpts); @@ -76,6 +85,7 @@ impl ConstraintSetter { FeatureLockedTransformer::always(Self { guess_points, constraint_idx, + waypoint_idx }) } } @@ -116,7 +126,7 @@ impl SwerveGenerationTransformer for ConstraintSetter { generator.wpt_linear_velocity_max_magnitude(from, 0.0f64); generator.wpt_angular_velocity_max_magnitude(from, 0.0f64); } - } + }, ConstraintData::KeepInCircle { x, y, r } => match to_opt { None => generator.wpt_keep_in_circle(from, x, y, r), Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r), @@ -128,43 +138,26 @@ impl SwerveGenerationTransformer for ConstraintSetter { None => generator.wpt_keep_in_polygon(from, xs, ys), Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys), } - } + }, ConstraintData::KeepInLane { - below_start_x, - below_start_y, - below_end_x, - below_end_y, - above_start_x, - above_start_y, - above_end_x, - above_end_y, + tolerance } => { - let center_line_start_x = (below_start_x + above_start_x) / 2.0; - let center_line_start_y = (below_start_y + above_start_y) / 2.0; - let center_line_end_x = (below_end_x + above_end_x) / 2.0; - let center_line_end_y = (below_end_y + above_end_y) / 2.0; - let tolerance = (above_start_x - below_start_x).hypot(above_start_y - below_start_y) / 2.0; - - match to_opt { - None => generator.wpt_keep_in_lane( + if let Some(idx_to) = to_opt { + if let Some(wpt_to) = self.waypoint_idx.get(idx_to) { + if let Some(wpt_from) = self.waypoint_idx.get(from) { + generator.sgmt_keep_in_lane( from, - center_line_start_x, - center_line_start_y, - center_line_end_x, - center_line_end_y, + idx_to, + wpt_from.x, + wpt_from.y, + wpt_to.x, + wpt_to.y, tolerance, - ), - Some(to) => generator.sgmt_keep_in_lane( - from, - to, - center_line_start_x, - center_line_start_y, - center_line_end_x, - center_line_end_y, - tolerance, - ), + ); } } + } + }, ConstraintData::KeepOutCircle { x, y, r } => match to_opt { None => generator.wpt_keep_out_circle(from, x, y, r), Some(to) => generator.sgmt_keep_out_circle(from, to, x, y, r), @@ -223,43 +216,26 @@ impl DifferentialGenerationTransformer for ConstraintSetter { None => generator.wpt_keep_in_polygon(from, xs, ys), Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys), } - } + }, ConstraintData::KeepInLane { - below_start_x, - below_start_y, - below_end_x, - below_end_y, - above_start_x, - above_start_y, - above_end_x, - above_end_y, + tolerance } => { - let center_line_start_x = (below_start_x + above_start_x) / 2.0; - let center_line_start_y = (below_start_y + above_start_y) / 2.0; - let center_line_end_x = (below_end_x + above_end_x) / 2.0; - let center_line_end_y = (below_end_y + above_end_y) / 2.0; - let tolerance = (above_start_x - below_start_x).hypot(above_start_y - below_start_y) / 2.0; - - match to_opt { - None => generator.wpt_keep_in_lane( + if let Some(idx_to) = to_opt { + if let Some(wpt_to) = self.waypoint_idx.get(idx_to) { + if let Some(wpt_from) = self.waypoint_idx.get(from) { + generator.sgmt_keep_in_lane( from, - center_line_start_x, - center_line_start_y, - center_line_end_x, - center_line_end_y, + idx_to, + wpt_from.x, + wpt_from.y, + wpt_to.x, + wpt_to.y, tolerance, - ), - Some(to) => generator.sgmt_keep_in_lane( - from, - to, - center_line_start_x, - center_line_start_y, - center_line_end_x, - center_line_end_y, - tolerance, - ), + ); + } } - } + } + }, ConstraintData::KeepOutCircle { x, y, r } => match to_opt { None => generator.wpt_keep_out_circle(from, x, y, r), Some(to) => generator.sgmt_keep_out_circle(from, to, x, y, r), diff --git a/src-core/src/spec/trajectory.rs b/src-core/src/spec/trajectory.rs index 674a70a3c..8d68a801d 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -148,16 +148,7 @@ pub enum ConstraintData { /// A constraint to contain the bumpers within a rectangular region of the field KeepInRectangle { x: T, y: T, w: T, h: T }, /// A constraint to contain the bumpers within two line - KeepInLane { - below_start_x: T, - below_start_y: T, - below_end_x: T, - below_end_y: T, - above_start_x: T, - above_start_y: T, - above_end_x: T, - above_end_y: T, - }, + KeepInLane { tolerance: T }, /// A constraint to contain the bumpers outside a circlular region of the field KeepOutCircle { x: T, y: T, r: T }, } @@ -167,6 +158,7 @@ impl ConstraintData { pub fn scope(&self) -> ConstraintScope { match self { ConstraintData::StopPoint {} => ConstraintScope::Waypoint, + ConstraintData::KeepInLane { tolerance: _ } => ConstraintScope::Segment, _ => ConstraintScope::Both, } } @@ -206,24 +198,8 @@ impl ConstraintData { w: w.snapshot(), h: h.snapshot(), }, - ConstraintData::KeepInLane { - below_start_x, - below_start_y, - below_end_x, - below_end_y, - above_start_x, - above_start_y, - above_end_x, - above_end_y, - } => ConstraintData::KeepInLane { - below_start_x: below_start_x.snapshot(), - below_start_y: below_start_y.snapshot(), - below_end_x: below_end_x.snapshot(), - below_end_y: below_end_y.snapshot(), - above_start_x: above_start_x.snapshot(), - above_start_y: above_start_y.snapshot(), - above_end_x: above_end_x.snapshot(), - above_end_y: above_end_y.snapshot(), + ConstraintData::KeepInLane { tolerance } => ConstraintData::KeepInLane { + tolerance: tolerance.snapshot(), }, ConstraintData::KeepOutCircle { x, y, r } => ConstraintData::KeepOutCircle { x: x.snapshot(), diff --git a/src/assets/KeepInLane.tsx b/src/assets/KeepInLane.tsx new file mode 100644 index 000000000..ef827f6c4 --- /dev/null +++ b/src/assets/KeepInLane.tsx @@ -0,0 +1,23 @@ +import * as React from "react"; +import { SvgIcon as MuiSvgIcon, SvgIconProps, styled } from "@mui/material"; +const SvgIcon = styled(MuiSvgIcon, { + name: "KeepInLaneIcon", + shouldForwardProp: (prop) => prop !== "fill" +})(() => ({ + fill: "currentColor", + stroke: "none" +})); + +const KeepInLane: React.FunctionComponent = (props) => { + return ( + + ); +}; +export default KeepInLane; diff --git a/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx b/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx index ad8765f86..d1fdf1e50 100644 --- a/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx +++ b/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx @@ -9,7 +9,7 @@ import * as d3 from "d3"; import { observer } from "mobx-react"; import { doc } from "../../../../document/DocumentManager"; -const STROKE = 0.1; +const STROKE = 0.02; const DOT = 0.1; type Props = { @@ -21,53 +21,9 @@ class KeepInLaneOverlay extends Component, object> { rootRef: React.RefObject = React.createRef(); componentDidMount() { if (this.rootRef.current) { - // Theres probably a better way to do this - // TODO doesn't work :( - const dragHandleDragAbove = d3 - .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) - .on("start", () => { - doc.history.startGroup(() => {}); - }) - .on("end", (_event) => { - doc.history.stopGroup(); - }) - .container(this.rootRef.current); - d3.select(`dragTarget-keepInLaneAbove`).call( - dragHandleDragAbove - ); - - const dragHandleDragBelow = d3 - .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) - .on("start", () => { - doc.history.startGroup(() => {}); - }) - .on("end", (_event) => { - doc.history.stopGroup(); - }) - .container(this.rootRef.current); - d3.select(`dragTarget-keepInLaneBelow`).call( - dragHandleDragBelow - ); - - const dragHandleDragBelowStart = d3 - .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) - .on("start", () => { - doc.history.startGroup(() => {}); - }) - .on("end", (_event) => { - doc.history.stopGroup(); - }) - .container(this.rootRef.current); - d3.select( - `dragTarget-keepInLaneBelowStart` - ).call(dragHandleDragBelowStart); - - const dragHandleDragBelowEnd = d3 + const dragHandleDrag = d3 .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) + .on("drag", (event) => this.dragPointTolerance(event)) .on("start", () => { doc.history.startGroup(() => {}); }) @@ -76,141 +32,109 @@ class KeepInLaneOverlay extends Component, object> { }) .container(this.rootRef.current); d3.select( - `dragTarget-keepInLaneBelowEnd` - ).call(dragHandleDragBelowEnd); - - const dragHandleDragAboveStart = d3 - .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) - .on("start", () => { - doc.history.startGroup(() => {}); - }) - .on("end", (_event) => { - doc.history.stopGroup(); - }) - .container(this.rootRef.current); + `#dragTarget-keepInLaneAbove` + ).call(dragHandleDrag); d3.select( - `dragTarget-keepInLaneAboveStart` - ).call(dragHandleDragAboveStart); - - const dragHandleDragAboveEnd = d3 - .drag() - .on("drag", (event) => this.dragPointTranslate(event, false, false)) - .on("start", () => { - doc.history.startGroup(() => {}); - }) - .on("end", (_event) => { - doc.history.stopGroup(); - }) - .container(this.rootRef.current); - d3.select( - `dragTarget-keepInLaneAboveEnd` - ).call(dragHandleDragAboveEnd); + `#dragTarget-keepInLaneBelow` + ).call(dragHandleDrag); } } - - dragPointTranslate(event: any, xOffset: boolean, yOffset: boolean) { + dragPointTolerance(event: DragEvent) { const data = this.props.data; - console.log(xOffset, yOffset); - data.below_start_x.set( - data.serialize.props.below_start_x.val + - event.dBelowStartX * (xOffset ? 0.0 : 1.0) - ); - data.below_start_y.set( - data.serialize.props.below_start_y.val + - event.dBelowStartY * (yOffset ? 0.0 : 1.0) - ); - data.below_end_x.set( - data.serialize.props.below_end_x.val + - event.dBelowEndX * (xOffset ? 0.0 : 1.0) - ); - data.below_end_y.set( - data.serialize.props.below_end_y.val + - event.dBelowEndY * (yOffset ? 0.0 : 1.0) - ); - data.above_start_x.set( - data.serialize.props.above_start_x.val + - event.dAboveStartX * (xOffset ? 0.0 : 1.0) - ); - data.above_start_y.set( - data.serialize.props.above_start_y.val + - event.dAboveStartY * (yOffset ? 0.0 : 1.0) - ); - data.above_end_x.set( - data.serialize.props.above_end_x.val + - event.dAboveEndX * (xOffset ? 0.0 : 1.0) - ); - data.above_end_y.set( - data.serialize.props.above_end_y.val + - event.dAboveEndY * (yOffset ? 0.0 : 1.0) - ); + const { x, y } = event; + const start = this.props.start; + const end = this.props.end; + if (start === undefined || end === undefined || start.uuid === end.uuid) { + return; + } + const startX = start.x.value; + const startY = start.y.value; + const endX = end.x.value; + const endY = end.y.value; + // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points + const dx = endX - startX; + const dy = endY - startY; + const dist = + Math.abs(dy * x - dx * y + endX * startY - endY * startX) / + Math.hypot(dx, dy); + data.tolerance.set(dist); } - render() { const data = this.props.data.serialize as DataMap["KeepInLane"]; - const belowStartX = data.props.below_start_x.val; - const belowStartY = data.props.below_start_y.val; - const belowEndX = data.props.below_end_x.val; - const belowEndY = data.props.below_end_y.val; - const aboveStartX = data.props.above_start_x.val; - const aboveStartY = data.props.above_start_y.val; - const aboveEndX = data.props.above_end_x.val; - const aboveEndY = data.props.above_end_y.val; + const tolerance = data.props.tolerance.val + STROKE / 2; + const start = this.props.start; + const end = this.props.end; + if (start === undefined || end === undefined || start.uuid === end.uuid) { + return <>; + } + const startX = start.x.value; + const startY = start.y.value; + const endX = end.x.value; + const endY = end.y.value; + + const dx = endX - startX; + const dy = endY - startY; + const dist = Math.hypot(dy, dx); + if (dist === 0) { + return <>; + } + const [offsetX, offsetY] = [ + -tolerance * (dy / dist), + tolerance * (dx / dist) + ]; + const [startAboveX, startAboveY] = [startX + offsetX, startY + offsetY]; + const [endAboveX, endAboveY] = [endX + offsetX, endY + offsetY]; + const [midAboveX, midAboveY] = [ + (endAboveX + startAboveX) / 2, + (endAboveY + startAboveY) / 2 + ]; + const [startBelowX, startBelowY] = [startX - offsetX, startY - offsetY]; + const [endBelowX, endBelowY] = [endX - offsetX, endY - offsetY]; + const [midBelowX, midBelowY] = [ + (endBelowX + startBelowX) / 2, + (endBelowY + startBelowY) / 2 + ]; return ( {/* Lines */} + - {/* Points */} - - ); diff --git a/src/document/ConstraintDefinitions.tsx b/src/document/ConstraintDefinitions.tsx index eafd7cda5..95f7ee3ca 100644 --- a/src/document/ConstraintDefinitions.tsx +++ b/src/document/ConstraintDefinitions.tsx @@ -1,4 +1,5 @@ import { + AddRoad, ArrowCircleDown, DoNotDisturb, NearMe, @@ -9,6 +10,7 @@ import { import { JSXElementConstructor, ReactElement } from "react"; import { Expr } from "./2025/DocumentTypes"; import { Dimension, DimensionName, Dimensions } from "./ExpressionStore"; +import KeepInLane from "../assets/KeepInLane"; export type ConstraintPropertyType = Expr | boolean; @@ -69,14 +71,7 @@ export type ConstraintDataTypeMap = { h: Expr; }; KeepInLane: { - below_start_x: Expr; - below_start_y: Expr; - below_end_x: Expr; - below_end_y: Expr; - above_start_x: Expr; - above_start_y: Expr; - above_end_x: Expr; - above_end_y: Expr; + tolerance: Expr; }; KeepOutCircle: { x: Expr; @@ -259,59 +254,17 @@ export const ConstraintDefinitions: defs = { type: "KeepInLane" as const, name: "Keep In Lane", shortName: "Keep In Lane", - description: "Keep the robot's bumpers within a lane", - icon: , // TODO + description: "Keep the robot's center within a lane", + icon: , properties: { - below_start_x: { - name: "Below Start X", - description: "The start of the blow lines X value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - below_start_y: { - name: "Below Start Y", - description: "The start of the blow lines Y value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - below_end_x: { - name: "Below End X", - description: "The end of the blow lines X value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - below_end_y: { - name: "Below End Y", - description: "The end of the blow lines Y value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - above_start_x: { - name: "Above Start X", - description: "The above of the blow lines X value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - above_start_y: { - name: "Above Start Y", - description: "The above of the blow lines Y value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - above_end_x: { - name: "Above End X", - description: "The above of the blow lines X value", - dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } - }, - above_end_y: { - name: "Above End Y", - description: "The above of the blow lines Y value", + tolerance: { + name: "Tolerance", + description: "Robot center max distance from line between waypoints", dimension: Dimensions.Length, - defaultVal: { exp: "0 m", val: 1 } + defaultVal: { exp: "0.01 m", val: 0.01 } } }, - wptScope: true, + wptScope: false, sgmtScope: true } satisfies ConstraintDefinition<"KeepInLane">, KeepOutCircle: { diff --git a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp index d2d20cd2b..59129b058 100644 --- a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp @@ -35,7 +35,7 @@ class TRAJOPT_DLLEXPORT LaneConstraint { double dx = centerLineEnd.X() - centerLineStart.X(); double dy = centerLineEnd.Y() - centerLineStart.Y(); double dist = std::hypot(dx, dy); - auto offset = Translation2d{tolerance, 0.0}.RotateBy( + auto offset = Translation2d{0.0, tolerance}.RotateBy( Rotation2d{dx / dist, dy / dist}); return PointLineRegionConstraint{ @@ -51,7 +51,7 @@ class TRAJOPT_DLLEXPORT LaneConstraint { double dx = centerLineEnd.X() - centerLineStart.X(); double dy = centerLineEnd.Y() - centerLineStart.Y(); double dist = std::hypot(dx, dy); - auto offset = Translation2d{tolerance, 0.0}.RotateBy( + auto offset = Translation2d{0.0, tolerance}.RotateBy( Rotation2d{dx / dist, dy / dist}); return PointLineRegionConstraint{ diff --git a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp index 41191822b..5bf767b34 100644 --- a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp @@ -93,18 +93,22 @@ class TRAJOPT_DLLEXPORT PointLineRegionConstraint { auto a = m_fieldLineStart.Y() - m_fieldLineEnd.Y(); auto b = m_fieldLineEnd.X() - m_fieldLineStart.X(); + auto line = m_fieldLineEnd - m_fieldLineStart; + auto startToPoint = bumperCorner - m_fieldLineStart; + auto cross = startToPoint.Cross(line); + // cross > 0, startToPoint is left of line + // cross = 0, startToPoint is on line + // cross < 0, startToPoint is right of line + switch (m_side) { case Side::kAbove: - problem.SubjectTo(a * bumperCorner.X() + b * bumperCorner.Y() > - a * m_fieldLineStart.X() + b * m_fieldLineStart.Y()); + problem.SubjectTo(cross < 0); break; case Side::kBelow: - problem.SubjectTo(a * bumperCorner.X() + b * bumperCorner.Y() < - a * m_fieldLineStart.X() + b * m_fieldLineStart.Y()); + problem.SubjectTo(cross > 0); break; case Side::kOn: - problem.SubjectTo(a * bumperCorner.X() + b * bumperCorner.Y() == - a * m_fieldLineStart.X() + b * m_fieldLineStart.Y()); + problem.SubjectTo(cross == 0); break; } } diff --git a/trajoptlib/src/RustFFI.cpp b/trajoptlib/src/RustFFI.cpp index f57057851..3ec68a5e1 100644 --- a/trajoptlib/src/RustFFI.cpp +++ b/trajoptlib/src/RustFFI.cpp @@ -162,16 +162,11 @@ void SwerveTrajectoryGenerator::wpt_keep_in_polygon( void SwerveTrajectoryGenerator::wpt_keep_in_lane( size_t index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { - for (const auto& bumper : path_builder.GetBumpers()) { - for (const auto& corner : bumper.points) { - path_builder.WptConstraint( - index, - trajopt::LaneConstraint{corner, - {center_line_start_x, center_line_start_y}, - {center_line_end_x, center_line_end_y}, - tolerance}); - } - } + path_builder.WptConstraint( + index, trajopt::LaneConstraint{{0.0, 0.0}, + {center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); } void SwerveTrajectoryGenerator::wpt_keep_out_circle(size_t index, double x, @@ -281,16 +276,12 @@ void SwerveTrajectoryGenerator::sgmt_keep_in_lane( size_t from_index, size_t to_index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { - for (const auto& bumper : path_builder.GetBumpers()) { - for (const auto& corner : bumper.points) { - path_builder.SgmtConstraint( - from_index, to_index, - trajopt::LaneConstraint{corner, - {center_line_start_x, center_line_start_y}, - {center_line_end_x, center_line_end_y}, - tolerance}); - } - } + path_builder.SgmtConstraint( + from_index, to_index, + trajopt::LaneConstraint{{0.0, 0.0}, + {center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); } void SwerveTrajectoryGenerator::sgmt_keep_out_circle(size_t from_index, @@ -511,16 +502,11 @@ void DifferentialTrajectoryGenerator::wpt_keep_in_polygon( void DifferentialTrajectoryGenerator::wpt_keep_in_lane( size_t index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { - for (const auto& bumper : path_builder.GetBumpers()) { - for (const auto& corner : bumper.points) { - path_builder.WptConstraint( - index, - trajopt::LaneConstraint{corner, - {center_line_start_x, center_line_start_y}, - {center_line_end_x, center_line_end_y}, - tolerance}); - } - } + path_builder.WptConstraint( + index, trajopt::LaneConstraint{{0.0, 0.0}, + {center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); } void DifferentialTrajectoryGenerator::wpt_keep_out_circle(size_t index, @@ -620,16 +606,12 @@ void DifferentialTrajectoryGenerator::sgmt_keep_in_lane( size_t from_index, size_t to_index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { - for (const auto& bumper : path_builder.GetBumpers()) { - for (const auto& corner : bumper.points) { - path_builder.SgmtConstraint( - from_index, to_index, - trajopt::LaneConstraint{corner, - {center_line_start_x, center_line_start_y}, - {center_line_end_x, center_line_end_y}, - tolerance}); - } - } + path_builder.SgmtConstraint( + from_index, to_index, + trajopt::LaneConstraint{{0.0, 0.0}, + {center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); } void DifferentialTrajectoryGenerator::sgmt_keep_out_circle(size_t from_index, From 9f59ab4dd89f473f09ecda25a6921729336b2348 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 11:39:49 -0700 Subject: [PATCH 06/12] Update derivation in PointLineRegionConstraint --- .../constraint/PointLineRegionConstraint.hpp | 57 ++++++++----------- 1 file changed, 24 insertions(+), 33 deletions(-) diff --git a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp index 5bf767b34..98849a22d 100644 --- a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp @@ -67,45 +67,36 @@ class TRAJOPT_DLLEXPORT PointLineRegionConstraint { [[maybe_unused]] const sleipnir::Variable& angularVelocity, [[maybe_unused]] const Translation2v& linearAcceleration, [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { - auto bumperCorner = - pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); - - // Rearrange y − y₀ = m(x − x₀) where m = (y₁ − y₀)/(x₁ − x₀) into ax + by = - // c form. - - // y − y₀ = m(x − x₀) - // y − y₀ = (y₁ − y₀)/(x₁ − x₀)(x − x₀) - // (x₁ − x₀)(y − y₀) = (y₁ − y₀)(x − x₀) - // (x₁ − x₀)y − (x₁ − x₀)y₀ = (y₁ − y₀)x − (y₁ − y₀)x₀ - // (y₀ − y₁)x + (x₁ − x₀)y = −(y₁ − y₀)x₀ + (x₁ − x₀)y₀ - // (y₀ − y₁)x + (x₁ − x₀)y = (y₀ − y₁)x₀ + (x₁ − x₀)y₀ - - // ax + by = c where - // a = y₀ − y₁ - // b = x₁ − x₀ - // c = (y₀ − y₁)x₀ + (x₁ − x₀)y₀ - // = ax₀ + by₀ - - // ax + by = ax₀ + by₀ where - // a = y₀ − y₁ - // b = x₁ − x₀ - - auto a = m_fieldLineStart.Y() - m_fieldLineEnd.Y(); - auto b = m_fieldLineEnd.X() - m_fieldLineStart.X(); - + auto point = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); + // Determine which side of the start-end field line bumperCorner is on. + // The cross product a x b gives ||a|| ||b|| std::sin(θ), for a and b + // vectors with the same tail. If a x b > 0, b is to the left of a + // b + // ^ + // / + // -----> a + // + // If a x b < 0, b is to the right of a + // -----> a + // \ + // v + // b + // + // If a = the field line, start->end + // and b = start->point, then: + // cross > 0 means point is to the left of line (above) + // cross = 0 means point is on line + // cross < 0 means point is to the right of line (below) auto line = m_fieldLineEnd - m_fieldLineStart; - auto startToPoint = bumperCorner - m_fieldLineStart; - auto cross = startToPoint.Cross(line); - // cross > 0, startToPoint is left of line - // cross = 0, startToPoint is on line - // cross < 0, startToPoint is right of line + auto startToPoint = point - m_fieldLineStart; + auto cross = line.Cross(startToPoint); switch (m_side) { case Side::kAbove: - problem.SubjectTo(cross < 0); + problem.SubjectTo(cross > 0); break; case Side::kBelow: - problem.SubjectTo(cross > 0); + problem.SubjectTo(cross < 0); break; case Side::kOn: problem.SubjectTo(cross == 0); From c322081d84cc7aba39ce471842de128218f66884 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 11:43:55 -0700 Subject: [PATCH 07/12] Remove hardcoded origin robot point --- .../trajopt/constraint/LaneConstraint.hpp | 18 ++++++++++-------- .../constraint/PointLineRegionConstraint.hpp | 12 ++++++------ trajoptlib/src/RustFFI.cpp | 12 ++++-------- 3 files changed, 20 insertions(+), 22 deletions(-) diff --git a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp index 59129b058..0d423a0e2 100644 --- a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp @@ -28,8 +28,8 @@ class TRAJOPT_DLLEXPORT LaneConstraint { * @param tolerance Distance from center line to lane edge. Passing zero * creates a line constraint. */ - LaneConstraint(Translation2d robotPoint, Translation2d centerLineStart, - Translation2d centerLineEnd, double tolerance) + LaneConstraint(Translation2d centerLineStart, Translation2d centerLineEnd, + double tolerance) : m_topLine{[&] { if (tolerance != 0.0) { double dx = centerLineEnd.X() - centerLineStart.X(); @@ -38,9 +38,10 @@ class TRAJOPT_DLLEXPORT LaneConstraint { auto offset = Translation2d{0.0, tolerance}.RotateBy( Rotation2d{dx / dist, dy / dist}); - return PointLineRegionConstraint{ - robotPoint, centerLineStart + offset, centerLineEnd + offset, - Side::kBelow}; + return PointLineRegionConstraint{{0.0, 0.0}, + centerLineStart + offset, + centerLineEnd + offset, + Side::kBelow}; } else { return PointLineRegionConstraint{robotPoint, centerLineStart, centerLineEnd, Side::kOn}; @@ -54,9 +55,10 @@ class TRAJOPT_DLLEXPORT LaneConstraint { auto offset = Translation2d{0.0, tolerance}.RotateBy( Rotation2d{dx / dist, dy / dist}); - return PointLineRegionConstraint{ - robotPoint, centerLineStart - offset, centerLineEnd - offset, - Side::kAbove}; + return PointLineRegionConstraint{{0.0, 0.0}, + centerLineStart - offset, + centerLineEnd - offset, + Side::kAbove}; } else { return std::nullopt; } diff --git a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp index 98849a22d..014645307 100644 --- a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp @@ -71,16 +71,16 @@ class TRAJOPT_DLLEXPORT PointLineRegionConstraint { // Determine which side of the start-end field line bumperCorner is on. // The cross product a x b gives ||a|| ||b|| std::sin(θ), for a and b // vectors with the same tail. If a x b > 0, b is to the left of a - // b - // ^ - // / + // b + // ^ + // | // -----> a // // If a x b < 0, b is to the right of a // -----> a - // \ - // v - // b + // | + // v + // b // // If a = the field line, start->end // and b = start->point, then: diff --git a/trajoptlib/src/RustFFI.cpp b/trajoptlib/src/RustFFI.cpp index 3ec68a5e1..4f4fe119f 100644 --- a/trajoptlib/src/RustFFI.cpp +++ b/trajoptlib/src/RustFFI.cpp @@ -163,8 +163,7 @@ void SwerveTrajectoryGenerator::wpt_keep_in_lane( size_t index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { path_builder.WptConstraint( - index, trajopt::LaneConstraint{{0.0, 0.0}, - {center_line_start_x, center_line_start_y}, + index, trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, {center_line_end_x, center_line_end_y}, tolerance}); } @@ -278,8 +277,7 @@ void SwerveTrajectoryGenerator::sgmt_keep_in_lane( double center_line_end_y, double tolerance) { path_builder.SgmtConstraint( from_index, to_index, - trajopt::LaneConstraint{{0.0, 0.0}, - {center_line_start_x, center_line_start_y}, + trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, {center_line_end_x, center_line_end_y}, tolerance}); } @@ -503,8 +501,7 @@ void DifferentialTrajectoryGenerator::wpt_keep_in_lane( size_t index, double center_line_start_x, double center_line_start_y, double center_line_end_x, double center_line_end_y, double tolerance) { path_builder.WptConstraint( - index, trajopt::LaneConstraint{{0.0, 0.0}, - {center_line_start_x, center_line_start_y}, + index, trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, {center_line_end_x, center_line_end_y}, tolerance}); } @@ -608,8 +605,7 @@ void DifferentialTrajectoryGenerator::sgmt_keep_in_lane( double center_line_end_y, double tolerance) { path_builder.SgmtConstraint( from_index, to_index, - trajopt::LaneConstraint{{0.0, 0.0}, - {center_line_start_x, center_line_start_y}, + trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, {center_line_end_x, center_line_end_y}, tolerance}); } From 2fb4ccfc673b86f63bb1fb326302fd549b11cbcb Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 11:46:16 -0700 Subject: [PATCH 08/12] Fix prior commit --- trajoptlib/include/trajopt/constraint/LaneConstraint.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp index 0d423a0e2..fc035b204 100644 --- a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp @@ -22,7 +22,6 @@ class TRAJOPT_DLLEXPORT LaneConstraint { /** * Constructs a LaneConstraint. * - * @param robotPoint Robot point. * @param centerLineStart Start point of the center line. * @param centerLineEnd End point of the center line. * @param tolerance Distance from center line to lane edge. Passing zero @@ -43,7 +42,7 @@ class TRAJOPT_DLLEXPORT LaneConstraint { centerLineEnd + offset, Side::kBelow}; } else { - return PointLineRegionConstraint{robotPoint, centerLineStart, + return PointLineRegionConstraint{{0.0, 0.0}, centerLineStart, centerLineEnd, Side::kOn}; } }()}, From 7ad6d4eb129ce75f2357853f77c61cd112587a1b Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 7 Oct 2024 11:49:53 -0700 Subject: [PATCH 09/12] Clean up notation a bit --- .../trajopt/constraint/LaneConstraint.hpp | 4 +- .../constraint/PointLineRegionConstraint.hpp | 40 +++++++++++-------- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp index fc035b204..9b19a42e4 100644 --- a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp @@ -42,8 +42,8 @@ class TRAJOPT_DLLEXPORT LaneConstraint { centerLineEnd + offset, Side::kBelow}; } else { - return PointLineRegionConstraint{{0.0, 0.0}, centerLineStart, - centerLineEnd, Side::kOn}; + return PointLineRegionConstraint{ + {0.0, 0.0}, centerLineStart, centerLineEnd, Side::kOn}; } }()}, m_bottomLine{[&]() -> std::optional { diff --git a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp index 014645307..3b1dd52d8 100644 --- a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp @@ -68,25 +68,31 @@ class TRAJOPT_DLLEXPORT PointLineRegionConstraint { [[maybe_unused]] const Translation2v& linearAcceleration, [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { auto point = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); - // Determine which side of the start-end field line bumperCorner is on. - // The cross product a x b gives ||a|| ||b|| std::sin(θ), for a and b - // vectors with the same tail. If a x b > 0, b is to the left of a - // b - // ^ - // | - // -----> a + + // Determine which side of the start-end field line a point is on. + // + // The cross product a x b = |a|₂|b|₂sinθ for a and b vectors with the same + // tail. If a x b > 0, b is to the left of a. + // + // b + // ^ + // | + // -----> a + // + // + // If a x b < 0, b is to the right of a. + // + // -----> a + // | + // v + // b // - // If a x b < 0, b is to the right of a - // -----> a - // | - // v - // b + // Let a be the field line start -> end and let b be the point start -> + // point. // - // If a = the field line, start->end - // and b = start->point, then: - // cross > 0 means point is to the left of line (above) - // cross = 0 means point is on line - // cross < 0 means point is to the right of line (below) + // cross > 0 means point is left of line (above) + // cross = 0 means point is on line + // cross < 0 means point is right of line (below) auto line = m_fieldLineEnd - m_fieldLineStart; auto startToPoint = point - m_fieldLineStart; auto cross = line.Cross(startToPoint); From cebfb7ca5b336ba513bc98d4190cd6d4e26c2ad8 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 12:10:11 -0700 Subject: [PATCH 10/12] Remove unused import --- src/document/ConstraintDefinitions.tsx | 1 - 1 file changed, 1 deletion(-) diff --git a/src/document/ConstraintDefinitions.tsx b/src/document/ConstraintDefinitions.tsx index 95f7ee3ca..8989b67fe 100644 --- a/src/document/ConstraintDefinitions.tsx +++ b/src/document/ConstraintDefinitions.tsx @@ -1,5 +1,4 @@ import { - AddRoad, ArrowCircleDown, DoNotDisturb, NearMe, From 09e3d4ebfde3d1abab248e8565261800e81e7275 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 12:13:53 -0700 Subject: [PATCH 11/12] Fix typescript --- src/document/DocumentManager.ts | 3 +-- src/util/ObjectTyped.ts | 34 --------------------------------- 2 files changed, 1 insertion(+), 36 deletions(-) delete mode 100644 src/util/ObjectTyped.ts diff --git a/src/document/DocumentManager.ts b/src/document/DocumentManager.ts index bc3274d1f..44964d62f 100644 --- a/src/document/DocumentManager.ts +++ b/src/document/DocumentManager.ts @@ -13,7 +13,6 @@ import { import { toast } from "react-toastify"; import "react-toastify/dist/ReactToastify.min.css"; import LocalStorageKeys from "../util/LocalStorageKeys"; -import { ObjectTyped } from "../util/ObjectTyped"; import { safeGetIdentifier } from "../util/mobxutils"; import { PplibCommand, @@ -135,7 +134,7 @@ function getConstructors(vars: () => IVariables): EnvConstructors { }); } - const keys = ObjectTyped.keys(ConstraintDefinitions); + const keys = Object.keys(ConstraintDefinitions) as ConstraintKey[]; const constraintDataConstructors = Object.fromEntries( keys.map( (key: K) => diff --git a/src/util/ObjectTyped.ts b/src/util/ObjectTyped.ts deleted file mode 100644 index 9010c3aed..000000000 --- a/src/util/ObjectTyped.ts +++ /dev/null @@ -1,34 +0,0 @@ -/** - * https://github.com/devinrhode2/ObjectTyped - * - * Nicely typed aliases for some `Object` Methods - - * - PSA: Don't mutate `yourObject`s - - * - Numerical keys are BAD, resolve that issue upstream - - * - Discussion: https://stackoverflow.com/a/65117465/565877 - - */ -export type TupleUnion< - U extends string | number | symbol, - R extends any[] = [] -> = { - [S in U]: Exclude extends never - ? [S, ...R] - : TupleUnion, [S, ...R]>; -}[U]; - -export const ObjectTyped = { - /** - - * Object.keys, but with nice typing (`Array`) - - */ - - keys: Object.keys as (yourObject: T) => TupleUnion -}; - -// Built-ins documented here: https://www.typescriptlang.org/docs/handbook/utility-types.html - -export type ValueOf = T[keyof T]; From ac491fc302952278f892bd454dfefa002dc4a62d Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Mon, 7 Oct 2024 12:26:41 -0700 Subject: [PATCH 12/12] Fix rust warning --- src-core/src/generation/transformers/constraints.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 33d92fd72..fa4478647 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -38,7 +38,7 @@ impl ConstraintSetter { // filtered out, save the index guess_points.push(idx); } else { - waypoint_idx.push(w.clone()); + waypoint_idx.push(*w); } });