diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp index 3d6b61c50b..80cb3b3682 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.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. */ @@ -47,30 +47,51 @@ DifferentialDriveVoltageConstraint::MinMaxAcceleration( // Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius // increased by half of the trackwidth T. Inner wheel has radius decreased // by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so - // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2) - // Inner wheel is similar. + // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + + // |curvature|T/2). Inner wheel is similar. // sgn(speed) term added to correctly account for which wheel is on // outside of turn: // If moving forward, max acceleration constraint corresponds to wheel on - // outside of turn - // If moving backward, max acceleration constraint corresponds to wheel on - // inside of turn - auto maxChassisAcceleration = - maxWheelAcceleration / - (1 + m_kinematics.trackWidth * units::math::abs(curvature) * - wpi::sgn(speed) / (2_rad)); - auto minChassisAcceleration = - minWheelAcceleration / - (1 - m_kinematics.trackWidth * units::math::abs(curvature) * - wpi::sgn(speed) / (2_rad)); + // outside of turn If moving backward, max acceleration constraint corresponds + // to wheel on inside of turn + + // When velocity is zero, then wheel velocities are uniformly zero (robot + // cannot be turning on its center) - we have to treat this as a special case, + // as it breaks the signum function. Both max and min acceleration are + // *reduced in magnitude* in this case. + + units::meters_per_second_squared_t maxChassisAcceleration; + units::meters_per_second_squared_t minChassisAcceleration; + + if (speed == 0_mps) { + maxChassisAcceleration = + maxWheelAcceleration / + (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad)); + minChassisAcceleration = + minWheelAcceleration / + (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad)); + } else { + maxChassisAcceleration = + maxWheelAcceleration / + (1 + m_kinematics.trackWidth * units::math::abs(curvature) * + wpi::sgn(speed) / (2_rad)); + minChassisAcceleration = + minWheelAcceleration / + (1 - m_kinematics.trackWidth * units::math::abs(curvature) * + wpi::sgn(speed) / (2_rad)); + } + + // When turning about a point inside of the wheelbase (i.e. radius less than + // half the trackwidth), the inner wheel's direction changes, but the + // magnitude remains the same. The formula above changes sign for the inner + // wheel when this happens. We can accurately account for this by simply + // negating the inner wheel. - // Negate acceleration corresponding to wheel on inside of turn - // if center of turn is inside of wheelbase if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) { if (speed > 0_mps) { minChassisAcceleration = -minChassisAcceleration; - } else { + } else if (speed < 0_mps) { maxChassisAcceleration = -maxChassisAcceleration; } } diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp index eb230dac39..2d5cbdd062 100644 --- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.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. */ @@ -11,7 +11,9 @@ #include +#include "frc/geometry/Pose2d.h" #include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/TrajectoryGenerator.h" #include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" @@ -57,3 +59,26 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) { -maxVoltage - 0.05_V); } } + +TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) { + SimpleMotorFeedforward feedforward{1_V, 1_V / 1_mps, + 3_V / 1_mps_sq}; + // Large trackwidth - need to test with radius of curvature less than half of + // trackwidth + const DifferentialDriveKinematics kinematics{3_m}; + const auto maxVoltage = 10_V; + + auto config = TrajectoryConfig(12_fps, 12_fps_sq); + config.AddConstraint( + DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage)); + + EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory( + Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector{}, + Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config)); + + config.SetReversed(true); + + EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory( + Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector{}, + Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java index 71432bfddf..8525cf6aa1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.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. */ @@ -82,20 +82,41 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint // If moving forward, max acceleration constraint corresponds to wheel on outside of turn // If moving backward, max acceleration constraint corresponds to wheel on inside of turn - double maxChassisAcceleration = - maxWheelAcceleration - / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) - * Math.signum(velocityMetersPerSecond) / 2); - double minChassisAcceleration = - minWheelAcceleration - / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) - * Math.signum(velocityMetersPerSecond) / 2); + // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be + // turning on its center) - we have to treat this as a special case, as it breaks + // the signum function. Both max and min acceleration are *reduced in magnitude* + // in this case. + + double maxChassisAcceleration; + double minChassisAcceleration; + + if (velocityMetersPerSecond == 0) { + maxChassisAcceleration = + maxWheelAcceleration + / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2); + minChassisAcceleration = + minWheelAcceleration + / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2); + } else { + maxChassisAcceleration = + maxWheelAcceleration + / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) + * Math.signum(velocityMetersPerSecond) / 2); + minChassisAcceleration = + minWheelAcceleration + / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) + * Math.signum(velocityMetersPerSecond) / 2); + } + + // When turning about a point inside of the wheelbase (i.e. radius less than half + // the trackwidth), the inner wheel's direction changes, but the magnitude remains + // the same. The formula above changes sign for the inner wheel when this happens. + // We can accurately account for this by simply negating the inner wheel. - // Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) { if (velocityMetersPerSecond > 0) { minChassisAcceleration = -minChassisAcceleration; - } else { + } else if (velocityMetersPerSecond < 0) { maxChassisAcceleration = -maxChassisAcceleration; } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java index f5c6ff32a4..fff4c61ad7 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.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. */ @@ -7,16 +7,21 @@ package edu.wpi.first.wpilibj.trajectory; +import java.util.ArrayList; import java.util.Collections; import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertTrue; class DifferentialDriveVoltageConstraintTest { @@ -67,4 +72,32 @@ class DifferentialDriveVoltageConstraintTest { ); } } + + @Test + void testEndpointHighCurvature() { + var feedforward = new SimpleMotorFeedforward(1, 1, 3); + + // Large trackwidth - need to test with radius of curvature less than half of trackwidth + var kinematics = new DifferentialDriveKinematics(3); + double maxVoltage = 10; + var constraint = new DifferentialDriveVoltageConstraint(feedforward, + kinematics, + maxVoltage); + + var config = new TrajectoryConfig(12, 12).addConstraint(constraint); + + // Radius of curvature should be ~1 meter. + assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory( + new Pose2d(1, 0, Rotation2d.fromDegrees(90)), + new ArrayList(), + new Pose2d(0, 1, Rotation2d.fromDegrees(180)), + config)); + + assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 1, Rotation2d.fromDegrees(180)), + new ArrayList(), + new Pose2d(1, 0, Rotation2d.fromDegrees(90)), + config.setReversed(true))); + + } }