diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp index 131ae8a421..976b4e26e6 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -31,6 +31,8 @@ #include "frc/trajectory/TrajectoryParameterizer.h" +#include + using namespace frc; Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( @@ -186,8 +188,9 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( // delta_x = v * t dt = ds / v; } else { - throw std::runtime_error( - "Something went wrong during trajectory generation."); + throw std::runtime_error("Something went wrong at iteration " + + std::to_string(i) + + " of time parameterization."); } } @@ -213,6 +216,14 @@ void TrajectoryParameterizer::EnforceAccelerationLimits( auto minMaxAccel = constraint->MinMaxAcceleration( state->pose.first, state->pose.second, state->maxVelocity * factor); + if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) { + throw std::runtime_error( + "The constraint's min acceleration was greater than its max " + "acceleration. To debug this, remove all constraints from the config " + "and add each one individually. If the offending constraint was " + "packaged with WPILib, please file a bug report."); + } + state->minAcceleration = units::math::max( state->minAcceleration, reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java index 9459dd0cd0..e2bda033fa 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -69,8 +69,7 @@ public final class TrajectoryParameterizer { * @return The trajectory. */ @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity", - "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops", - "PMD.AvoidThrowingRawExceptionTypes"}) + "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops"}) public static Trajectory timeParameterizeTrajectory( List points, List constraints, @@ -236,7 +235,8 @@ public final class TrajectoryParameterizer { // delta_x = v * t dt = ds / velocityMetersPerSecond; } else { - throw new RuntimeException("Something went wrong"); + throw new TrajectoryGenerationException("Something went wrong at iteration " + i + + " of time parameterization."); } } @@ -266,6 +266,14 @@ public final class TrajectoryParameterizer { state.pose.poseMeters, state.pose.curvatureRadPerMeter, state.maxVelocityMetersPerSecond * factor); + if (minMaxAccel.minAccelerationMetersPerSecondSq + > minMaxAccel.maxAccelerationMetersPerSecondSq) { + throw new TrajectoryGenerationException("The constraint's min acceleration " + + "was greater than its max acceleration.\n Offending Constraint: " + + constraint.getClass().getName() + + "\n If the offending constraint was packaged with WPILib, please file a bug report."); + } + state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq, reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq : minMaxAccel.minAccelerationMetersPerSecondSq); @@ -300,4 +308,10 @@ public final class TrajectoryParameterizer { pose = new PoseWithCurvature(); } } + + public static class TrajectoryGenerationException extends RuntimeException { + public TrajectoryGenerationException(String message) { + super(message); + } + } }