diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 936f5e5e22..fa4478647d 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -1,6 +1,9 @@ -use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope}; +use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope, Waypoint}; -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; @@ -14,19 +17,30 @@ fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { pub struct ConstraintSetter { guess_points: Vec, - constraint_idx: 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() + 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); + } + }); for constraint in context.params.get_enabled_constraints() { let from = constraint.from.get_idx(num_wpts); @@ -61,7 +75,7 @@ impl ConstraintSetter { from: fixed_from, to: fixed_to, data: constraint.data, - enabled: constraint.enabled + enabled: constraint.enabled, }); } } @@ -70,7 +84,8 @@ impl ConstraintSetter { FeatureLockedTransformer::always(Self { guess_points, - constraint_idx + constraint_idx, + waypoint_idx }) } } @@ -92,7 +107,7 @@ impl SwerveGenerationTransformer for ConstraintSetter { flip, } => match to_opt { None => generator.wpt_point_at(from, x, y, tolerance, flip), - Some(to) => generator.sgmt_point_at(from, to, x, y, tolerance, flip) + Some(to) => generator.sgmt_point_at(from, to, x, y, tolerance, flip), }, ConstraintData::MaxVelocity { max } => match to_opt { None => generator.wpt_linear_velocity_max_magnitude(from, max), @@ -123,13 +138,30 @@ 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 { + tolerance + } => { + 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, + idx_to, + wpt_from.x, + wpt_from.y, + wpt_to.x, + wpt_to.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), } }, + 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), + }, }; } } @@ -150,7 +182,11 @@ impl DifferentialGenerationTransformer for ConstraintSetter { y, tolerance, flip, - } => if to_opt.is_none() { generator.wpt_point_at(from, x, y, tolerance, flip) }, + } => { + if to_opt.is_none() { + generator.wpt_point_at(from, x, y, tolerance, flip) + } + } ConstraintData::MaxVelocity { max } => match to_opt { None => generator.wpt_linear_velocity_max_magnitude(from, max), Some(to) => generator.sgmt_linear_velocity_max_magnitude(from, to, max), @@ -168,7 +204,7 @@ impl DifferentialGenerationTransformer 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), @@ -180,13 +216,30 @@ 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::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), + }, + ConstraintData::KeepInLane { + tolerance + } => { + 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, + idx_to, + wpt_from.x, + wpt_from.y, + wpt_to.x, + wpt_to.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 63f7f67eae..8d68a801d0 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -147,6 +147,8 @@ 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 { tolerance: T }, /// A constraint to contain the bumpers outside a circlular region of the field KeepOutCircle { x: T, y: T, r: T }, } @@ -156,6 +158,7 @@ impl ConstraintData { pub fn scope(&self) -> ConstraintScope { match self { ConstraintData::StopPoint {} => ConstraintScope::Waypoint, + ConstraintData::KeepInLane { tolerance: _ } => ConstraintScope::Segment, _ => ConstraintScope::Both, } } @@ -195,6 +198,9 @@ impl ConstraintData { w: w.snapshot(), h: h.snapshot(), }, + ConstraintData::KeepInLane { tolerance } => ConstraintData::KeepInLane { + tolerance: tolerance.snapshot(), + }, ConstraintData::KeepOutCircle { x, y, r } => ConstraintData::KeepOutCircle { x: x.snapshot(), y: y.snapshot(), diff --git a/src/assets/KeepInLane.tsx b/src/assets/KeepInLane.tsx new file mode 100644 index 0000000000..ef827f6c4f --- /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/FieldConstraintDisplayLayer.tsx b/src/components/field/svg/constraintDisplay/FieldConstraintDisplayLayer.tsx index ca5b9e9be9..5898f5b2f2 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 0000000000..d1fdf1e50c --- /dev/null +++ b/src/components/field/svg/constraintDisplay/KeepInLaneOverlay.tsx @@ -0,0 +1,143 @@ +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.02; +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) { + const dragHandleDrag = d3 + .drag() + .on("drag", (event) => this.dragPointTolerance(event)) + .on("start", () => { + doc.history.startGroup(() => {}); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `#dragTarget-keepInLaneAbove` + ).call(dragHandleDrag); + d3.select( + `#dragTarget-keepInLaneBelow` + ).call(dragHandleDrag); + } + } + dragPointTolerance(event: DragEvent) { + const data = this.props.data; + 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 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 */} + + + + + + + ); + } +} +export default observer(KeepInLaneOverlay); diff --git a/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx b/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx index a0b93e8ea3..8666c821a2 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 c3c9277bfc..8989b67fe8 100644 --- a/src/document/ConstraintDefinitions.tsx +++ b/src/document/ConstraintDefinitions.tsx @@ -9,6 +9,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; @@ -68,6 +69,9 @@ export type ConstraintDataTypeMap = { w: Expr; h: Expr; }; + KeepInLane: { + tolerance: Expr; + }; KeepOutCircle: { x: Expr; y: Expr; @@ -245,6 +249,23 @@ 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 center within a lane", + icon: , + properties: { + tolerance: { + name: "Tolerance", + description: "Robot center max distance from line between waypoints", + dimension: Dimensions.Length, + defaultVal: { exp: "0.01 m", val: 0.01 } + } + }, + wptScope: false, + sgmtScope: true + } satisfies ConstraintDefinition<"KeepInLane">, KeepOutCircle: { type: "KeepOutCircle" as const, name: "Keep Out Circle", diff --git a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp index d2d20cd2be..9b19a42e42 100644 --- a/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LaneConstraint.hpp @@ -22,28 +22,28 @@ 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 * 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(); 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{ - 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}; + return PointLineRegionConstraint{ + {0.0, 0.0}, centerLineStart, centerLineEnd, Side::kOn}; } }()}, m_bottomLine{[&]() -> std::optional { @@ -51,12 +51,13 @@ 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{ - 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 41191822b7..3b1dd52d8e 100644 --- a/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/PointLineRegionConstraint.hpp @@ -67,44 +67,45 @@ 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 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 + // + // Let a be the field line start -> end and let b be the point start -> + // point. + // + // 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); 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 f57057851f..4f4fe119f0 100644 --- a/trajoptlib/src/RustFFI.cpp +++ b/trajoptlib/src/RustFFI.cpp @@ -162,16 +162,10 @@ 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{{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 +275,11 @@ 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{{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 +500,10 @@ 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{{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 +603,11 @@ 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{{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,