From c8fa3739faf0243727d7f5f2718b289ac09cc3b1 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 17:31:28 -0400 Subject: [PATCH 1/6] global constraints unlimited check --- lib/path/path_constraints.dart | 12 ++- .../tree_widgets/global_constraints_tree.dart | 94 +++++++++++++------ .../global_constraints_tree_test.dart | 46 ++++++++- 3 files changed, 120 insertions(+), 32 deletions(-) diff --git a/lib/path/path_constraints.dart b/lib/path/path_constraints.dart index 18a297ed..07d5c4d1 100644 --- a/lib/path/path_constraints.dart +++ b/lib/path/path_constraints.dart @@ -3,19 +3,22 @@ class PathConstraints { num maxAccelerationMPSSq; num maxAngularVelocityDeg; num maxAngularAccelerationDeg; + bool unlimited; PathConstraints({ this.maxVelocityMPS = 3, this.maxAccelerationMPSSq = 3, this.maxAngularVelocityDeg = 540, this.maxAngularAccelerationDeg = 720, + this.unlimited = false, }); PathConstraints.fromJson(Map json) : maxVelocityMPS = json['maxVelocity'] ?? 3, maxAccelerationMPSSq = json['maxAcceleration'] ?? 3, maxAngularVelocityDeg = json['maxAngularVelocity'] ?? 540, - maxAngularAccelerationDeg = json['maxAngularAcceleration'] ?? 720; + maxAngularAccelerationDeg = json['maxAngularAcceleration'] ?? 720, + unlimited = json['unlimited'] ?? false; PathConstraints clone() { return PathConstraints( @@ -23,6 +26,7 @@ class PathConstraints { maxAccelerationMPSSq: maxAccelerationMPSSq, maxAngularVelocityDeg: maxAngularVelocityDeg, maxAngularAccelerationDeg: maxAngularAccelerationDeg, + unlimited: unlimited, ); } @@ -32,6 +36,7 @@ class PathConstraints { 'maxAcceleration': maxAccelerationMPSSq, 'maxAngularVelocity': maxAngularVelocityDeg, 'maxAngularAcceleration': maxAngularAccelerationDeg, + 'unlimited': unlimited, }; } @@ -42,9 +47,10 @@ class PathConstraints { other.maxVelocityMPS == maxVelocityMPS && other.maxAccelerationMPSSq == maxAccelerationMPSSq && other.maxAngularVelocityDeg == maxAngularVelocityDeg && - other.maxAngularAccelerationDeg == other.maxAngularAccelerationDeg; + other.maxAngularAccelerationDeg == other.maxAngularAccelerationDeg && + other.unlimited == unlimited; @override int get hashCode => Object.hash(maxVelocityMPS, maxAccelerationMPSSq, - maxAngularVelocityDeg, maxAngularAccelerationDeg); + maxAngularVelocityDeg, maxAngularAccelerationDeg, unlimited); } diff --git a/lib/widgets/editor/tree_widgets/global_constraints_tree.dart b/lib/widgets/editor/tree_widgets/global_constraints_tree.dart index 5d2f4dda..a6c4068b 100644 --- a/lib/widgets/editor/tree_widgets/global_constraints_tree.dart +++ b/lib/widgets/editor/tree_widgets/global_constraints_tree.dart @@ -40,7 +40,8 @@ class GlobalConstraintsTree extends StatelessWidget { child: NumberTextField( initialValue: path.globalConstraints.maxVelocityMPS, label: 'Max Velocity (M/S)', - enabled: !path.useDefaultConstraints, + enabled: !path.useDefaultConstraints && + !path.globalConstraints.unlimited, minValue: 0.1, onSubmitted: (value) { if (value != null) { @@ -55,7 +56,8 @@ class GlobalConstraintsTree extends StatelessWidget { child: NumberTextField( initialValue: path.globalConstraints.maxAccelerationMPSSq, label: 'Max Acceleration (M/S²)', - enabled: !path.useDefaultConstraints, + enabled: !path.useDefaultConstraints && + !path.globalConstraints.unlimited, minValue: 0.1, onSubmitted: (value) { if (value != null) { @@ -78,7 +80,8 @@ class GlobalConstraintsTree extends StatelessWidget { initialValue: path.globalConstraints.maxAngularVelocityDeg, label: 'Max Angular Velocity (Deg/S)', arrowKeyIncrement: 1.0, - enabled: !path.useDefaultConstraints, + enabled: !path.useDefaultConstraints && + !path.globalConstraints.unlimited, minValue: 0.1, onSubmitted: (value) { if (value != null) { @@ -95,7 +98,8 @@ class GlobalConstraintsTree extends StatelessWidget { path.globalConstraints.maxAngularAccelerationDeg, label: 'Max Angular Acceleration (Deg/S²)', arrowKeyIncrement: 1.0, - enabled: !path.useDefaultConstraints, + enabled: !path.useDefaultConstraints && + !path.globalConstraints.unlimited, minValue: 0.1, onSubmitted: (value) { if (value != null) { @@ -113,31 +117,67 @@ class GlobalConstraintsTree extends StatelessWidget { padding: const EdgeInsets.symmetric(horizontal: 6.0), child: Row( children: [ - Checkbox( - value: path.useDefaultConstraints, - onChanged: (value) { - undoStack.add(Change( - ( - path.useDefaultConstraints, - path.globalConstraints.clone() + Expanded( + child: Row( + children: [ + Checkbox( + value: path.useDefaultConstraints, + onChanged: path.globalConstraints.unlimited + ? null + : (value) { + undoStack.add(Change( + ( + path.useDefaultConstraints, + path.globalConstraints.clone() + ), + () { + path.useDefaultConstraints = value ?? false; + path.globalConstraints = + defaultConstraints.clone(); + onPathChanged?.call(); + }, + (oldValue) { + path.useDefaultConstraints = oldValue.$1; + path.globalConstraints = oldValue.$2.clone(); + onPathChanged?.call(); + }, + )); + }, + ), + const SizedBox(width: 4), + const Text( + 'Use Default Constraints', + style: TextStyle(fontSize: 15), ), - () { - path.useDefaultConstraints = value ?? false; - path.globalConstraints = defaultConstraints.clone(); - onPathChanged?.call(); - }, - (oldValue) { - path.useDefaultConstraints = oldValue.$1; - path.globalConstraints = oldValue.$2.clone(); - onPathChanged?.call(); - }, - )); - }, + ], + ), ), - const SizedBox(width: 4), - const Text( - 'Use Default Constraints', - style: TextStyle(fontSize: 15), + Expanded( + child: Row( + children: [ + Checkbox( + value: path.globalConstraints.unlimited, + onChanged: (value) { + undoStack.add(Change( + path.globalConstraints.unlimited, + () { + path.globalConstraints.unlimited = value ?? false; + onPathChanged?.call(); + }, + (oldValue) { + path.globalConstraints.unlimited = oldValue; + onPathChanged?.call(); + }, + )); + }, + ), + const SizedBox(width: 4), + const Text( + 'Unlimited', + style: TextStyle(fontSize: 15), + ), + ], + ), ), ], ), diff --git a/test/widgets/editor/tree_widgets/global_constraints_tree_test.dart b/test/widgets/editor/tree_widgets/global_constraints_tree_test.dart index 19068a30..d8ad9ec3 100644 --- a/test/widgets/editor/tree_widgets/global_constraints_tree_test.dart +++ b/test/widgets/editor/tree_widgets/global_constraints_tree_test.dart @@ -25,11 +25,14 @@ void main() { maxAccelerationMPSSq: 1.0, maxAngularVelocityDeg: 1.0, maxAngularAccelerationDeg: 1.0, + unlimited: false, ); pathChanged = false; }); testWidgets('tapping expands/collapses tree', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + path.globalConstraintsExpanded = false; await widgetTester.pumpWidget(MaterialApp( home: Scaffold( @@ -57,6 +60,8 @@ void main() { }); testWidgets('max vel text field', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + await widgetTester.pumpWidget(MaterialApp( home: Scaffold( body: GlobalConstraintsTree( @@ -86,6 +91,8 @@ void main() { }); testWidgets('max accel text field', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + await widgetTester.pumpWidget(MaterialApp( home: Scaffold( body: GlobalConstraintsTree( @@ -115,6 +122,8 @@ void main() { }); testWidgets('max ang vel text field', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + await widgetTester.pumpWidget(MaterialApp( home: Scaffold( body: GlobalConstraintsTree( @@ -144,6 +153,8 @@ void main() { }); testWidgets('max ang accel text field', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + await widgetTester.pumpWidget(MaterialApp( home: Scaffold( body: GlobalConstraintsTree( @@ -173,6 +184,8 @@ void main() { }); testWidgets('use defaults checkbox', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + await widgetTester.pumpWidget(MaterialApp( home: Scaffold( body: GlobalConstraintsTree( @@ -186,9 +199,9 @@ void main() { final check = find.byType(Checkbox); - expect(check, findsOneWidget); + expect(check, findsNWidgets(2)); - await widgetTester.tap(check); + await widgetTester.tap(check.first); await widgetTester.pump(); expect(pathChanged, true); @@ -205,4 +218,33 @@ void main() { maxAngularAccelerationDeg: 1.0, )); }); + + testWidgets('unlimited checkbox', (widgetTester) async { + await widgetTester.binding.setSurfaceSize(const Size(1280, 800)); + + await widgetTester.pumpWidget(MaterialApp( + home: Scaffold( + body: GlobalConstraintsTree( + path: path, + onPathChanged: () => pathChanged = true, + undoStack: undoStack, + defaultConstraints: PathConstraints(), + ), + ), + )); + + final check = find.byType(Checkbox); + + expect(check, findsNWidgets(2)); + + await widgetTester.tap(check.last); + await widgetTester.pump(); + + expect(pathChanged, true); + expect(path.globalConstraints.unlimited, isTrue); + + undoStack.undo(); + await widgetTester.pump(); + expect(path.globalConstraints.unlimited, isFalse); + }); } From 4a372d4a760dffd462119cca3f12fc5cf5ab1888 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 17:35:19 -0400 Subject: [PATCH 2/6] generate with unlimited constraints --- lib/path/pathplanner_path.dart | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/lib/path/pathplanner_path.dart b/lib/path/pathplanner_path.dart index 35bdd930..fdbd5623 100644 --- a/lib/path/pathplanner_path.dart +++ b/lib/path/pathplanner_path.dart @@ -364,6 +364,18 @@ class PathPlannerPath { return z.constraints; } } + + // Check if the constraints should be unlimited + if (globalConstraints.unlimited) { + return PathConstraints( + maxVelocityMPS: double.infinity, + maxAccelerationMPSSq: double.infinity, + maxAngularVelocityDeg: double.infinity, + maxAngularAccelerationDeg: double.infinity, + unlimited: true, + ); + } + return globalConstraints; } From d5ad0ceb9d687315375523a98b9ce0bc712796ee Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 18:05:35 -0400 Subject: [PATCH 3/6] [java] support unlimited global constraints --- .../tree_widgets/constraint_zones_tree.dart | 4 +- .../pathplanner/lib/path/PathConstraints.java | 49 ++++++++++++++++++- .../pathplanner/lib/path/PathPlannerPath.java | 37 +++----------- .../lib/trajectory/PathPlannerTrajectory.java | 2 +- 4 files changed, 59 insertions(+), 33 deletions(-) diff --git a/lib/widgets/editor/tree_widgets/constraint_zones_tree.dart b/lib/widgets/editor/tree_widgets/constraint_zones_tree.dart index 8256f676..802657ad 100644 --- a/lib/widgets/editor/tree_widgets/constraint_zones_tree.dart +++ b/lib/widgets/editor/tree_widgets/constraint_zones_tree.dart @@ -65,8 +65,10 @@ class _ConstraintZonesTreeState extends State { widget.undoStack.add(Change( PathPlannerPath.cloneConstraintZones(constraintZones), () { + final constraints = widget.path.globalConstraints.clone(); + constraints.unlimited = false; constraintZones.add(ConstraintsZone.defaultZone( - constraints: widget.path.globalConstraints.clone(), + constraints: constraints, )); widget.onPathChangedNoSim?.call(); }, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java index e0bc9d5c..0411cb61 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java @@ -10,12 +10,52 @@ * @param maxAccelerationMps Max linear acceleration (M/S^2) * @param maxAngularVelocityRps Max angular velocity (Rad/S) * @param maxAngularAccelerationRpsSq Max angular acceleration (Rad/S^2) + * @param unlimited Should the constraints be unlimited */ public record PathConstraints( double maxVelocityMps, double maxAccelerationMps, double maxAngularVelocityRps, - double maxAngularAccelerationRpsSq) { + double maxAngularAccelerationRpsSq, + boolean unlimited) { + private static final PathConstraints kUnlimited = + new PathConstraints( + Double.POSITIVE_INFINITY, + Double.POSITIVE_INFINITY, + Double.POSITIVE_INFINITY, + Double.POSITIVE_INFINITY, + true); + + /** + * Kinematic path following constraints + * + * @param maxVelocityMps Max linear velocity (M/S) + * @param maxAccelerationMps Max linear acceleration (M/S^2) + * @param maxAngularVelocityRps Max angular velocity (Rad/S) + * @param maxAngularAccelerationRpsSq Max angular acceleration (Rad/S^2) + */ + public PathConstraints( + double maxVelocityMps, + double maxAccelerationMps, + double maxAngularVelocityRps, + double maxAngularAccelerationRpsSq) { + this( + maxVelocityMps, + maxAccelerationMps, + maxAngularVelocityRps, + maxAngularAccelerationRpsSq, + false); + } + + /** + * Get unlimited PathConstraints + * + * @return Unlimited constraints + */ + public static PathConstraints unlimitedConstraints() { + return kUnlimited; + } + /** * Create a path constraints object from json * @@ -30,11 +70,16 @@ static PathConstraints fromJson(JSONObject constraintsJson) { ((Number) constraintsJson.get("maxAngularVelocity")).doubleValue(); // Degrees double maxAngularAccel = ((Number) constraintsJson.get("maxAngularAcceleration")).doubleValue(); // Degrees + boolean unlimited = false; + if (constraintsJson.containsKey("unlimited")) { + unlimited = ((boolean) constraintsJson.get("unlimited")); + } return new PathConstraints( maxVel, maxAccel, Units.degreesToRadians(maxAngularVel), - Units.degreesToRadians(maxAngularAccel)); + Units.degreesToRadians(maxAngularAccel), + unlimited); } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 04b4d3c8..1699c811 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -404,12 +404,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) // Add the full path to the cache PathPlannerPath fullPath = new PathPlannerPath(); - fullPath.globalConstraints = - new PathConstraints( - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY); + fullPath.globalConstraints = PathConstraints.unlimitedConstraints(); fullPath.goalEndState = new GoalEndState( fullTrajStates.get(fullTrajStates.size() - 1).linearVelocity, @@ -457,12 +452,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) } PathPlannerPath path = new PathPlannerPath(); - path.globalConstraints = - new PathConstraints( - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY, - Double.POSITIVE_INFINITY); + path.globalConstraints = PathConstraints.unlimitedConstraints(); path.goalEndState = new GoalEndState( states.get(states.size() - 1).linearVelocity, @@ -652,26 +642,18 @@ public Pose2d getStartingDifferentialPose() { return new Pose2d(startPos, heading); } - /** - * Get the constraints for a point along the path - * - * @param idx Index of the point to get constraints for - * @return The constraints that should apply to the point - */ - public PathConstraints getConstraintsForPoint(int idx) { - if (getPoint(idx).constraints != null) { - return getPoint(idx).constraints; - } - - return globalConstraints; - } - private PathConstraints constraintsForWaypointPos(double pos) { for (ConstraintsZone z : constraintZones) { if (pos >= z.minPosition() && pos <= z.maxPosition()) { return z.constraints(); } } + + // Check if constraints should be unlimited + if (globalConstraints.unlimited()) { + return PathConstraints.unlimitedConstraints(); + } + return globalConstraints; } @@ -915,9 +897,6 @@ private void precalcValues() { if (numPoints() > 0) { for (int i = 0; i < allPoints.size(); i++) { PathPoint point = allPoints.get(i); - if (point.constraints == null) { - point.constraints = globalConstraints; - } double curveRadius = getCurveRadiusAtPoint(i, allPoints); if (Double.isFinite(curveRadius)) { diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index b8dee1a8..9df86bed 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java @@ -219,7 +219,7 @@ private static void generateStates( Pose2d robotPose = new Pose2d(p.position, holonomicRot); var state = new PathPlannerTrajectoryState(); state.pose = robotPose; - state.constraints = path.getConstraintsForPoint(i); + state.constraints = p.constraints; state.waypointRelativePos = p.waypointRelativePos; // Calculate robot heading From 93e4b6ebef29f58e1c2b3e24ccd2ef5565de59a6 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 18:39:51 -0400 Subject: [PATCH 4/6] [cpp] support unlimited global constraints --- .../pathplanner/lib/path/PathConstraints.cpp | 6 +++- .../pathplanner/lib/path/PathPlannerPath.cpp | 29 ++++--------------- .../lib/trajectory/PathPlannerTrajectory.cpp | 3 +- .../pathplanner/lib/path/PathConstraints.h | 25 +++++++++++++--- .../pathplanner/lib/path/PathPlannerPath.h | 16 ++++------ 5 files changed, 39 insertions(+), 40 deletions(-) diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathConstraints.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathConstraints.cpp index f1fea73c..b8fd78b3 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathConstraints.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathConstraints.cpp @@ -12,6 +12,10 @@ PathConstraints PathConstraints::fromJson(const wpi::json &json) { json.at("maxAngularVelocity").get()); auto maxAngAccel = units::degrees_per_second_squared_t( json.at("maxAngularAcceleration").get()); + bool unlimited = false; + if (json.contains("unlimited")) { + unlimited = json.at("unlimited").get(); + } - return PathConstraints(maxVel, maxAccel, maxAngVel, maxAngAccel); + return PathConstraints(maxVel, maxAccel, maxAngVel, maxAngAccel, unlimited); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 060911bc..2020f033 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -219,14 +218,7 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( // Add the full path to the cache auto fullPath = std::make_shared < PathPlannerPath - > (PathConstraints( - units::meters_per_second_t { - std::numeric_limits::infinity() }, - units::meters_per_second_squared_t { std::numeric_limits< - double>::infinity() }, units::radians_per_second_t { - std::numeric_limits::infinity() }, - units::radians_per_second_squared_t { std::numeric_limits< - double>::infinity() }), GoalEndState( + > (PathConstraints::unlimitedConstraints(), GoalEndState( fullTrajStates[fullTrajStates.size() - 1].linearVelocity, fullTrajStates[fullTrajStates.size() - 1].pose.Rotation())); @@ -277,18 +269,10 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( } } - auto path = - std::make_shared < PathPlannerPath - > (PathConstraints(units::meters_per_second_t { - std::numeric_limits::infinity() }, - units::meters_per_second_squared_t { - std::numeric_limits::infinity() }, - units::radians_per_second_t { - std::numeric_limits::infinity() }, - units::radians_per_second_squared_t { - std::numeric_limits::infinity() }), GoalEndState( - states[states.size() - 1].linearVelocity, - states[states.size() - 1].pose.Rotation())); + auto path = std::make_shared < PathPlannerPath + > (PathConstraints::unlimitedConstraints(), GoalEndState( + states[states.size() - 1].linearVelocity, + states[states.size() - 1].pose.Rotation())); std::vector < PathPoint > pathPoints; for (auto state : states) { @@ -621,9 +605,6 @@ void PathPlannerPath::precalcValues() { for (size_t i = 0; i < m_allPoints.size(); i++) { PathConstraints constraints = m_allPoints[i].constraints.value_or( m_globalConstraints); - if (!m_allPoints[i].constraints) { - m_allPoints[i].constraints = m_globalConstraints; - } units::meter_t curveRadius = units::math::abs( getCurveRadiusAtPoint(i, m_allPoints)); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp index 976090de..642887d7 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp @@ -247,7 +247,8 @@ void PathPlannerTrajectory::generateStates( frc::Pose2d robotPose(p.position, holonomicRot); PathPlannerTrajectoryState state; state.pose = robotPose; - state.constraints = path->getConstraintsForPoint(i); + state.constraints = p.constraints.value_or( + path->getGlobalConstraints()); state.waypointRelativePos = p.waypointRelativePos; // Calculate robot heading diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathConstraints.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathConstraints.h index 00647dd6..f2932c4c 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathConstraints.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathConstraints.h @@ -5,6 +5,7 @@ #include #include #include +#include namespace pathplanner { class PathConstraints { @@ -16,13 +17,15 @@ class PathConstraints { * @param maxAccel Max linear acceleration (M/S^2) * @param maxAngularVel Max angular velocity (Deg/S) * @param maxAngularAccel Max angular acceleration (Deg/S^2) + * @param unlimited Should the constraints be unlimited */ constexpr PathConstraints(units::meters_per_second_t maxVel, units::meters_per_second_squared_t maxAccel, units::radians_per_second_t maxAngularVel, - units::radians_per_second_squared_t maxAngularAccel) : m_maxVelocity( - maxVel), m_maxAcceleration(maxAccel), m_maxAngularVelocity( - maxAngularVel), m_maxAngularAcceleration(maxAngularAccel) { + units::radians_per_second_squared_t maxAngularAccel, + bool unlimited = false) : m_maxVelocity(maxVel), m_maxAcceleration( + maxAccel), m_maxAngularVelocity(maxAngularVel), m_maxAngularAcceleration( + maxAngularAccel), m_unlimited(unlimited) { } /** @@ -33,6 +36,14 @@ class PathConstraints { */ static PathConstraints fromJson(const wpi::json &json); + static constexpr PathConstraints unlimitedConstraints() { + double inf = std::numeric_limits::infinity(); + return PathConstraints(units::meters_per_second_t { inf }, + units::meters_per_second_squared_t { inf }, + units::radians_per_second_t { inf }, + units::radians_per_second_squared_t { inf }, true); + } + /** * Get the max linear velocity * @@ -69,6 +80,10 @@ class PathConstraints { return m_maxAngularAcceleration; } + constexpr bool isUnlimited() const { + return m_unlimited; + } + bool operator==(const PathConstraints &other) const { return std::abs(m_maxVelocity() - other.m_maxVelocity()) < 1E-9 && std::abs(m_maxAcceleration() - other.m_maxAcceleration()) @@ -78,7 +93,8 @@ class PathConstraints { < 1E-9 && std::abs( m_maxAngularAcceleration() - - other.m_maxAngularAcceleration()) < 1E-9; + - other.m_maxAngularAcceleration()) < 1E-9 + && m_unlimited == other.m_unlimited; } private: @@ -86,5 +102,6 @@ class PathConstraints { units::meters_per_second_squared_t m_maxAcceleration; units::radians_per_second_t m_maxAngularVelocity; units::radians_per_second_squared_t m_maxAngularAcceleration; + bool m_unlimited; }; } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h index ab907e79..a655271e 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -146,16 +146,6 @@ class PathPlannerPath: public std::enable_shared_from_this { */ frc::Pose2d getStartingDifferentialPose(); - /** - * Get the constraints for a point along the path - * - * @param idx Index of the point to get constraints for - * @return The constraints that should apply to the point - */ - inline PathConstraints getConstraintsForPoint(size_t idx) { - return getPoint(idx).constraints.value_or(m_globalConstraints); - } - /** * Create a path planner path from pre-generated path points. This is used internally, and you * likely should not use this @@ -366,6 +356,12 @@ class PathPlannerPath: public std::enable_shared_from_this { return z.getConstraints(); } } + + // Check if constraints should be unlimited + if (m_globalConstraints.isUnlimited()) { + return PathConstraints::unlimitedConstraints(); + } + return m_globalConstraints; } From 5d88fd4ed595bd4159d0005136341a677cf66b65 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 18:45:59 -0400 Subject: [PATCH 5/6] [python] support unlimited global constraints --- pathplannerlib-python/pathplannerlib/path.py | 55 +++++++++---------- .../pathplannerlib/trajectory.py | 2 +- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 285e6c1a..985d56c4 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -34,11 +34,13 @@ class PathConstraints: maxAccelerationMpsSq (float): Max linear acceleration (M/S^2) maxAngularVelocityRps (float): Max angular velocity (Rad/S) maxAngularAccelerationRpsSq (float): Max angular acceleration (Rad/S^2) + unlimited (bool): Should the constraints be unlimited """ maxVelocityMps: float maxAccelerationMpsSq: float maxAngularVelocityRps: float maxAngularAccelerationRpsSq: float + unlimited: bool = False @staticmethod def fromJson(json_dict: dict) -> PathConstraints: @@ -46,19 +48,32 @@ def fromJson(json_dict: dict) -> PathConstraints: maxAccel = float(json_dict['maxAcceleration']) maxAngularVel = float(json_dict['maxAngularVelocity']) maxAngularAccel = float(json_dict['maxAngularAcceleration']) + unlimited = bool(json_dict['unlimited']) if 'unlimited' in json_dict else False return PathConstraints( maxVel, maxAccel, units.degreesToRadians(maxAngularVel), - units.degreesToRadians(maxAngularAccel)) + units.degreesToRadians(maxAngularAccel), + unlimited) + + @staticmethod + def unlimitedConstraints() -> PathConstraints: + return PathConstraints( + float('inf'), + float('inf'), + float('inf'), + float('inf'), + True + ) def __eq__(self, other): return (isinstance(other, PathConstraints) and other.maxVelocityMps == self.maxVelocityMps and other.maxAccelerationMpsSq == self.maxAccelerationMpsSq and other.maxAngularVelocityRps == self.maxAngularVelocityRps - and other.maxAngularAccelerationRpsSq == self.maxAngularAccelerationRpsSq) + and other.maxAngularAccelerationRpsSq == self.maxAngularAccelerationRpsSq + and other.unlimited == self.unlimited) @dataclass(frozen=True) @@ -543,12 +558,9 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None: fullEvents.sort(key=lambda e: e.getTimestamp()) # Add the full path to the cache - fullPath = PathPlannerPath([], PathConstraints( - float('inf'), - float('inf'), - float('inf'), - float('inf') - ), None, GoalEndState(fullTrajStates[-1].linearVelocity, fullTrajStates[-1].pose.rotation())) + fullPath = PathPlannerPath([], PathConstraints.unlimitedConstraints(), None, + GoalEndState(fullTrajStates[-1].linearVelocity, + fullTrajStates[-1].pose.rotation())) fullPathPoints = [PathPoint(state.pose.translation()) for state in fullTrajStates] fullPath._allPoints = fullPathPoints fullPath._isChoreoPath = True @@ -580,12 +592,8 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None: if startTime <= originalEvent.getTimestamp() < endTime: events.append(originalEvent.copyWithTime(originalEvent.getTimestamp() - startTime)) - path = PathPlannerPath([], PathConstraints( - float('inf'), - float('inf'), - float('inf'), - float('inf') - ), None, GoalEndState(states[-1].linearVelocity, states[-1].pose.rotation())) + path = PathPlannerPath([], PathConstraints.unlimitedConstraints(), None, + GoalEndState(states[-1].linearVelocity, states[-1].pose.rotation())) pathPoints = [PathPoint(state.pose.translation()) for state in states] path._allPoints = pathPoints path._isChoreoPath = True @@ -662,18 +670,6 @@ def getPoint(self, index: int) -> PathPoint: """ return self._allPoints[index] - def getConstraintsForPoint(self, idx: int) -> PathConstraints: - """ - Get the constraints for a point along the path - - :param idx: Index of the point to get constraints for - :return: The constraints that should apply to the point - """ - if self.getPoint(idx).constraints is None: - return self.getPoint(idx).constraints - - return self._globalConstraints - def getGlobalConstraints(self) -> PathConstraints: """ Get the global constraints for this path @@ -859,6 +855,11 @@ def _constraintsForWaypointPos(self, pos: float) -> PathConstraints: for z in self._constraintZones: if z.minWaypointPos <= pos <= z.maxWaypointPos: return z.constraints + + # Check if constraints should be unlimited + if self._globalConstraints.unlimited: + return PathConstraints.unlimitedConstraints() + return self._globalConstraints def _pointZoneForWaypointPos(self, pos: float) -> Union[PointTowardsZone, None]: @@ -1079,8 +1080,6 @@ def _precalcValues(self) -> None: if self.numPoints() > 0: for i in range(self.numPoints()): point = self.getPoint(i) - if point.constraints is None: - point.constraints = self._globalConstraints curveRadius = self._getCurveRadiusAtPoint(i) if math.isfinite(curveRadius): diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 14d4b451..f7789496 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -422,7 +422,7 @@ def _generateStates(states: List[PathPlannerTrajectoryState], path: PathPlannerP robotPose = Pose2d(p.position, holonomicRot) state = PathPlannerTrajectoryState() state.pose = robotPose - state.constraints = path.getConstraintsForPoint(i) + state.constraints = p.constraints state.waypointRelativePos = p.waypointRelativePos # Calculate robot heading From cc3b563fc6d1aa9ae7a9c97457172a4980c4f543 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 8 Oct 2024 18:50:40 -0400 Subject: [PATCH 6/6] document unlimited constraints --- Writerside/images/global_constraints_tree.png | Bin 9919 -> 10080 bytes .../topics/gui-Editing-Paths-and-Autos.md | 3 +++ Writerside/topics/gui-Settings.md | 4 ++++ 3 files changed, 7 insertions(+) diff --git a/Writerside/images/global_constraints_tree.png b/Writerside/images/global_constraints_tree.png index 62a1d382075ac109c7a0c510ce8138bd988ffaa7..73a327f974731cf8e036070a5941e489b1e4e9a7 100644 GIT binary patch literal 10080 zcmb7qcUV(jv#tdzAkv$FB8W7RCLm3ucL8aEfRsoNBE3dHYADjB_a;qBr~)b_LMT!U zC4|re1dz}n1aA0!-+j)x&wb9h_xzD$?>u|WT5D$QnfINUs8>3wl($)KU%GUOQe90+ z|I($)W29^N&FiG^mqRVdq@T;~`l>H4RS(@;Bi)eMDQGKPx>Oraerk1%bbsrenyLGx zOLw|1K9}LHWww_tJ=#}SQh4oSv5BVqWVoD-#YbdYHL0|&{kTo8QCP&nO76+{!10mU z?FQRt1&@VA6`!U)0%&B#7)A-DeEFQQs+GuhLp)uWIW$=%R>;cB#^cqLk}tKBX!tKg zC4l}_$nD4N1N~RfMn)PY9i_wPJA3n4{zL7LWaRiJrbk{`NvTLjqlA-_GdNj`baRC_JvuUyOkaean)+6eEgvT*jauJpB_*y; ziRF=zYWKMfsHqcPbe##^Cr&_XJQ;;fqgiE7; z(dQlqGmm+pq}gmHc}Raao~?dt^idLwz++;_WlI_o*wQu+wNuK4oSqra+-SeC z&_a^1@04NNx^)bnsrGK{wHaz}1!eoYqk4TR@fPO0A%qndOe8$o4Bu_D6}*gCs67#E zIcv4&Sh4iblPUJqyS|@8MV%m4%rn+wOoS0KrHy-R6uaU!&)3heC+F<(`%+H5_atEM zMluZHAK`;j=TBWE>zAYx=POqW1`Ynf^y7`liL<{p>fhB zKdhI{Q%M4%NsN^SqC_DWg zv9ND^>?c=|C#+$VyF73^{7*s0Gt1=YOjJ`F=ujxWbUe<^D@cM*@TWi@$f4?ZLn`mQ zoF4aidSsqGXlcYp!_xs}n=zboJepAt8jVW=o$gQKg|jutopW45t@^|Ek1>7x{N0q^1uW_q0+IItD*C{Ak>lI0PON_Z7bZd`y8kUBQhqe+)dURqbcXE z;bA|{RM0N-Z^O6^pU2-Qv^u-r1(!Z^7c);4wgJq{w)-#E%FTQwBBOFQ67A8S0#3WX z52o9u1|X7Io6*e*O(KW{F^aKMqn&WfHyT)hquWBWn;xSpHP9Waq}~tN`t@{dNEStkH^OfbNiGW zPd`pipTPBuaWAI!`gc=^$3l#!rhE0yi}3jPh?8xtS~WyB0PG<2{nLPb*omK?-$K4k zv#&et;W027DGsP85YkD=Qqxx_9CAoLxm>&{pF-W`XHQ>;fqBJubh=u~j9be7A2XkL zL;RJK1xT8Ckbkx?bz{bc3a#f$U8Xk~N7M^1JwwTkG=xEd4EDU-rtEvTl4jp5sK_~t z_9*~k&6-@GiK;LL&xgOGZE_vTla3@)KLtK66mZl#ID`J$rac`j~LQ-IO>74EJ*9}BGBnFV*Aq0fj zZU0StJKx*4+uFbgj#l%J`eosz?sYy|ye_W_k^+3{TRtVu^Ni%_L}Y+&>{b#Ovf zLvsixgr27u;#AURsQ&Tj4T4iB5OWN8m3F|~&pOHxU zUvF~Vh5N%LFPZ(QCO)Ki3xCe{lY zxK5O`!)}|U1s*NZg}c;I(xeR9eg11Cy(Px=7$V)^Ok@l;;Q+lVB8Lr-=2Q;ur^Lbf zb^kT(#(6F;WTY%czWW8E@US__l{<=Wn3W96?RTx}UJswk4dqXWj#O(84@8jMx$W23 zGP?_}AFA+wd-wlwIQV}Lu>E*4%2?wTFSp~fXZo%$#OI6H&wm2Zf)zD2bFkW%#ok!= z!+^hAKGDfl^MYfjPR|H4FU*clWoKrGm-}lQzNAXg zDgWL9`=hO53^aAoX8F5riKctm;q%)+p0NhTid%OkJ#P9&P;EDnIK0E!*sNP{Z>&G@ z@3gnQCm^dd1(9x-MbE8orqeHRAN|dyH>TWNtivI@6s|artOnvrys{YL*}583CVGQT ze}``MS5!yBPoA9lwZ2ySz@sfQg${x@E%3t9*ieD4!)@T*RBgSy_Jfm(k&a`6-EuxS zcSZQF@ap38aKrU*sO`p+migB38F@*@Zr;N?UG03P7BRY?pFUv6#giTHjp?c)%-`dS zQzSbHIcsno3kNjM>SW_n|5l)9<}ZP2NLW(8J964w$15E|}GkxXKZv*^ED z(bW!8?AV_RE;~rj9mL3)tfa11f!u&C$L9SQ)kVvlGQ7|#}nr?!24wD1)U}{ zHN~Kt7Bplqp)9}ka=p=~6moIQ!~m#jrX4InqKl zImx(R30>gxGa~qK3k^m!R)Hs~v+-Fc6kZe|Ot${!HkKCe6=n9BAgnOsILU%Vz^(tI?W0+Hz!UZ#9fYzxF$q7|Z1~!~<<#2z}W~IgMOAwlZAgIfM zQfI%>m$G>>$v}X&>?Ck)&i49E+8qe`>hAsp_8v@hG7Q|}$(pMsodustsbu=lV(u|b zAz~$}>2KiSsHRY_!AW6{vLG}GuKp27)+>)UXYz7;r{|vk-s*j=+vU>y-sKxY!D~18 z7;`;w>O-)TLGe6QIa@UHwwfOfBWP+ZDH8?j1!9Ya8VB zax|%%w2KU-CBNR^Yl}B|(_$OekYRgDYu2Z#nL775&CvAPVY%3hYbCt~ch>cnG@z`2 zdt`i9yPw0mMgH8|kh)8C>!r=zAANgS?nAPz`1lzi$T8GM<5@^dlGdQO?-=%H6M#Lp zlEsugdUn@ zia1XkQVFPo&4nn)aARIYE96|O18X|Ie#2bDNG~V<_PB_9rvMSGbTUwXeos)5|H*5B zt3JD5{^&c`H|*F*A8qOD{gQ%xEOETY0WwjCyj(z8;Ybp&2s}xzZYRnWHmL&} zinu{*KK?snC8a$!83l-Afjg&0qNCOmT#LEUyir2@4t7?@j`XHm)XBnC<6ITa1k_S9 zQ9~p)>f7}f$cNl(2AUKZdA8HDvs+$UGi!1uWhR(9PiWDfFU^u_kBkcU%}is)ddP5& z{nj{qTgHn+@~W3+5ae^8XWe1Wh~Kh~>gl=WtIvd{TpVvcN6rspAE|`Y7!}%@k-f}t z1gU<|8`Em^nW91&%Z-}|-dtNaK$wK53aSnp*kK!d1W+XXvxB%6@9Z&{YDP&e0bnjB zVM0#Nh1QSpzubR_DW_c(b_V`bcc^tr#Or#0Ik&bFOhk9d`6ZN%b1gEzmP}MnB`zFS zZZgIfpJL0^9d4P%8H-)l{%!f9^<>>?=49Uq9mgiUz?&HMJ)a@VJ7M>VVb$gS*oe#z z7@A>&O=WWCfNyte-_0*?@yPp1<|OI}OUjI7EHOL72nzb`hY?&Be2P0#b$b^=Um=ni zIq153egm^M!nF%Z)ST{5$!11;sN_xnJmpQe1N?S(1c_vb2N_V@(e37!1;ISD-61!% zi(aq$)@ntGS&lG?{SdmZ?rHwbqkWsAP;G}(7^!bobt8D`JR^oeyHJH!MpcByMlfYg z(5R3jb9HhciqfhV&nJZhxCF7y9J2LGE`=7sZ&aZgxcN#yS3vimSlb5UyQbh*

fZwPO3c8q5hojKTV9Ls(sSrT z@@-cLwrymX#FV^U!I-TTHp;h(Z_YOQN*9C~uMth{a;W2%hZ!B7MJpuqvi`O!AzSah zMavhQp+tGoE{n#SK69`aIOHEKrqf9g6Zgo6qh~={91Z0-!R+ap2jL#G8vHYwwQ^|p^_B* zcNqm@uJ{a2d+NQw9Y_88u1^syhWy~QyK`DRy-V9E-IpZ^Hs2GuI@$_-qzjI3P5E@O zS6-5`Y+fWeu#+CRJ1ITuC(WXRds1N5mxEs#WK3A63+3E*@Db5Ii({^t>O7go;Y~30 z_i*nQif>xPeRZ>VJ2&{k-!vqR#fNspk_U&{&NMEV|8~qds3!b))Q3{HWchA|lkn^C4>1h^S>V&$x#@)u^7Z&5C3GK)O&!XGB+v4KN@t-4;!~yvN!aub_-HBhzci<|^d%`LR`23n5<$b9O8v3fO*2_iKs+7wEoB*Ml2~@q zqK_S;6ljyR@S~bIpqW`_h-nd7H#J_#oSw8IUR{#6m6ybKBovUkT{HjcvRJ zCCyXygpih|ris9#`G}FPk+0#Pfu_I2DJO}l?N>c2_%IYIDn7Z_$wxxy25h5a8E*oS2;6*oxcV?2ZnJEa=`BRG zkU{!5JN@7$CnaiLF>H9#jD0rNwr5T?waCH=1>!UMpAz2YR^slHq3Yf*-aA5xQbgqB z&d2QNQ6Ni32*VwBl#buc?rw4HOOo*ZYU_L{C4mFlC*mgHOzwr=QBQpc@}K)2{5YX* z<^?d~>dt`2z-QqU)=F0Zb`AfkMJL1O{odH^AW4%pFPCEfpYR6%4sUN6BvTUxFX+8n zWeJ6^>keft9v*zdT|j+&a-l?v|5kLX*^x5|Q`@6KIccK9k_cJi-q^@I5_ zpgaHB+ARs5vrO2ahOkNIeCv7NB=CKgCuIx7+-nL1-p^p1SrN=Ea64mY=wZ)Arq&i4 z<*0v}F`Hzcz)#lN@T2@T_l^0nQNG=+Z1u!@k^OrgxW*)!I;qin0Y;Lb`u(jS@3c&5 zZ?KH#>LTab*4CEyXvaeg&eqr=icr{MkU*jN%ExB^~jnp-ww)*H);oed&v+B0oLaPMD$dGODV zxAcmCHfq-`6Jie{@WcAA2*u%v-NEjkW(GF$%4IA4-LyNAOu(yWlCV3ky8c1=^el`g zB$RKSo~D#v+b(F<*eoQ}@*rm#-YQ)di|WvK7d<;@S*yRdJLQT5As)d4v%ZtJt@PLg zR&nB*uw?bK>2|}>039QS@Hx=A%dvs3ta$6)7)#vVMCOtZTst|n6n{7zhv^TqwZkm) zV8k2a9N|OCSuov|Sc%4cC+_KEu`}Q0Z|*rSTJ%`=KCU2#6;(*6qgsfXmUBFS*M<#4|%sSy!P3L(78_Xd23T-EO{RSJYcM*=t@D zGm4KZAcUrNcnWBG|GV$fw66Q?$Qm}wU58iL1c2BZ)s)A*9U2%q^T3-5#{X2PyDxcc z&J^(2nOx@fFP*1AUor)TCn7pm6kZNS(*AuI8Pu@RVb2;g7?qp#6eTCI<@eRaOx5U+ zb9Xf4bF~G|B7}|n=Ic&G@_V(e_hi9>s@~9)hwxv@pxx0YG9@V8p&P)mGdZ_AE#;5g zbjH97>TO<|ap2o?(m@jf%RRAIPPxjQEdbtJgykF32yGI-YUSk`mL7g}`u#xmXzsoD z-*yZ~@-S7+A9J=ER)TQjcW_@Bk^jwb8Ef4+SlmO)QOR*1>aaGG_;PaP67Xc-eAKh-$* zIX_Gqca}Je0nbI+vIwj{6_nF=&$O$K|fr0c=5ui0dLw(U24>-|^rZ_Lbhsm`1p zr;XDMME_X)!y5YMKwhxpm5X0Oa}~plJo;5*zc1N^j)}s`QYM-sCNyvTHw+l~N_{13 z8tMBWkxy`;8mr`&)?%kh=|ciePF?E?EmwEHVoBYN+LB&|tdv-~|4Ia(~wb{O%= z&U7@46_^DPy6cQ=*SaGh_f~Z3uB)S3Hz2x1TYBUbSS#{xU9-;MGpBxSDt$Sl8>Czgvr| z0dULw%~2dJrP#@FF|J_Z`a>IfQ=^0w+I$;|-J<<>`f}k0UluQmFxnxn&E-zoHnC$C z$+kr)8JS^h=eGEKBW%;Fx>un)ZK0=Ww#`RG7SL=|AwO*&i!b%wU-yl(!J!bLzmlV= zi=PE}cSS33y>Q>JHrE&h#5Lo1aMvF47do!4Tgf6t=1in?AZFp0>bLlh2rdYM1b5sN zU^wsPf-&dKv;3Lo`_u!aSt8sgsu)s~Ut5p*@r^hf-32J+F@%PS{Tb2|@e1VP?$nB^ zag5?4r)Ov9FjNc+_cVh(Z*fs%)>~m{+i0`&Rd2%{0q63tRRHIDoz8*iGVTkk;Nt4) zE$zMK!4dd+hP- zQnmkKGnv-gKKs3I5{SCJY25vlYsw3afO}kq(yD4uprXttvdy+WCf;bLNV3(!0vp`2 zqIY{II6O)L!mRlBG7zP;cwC-MGCU#k8}_&1W(~X(UYsv2WvZi07pp+K$M~tb1BpItE+-nc*|!95&s5}G8nlsMOniYf%Q6jLY;ksR@g!%o z*b3T7idf`bqYD*bQbV4xHh~I$TE(aKPli|302J?QlZc6epn+q=L*YmTemJCYV#A($ z1voxXdXPR*WytSX=A4Ta8Xs^{FUmt^I``f2{12wn|0d5<4Juus*{pb|fy7KGeC& zdAN|lcoNugL3>(=!r8|GaBmV9;>QGnmksda&ipw(m360A5otXJ@wc*tM9hnNasyM% zF^6eIiHIWLE(znVv*c*;=z_i5+hxIC6W{v(Rc>nP)D7I3@mXb}*;pIXsR%fHqwD%- z&YB%^b2>G5dSpb%H)4p9StrK~_T{}G3VZy=X(ne2+_Z7l0rA*wK`FtP- zW(c@CI;gq5{J>-uHO3at=8jCaYqo6v_JG}eYhIN-HiVyiH|SugHbYe64$|r+7E0m$ z8W8o+ebZBOAz!W&eG=~EBz<6MdVxSmYlX*ia8MZ@u93IJkvnQ-J^kS_-5|w|t8jQY zMNiRMWq%DNGTD}P-ac^%d8T+F%*bKC1~2okb%<~??dHV4B52jU(Y`MCL*>woCQ5qh z_P?nUH_IK8j%G&q(A$}NkZthWL7BF?7!v#X9L{5q6F*%~B_|V)r~M*J-scy`$GM#M z=Z3#ghOH2%UhN=2MQM~$4KaSW*Kb$&@pees7FrqFc{i5F)J1YCTS+iw+EE|i{i*#@ z*zaS`uBjGm0H+^&M!3f2gloIhm+cdJGZHZnzM7-8)+_u?`6lkl%GHo*KqVRV58IoL zCERVwMX=$St}%jifm@$Ecdikhi3|u*m2I2l%f`#d)nsYpWxL=!Az6i^9~r zW)=Y#JhUS9=!pQtuC~2lzGGn zdQcr%b7Ab>$1I_b9;cJ?x_<^g3yYE%<3FtKf_~XtxSM1x@6GA8SzoN6gq@DDjIz>8 zYF(1p6jkBJ`W*o4-(=hd-}XlRCXmRIHw?U&-IPe|O_{W!egj>K1u7jGx6@7hJK!A- zxs9O1woPzA;#OkQK)iFB0yP1%%U9|t6$bl!L&MeG47uEvxsCRv-x^0~h5F0~#aR%xZlmcJ;4@>j^*{A$})$;!};NP}R zTMr?B+tveRXhR}v@yXNoWPHBM)-K#4d2uL-^Oma!^N9GcAVnx|Ic@`y4>)@suP9yD z?E;kiQ`y2Fut%}QSCuG@^MJFF~O!@14IvH-YR9d&1bQ-r6723 z?VTmV;eV$tpGGU6RH_$XyZ&{bJRPXaN0j1<}g7idO@|w*Rdvs5vLAtjyNRItn^WuhQ9|MM{r#y}1C@#v=2u32yUtia z?S$QBOP+(i=U55|sUBWv6;xMQdsI(_cr*n|6}QYV9(!bPWqZ$|bRX9!Hn4czahLsk zExOEe?S{SJfrodSLw5GaQ{4SX)6yyka-g1&i1}hT;^XS7Ta5VZ&QGbZcQ@nOfB7e5<_eoYq9VXz$dOJ0qaRv6$MbU zdeUoBs&4vKZd-tr$5pHBaD4rc%tLl*cEWPg9ZN~StITEhx7OC8BI@)kX2vT|x3?e0 zt4PRdg6}^#j{1~23op%Lq14TpVJ9|tZr7_xh~Tynbvbok8ZAvtu>TbPJ2D&Sx?t8D zr$-a&?B@Yf;x8%E*<9P%WckXKEzAM>`!!XZvd>0kGM=T=#LR4zk34EizX^66MrL=x z9lQ4H?D7N!1@&SlEF~6wo2MpsklKa_$})=$SN49VByZ8H>am$Q-0DXujK4GL_szn> z7AuMg0#(W;PNs&a8i5)67L~***FLQA%gUW4OCH}uEEG2Yfq&S?8s;VLEF|_zRE=^q z3Wk(umIU1&GqeFVlf@7Jt!vP2ucD%d<{-*}PYNSI3cGIw;tPTdYE78{egT}X7a ziqrWSb}c3hg;ZdwEGNh59bLXIf+~5;H|zb35%EW*Im@^ z_EpQ{_)a#F5(Q~2r^ox>s`}*aehAw1>TpPHN;yNVw%AuAwEU(m_lcKvSxway`Mw=EWIE5g^-Q5ecSa2`yF2$X+MS>J4UI>c|PBn2vrqX;)gU3@$m47<>jQ*@$ha< z;{L-Q;Ny6WXytR!Cb2;DaB;;!Xe<+pfvwXuYk#`kd71Wt0gu6TGP-8a8m zaK})Iz=N6G~6pj5n?=}BG{wA?9I%5hOtF)WGOi*X}tQi z@}-89l$3(1Bn2rc!5acCDJf<=7EN3dUXrS`l+^A2Rl-LmRI?xUh-Ic#kNNUnYT^yjhOx}>#Gs8Ad#T_Wp*(}Ri$_d=gkmNF_pzx! z?U@Y7xOVGb5W1R0L17}zvoOlk)YtLJM7nBquFIA`(%&M8N z{W*(pp#@da0>PqFQIps1XMeG-6*>!y#LZd!Q&(4!!~0!fhMs}v(TwfnGSM-`GeZd@ z`wIu{x>>FmKRt*{uOhMdXqC^P!FkbVIK4(-tqaL|>r_R->iItX613!rY|%>g!4gGm zMELQ;bXDM&^d@=wJAkr%FIUCMPUyK&3*T==B{$jxHwNsvg6G8W&`KHnX9nyU1C9Qc zq4?xfR{9$%?h6+XGXT)_fnW+;>{jD&Is9_WDUemm@byLSD3+V17@BGnORx^r;c{|B z{$5U5f&sGCMYEUB4aysc>X}oErZ(y~gRe`pD@_%A+kMaCWmnm!itH1u#eMOWDn?S| zMO9KzGp2C38wdAS??+`HFdsGt}5b84i3AK;(T#Sa>z@ss)RPJq} z2W{=+MHk<*JI97!MKGUk50q%9AR4?ClgutZ@ito;G^E&5C-_&5f-H}jBlq{KOBDqk z5v-<|D=r+_PHQ&}@J=KQe7Xt)-j|?Oqf9Cp-hB1b9h$1yXWs=2Oj?e;Hue@+;1E7Z zk#4+da99!ZJ(XD><;8^BV0ZxC@3udsrzin~#8CZ9$F}MV&8K3gmsl@envGZ?zqTt0 zneMzOnyD2u5dV;b#(A4ys`M1Hl}9&!--BB za?RXqb}x6at{*YVWsWwI3Jo16a33!#k(Edsm^_njY&-$yJIq+jUJf25U!8-gu2Z)x ztycS@p5YJ&3eTzL!v&l^jZf6;Z_Gwiq0lc?w}>VW7HQ+PI| zHy!=PHpaiWSLJZ^{ovla@VZjztt+bd@2veVHOVZr`30GqWmqTfU{3>e5=sw(X9F1? zeLmT-t&y!t@NvkTrcawr&-8j>xv5#@DHbf)t(`(hJ^Ta^LMj3yj&g+qJm~Hy!4A#+u zal;?%0Aw$~_tuM93Z>I>tbX#kmHdm;qtlR7_nq&A;+jv!4OdVlRR+!CJ|uewRNa23 zRA38ORpyIUAP0pTeMXk3r?iAE`{rZw_)rI9Jy=%x6OJ*<{5c<$GV59Z2w+Vt1Sy)zGbNV41bP<8Pd+W$+K z`I?a<@SLrO(Kfd|&GPGw2Gig)*fQtB`FQi*&>zMq&M%SSa-E@4ggDhNgb}w|mf|v9 zz5V~31pd=$!|VZON2l67R{YTG_)FmSrB}JYk8m{FT6Wc#l$1JOuF@19<9>SiXWuLG z({vK$j{Q>W0ubgX4Vs=&Uec$T%9Q+IV8Lx|7bxISRz|<2S&E@z3?dsZBq+4pKf9>;b6|w%3m+D zBzfqIWhMoS21MnDtzPOusagRCRg9wzd>f_ey*pjysxb$K!efl}3T(|K{I}kEPEG0b znReb%1)n~Y|654F+KzC*s(^~nb*Al%d)`a-vuz>|n`R6H1?PkVv6Z07djfs}&++~B z;a?*cKEy{Yun2QG^k0RzzRh$w;GM5XSnoYML#?jXcF84xC9d2Weio(bYeyWlADjoQ z>%WY>+vYYaT!7LVt#rv~wzGN9V*JWhPtxBOo-sCXIvXwVgsgwD(-(914i&`&K2*eY zqs1Pv6b|>9((`y}8PI1xcdQ^VKgU03ztAQ~6B=mxY^<)5=+SWaThrZztLy5m^psU+ zZY~w#O*UP_sFz{`WIU&xO-&L0BhM$e)Qkeo*T=vK<&75BegfqH!OSb?%a>1WzAxY= z?B9F`U6^W7Y==ZN_iYs+i_smoT_b%&iPiwYDZENc+OvNMcD1<$V0uq=-J35Cj z$Xxn)c~S!$O%|4iwr&)&vDOs?!xTS3XC3(x3VC)iX^CAcCqK-x#)RqR+K2cTPWK16 z?r~YZ%Ff|})u>ZitPI$zeL0Ivp?@Em5=1YtUVB`ep)nLGHRLmq>AUZ}-x1|aHa%7# zJ0EoC{`~$z`vOj`=c^4wS4B>?51!uT>qocrbv3PpdhPIC3tgTKrtia72Lt``UHnf#_J*_2+7HozW*>NB1Cy*&9pkQOKG=U>do~tM3M$3n!@Ge;;$;hrE~^;>H9c zVAyx3?1n%Iv!`w-w6+9$yIaS9G#ALsL>xvJ+cH8AOZaW`oRIwyDuy zkZK+clgQuS+gi?xXIoIsW3uT5jg3={&0uHO&Fe3|2c4ciCRQP$erHag=9ZwxA%9n^ zD=IkK@PU-!@0d;5Y+s|cSb=@^23>%m**4w26$D2&R|aLf_)1PnR#n)+3(iW-6>3X{ z^`sP4c1(l9quPU1DntPw?!~!PxP9G0W`RvINwb{5tF_C|rHJsi z59!5x8XzrB@tAH#@j)W9$4T1tN47fR+ydMW!~=kjnMH~dc@wCDjN1(2{4T4>xZl{s zGzHj%Pip#mB%(q-{>fi|5ZnuXFFhMg6p(6{-eAh9?(yt*;w_~Qq;?J3tpjJ{!FuTj zzw+LRRkZ^5^THUMWMk;uk2VZ;WMb&3M~8*uOyZdT!08cx8G(<(K&Ofd86#b)+t00M z#rYhZZ6dkgwV3dGDT>+(74m%&waI=z&E#Xebt$x+aum9+`t%Akd#st6)%pfx-{nfa zsdQE1(iqOG4nf7`+7SWdB*iu}U41?Nke}`vVL7rBqTj4aX@n>*p0%D;HNjacEKs=Lz5-tO>@?Ue`s=wKAxP)4y%2nJNwfB#8_f}49JtrxFdw2U~=>7qa3l=eV8Excc&rYz%)qCd2Od5*-=2v8W+F)dN z!?ralt$52t9C`wnMPHsCn(l*@8+g zXXtcZm4q!lh;zlH$$X2WvF9h4+an`&Pg3wM!|vZr*XPd%bSiY~fWb`4jdjoaOPyap zy7t15oQyWrl;Lik7;~;WL(|zulVf~>3`0V=;RB}`Xt~oVGi?GOY1Ou;)!rUT9~tru z03BxXD&t-R5#-cKU2iYl>~f-U2WIykYNk^gyU5HTG~k7w9AlPm4)Zd!#q>Vsz@FQv z>#3)#QbfPixZq(ia2;-R=ni$?M>a3Tl*myAhs`-HejZWOYQ$$3W^}k400n^?Q+W!p zsDz&2dpa#SS9{A|3a@+r{E@0iu&kTfzb}8X$e7?YMl`TFIIc7HRG)jVqtjrG2?$gm$~c&?l^vHr<&>dwRwbuz$iwQQ=9h&Ss5yR27c&0 z#7VdJk#s2MN<_;u?uy#8<$|YA24<2zy|z}**xlcEseiqxExy#b>Wi9R6>HvMKfO$E z9CL}q`W0d3rL+8lbN;M7@Svh(#(q*ia6+?B`$HNuNC**NEF7Ec)mhG zn4g30|NO{7w@Kh+7}A?7=bxUSnz+zDnX>d7Z2_9bM0PE)^d4!`Zr;1U__U##!_e(_ zy{BW2dtFyF%$J2`B!h>4{x4Yq7Wa_-j@^Az=c1=i*sH98nlO(J>m$m4z*YqQ{1?HS zl+Hs%!UAVT106SN#D&*buntCf(T}uDg^x|WofL_ji0SKgZ$SHGh=^8iaYWFYr}vvAtcf1`KZwf2?OZQbfver&vGdnu3JRRO&=Fw5lZS11oO-o-;{FZ zE55jf--QzCcBG7EeCoLTc9gEymNt%Sj14)?rm$)G&``7DYT}o- zQkbWubFWUWOQtWvM3vd7^p%=XTzI$~p27`2++xd%3=hAX`-uYpAp1yGev?3a)kuwt zC|R^2MVs5%ktNdIItKpc$Mw25Attkardo%&<4DNK&vGw?Rl%a6)H{%XrAV*JnvAa2 zNwJImRn+@ec1Qc|0gqz^84&3=s8A`MbPto&Be0>R(M0qvg=+Jn!l<>0|7X`M>+Wl| z5PRKLXOc@Kb?8ub&iAN8HKOU_G0bR}P=Vj81MkyVpG+}-8MlSM6QNhJy=HQBgot2+ zX1|1WxmgB~7q3Dw*G%Q^SL;z#*=QQA)`5#+k4zkex{rTKEXdJ53;EvWzRO`iAzr+2 z6wZRMZ+!pNao+t^b}`{L-!4Y?&cCxwSJ74rOrtk&v7Q}p zv2B%=YUrYC4zZi5ZcWCXeafK6MkAIL_}*uHZ^I7!HgM@)37wbVhF!WeTObJ?hNg$d zUImKInZ$x^hqiJltP+?{DVyN#AfMx{!6rlBtEoY66NXv`xSd3XTAstdi6DB^^4^p5 zD$i3)`+TN|>q^-jMykOZJjnFoCMhPS%rtG&t#fjAT&!b%*!P$#oUQ%F>Q}QC>zb4l z&yA1sTRHACvEf5$_b^LDn#Wg>u4EL))z#IL3i@IeBbQ3+i~JsY?@#yJ4`jMONO0BW z1TN;Zp;qCt6od#Ng46@yIYzidNFJqh7<~+3a70Z+VGyU9}Y1s9&laN>Pr% zyo#W5ABl$hKxC(5N98R#Uyfu`H|ADy5#9QEjW{gH_OWr}dkRBu*9Z?7#n7??RaXyk z=wZo(1fyL;;4WJ``l*;KLJ`}XpBau02NQY?N7k1|gWj8gA@6t_D3 zT6W+!i}u$38XZE2npGv9%eM<20GU09|1r&p)I4s@iNB13*^dPEvTwRm!+%dwsFFBik@ep%jXhlzC&b++JbR8ie3waAEO*KH@6E>Cc0DjyT zuRyB3k)y;M9DMrk+rGJ3m4xTy?FQOa&N*DBDXzavfy`Rs;c($Vf8oP~7JbLR{ z4g+VtbEuThoM@o+9F5 zj5)d!M=MQwbRU_e)Em5TJsE`nr^uURg|(4rcMYAy`E|*$WP9(u(w8zNfI!_ts9|nQaXwEG8RhTIz7@1Bcb0ZlXi4|BzF8! z)Mc$SL-=4k{k9&TSBH=3*SYJGUaE3s9A?z~VeUxcL{<4xsw^-gg`KpdD{29RsZXh9NgTV7O zfman?3PKMf38x*ssULnKjFnt@_iHNBQmPW6Lc_|dizo}_{#5s1%`*F{WN@IG;+Ef> zHGQ%JYxg~4bJLv43_bPuIWs|Kb97jtLIoeR5maF;HZMgy^C?g%prUmSt&W|_Bl zWf`hlH4$WSbd(mwlLor0wnQc8i}uwt5Ht}blzu{Y_p7B zQr|S4F=~wTS`-WG0aLGu?N)1x$d$9=tG&)s$?&iMLsy@it|h{vN;Ki`%(;J6LcdA0 z4seTi!R7y0(r^pdh+Pw`zC(^UmYL;yEl6L-ODtdJl^t(0~nnLpeo_I1IpR6Y6hWW~1%j8r^R{{gRi__Jiyu4~! zzoI3F+f`YqvZFZ3=JD`dIHzRa`V8uT|OK_6dW@T#vNyarrabj>1 zpLWU5(pAn?W>R&*^Li%B#`u4AtBM67?-U~+r z*u&cCPF;0|P2cwH>bfiU+eyam=Pu&ZS$DQCsjk*>h0Q=Iu$>R+n}B8Kw)+31pHVLGR#P{ z?bqLP8bb8+8mqN-k>6LRXhWpY64XX1T=m6N5eOUSI87uS6VQVg^#;&z@WQl^t(qwz zH+C?lD)o`U>2(|R3;Zi{qRt0yt}mlE%RETrKJ7UkX1{B0gfU$9$~eU1ZK7ym4x%t+mD#-u*&msrEb4XiytGto@m(;6|&Rdpph_HZ$*{J?AIJB z=Y-g^f%1skZbO4`cz30oLQtz|^ysP0RwJ{9Dn_}<>i%oPgr_eplt3xUd~8=i+5q4| z<^H;l^Fi*3;$eY_q0kpS>5gf$$b3F4g=SU94ixQJ1@_&oMq_OBn>Koyv2hx^mS^%7 zZt{ZpLg~Ftw&|T$LAi2%?D*zi3+`=uG94X~QEUd~rktruQc!;jh2V}>I3xNDCiENt z;Gbvz@4o848?HAv>ga&-c<@yF=Z$V6ngckq5mXajMm_yb{jk-8o;i>cO^_hs2B5gG z-y+vhlcs|sRMbi-`mh!ku_LDJ@y8I|nNFMsZYyS~K%(K%)&IfuX0xiK!j9L$Y}eD(5Jr}tYcJ*7G=VZ@ z^NV$M)*xA_NGmFKYMmJXl1|Axe5o0b~!I zrTx}%qfEzfx6 zY?8Dr8}(rP0VQ>v7cfVd!23LCQ?SvEuL#$e$rqYL_~26U0NLtkA{=%L@T=VPxMZ*V zyNc?EX7PT}QRFuRU)4Fk>)C&}=Kr5A_x#^|1^<5E_dSB?aPnKWQT_DrPA25s@2jO_k{3=GUm517*;aQA+Rt^c=)skRd7ves z(!KdsCd*G>sDN5WYn>~j%QFkTaq*}vjp-lKMjl5SJmK)jvZ6{UGcR>X`>(r{JP~B0 z$wqol!R*D2=1CYQ?6p^A;8?z|Ej)&e^t{U#ewrT}9?q7>FcU4WQ<0~&GFs7?Y|XWO zwshL=cK*J(TA5L>yw`s}f5hrHI%4_@mOjvEqRq9UvHX3?Nj?2^@)X5Saqv?K@$#o@ z#=3#ngxv=J;~`M1k_-LxZ3{02U*BWn--AQbaA*F7!IqS|K?F}|{or8ACVBn|7a(Nj zt4is7B{;`NFNa&H8|08Lbb!jKiPHOW9o&A}?Pm&#f3~b8S7pxZGF7Od=jKQDYD%?GKTOQ^<#8L=j9?q7Ct- zcmD=!)~n*vL2yOokO9<=e93#VY0mcm7V){{OrHadH+hV9=Qsf ZNRO8;C8rL?Z92n~msXLgmi!R>e*kaexvu~K diff --git a/Writerside/topics/gui-Editing-Paths-and-Autos.md b/Writerside/topics/gui-Editing-Paths-and-Autos.md index 1958effb..916661bb 100644 --- a/Writerside/topics/gui-Editing-Paths-and-Autos.md +++ b/Writerside/topics/gui-Editing-Paths-and-Autos.md @@ -81,6 +81,9 @@ Max Angular Acceleration Use Default Constraints : Ties these constraints to the default constraints in the settings menu. +Unlimited +: Use unlimited constraints for this path. If unlimited, the robot will move as fast as the torque output will allow. + ### Ideal Starting State The ideal starting state defines the state the robot should be at when starting the path. This will be used to diff --git a/Writerside/topics/gui-Settings.md b/Writerside/topics/gui-Settings.md index 9b6d556a..5cd1644e 100644 --- a/Writerside/topics/gui-Settings.md +++ b/Writerside/topics/gui-Settings.md @@ -55,6 +55,10 @@ other configuration options. These values will be used as the default global constraints when creating new paths. Updating these values will also update the constraints of any paths set to use the defaults. +The default constraints do not support being unlimited, as a conscious decision should be made per-path to have +unlimited constraints. Running a path as fast as possible is not only dangerous, but can lead to the path following +controller falling behind. + Max Velocity : Max linear velocity in meters per second.