diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 23f36a77..4c1622eb 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -31,8 +31,8 @@ class PathPlannerTrajectoryState: pose: Pose2d = field(default_factory=Pose2d) linearVelocity: float = 0.0 feedforwards: DriveFeedforwards = None - heading: Rotation2d = field(default_factory=Rotation2d) + deltaPos: float = 0.0 deltaRot: Rotation2d = field(default_factory=Rotation2d) moduleStates: List[SwerveModuleTrajectoryState] = field(default_factory=list) @@ -106,6 +106,7 @@ def reverse(self) -> PathPlannerTrajectoryState: reversedState.pose = Pose2d(self.pose.translation(), self.pose.rotation() + Rotation2d.fromDegrees(180)) reversedState.linearVelocity = -self.linearVelocity reversedState.feedforwards = self.feedforwards.reverse() + reversedState.heading = self.heading + Rotation2d.fromDegrees(180) return reversedState @@ -122,6 +123,7 @@ def flip(self) -> PathPlannerTrajectoryState: flipped.pose = FlippingUtil.flipFieldPose(self.pose) flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(self.fieldSpeeds) flipped.feedforwards = self.feedforwards.flip() + flipped.heading = FlippingUtil.flipFieldRotation(self.heading) return flipped diff --git a/pathplannerlib-python/pathplannerlib/util/swerve.py b/pathplannerlib-python/pathplannerlib/util/swerve.py index 529462a1..9a77fe05 100644 --- a/pathplannerlib-python/pathplannerlib/util/swerve.py +++ b/pathplannerlib-python/pathplannerlib/util/swerve.py @@ -25,8 +25,6 @@ class SwerveSetpointGenerator: forces acting on a module's wheel from exceeding the force of friction. """ _k_epsilon = 1e-8 - _MAX_STEER_ITERATIONS = 8 - _MAX_DRIVE_ITERATIONS = 10 def __init__(self, config: RobotConfig, max_steer_velocity_rads_per_sec: float) -> None: """ @@ -221,11 +219,13 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re / self._config.moduleConfig.wheelRadiusMeters # Use the current battery voltage since we won't be able to supply 12v if the # battery is sagging down to 11v, which will affect the max torque output - current_draw = \ - self._config.moduleConfig.driveMotor.current( - math.fabs(last_vel_rad_per_sec), input_voltage) + current_draw = self._config.moduleConfig.driveMotor.current(abs(last_vel_rad_per_sec), input_voltage) + reverse_current_draw = abs( + self._config.moduleConfig.driveMotor.getCurrent(abs(last_vel_rad_per_sec), -input_voltage)) current_draw = min(current_draw, self._config.moduleConfig.driveCurrentLimit) - module_torque = self._config.moduleConfig.driveMotor.torque(current_draw) + reverse_current_draw = min(reverse_current_draw, self._config.moduleConfig.driveCurrentLimit) + forward_module_torque = self._config.moduleConfig.driveMotor.torque(current_draw) + reverse_module_torque = self._config.moduleConfig.driveMotor.torque(reverse_current_draw) prev_speed = prev_setpoint.module_states[m].speed desired_module_states[m].optimize(prev_setpoint.module_states[m].angle) @@ -233,15 +233,18 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re force_sign = 1 force_angle = prev_setpoint.module_states[m].angle + module_torque = 0.0 if self._epsilonEquals(prev_speed, 0.0) \ or (prev_speed > 0 and desired_speed >= prev_speed) \ or (prev_speed < 0 and desired_speed <= prev_speed): + module_torque = forward_module_torque # Torque loss will be fighting motor module_torque -= self._config.moduleConfig.torqueLoss force_sign = 1 # Force will be applied in direction of module if prev_speed < 0: force_angle = force_angle + Rotation2d(math.pi) else: + module_torque = reverse_module_torque # Torque loss will be helping the motor module_torque += self._config.moduleConfig.torqueLoss force_sign = -1 # Force will be applied in opposite direction of module @@ -283,15 +286,7 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re desired_vy[m] if min_s == 1.0 else (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m] # Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we # already know we can't go faster than that. - s = self._findDriveMaxS( - prev_vx[m], - prev_vy[m], - math.hypot(prev_vx[m], prev_vy[m]), - vx_min_s, - vy_min_s, - math.hypot(vx_min_s, vy_min_s), - max_vel_step - ) + s = self._findDriveMaxS(prev_vx[m], prev_vy[m], vx_min_s, vy_min_s, max_vel_step) min_s = min(min_s, s) ret_speeds = ChassisSpeeds( @@ -378,97 +373,108 @@ def _unwrapAngle(ref: float, angle: float) -> float: else: return angle - @classmethod - def _findRoot( - cls, - func: Callable[[float, float], float], - x_0: float, - y_0: float, - f_0: float, - x_1: float, - y_1: float, - f_1: float, - iterations_left: int - ) -> float: - """ - Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - This is a pretty naive way to do root finding, but it's usually faster than simple bisection - while being robust in ways that e.g. the Newton-Raphson method isn't. - :param func: The Function2d to take the root of. - :param x_0: x value of the lower bracket. - :param y_0: y value of the lower bracket. - :param f_0: value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - recursion) - :param x_1: x value of the upper bracket. - :param y_1: y value of the upper bracket. - :param f_1: value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - recursion) - :param iterations_left: Number of iterations of root finding left. - :return: The parameter value 's' that interpolating between 0 and 1 that corresponds to the - (approximate) root. - """ - s_guess = max(0.0, min(1.0, -f_0 / (f_1 - f_0))) - - if iterations_left < 0 or cls._epsilonEquals(f_0, f_1): - return s_guess - - x_guess = (x_1 - x_0) * s_guess + x_0 - y_guess = (y_1 - y_0) * s_guess + y_0 - f_guess = func(x_guess, y_guess) - if cls._signum(f_0) == cls._signum(f_guess): - # 0 and guess on same side of root, so use upper bracket. - return s_guess \ - + (1.0 - s_guess) \ - * cls._findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1) - else: - # Use lower bracket. - return s_guess \ - * cls._findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1) - @classmethod def _findSteeringMaxS( cls, x_0: float, y_0: float, - f_0: float, + theta_0: float, x_1: float, y_1: float, - f_1: float, + theta_1: float, max_deviation: float ) -> float: - f_1 = cls._unwrapAngle(f_0, f_1) - diff = f_1 - f_0 + theta_1 = cls._unwrapAngle(theta_0, theta_1) + diff = theta_1 - theta_0 if math.fabs(diff) <= max_deviation: # Can go all the way to s=1 return 1.0 - offset = f_0 + cls._signum(diff) * max_deviation - def func(x: float, y: float): - return cls._unwrapAngle(f_0, math.atan2(y, x)) - offset - - return cls._findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, cls._MAX_STEER_ITERATIONS) + target = theta_0 + math.copysign(max_deviation, diff) + + # Rotate the velocity vectors such that the target angle becomes the +X + # axis. We only need find the Y components, h_0 and h_1, since they are + # proportional to the distances from the two points to the solution + # point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s). + sin = math.sin(-target) + cos = math.cos(-target) + h_0 = sin * x_0 + cos * y_0 + h_1 = sin * x_1 + cos * y_1 + + # Undo linear interpolation from h_0 to h_1: + # 0 = h_0 + (h_1 - h_0) * s + # -h_0 = (h_1 - h_0) * s + # -h_0 / (h_1 - h_0) = s + # h_0 / (h_0 - h_1) = s + # Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0 + # would be equal to theta_1, which is caught by the difference check. + return h_0 / (h_0 - h_1) @classmethod def _findDriveMaxS( cls, x_0: float, y_0: float, - f_0: float, x_1: float, y_1: float, - f_1: float, max_vel_step: float ) -> float: - diff = f_1 - f_0 + # Derivation: + # Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + # length of P(s) is the target T. P(s) is linearly interpolated between the + # points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + # Then, + # T = sqrt(P(s).x^2 + P(s).y^2) + # T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + # T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + # + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + # T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + # + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + # 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + # + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + # + (x_0^2 + y_0^2 - T^2). + # + # To simplify, we can factor out some common parts: + # Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + # p = x_0 * x_1 + y_0 * y_1. + # Then we have + # 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + # with which we can solve for s using the quadratic formula. + + l_0 = x_0 * x_0 + y_0 * y_0 + l_1 = x_1 * x_1 + y_1 * y_1 + sqrt_l_0 = math.sqrt(l_0) + diff = math.sqrt(l_1) - sqrt_l_0 if math.fabs(diff) <= max_vel_step: # Can go all the way to s=1. return 1.0 - offset = f_0 + cls._signum(diff) * max_vel_step - - def func(x: float, y: float): - return math.hypot(x, y) - offset - return cls._findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, cls._MAX_DRIVE_ITERATIONS) + target = sqrt_l_0 + math.copysign(max_vel_step, diff) + p = x_0 * x_1 + y_0 * y_1 + + # Quadratic of s + a = l_0 + l_1 - 2 * p + b = 2 * (p - l_0) + c = l_0 - target * target + root = math.sqrt(b * b - 4 * a * c) + + def is_valid_s(s): + return math.isfinite(s) and s >= 0 and s <= 1 + + # Check if either of the solutions are valid + # Won't divide by zero because it is only possible for a to be zero if the + # target velocity is exactly the same or the reverse of the current + # velocity, which would be caught by the difference check. + s_1 = (-b + root) / (2 * a) + if is_valid_s(s_1): + return s_1 + s_2 = (-b - root) / (2 * a) + if is_valid_s(s_2): + return s_2 + + # Since we passed the initial max_vel_step check, a solution should exist, + # but if no solution was found anyway, just don't limit movement + return 1.0 @classmethod def _epsilonEquals( diff --git a/pathplannerlib-python/tests/import_test.py b/pathplannerlib-python/tests/import_test.py index 2c2b5c54..ecbd2fe0 100644 --- a/pathplannerlib-python/tests/import_test.py +++ b/pathplannerlib-python/tests/import_test.py @@ -10,6 +10,7 @@ from pathplannerlib.telemetry import * from pathplannerlib.trajectory import * from pathplannerlib.util import * +from pathplannerlib.util.swerve import * # Test that just imports everything to make sure there are no circular imports def importTest(): diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java index 9a98bda3..4e0565b4 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java @@ -112,14 +112,14 @@ public PathPlannerTrajectoryState reverse() { reversed.timeSeconds = timeSeconds; Translation2d reversedSpeeds = new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond) - .rotateBy(Rotation2d.fromDegrees(180)); + .rotateBy(Rotation2d.k180deg); reversed.fieldSpeeds = new ChassisSpeeds( reversedSpeeds.getX(), reversedSpeeds.getY(), fieldSpeeds.omegaRadiansPerSecond); - reversed.pose = - new Pose2d(pose.getTranslation(), pose.getRotation().plus(Rotation2d.fromDegrees(180))); + reversed.pose = new Pose2d(pose.getTranslation(), pose.getRotation().plus(Rotation2d.k180deg)); reversed.linearVelocity = -linearVelocity; reversed.feedforwards = feedforwards.reverse(); + reversed.heading = heading.plus(Rotation2d.k180deg); return reversed; } @@ -137,6 +137,7 @@ public PathPlannerTrajectoryState flip() { flipped.pose = FlippingUtil.flipFieldPose(pose); flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(fieldSpeeds); flipped.feedforwards = feedforwards.flip(); + flipped.heading = FlippingUtil.flipFieldRotation(heading); return flipped; } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java index 8f046864..107cc266 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java @@ -26,8 +26,6 @@ */ public class SwerveSetpointGenerator { private static final double kEpsilon = 1E-8; - private static final int MAX_STEER_ITERATIONS = 8; - private static final int MAX_DRIVE_ITERATIONS = 10; private final RobotConfig config; private final double maxSteerVelocityRadsPerSec; @@ -252,8 +250,13 @@ public SwerveSetpoint generateSetpoint( // battery is sagging down to 11v, which will affect the max torque output double currentDraw = config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), inputVoltage); + double reverseCurrentDraw = + Math.abs( + config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), -inputVoltage)); currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit); - double moduleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); + reverseCurrentDraw = Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit); + double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); + double reverseModuleTorque = config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw); double prevSpeed = prevSetpoint.moduleStates()[m].speedMetersPerSecond; desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); @@ -261,9 +264,11 @@ public SwerveSetpoint generateSetpoint( int forceSign; Rotation2d forceAngle = prevSetpoint.moduleStates()[m].angle; + double moduleTorque; if (epsilonEquals(prevSpeed, 0.0) || (prevSpeed > 0 && desiredSpeed >= prevSpeed) || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + moduleTorque = forwardModuleTorque; // Torque loss will be fighting motor moduleTorque -= config.moduleConfig.torqueLoss; forceSign = 1; // Force will be applied in direction of module @@ -271,6 +276,7 @@ public SwerveSetpoint generateSetpoint( forceAngle = forceAngle.plus(Rotation2d.k180deg); } } else { + moduleTorque = reverseModuleTorque; // Torque loss will be helping the motor moduleTorque += config.moduleConfig.torqueLoss; forceSign = -1; // Force will be applied in opposite direction of module @@ -318,15 +324,7 @@ public SwerveSetpoint generateSetpoint( min_s == 1.0 ? desired_vy[m] : (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m]; // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we // already know we can't go faster than that. - double s = - findDriveMaxS( - prev_vx[m], - prev_vy[m], - Math.hypot(prev_vx[m], prev_vy[m]), - vx_min_s, - vy_min_s, - Math.hypot(vx_min_s, vy_min_s), - maxVelStep); + double s = findDriveMaxS(prev_vx[m], prev_vy[m], vx_min_s, vy_min_s, maxVelStep); min_s = Math.min(min_s, s); } @@ -493,88 +491,104 @@ private static double unwrapAngle(double ref, double angle) { } } - @FunctionalInterface - private interface Function2d { - double f(double x, double y); - } - - /** - * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - * This is a pretty naive way to do root finding, but it's usually faster than simple bisection - * while being robust in ways that e.g. the Newton-Raphson method isn't. - * - * @param func The Function2d to take the root of. - * @param x_0 x value of the lower bracket. - * @param y_0 y value of the lower bracket. - * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - * recursion) - * @param x_1 x value of the upper bracket. - * @param y_1 y value of the upper bracket. - * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - * recursion) - * @param iterations_left Number of iterations of root finding left. - * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the - * (approximate) root. - */ - private static double findRoot( - Function2d func, - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - int iterations_left) { - var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); - - if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { - return s_guess; - } - - var x_guess = (x_1 - x_0) * s_guess + x_0; - var y_guess = (y_1 - y_0) * s_guess + y_0; - var f_guess = func.f(x_guess, y_guess); - if (Math.signum(f_0) == Math.signum(f_guess)) { - // 0 and guess on same side of root, so use upper bracket. - return s_guess - + (1.0 - s_guess) - * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); - } else { - // Use lower bracket. - return s_guess - * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); - } - } - private static double findSteeringMaxS( double x_0, double y_0, - double f_0, + double theta_0, double x_1, double y_1, - double f_1, + double theta_1, double max_deviation) { - f_1 = unwrapAngle(f_0, f_1); - double diff = f_1 - f_0; + theta_1 = unwrapAngle(theta_0, theta_1); + double diff = theta_1 - theta_0; if (Math.abs(diff) <= max_deviation) { // Can go all the way to s=1. return 1.0; } - double offset = f_0 + Math.signum(diff) * max_deviation; - Function2d func = (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_STEER_ITERATIONS); + + double target = theta_0 + Math.copySign(max_deviation, diff); + + // Rotate the velocity vectors such that the target angle becomes the +X + // axis. We only need find the Y components, h_0 and h_1, since they are + // proportional to the distances from the two points to the solution + // point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s). + double sin = Math.sin(-target); + double cos = Math.cos(-target); + double h_0 = sin * x_0 + cos * y_0; + double h_1 = sin * x_1 + cos * y_1; + + // Undo linear interpolation from h_0 to h_1: + // 0 = h_0 + (h_1 - h_0) * s + // -h_0 = (h_1 - h_0) * s + // -h_0 / (h_1 - h_0) = s + // h_0 / (h_0 - h_1) = s + // Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0 + // would be equal to theta_1, which is caught by the difference check. + return h_0 / (h_0 - h_1); + } + + private static boolean isValidS(double s) { + return Double.isFinite(s) && s >= 0 && s <= 1; } private static double findDriveMaxS( - double x_0, double y_0, double f_0, double x_1, double y_1, double f_1, double max_vel_step) { - double diff = f_1 - f_0; + double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Derivation: + // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + // length of P(s) is the target T. P(s) is linearly interpolated between the + // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + // Then, + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). + // + // To simplify, we can factor out some common parts: + // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + // p = x_0 * x_1 + y_0 * y_1. + // Then we have + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // with which we can solve for s using the quadratic formula. + + double l_0 = x_0 * x_0 + y_0 * y_0; + double l_1 = x_1 * x_1 + y_1 * y_1; + double sqrt_l_0 = Math.sqrt(l_0); + double diff = Math.sqrt(l_1) - sqrt_l_0; if (Math.abs(diff) <= max_vel_step) { // Can go all the way to s=1. return 1.0; } - double offset = f_0 + Math.signum(diff) * max_vel_step; - Function2d func = (x, y) -> Math.hypot(x, y) - offset; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); + + double target = sqrt_l_0 + Math.copySign(max_vel_step, diff); + double p = x_0 * x_1 + y_0 * y_1; + + // Quadratic of s + double a = l_0 + l_1 - 2 * p; + double b = 2 * (p - l_0); + double c = l_0 - target * target; + double root = Math.sqrt(b * b - 4 * a * c); + + // Check if either of the solutions are valid + // Won't divide by zero because it is only possible for a to be zero if the + // target velocity is exactly the same or the reverse of the current + // velocity, which would be caught by the difference check. + double s_1 = (-b + root) / (2 * a); + if (isValidS(s_1)) { + return s_1; + } + double s_2 = (-b - root) / (2 * a); + if (isValidS(s_2)) { + return s_2; + } + + // Since we passed the initial max_vel_step check, a solution should exist, + // but if no solution was found anyway, just don't limit movement + return 1.0; } private static boolean epsilonEquals(double a, double b, double epsilon) { diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp index 2fe32510..e8ea6b7f 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp @@ -72,6 +72,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::reverse() const { pose.Rotation() + frc::Rotation2d(180_deg)); reversed.linearVelocity = -linearVelocity; reversed.feedforwards = feedforwards.reverse(); + reversed.heading = heading + frc::Rotation2d(180_deg); return reversed; } @@ -84,6 +85,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::flip() const { flipped.pose = FlippingUtil::flipFieldPose(pose); flipped.fieldSpeeds = FlippingUtil::flipFieldSpeeds(fieldSpeeds); flipped.feedforwards = feedforwards.flip(); + flipped.heading = FlippingUtil::flipFieldRotation(heading); return flipped; }