diff --git a/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp index 8114bb36dc..0448437349 100644 --- a/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp @@ -6,7 +6,6 @@ #include #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp index 625a3c0669..5e630edf16 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/AnalogGyroSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp index 14a659674d..aae5c0a629 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/AnalogInputSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp index c2b01cf87f..02692ca37b 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/AnalogOutputSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp index 09e0a8d09f..8faba0feff 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp @@ -6,7 +6,6 @@ #include #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp index 0f6aa88e10..58f3efb901 100644 --- a/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/BuiltInAccelerometerSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp b/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp index 0081928782..c9645d51a4 100644 --- a/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/CTREPCMSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index dcceec9701..bb46674899 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -6,7 +6,7 @@ #include -#include "frc/system/plant/LinearSystemId.h" +#include "frc/RobotController.h" using namespace frc; using namespace frc::sim; diff --git a/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp index 8b73b7c8f2..52f6899546 100644 --- a/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/DIOSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index d5addb2338..19f3ab6c7d 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -11,6 +11,7 @@ #include #include "frc/RobotController.h" +#include "frc/StateSpaceUtil.h" #include "frc/system/NumericalIntegration.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp index eb1e02ec9a..567637e82f 100644 --- a/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp @@ -6,7 +6,6 @@ #include #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp index bdefda9002..7b7441c858 100644 --- a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/DriverStationSim.h" #include -#include #include #include diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp index 29f2df4b41..c95093ccb4 100644 --- a/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp @@ -6,7 +6,6 @@ #include #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 13b3d2e4a3..4553dc4d0a 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -6,6 +6,7 @@ #include +#include "frc/RobotController.h" #include "frc/system/NumericalIntegration.h" #include "frc/system/plant/LinearSystemId.h" diff --git a/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp index 9709b4c4c9..b5a09e1f60 100644 --- a/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp @@ -6,7 +6,6 @@ #include #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 4f1daaa83e..b285f1f2b4 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -6,7 +6,7 @@ #include -#include "frc/system/plant/LinearSystemId.h" +#include "frc/RobotController.h" using namespace frc; using namespace frc::sim; diff --git a/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp index 9332c78b0b..fa0e3ec7a5 100644 --- a/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/PWMSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp b/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp index 844a1b677f..4c03f22db6 100644 --- a/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/PowerDistributionSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp index df7bc0284f..1f176bf93b 100644 --- a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/REVPHSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp index 84ce120d9f..10e95bf318 100644 --- a/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/RelaySim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp index 9483af30c8..d0737e642c 100644 --- a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp @@ -5,7 +5,6 @@ #include "frc/simulation/SPIAccelerometerSim.h" #include -#include #include diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 945decc320..f318d08300 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -9,6 +9,7 @@ #include #include +#include "frc/RobotController.h" #include "frc/system/NumericalIntegration.h" #include "frc/system/plant/LinearSystemId.h" diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 8685435394..788d5ceeed 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -10,7 +10,6 @@ #include #include "frc/EigenCore.h" -#include "frc/RobotController.h" #include "frc/StateSpaceUtil.h" #include "frc/system/LinearSystem.h" diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index e9848593a4..7c9c44609d 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -15,20 +15,6 @@ using namespace frc; -units::volt_t ArmFeedforward::Calculate(units::unit_t currentAngle, - units::unit_t currentVelocity, - units::unit_t nextVelocity, - units::second_t dt) const { - return Calculate(currentAngle, currentVelocity, nextVelocity); -} - -units::volt_t ArmFeedforward::Calculate( - units::unit_t currentAngle, - units::unit_t currentVelocity) const { - return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) + - kV * currentVelocity; -} - units::volt_t ArmFeedforward::Calculate( units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity) const { diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp index c60517a095..7c27f4e574 100644 --- a/wpimath/src/main/native/cpp/controller/BangBangController.cpp +++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp @@ -6,52 +6,8 @@ #include -#include "wpimath/MathShared.h" - using namespace frc; -BangBangController::BangBangController(double tolerance) - : m_tolerance(tolerance) {} - -void BangBangController::SetSetpoint(double setpoint) { - m_setpoint = setpoint; -} - -double BangBangController::GetSetpoint() const { - return m_setpoint; -} - -bool BangBangController::AtSetpoint() const { - return std::abs(m_setpoint - m_measurement) < m_tolerance; -} - -void BangBangController::SetTolerance(double tolerance) { - m_tolerance = tolerance; -} - -double BangBangController::GetTolerance() const { - return m_tolerance; -} - -double BangBangController::GetMeasurement() const { - return m_measurement; -} - -double BangBangController::GetError() const { - return m_setpoint - m_measurement; -} - -double BangBangController::Calculate(double measurement, double setpoint) { - m_measurement = measurement; - m_setpoint = setpoint; - - return measurement < setpoint ? 1 : 0; -} - -double BangBangController::Calculate(double measurement) { - return Calculate(measurement, m_setpoint); -} - void BangBangController::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("BangBangController"); builder.AddDoubleProperty( diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp index 05be6459f8..bcf625c319 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -4,35 +4,10 @@ #include "frc/controller/DifferentialDriveAccelerationLimiter.h" -#include - #include using namespace frc; -DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( - LinearSystem<2, 2, 2> system, units::meter_t trackwidth, - units::meters_per_second_squared_t maxLinearAccel, - units::radians_per_second_squared_t maxAngularAccel) - : DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel, - maxLinearAccel, maxAngularAccel) {} - -DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( - LinearSystem<2, 2, 2> system, units::meter_t trackwidth, - units::meters_per_second_squared_t minLinearAccel, - units::meters_per_second_squared_t maxLinearAccel, - units::radians_per_second_squared_t maxAngularAccel) - : m_system{std::move(system)}, - m_trackwidth{trackwidth}, - m_minLinearAccel{minLinearAccel}, - m_maxLinearAccel{maxLinearAccel}, - m_maxAngularAccel{maxAngularAccel} { - if (minLinearAccel > maxLinearAccel) { - throw std::invalid_argument( - "maxLinearAccel must be greater than minLinearAccel"); - } -} - DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate( units::meters_per_second_t leftVelocity, units::meters_per_second_t rightVelocity, units::volt_t leftVoltage, diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp index e458526cb6..d058fdf17c 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp @@ -7,31 +7,9 @@ #include #include "frc/controller/LinearPlantInversionFeedforward.h" -#include "frc/system/plant/LinearSystemId.h" using namespace frc; -DifferentialDriveFeedforward::DifferentialDriveFeedforward( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_rad_per_s) kVAngular, - decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) - // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps), - // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V / - // 1_rad_per_s_sq)) - : DifferentialDriveFeedforward{kVLinear, kALinear, - kVAngular * 2.0 / trackwidth * 1_rad, - kAAngular * 2.0 / trackwidth * 1_rad} {} - -DifferentialDriveFeedforward::DifferentialDriveFeedforward( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) - : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( - kVLinear, kALinear, kVAngular, kAAngular)}, - m_kVLinear{kVLinear}, - m_kALinear{kALinear}, - m_kVAngular{kVAngular}, - m_kAAngular{kAAngular} {} - DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate( units::meters_per_second_t currentLeftVelocity, units::meters_per_second_t nextLeftVelocity, diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp deleted file mode 100644 index 40ffecbb50..0000000000 --- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/controller/HolonomicDriveController.h" - -#include - -#include "units/angular_velocity.h" - -using namespace frc; - -HolonomicDriveController::HolonomicDriveController( - PIDController xController, PIDController yController, - ProfiledPIDController thetaController) - : m_xController(std::move(xController)), - m_yController(std::move(yController)), - m_thetaController(std::move(thetaController)) { - m_thetaController.EnableContinuousInput(0_deg, 360.0_deg); -} - -bool HolonomicDriveController::AtReference() const { - const auto& eTranslate = m_poseError.Translation(); - const auto& eRotate = m_rotationError; - const auto& tolTranslate = m_poseTolerance.Translation(); - const auto& tolRotate = m_poseTolerance.Rotation(); - return units::math::abs(eTranslate.X()) < tolTranslate.X() && - units::math::abs(eTranslate.Y()) < tolTranslate.Y() && - units::math::abs(eRotate.Radians()) < tolRotate.Radians(); -} - -void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) { - m_poseTolerance = tolerance; -} - -ChassisSpeeds HolonomicDriveController::Calculate( - const Pose2d& currentPose, const Pose2d& trajectoryPose, - units::meters_per_second_t desiredLinearVelocity, - const Rotation2d& desiredHeading) { - // If this is the first run, then we need to reset the theta controller to the - // current pose's heading. - if (m_firstRun) { - m_thetaController.Reset(currentPose.Rotation().Radians()); - m_firstRun = false; - } - - // Calculate feedforward velocities (field-relative) - auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos(); - auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin(); - auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate( - currentPose.Rotation().Radians(), desiredHeading.Radians())}; - - m_poseError = trajectoryPose.RelativeTo(currentPose); - m_rotationError = desiredHeading - currentPose.Rotation(); - - if (!m_enabled) { - return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF, - currentPose.Rotation()); - } - - // Calculate feedback velocities (based on position error). - auto xFeedback = units::meters_per_second_t{m_xController.Calculate( - currentPose.X().value(), trajectoryPose.X().value())}; - auto yFeedback = units::meters_per_second_t{m_yController.Calculate( - currentPose.Y().value(), trajectoryPose.Y().value())}; - - // Return next output. - return ChassisSpeeds::FromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation()); -} - -ChassisSpeeds HolonomicDriveController::Calculate( - const Pose2d& currentPose, const Trajectory::State& desiredState, - const Rotation2d& desiredHeading) { - return Calculate(currentPose, desiredState.pose, desiredState.velocity, - desiredHeading); -} - -void HolonomicDriveController::SetEnabled(bool enabled) { - m_enabled = enabled; -} - -PIDController& HolonomicDriveController::getXController() { - return m_xController; -} - -PIDController& HolonomicDriveController::getYController() { - return m_yController; -} - -ProfiledPIDController& -HolonomicDriveController::getThetaController() { - return m_thetaController; -} - -PIDController& HolonomicDriveController::GetXController() { - return m_xController; -} - -PIDController& HolonomicDriveController::GetYController() { - return m_yController; -} - -ProfiledPIDController& -HolonomicDriveController::GetThetaController() { - return m_thetaController; -} diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index eeb3e758c5..9685f10a4e 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -4,232 +4,10 @@ #include "frc/controller/PIDController.h" -#include -#include - #include -#include - -#include "frc/MathUtil.h" -#include "wpimath/MathShared.h" using namespace frc; -PIDController::PIDController(double Kp, double Ki, double Kd, - units::second_t period) - : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { - bool invalidGains = false; - if (Kp < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Kp must be a non-negative number, got {}!", Kp); - invalidGains = true; - } - if (Ki < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Ki must be a non-negative number, got {}!", Ki); - invalidGains = true; - } - if (Kd < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Kd must be a non-negative number, got {}!", Kd); - invalidGains = true; - } - if (invalidGains) { - m_Kp = 0.0; - m_Ki = 0.0; - m_Kd = 0.0; - wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0."); - } - - if (period <= 0_s) { - wpi::math::MathSharedStore::ReportError( - "Controller period must be a positive number, got {}!", period.value()); - m_period = 20_ms; - wpi::math::MathSharedStore::ReportWarning( - "Controller period defaulted to 20ms."); - } - static int instances = 0; - instances++; - - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kController_PIDController2, instances); - wpi::SendableRegistry::Add(this, "PIDController", instances); -} - -void PIDController::SetPID(double Kp, double Ki, double Kd) { - m_Kp = Kp; - m_Ki = Ki; - m_Kd = Kd; -} - -void PIDController::SetP(double Kp) { - m_Kp = Kp; -} - -void PIDController::SetI(double Ki) { - m_Ki = Ki; -} - -void PIDController::SetD(double Kd) { - m_Kd = Kd; -} - -void PIDController::SetIZone(double iZone) { - if (iZone < 0) { - wpi::math::MathSharedStore::ReportError( - "IZone must be a non-negative number, got {}!", iZone); - } - m_iZone = iZone; -} - -double PIDController::GetP() const { - return m_Kp; -} - -double PIDController::GetI() const { - return m_Ki; -} - -double PIDController::GetD() const { - return m_Kd; -} - -double PIDController::GetIZone() const { - return m_iZone; -} - -units::second_t PIDController::GetPeriod() const { - return m_period; -} - -double PIDController::GetPositionTolerance() const { - return m_errorTolerance; -} - -double PIDController::GetVelocityTolerance() const { - return m_errorDerivativeTolerance; -} - -double PIDController::GetAccumulatedError() const { - return m_totalError; -} - -void PIDController::SetSetpoint(double setpoint) { - m_setpoint = setpoint; - m_haveSetpoint = true; - - if (m_continuous) { - double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); - } else { - m_error = m_setpoint - m_measurement; - } - - m_errorDerivative = (m_error - m_prevError) / m_period.value(); -} - -double PIDController::GetSetpoint() const { - return m_setpoint; -} - -bool PIDController::AtSetpoint() const { - return m_haveMeasurement && m_haveSetpoint && - std::abs(m_error) < m_errorTolerance && - std::abs(m_errorDerivative) < m_errorDerivativeTolerance; -} - -void PIDController::EnableContinuousInput(double minimumInput, - double maximumInput) { - m_continuous = true; - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; -} - -void PIDController::DisableContinuousInput() { - m_continuous = false; -} - -bool PIDController::IsContinuousInputEnabled() const { - return m_continuous; -} - -void PIDController::SetIntegratorRange(double minimumIntegral, - double maximumIntegral) { - m_minimumIntegral = minimumIntegral; - m_maximumIntegral = maximumIntegral; -} - -void PIDController::SetTolerance(double errorTolerance, - double errorDerivativeTolerance) { - m_errorTolerance = errorTolerance; - m_errorDerivativeTolerance = errorDerivativeTolerance; -} - -double PIDController::GetErrorTolerance() const { - return m_errorTolerance; -} - -double PIDController::GetErrorDerivativeTolerance() const { - return m_errorDerivativeTolerance; -} - -double PIDController::GetError() const { - return m_error; -} - -double PIDController::GetErrorDerivative() const { - return m_errorDerivative; -} - -double PIDController::GetPositionError() const { - return m_error; -} - -double PIDController::GetVelocityError() const { - return m_errorDerivative; -} - -double PIDController::Calculate(double measurement) { - m_measurement = measurement; - m_prevError = m_error; - m_haveMeasurement = true; - - if (m_continuous) { - double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); - } else { - m_error = m_setpoint - m_measurement; - } - - m_errorDerivative = (m_error - m_prevError) / m_period.value(); - - // If the absolute value of the position error is outside of IZone, reset the - // total error - if (std::abs(m_error) > m_iZone) { - m_totalError = 0; - } else if (m_Ki != 0) { - m_totalError = - std::clamp(m_totalError + m_error * m_period.value(), - m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); - } - - return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative; -} - -double PIDController::Calculate(double measurement, double setpoint) { - m_setpoint = setpoint; - m_haveSetpoint = true; - return Calculate(measurement); -} - -void PIDController::Reset() { - m_error = 0; - m_prevError = 0; - m_totalError = 0; - m_errorDerivative = 0; - m_haveMeasurement = false; -} - void PIDController::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("PIDController"); builder.AddDoubleProperty( diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp deleted file mode 100644 index 70a64af1b5..0000000000 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/controller/RamseteController.h" - -#include - -#include "units/angle.h" -#include "units/math.h" - -using namespace frc; - -/** - * Returns sin(x) / x. - * - * @param x Value of which to take sinc(x). - */ -static decltype(1 / 1_rad) Sinc(units::radian_t x) { - if (units::math::abs(x) < 1e-9_rad) { - return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()}; - } else { - return units::math::sin(x) / x; - } -} - -RamseteController::RamseteController(units::unit_t b, - units::unit_t zeta) - : m_b{b}, m_zeta{zeta} {} - -WPI_IGNORE_DEPRECATED - -RamseteController::RamseteController() - : RamseteController{units::unit_t{2.0}, - units::unit_t{0.7}} {} - -WPI_UNIGNORE_DEPRECATED - -bool RamseteController::AtReference() const { - const auto& eTranslate = m_poseError.Translation(); - const auto& eRotate = m_poseError.Rotation(); - const auto& tolTranslate = m_poseTolerance.Translation(); - const auto& tolRotate = m_poseTolerance.Rotation(); - return units::math::abs(eTranslate.X()) < tolTranslate.X() && - units::math::abs(eTranslate.Y()) < tolTranslate.Y() && - units::math::abs(eRotate.Radians()) < tolRotate.Radians(); -} - -void RamseteController::SetTolerance(const Pose2d& poseTolerance) { - m_poseTolerance = poseTolerance; -} - -ChassisSpeeds RamseteController::Calculate( - const Pose2d& currentPose, const Pose2d& poseRef, - units::meters_per_second_t linearVelocityRef, - units::radians_per_second_t angularVelocityRef) { - if (!m_enabled) { - return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef}; - } - - m_poseError = poseRef.RelativeTo(currentPose); - - // Aliases for equation readability - const auto& eX = m_poseError.X(); - const auto& eY = m_poseError.Y(); - const auto& eTheta = m_poseError.Rotation().Radians(); - const auto& vRef = linearVelocityRef; - const auto& omegaRef = angularVelocityRef; - - // k = 2ζ√(ω_ref² + b v_ref²) - auto k = 2.0 * m_zeta * - units::math::sqrt(units::math::pow<2>(omegaRef) + - m_b * units::math::pow<2>(vRef)); - - // v_cmd = v_ref cos(e_θ) + k e_x - // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y - return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps, - omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY}; -} - -ChassisSpeeds RamseteController::Calculate( - const Pose2d& currentPose, const Trajectory::State& desiredState) { - return Calculate(currentPose, desiredState.pose, desiredState.velocity, - desiredState.velocity * desiredState.curvature); -} - -void RamseteController::SetEnabled(bool enabled) { - m_enabled = enabled; -} diff --git a/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp b/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp deleted file mode 100644 index f594cd6629..0000000000 --- a/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/interpolation/TimeInterpolatableBuffer.h" - -namespace frc { - -// Template specialization to ensure that Pose2d uses pose exponential -template <> -TimeInterpolatableBuffer::TimeInterpolatableBuffer( - units::second_t historySize) - : m_historySize(historySize), - m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) { - if (t < 0) { - return start; - } else if (t >= 1) { - return end; - } else { - Twist2d twist = start.Log(end); - Twist2d scaledTwist = twist * t; - return start.Exp(scaledTwist); - } - }) {} - -} // namespace frc diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp deleted file mode 100644 index 3885443719..0000000000 --- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/spline/CubicHermiteSpline.h" - -using namespace frc; - -CubicHermiteSpline::CubicHermiteSpline( - wpi::array xInitialControlVector, - wpi::array xFinalControlVector, - wpi::array yInitialControlVector, - wpi::array yFinalControlVector) - : m_initialControlVector{xInitialControlVector, yInitialControlVector}, - m_finalControlVector{xFinalControlVector, yFinalControlVector} { - const auto hermite = MakeHermiteBasis(); - const auto x = - ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); - const auto y = - ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); - - // Populate first two rows with coefficients. - m_coefficients.template block<1, 4>(0, 0) = hermite * x; - m_coefficients.template block<1, 4>(1, 0) = hermite * y; - - // Populate Row 2 and Row 3 with the derivatives of the equations above. - // Then populate row 4 and 5 with the second derivatives. - for (int i = 0; i < 4; i++) { - // Here, we are multiplying by (3 - i) to manually take the derivative. The - // power of the term in index 0 is 3, index 1 is 2 and so on. To find the - // coefficient of the derivative, we can use the power rule and multiply - // the existing coefficient by its power. - m_coefficients.template block<2, 1>(2, i) = - m_coefficients.template block<2, 1>(0, i) * (3 - i); - } - - for (int i = 0; i < 3; i++) { - // Here, we are multiplying by (2 - i) to manually take the derivative. The - // power of the term in index 0 is 2, index 1 is 1 and so on. To find the - // coefficient of the derivative, we can use the power rule and multiply - // the existing coefficient by its power. - m_coefficients.template block<2, 1>(4, i) = - m_coefficients.template block<2, 1>(2, i) * (2 - i); - } -} diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp deleted file mode 100644 index 65d7986a4e..0000000000 --- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/spline/QuinticHermiteSpline.h" - -using namespace frc; - -QuinticHermiteSpline::QuinticHermiteSpline( - wpi::array xInitialControlVector, - wpi::array xFinalControlVector, - wpi::array yInitialControlVector, - wpi::array yFinalControlVector) - : m_initialControlVector{xInitialControlVector, yInitialControlVector}, - m_finalControlVector{xFinalControlVector, yFinalControlVector} { - const auto hermite = MakeHermiteBasis(); - const auto x = - ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); - const auto y = - ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); - - // Populate first two rows with coefficients. - m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose(); - m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose(); - - // Populate Row 2 and Row 3 with the derivatives of the equations above. - // Then populate row 4 and 5 with the second derivatives. - for (int i = 0; i < 6; i++) { - // Here, we are multiplying by (5 - i) to manually take the derivative. The - // power of the term in index 0 is 5, index 1 is 4 and so on. To find the - // coefficient of the derivative, we can use the power rule and multiply - // the existing coefficient by its power. - m_coefficients.template block<2, 1>(2, i) = - m_coefficients.template block<2, 1>(0, i) * (5 - i); - } - for (int i = 0; i < 5; i++) { - // Here, we are multiplying by (4 - i) to manually take the derivative. The - // power of the term in index 0 is 4, index 1 is 3 and so on. To find the - // coefficient of the derivative, we can use the power rule and multiply - // the existing coefficient by its power. - m_coefficients.template block<2, 1>(4, i) = - m_coefficients.template block<2, 1>(2, i) * (4 - i); - } -} diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp deleted file mode 100644 index cd47ab0383..0000000000 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/spline/SplineHelper.h" - -#include -#include - -using namespace frc; - -std::vector SplineHelper::CubicSplinesFromControlVectors( - const Spline<3>::ControlVector& start, std::vector waypoints, - const Spline<3>::ControlVector& end) { - std::vector splines; - - wpi::array xInitial = start.x; - wpi::array yInitial = start.y; - wpi::array xFinal = end.x; - wpi::array yFinal = end.y; - - if (waypoints.size() > 1) { - waypoints.emplace(waypoints.begin(), - Translation2d{units::meter_t{xInitial[0]}, - units::meter_t{yInitial[0]}}); - waypoints.emplace_back( - Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}}); - - // Populate tridiagonal system for clamped cubic - /* See: - https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08 - /undervisningsmateriale/chap7alecture.pdf - */ - - // Above-diagonal of tridiagonal matrix, zero-padded - std::vector a; - // Diagonal of tridiagonal matrix - std::vector b(waypoints.size() - 2, 4.0); - // Below-diagonal of tridiagonal matrix, zero-padded - std::vector c; - // rhs vectors - std::vector dx, dy; - // solution vectors - std::vector fx(waypoints.size() - 2, 0.0), - fy(waypoints.size() - 2, 0.0); - - // populate above-diagonal and below-diagonal vectors - a.emplace_back(0); - for (size_t i = 0; i < waypoints.size() - 3; ++i) { - a.emplace_back(1); - c.emplace_back(1); - } - c.emplace_back(0); - - // populate rhs vectors - dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) - - xInitial[1]); - dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) - - yInitial[1]); - if (waypoints.size() > 4) { - for (size_t i = 1; i <= waypoints.size() - 4; ++i) { - // dx and dy represent the derivatives of the internal waypoints. The - // derivative of the second internal waypoint should involve the third - // and first internal waypoint, which have indices of 1 and 3 in the - // waypoints list (which contains ALL waypoints). - dx.emplace_back( - 3 * (waypoints[i + 2].X().value() - waypoints[i].X().value())); - dy.emplace_back( - 3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value())); - } - } - dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() - - waypoints[waypoints.size() - 3].X().value()) - - xFinal[1]); - dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() - - waypoints[waypoints.size() - 3].Y().value()) - - yFinal[1]); - - // Compute solution to tridiagonal system - ThomasAlgorithm(a, b, c, dx, &fx); - ThomasAlgorithm(a, b, c, dy, &fy); - - fx.emplace(fx.begin(), xInitial[1]); - fx.emplace_back(xFinal[1]); - fy.emplace(fy.begin(), yInitial[1]); - fy.emplace_back(yFinal[1]); - - for (size_t i = 0; i < fx.size() - 1; ++i) { - // Create the spline. - const CubicHermiteSpline spline{ - {waypoints[i].X().value(), fx[i]}, - {waypoints[i + 1].X().value(), fx[i + 1]}, - {waypoints[i].Y().value(), fy[i]}, - {waypoints[i + 1].Y().value(), fy[i + 1]}}; - - splines.push_back(spline); - } - } else if (waypoints.size() == 1) { - const double xDeriv = - (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0; - const double yDeriv = - (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0; - - wpi::array midXControlVector{waypoints[0].X().value(), xDeriv}; - wpi::array midYControlVector{waypoints[0].Y().value(), yDeriv}; - - splines.emplace_back(xInitial, midXControlVector, yInitial, - midYControlVector); - splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal); - - } else { - // Create the spline. - const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal}; - splines.push_back(spline); - } - - return splines; -} - -std::vector -SplineHelper::QuinticSplinesFromControlVectors( - const std::vector::ControlVector>& controlVectors) { - std::vector splines; - for (size_t i = 0; i < controlVectors.size() - 1; ++i) { - auto& xInitial = controlVectors[i].x; - auto& yInitial = controlVectors[i].y; - auto& xFinal = controlVectors[i + 1].x; - auto& yFinal = controlVectors[i + 1].y; - splines.emplace_back(xInitial, xFinal, yInitial, yFinal); - } - return splines; -} - -wpi::array::ControlVector, 2> -SplineHelper::CubicControlVectorsFromWaypoints( - const Pose2d& start, const std::vector& interiorWaypoints, - const Pose2d& end) { - double scalar; - if (interiorWaypoints.empty()) { - scalar = 1.2 * start.Translation().Distance(end.Translation()).value(); - } else { - scalar = - 1.2 * start.Translation().Distance(interiorWaypoints.front()).value(); - } - const auto initialCV = CubicControlVector(scalar, start); - if (!interiorWaypoints.empty()) { - scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value(); - } - const auto finalCV = CubicControlVector(scalar, end); - return {initialCV, finalCV}; -} - -std::vector SplineHelper::QuinticSplinesFromWaypoints( - const std::vector& waypoints) { - std::vector splines; - splines.reserve(waypoints.size() - 1); - for (size_t i = 0; i < waypoints.size() - 1; ++i) { - auto& p0 = waypoints[i]; - auto& p1 = waypoints[i + 1]; - - // This just makes the splines look better. - const auto scalar = - 1.2 * p0.Translation().Distance(p1.Translation()).value(); - - auto controlVectorA = QuinticControlVector(scalar, p0); - auto controlVectorB = QuinticControlVector(scalar, p1); - splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y, - controlVectorB.y); - } - return splines; -} - -std::vector SplineHelper::OptimizeCurvature( - const std::vector& splines) { - // If there's only one spline in the vector, we can't optimize anything so - // just return that. - if (splines.size() < 2) { - return splines; - } - - // Implements Section 4.1.2 of - // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf. - - // Cubic splines minimize the integral of the second derivative's absolute - // value. Therefore, we can create cubic splines with the same 0th and 1st - // derivatives and the provided quintic splines, find the second derivative of - // those cubic splines and then use a weighted average for the second - // derivatives of the quintic splines. - - std::vector optimizedSplines; - optimizedSplines.reserve(splines.size()); - optimizedSplines.push_back(splines[0]); - - for (size_t i = 0; i < splines.size() - 1; ++i) { - const auto& a = splines[i]; - const auto& b = splines[i + 1]; - - // Get the control vectors that created the quintic splines above. - const auto& aInitial = a.GetInitialControlVector(); - const auto& aFinal = a.GetFinalControlVector(); - const auto& bInitial = b.GetInitialControlVector(); - const auto& bFinal = b.GetFinalControlVector(); - - // Create cubic splines with the same control vectors. - auto Trim = [](const wpi::array& a) { - return wpi::array{a[0], a[1]}; - }; - CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y), - Trim(aFinal.y)}; - CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y), - Trim(bFinal.y)}; - - // Calculate the second derivatives at the knot points. - frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0}; - frc::Vectord<6> combinedA = ca.Coefficients() * bases; - - double ddxA = combinedA(4); - double ddyA = combinedA(5); - double ddxB = cb.Coefficients()(4, 1); - double ddyB = cb.Coefficients()(5, 1); - - // Calculate the parameters for weighted average. - double dAB = - std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]); - double dBC = - std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]); - double alpha = dBC / (dAB + dBC); - double beta = dAB / (dAB + dBC); - - // Calculate the weighted average. - double ddx = alpha * ddxA + beta * ddxB; - double ddy = alpha * ddyA + beta * ddyB; - - // Create new splines. - optimizedSplines[i] = {aInitial.x, - {aFinal.x[0], aFinal.x[1], ddx}, - aInitial.y, - {aFinal.y[0], aFinal.y[1], ddy}}; - optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx}, - bFinal.x, - {bInitial.y[0], bInitial.y[1], ddy}, - bFinal.y}); - } - - return optimizedSplines; -} - -void SplineHelper::ThomasAlgorithm(const std::vector& a, - const std::vector& b, - const std::vector& c, - const std::vector& d, - std::vector* solutionVector) { - auto& f = *solutionVector; - size_t N = d.size(); - - // Create the temporary vectors - // Note that this is inefficient as it is possible to call - // this function many times. A better implementation would - // pass these temporary matrices by non-const reference to - // save excess allocation and deallocation - std::vector c_star(N, 0.0); - std::vector d_star(N, 0.0); - - // This updates the coefficients in the first row - // Note that we should be checking for division by zero here - c_star[0] = c[0] / b[0]; - d_star[0] = d[0] / b[0]; - - // Create the c_star and d_star coefficients in the forward sweep - for (size_t i = 1; i < N; ++i) { - double m = 1.0 / (b[i] - a[i] * c_star[i - 1]); - c_star[i] = c[i] * m; - d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m; - } - - f[N - 1] = d_star[N - 1]; - // This is the reverse sweep, used to update the solution vector f - for (int i = N - 2; i >= 0; i--) { - f[i] = d_star[i] - c_star[i] * f[i + 1]; - } -} diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp deleted file mode 100644 index 26d239fed4..0000000000 --- a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/system/plant/LinearSystemId.h" - -using namespace frc; - -LinearSystem<2, 1, 2> LinearSystemId::ElevatorSystem(DCMotor motor, - units::kilogram_t mass, - units::meter_t radius, - double gearing) { - if (mass <= 0_kg) { - throw std::domain_error("mass must be greater than zero."); - } - if (radius <= 0_m) { - throw std::domain_error("radius must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, (-std::pow(gearing, 2) * motor.Kt / - (motor.R * units::math::pow<2>(radius) * mass * motor.Kv)) - .value()}}; - Matrixd<2, 1> B{0.0, - (gearing * motor.Kt / (motor.R * radius * mass)).value()}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 1> D{0.0, 0.0}; - - return LinearSystem<2, 1, 2>(A, B, C, D); -} - -LinearSystem<2, 1, 2> LinearSystemId::SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double gearing) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, - (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 1> D{{0.0}, {0.0}}; - - return LinearSystem<2, 1, 2>(A, B, C, D); -} - -LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) { - if (kVLinear <= decltype(kVLinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kALinear <= decltype(kALinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVAngular <= decltype(kVAngular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAAngular <= decltype(kAAngular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - - double A1 = -(kVLinear.value() / kALinear.value() + - kVAngular.value() / kAAngular.value()); - double A2 = -(kVLinear.value() / kALinear.value() - - kVAngular.value() / kAAngular.value()); - double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value(); - double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value(); - - Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}}; - Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; - - return LinearSystem<2, 2, 2>(A, B, C, D); -} - -LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_rad_per_s) kVAngular, - decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) { - if (kVLinear <= decltype(kVLinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kALinear <= decltype(kALinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVAngular <= decltype(kVAngular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAAngular <= decltype(kAAngular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - if (trackwidth <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - - // We want to find a factor to include in Kv,angular that will convert - // `u = Kv,angular omega` to `u = Kv,angular v`. - // - // v = omega r - // omega = v/r - // omega = 1/r v - // omega = 1/(trackwidth/2) v - // omega = 2/trackwidth v - // - // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) - // to V/(m/s). - return IdentifyDrivetrainSystem(kVLinear, kALinear, - kVAngular * 2.0 / trackwidth * 1_rad, - kAAngular * 2.0 / trackwidth * 1_rad); -} - -LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( - DCMotor motor, units::kilogram_square_meter_t J, double gearing) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<1, 1> A{ - (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; - Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()}; - Matrixd<1, 1> C{1.0}; - Matrixd<1, 1> D{0.0}; - - return LinearSystem<1, 1, 1>(A, B, C, D); -} - -LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( - DCMotor motor, units::kilogram_square_meter_t J, double gearing) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, - (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 1> D{0.0, 0.0}; - - return LinearSystem<2, 1, 2>(A, B, C, D); -} - -LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem( - const DCMotor& motor, units::kilogram_t mass, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double gearing) { - if (mass <= 0_kg) { - throw std::domain_error("mass must be greater than zero."); - } - if (r <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - if (rb <= 0_m) { - throw std::domain_error("rb must be greater than zero."); - } - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - auto C1 = -std::pow(gearing, 2) * motor.Kt / - (motor.Kv * motor.R * units::math::pow<2>(r)); - auto C2 = gearing * motor.Kt / (motor.R * r); - - Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(), - ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()}, - {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(), - ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}}; - Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(), - ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()}, - {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(), - ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; - - return LinearSystem<2, 2, 2>(A, B, C, D); -} diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index 467c3a89d0..ef3591998b 100644 --- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -4,150 +4,10 @@ #include "frc/trajectory/Trajectory.h" -#include -#include -#include - -#include #include -#include "units/math.h" - using namespace frc; -Trajectory::State Trajectory::State::Interpolate(State endValue, - double i) const { - // Find the new [t] value. - const auto newT = wpi::Lerp(t, endValue.t, i); - - // Find the delta time between the current state and the interpolated state. - const auto deltaT = newT - t; - - // If delta time is negative, flip the order of interpolation. - if (deltaT < 0_s) { - return endValue.Interpolate(*this, 1.0 - i); - } - - // Check whether the robot is reversing at this stage. - const auto reversing = - velocity < 0_mps || - (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq); - - // Calculate the new velocity. - // v = v_0 + at - const units::meters_per_second_t newV = velocity + (acceleration * deltaT); - - // Calculate the change in position. - // delta_s = v_0 t + 0.5at² - const units::meter_t newS = - (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) * - (reversing ? -1.0 : 1.0); - - // Return the new state. To find the new position for the new state, we need - // to interpolate between the two endpoint poses. The fraction for - // interpolation is the change in position (delta s) divided by the total - // distance between the two endpoints. - const double interpolationFrac = - newS / endValue.pose.Translation().Distance(pose.Translation()); - - return {newT, newV, acceleration, - wpi::Lerp(pose, endValue.pose, interpolationFrac), - wpi::Lerp(curvature, endValue.curvature, interpolationFrac)}; -} - -Trajectory::Trajectory(const std::vector& states) : m_states(states) { - if (m_states.empty()) { - throw std::invalid_argument( - "Trajectory manually initialized with no states."); - } - - m_totalTime = states.back().t; -} - -Trajectory::State Trajectory::Sample(units::second_t t) const { - if (m_states.empty()) { - throw std::runtime_error( - "Trajectory cannot be sampled if it has no states."); - } - - if (t <= m_states.front().t) { - return m_states.front(); - } - if (t >= m_totalTime) { - return m_states.back(); - } - - // Use binary search to get the element with a timestamp no less than the - // requested timestamp. This starts at 1 because we use the previous state - // later on for interpolation. - auto sample = - std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t, - [](const auto& a, const auto& b) { return a.t < b; }); - - auto prevSample = sample - 1; - - // The sample's timestamp is now greater than or equal to the requested - // timestamp. If it is greater, we need to interpolate between the - // previous state and the current state to get the exact state that we - // want. - - // If the difference in states is negligible, then we are spot on! - if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) { - return *sample; - } - // Interpolate between the two states for the state that we want. - return prevSample->Interpolate( - *sample, (t - prevSample->t) / (sample->t - prevSample->t)); -} - -Trajectory Trajectory::TransformBy(const Transform2d& transform) { - auto& firstState = m_states[0]; - auto& firstPose = firstState.pose; - - // Calculate the transformed first pose. - auto newFirstPose = firstPose + transform; - auto newStates = m_states; - newStates[0].pose = newFirstPose; - - for (unsigned int i = 1; i < newStates.size(); i++) { - auto& state = newStates[i]; - // We are transforming relative to the coordinate frame of the new initial - // pose. - state.pose = newFirstPose + (state.pose - firstPose); - } - - return Trajectory(newStates); -} - -Trajectory Trajectory::RelativeTo(const Pose2d& pose) { - auto newStates = m_states; - for (auto& state : newStates) { - state.pose = state.pose.RelativeTo(pose); - } - return Trajectory(newStates); -} - -Trajectory Trajectory::operator+(const Trajectory& other) const { - // If this is a default constructed trajectory with no states, then we can - // simply return the rhs trajectory. - if (m_states.empty()) { - return other; - } - - auto states = m_states; - auto otherStates = other.States(); - for (auto& otherState : otherStates) { - otherState.t += m_totalTime; - } - - // Here we omit the first state of the other trajectory because we don't want - // two time points with different states. Sample() will automatically - // interpolate between the end of this trajectory and the second state of the - // other trajectory. - states.insert(states.end(), otherStates.begin() + 1, otherStates.end()); - return Trajectory(states); -} - void frc::to_json(wpi::json& json, const Trajectory::State& state) { json = wpi::json{{"time", state.t.value()}, {"velocity", state.velocity.value()}, diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp deleted file mode 100644 index 40913a806b..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" - -#include "units/math.h" - -using namespace frc; - -CentripetalAccelerationConstraint::CentripetalAccelerationConstraint( - units::meters_per_second_squared_t maxCentripetalAcceleration) - : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {} - -units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - // ac = v²/r - // k (curvature) = 1/r - - // therefore, ac = v²k - // ac/k = v² - // v = √(ac/k) - - // We have to multiply by 1_rad here to get the units to cancel out nicely. - // The units library defines a unit for radians although it is technically - // unitless. - return units::math::sqrt(m_maxCentripetalAcceleration / - units::math::abs(curvature) * 1_rad); -} - -TrajectoryConstraint::MinMax -CentripetalAccelerationConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - // The acceleration of the robot has no impact on the centripetal acceleration - // of the robot. - return {}; -} diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp deleted file mode 100644 index 8e4093ca70..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" - -#include - -using namespace frc; - -DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint( - DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed) - : m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {} - -units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - auto wheelSpeeds = - m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature}); - wheelSpeeds.Desaturate(m_maxSpeed); - - return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx; -} - -TrajectoryConstraint::MinMax -DifferentialDriveKinematicsConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - return {}; -} diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp deleted file mode 100644 index f18012804c..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h" - -#include -#include -#include - -#include - -#include "units/math.h" - -using namespace frc; - -DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint( - const SimpleMotorFeedforward& feedforward, - DifferentialDriveKinematics kinematics, units::volt_t maxVoltage) - : m_feedforward(feedforward), - m_kinematics(std::move(kinematics)), - m_maxVoltage(maxVoltage) {} - -units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - return units::meters_per_second_t{std::numeric_limits::max()}; -} - -TrajectoryConstraint::MinMax -DifferentialDriveVoltageConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - auto wheelSpeeds = - m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature}); - - auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right); - auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right); - - // Calculate maximum/minimum possible accelerations from motor dynamics - // and max/min wheel speeds - auto maxWheelAcceleration = - m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed); - auto minWheelAcceleration = - m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed); - - // 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. - - // 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 - - // 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. - - if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) { - if (speed > 0_mps) { - minChassisAcceleration = -minChassisAcceleration; - } else if (speed < 0_mps) { - maxChassisAcceleration = -maxChassisAcceleration; - } - } - - return {minChassisAcceleration, maxChassisAcceleration}; -} diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp deleted file mode 100644 index 9e6e712923..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/trajectory/constraint/MaxVelocityConstraint.h" - -using namespace frc; - -MaxVelocityConstraint::MaxVelocityConstraint( - units::meters_per_second_t maxVelocity) - : m_maxVelocity(units::math::abs(maxVelocity)) {} - -units::meters_per_second_t MaxVelocityConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - return m_maxVelocity; -} - -TrajectoryConstraint::MinMax MaxVelocityConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - return {}; -} diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp deleted file mode 100644 index 391b99f7ed..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h" - -#include "units/math.h" - -using namespace frc; - -MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint( - const MecanumDriveKinematics& kinematics, - units::meters_per_second_t maxSpeed) - : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {} - -units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - auto xVelocity = velocity * pose.Rotation().Cos(); - auto yVelocity = velocity * pose.Rotation().Sin(); - auto wheelSpeeds = - m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature}); - wheelSpeeds.Desaturate(m_maxSpeed); - - auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); - - return units::math::hypot(normSpeeds.vx, normSpeeds.vy); -} - -TrajectoryConstraint::MinMax -MecanumDriveKinematicsConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - return {}; -} diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 9503b5ecb4..ae90ec0088 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -81,9 +81,9 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @return The computed feedforward, in volts. */ [[deprecated("Use the current/next velocity overload instead.")]] - units::volt_t Calculate(units::unit_t angle, - units::unit_t velocity, - units::unit_t acceleration) const { + constexpr units::volt_t Calculate( + units::unit_t angle, units::unit_t velocity, + units::unit_t acceleration) const { return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) + kV * velocity + kA * acceleration; } @@ -105,7 +105,9 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::volt_t Calculate(units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity, - units::second_t dt) const; + units::second_t dt) const { + return Calculate(currentAngle, currentVelocity, nextVelocity); + } /** * Calculates the feedforward from the gains and setpoint assuming discrete @@ -118,8 +120,12 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param currentVelocity The current velocity. * @return The computed feedforward in volts. */ - units::volt_t Calculate(units::unit_t currentAngle, - units::unit_t currentVelocity) const; + constexpr units::volt_t Calculate( + units::unit_t currentAngle, + units::unit_t currentVelocity) const { + return kS * wpi::sgn(currentVelocity) + + kG * units::math::cos(currentAngle) + kV * currentVelocity; + } /** * Calculates the feedforward from the gains and setpoints assuming discrete @@ -156,7 +162,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param acceleration The acceleration of the arm. * @return The maximum possible velocity at the given acceleration and angle. */ - units::unit_t MaxAchievableVelocity( + constexpr units::unit_t MaxAchievableVelocity( units::volt_t maxVoltage, units::unit_t angle, units::unit_t acceleration) { // Assume max velocity is positive @@ -181,7 +187,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param acceleration The acceleration of the arm. * @return The minimum possible velocity at the given acceleration and angle. */ - units::unit_t MinAchievableVelocity( + constexpr units::unit_t MinAchievableVelocity( units::volt_t maxVoltage, units::unit_t angle, units::unit_t acceleration) { // Assume min velocity is negative, ks flips sign @@ -206,7 +212,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param velocity The velocity of the arm. * @return The maximum possible acceleration at the given velocity and angle. */ - units::unit_t MaxAchievableAcceleration( + constexpr units::unit_t MaxAchievableAcceleration( units::volt_t maxVoltage, units::unit_t angle, units::unit_t velocity) { return (maxVoltage - kS * wpi::sgn(velocity) - @@ -230,7 +236,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param velocity The velocity of the arm. * @return The minimum possible acceleration at the given velocity and angle. */ - units::unit_t MinAchievableAcceleration( + constexpr units::unit_t MinAchievableAcceleration( units::volt_t maxVoltage, units::unit_t angle, units::unit_t velocity) { return MaxAchievableAcceleration(-maxVoltage, angle, velocity); @@ -241,28 +247,28 @@ class WPILIB_DLLEXPORT ArmFeedforward { * * @return The static gain. */ - units::volt_t GetKs() const { return kS; } + constexpr units::volt_t GetKs() const { return kS; } /** * Returns the gravity gain. * * @return The gravity gain. */ - units::volt_t GetKg() const { return kG; } + constexpr units::volt_t GetKg() const { return kG; } /** * Returns the velocity gain. * * @return The velocity gain. */ - units::unit_t GetKv() const { return kV; } + constexpr units::unit_t GetKv() const { return kV; } /** * Returns the acceleration gain. * * @return The acceleration gain. */ - units::unit_t GetKa() const { return kA; } + constexpr units::unit_t GetKa() const { return kA; } private: /// The static gain, in volts. diff --git a/wpimath/src/main/native/include/frc/controller/BangBangController.h b/wpimath/src/main/native/include/frc/controller/BangBangController.h index 6058c81ebc..3698d830a9 100644 --- a/wpimath/src/main/native/include/frc/controller/BangBangController.h +++ b/wpimath/src/main/native/include/frc/controller/BangBangController.h @@ -6,6 +6,7 @@ #include +#include #include #include #include @@ -37,57 +38,60 @@ class WPILIB_DLLEXPORT BangBangController * * @param tolerance Tolerance for atSetpoint. */ - explicit BangBangController( - double tolerance = std::numeric_limits::infinity()); + constexpr explicit BangBangController( + double tolerance = std::numeric_limits::infinity()) + : m_tolerance(tolerance) {} /** * Sets the setpoint for the bang-bang controller. * * @param setpoint The desired setpoint. */ - void SetSetpoint(double setpoint); + constexpr void SetSetpoint(double setpoint) { m_setpoint = setpoint; } /** * Returns the current setpoint of the bang-bang controller. * * @return The current setpoint. */ - double GetSetpoint() const; + constexpr double GetSetpoint() const { return m_setpoint; } /** * Returns true if the error is within the tolerance of the setpoint. * * @return Whether the error is within the acceptable bounds. */ - bool AtSetpoint() const; + constexpr bool AtSetpoint() const { + return gcem::abs(m_setpoint - m_measurement) < m_tolerance; + } /** * Sets the error within which AtSetpoint will return true. * * @param tolerance Position error which is tolerable. */ - void SetTolerance(double tolerance); + constexpr void SetTolerance(double tolerance) { m_tolerance = tolerance; } /** * Returns the current tolerance of the controller. * * @return The current tolerance. */ - double GetTolerance() const; + constexpr double GetTolerance() const { return m_tolerance; } /** * Returns the current measurement of the process variable. * * @return The current measurement of the process variable. */ - double GetMeasurement() const; + constexpr double GetMeasurement() const { return m_measurement; } /** * Returns the current error. * * @return The current error. */ - double GetError() const; + constexpr double GetError() const { return m_setpoint - m_measurement; } /** * Returns the calculated control output. @@ -99,7 +103,12 @@ class WPILIB_DLLEXPORT BangBangController * @param setpoint The setpoint for the process variable. * @return The calculated motor output (0 or 1). */ - double Calculate(double measurement, double setpoint); + constexpr double Calculate(double measurement, double setpoint) { + m_measurement = measurement; + m_setpoint = setpoint; + + return measurement < setpoint ? 1 : 0; + } /** * Returns the calculated control output. @@ -107,7 +116,9 @@ class WPILIB_DLLEXPORT BangBangController * @param measurement The most recent measurement of the process variable. * @return The calculated motor output (0 or 1). */ - double Calculate(double measurement); + constexpr double Calculate(double measurement) { + return Calculate(measurement, m_setpoint); + } void InitSendable(wpi::SendableBuilder& builder) override; diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h index 108d590b9b..e2741e5d00 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -38,7 +40,10 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { DifferentialDriveAccelerationLimiter( LinearSystem<2, 2, 2> system, units::meter_t trackwidth, units::meters_per_second_squared_t maxLinearAccel, - units::radians_per_second_squared_t maxAngularAccel); + units::radians_per_second_squared_t maxAngularAccel) + : DifferentialDriveAccelerationLimiter(system, trackwidth, + -maxLinearAccel, maxLinearAccel, + maxAngularAccel) {} /** * Constructs a DifferentialDriveAccelerationLimiter. @@ -56,7 +61,17 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { LinearSystem<2, 2, 2> system, units::meter_t trackwidth, units::meters_per_second_squared_t minLinearAccel, units::meters_per_second_squared_t maxLinearAccel, - units::radians_per_second_squared_t maxAngularAccel); + units::radians_per_second_squared_t maxAngularAccel) + : m_system{std::move(system)}, + m_trackwidth{trackwidth}, + m_minLinearAccel{minLinearAccel}, + m_maxLinearAccel{maxLinearAccel}, + m_maxAngularAccel{maxAngularAccel} { + if (minLinearAccel > maxLinearAccel) { + throw std::invalid_argument( + "maxLinearAccel must be greater than minLinearAccel"); + } + } /** * Returns the next voltage pair subject to acceleration constraints. diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h index ff4ed61015..7947e5e8c8 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h @@ -8,6 +8,7 @@ #include "frc/controller/DifferentialDriveWheelVoltages.h" #include "frc/system/LinearSystem.h" +#include "frc/system/plant/LinearSystemId.h" #include "units/acceleration.h" #include "units/angular_acceleration.h" #include "units/angular_velocity.h" @@ -38,11 +39,16 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward { * @param trackwidth The distance between the differential drive's left and * right wheels, in meters. */ - DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear, - decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_rad_per_s) kVAngular, - decltype(1_V / 1_rad_per_s_sq) kAAngular, - units::meter_t trackwidth); + constexpr DifferentialDriveFeedforward( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_rad_per_s) kVAngular, + decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) + // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps), + // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V / + // 1_rad_per_s_sq)) + : DifferentialDriveFeedforward{kVLinear, kALinear, + kVAngular * 2.0 / trackwidth * 1_rad, + kAAngular * 2.0 / trackwidth * 1_rad} {} /** * Creates a new DifferentialDriveFeedforward with the specified parameters. @@ -55,10 +61,16 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward { * @param kAAngular The angular acceleration gain in volts per (meters per * second squared). */ - DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear, - decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_mps) kVAngular, - decltype(1_V / 1_mps_sq) kAAngular); + constexpr DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear, + decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, + decltype(1_V / 1_mps_sq) kAAngular) + : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular)}, + m_kVLinear{kVLinear}, + m_kALinear{kALinear}, + m_kVAngular{kVAngular}, + m_kAAngular{kAAngular} {} /** * Calculates the differential drive feedforward inputs given velocity diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index d3f7706601..2ccfef1add 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -77,8 +77,9 @@ class ElevatorFeedforward { * @deprecated Use the current/next velocity overload instead. */ [[deprecated("Use the current/next velocity overload instead.")]] - units::volt_t Calculate(units::unit_t velocity, - units::unit_t acceleration) const { + constexpr units::volt_t Calculate( + units::unit_t velocity, + units::unit_t acceleration) const { return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration; } @@ -99,8 +100,8 @@ class ElevatorFeedforward { auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; - Vectord<1> r{currentVelocity.value()}; - Vectord<1> nextR{nextVelocity.value()}; + Vectord<1> r{{currentVelocity.value()}}; + Vectord<1> nextR{{nextVelocity.value()}}; return kG + kS * wpi::sgn(currentVelocity.value()) + units::volt_t{feedforward.Calculate(r, nextR)(0)}; @@ -220,28 +221,28 @@ class ElevatorFeedforward { * * @return The static gain. */ - units::volt_t GetKs() const { return kS; } + constexpr units::volt_t GetKs() const { return kS; } /** * Returns the gravity gain. * * @return The gravity gain. */ - units::volt_t GetKg() const { return kG; } + constexpr units::volt_t GetKg() const { return kG; } /** * Returns the velocity gain. * * @return The velocity gain. */ - units::unit_t GetKv() const { return kV; } + constexpr units::unit_t GetKv() const { return kV; } /** * Returns the acceleration gain. * * @return The acceleration gain. */ - units::unit_t GetKa() const { return kA; } + constexpr units::unit_t GetKa() const { return kA; } private: /// The static gain. diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index 942126df2c..a78fdd85c1 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include "frc/controller/PIDController.h" @@ -13,6 +15,7 @@ #include "frc/kinematics/ChassisSpeeds.h" #include "frc/trajectory/Trajectory.h" #include "units/angle.h" +#include "units/angular_velocity.h" #include "units/velocity.h" namespace frc { @@ -41,20 +44,34 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @param thetaController A profiled PID controller to respond to error in * angle. */ - HolonomicDriveController( + constexpr HolonomicDriveController( PIDController xController, PIDController yController, - ProfiledPIDController thetaController); + ProfiledPIDController thetaController) + : m_xController(std::move(xController)), + m_yController(std::move(yController)), + m_thetaController(std::move(thetaController)) { + m_thetaController.EnableContinuousInput(0_deg, 360.0_deg); + } - HolonomicDriveController(const HolonomicDriveController&) = default; - HolonomicDriveController& operator=(const HolonomicDriveController&) = + constexpr HolonomicDriveController(const HolonomicDriveController&) = default; + constexpr HolonomicDriveController& operator=( + const HolonomicDriveController&) = default; + constexpr HolonomicDriveController(HolonomicDriveController&&) = default; + constexpr HolonomicDriveController& operator=(HolonomicDriveController&&) = default; - HolonomicDriveController(HolonomicDriveController&&) = default; - HolonomicDriveController& operator=(HolonomicDriveController&&) = default; /** * Returns true if the pose error is within tolerance of the reference. */ - bool AtReference() const; + constexpr bool AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_rotationError; + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); + } /** * Sets the pose error which is considered tolerable for use with @@ -62,7 +79,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * * @param tolerance Pose error which is tolerable. */ - void SetTolerance(const Pose2d& tolerance); + constexpr void SetTolerance(const Pose2d& tolerance) { + m_poseTolerance = tolerance; + } /** * Returns the next output of the holonomic drive controller. @@ -75,10 +94,41 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @param desiredHeading The desired heading. * @return The next output of the holonomic drive controller. */ - ChassisSpeeds Calculate(const Pose2d& currentPose, - const Pose2d& trajectoryPose, - units::meters_per_second_t desiredLinearVelocity, - const Rotation2d& desiredHeading); + constexpr ChassisSpeeds Calculate( + const Pose2d& currentPose, const Pose2d& trajectoryPose, + units::meters_per_second_t desiredLinearVelocity, + const Rotation2d& desiredHeading) { + // If this is the first run, then we need to reset the theta controller to + // the current pose's heading. + if (m_firstRun) { + m_thetaController.Reset(currentPose.Rotation().Radians()); + m_firstRun = false; + } + + // Calculate feedforward velocities (field-relative) + auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos(); + auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin(); + auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate( + currentPose.Rotation().Radians(), desiredHeading.Radians())}; + + m_poseError = trajectoryPose.RelativeTo(currentPose); + m_rotationError = desiredHeading - currentPose.Rotation(); + + if (!m_enabled) { + return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF, + currentPose.Rotation()); + } + + // Calculate feedback velocities (based on position error). + auto xFeedback = units::meters_per_second_t{m_xController.Calculate( + currentPose.X().value(), trajectoryPose.X().value())}; + auto yFeedback = units::meters_per_second_t{m_yController.Calculate( + currentPose.Y().value(), trajectoryPose.Y().value())}; + + // Return next output. + return ChassisSpeeds::FromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation()); + } /** * Returns the next output of the holonomic drive controller. @@ -90,9 +140,12 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @param desiredHeading The desired heading. * @return The next output of the holonomic drive controller. */ - ChassisSpeeds Calculate(const Pose2d& currentPose, - const Trajectory::State& desiredState, - const Rotation2d& desiredHeading); + constexpr ChassisSpeeds Calculate(const Pose2d& currentPose, + const Trajectory::State& desiredState, + const Rotation2d& desiredHeading) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + desiredHeading); + } /** * Enables and disables the controller for troubleshooting purposes. When @@ -101,7 +154,7 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * * @param enabled If the controller is enabled or not. */ - void SetEnabled(bool enabled); + constexpr void SetEnabled(bool enabled) { m_enabled = enabled; } /** * Returns the X PIDController @@ -109,7 +162,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @deprecated Use GetXController() instead. */ [[deprecated("Use GetXController() instead")]] - PIDController& getXController(); + constexpr PIDController& getXController() { + return m_xController; + } /** * Returns the Y PIDController @@ -117,7 +172,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @deprecated Use GetYController() instead. */ [[deprecated("Use GetYController() instead")]] - PIDController& getYController(); + constexpr PIDController& getYController() { + return m_yController; + } /** * Returns the rotation ProfiledPIDController @@ -125,22 +182,26 @@ class WPILIB_DLLEXPORT HolonomicDriveController { * @deprecated Use GetThetaController() instead. */ [[deprecated("Use GetThetaController() instead")]] - ProfiledPIDController& getThetaController(); + constexpr ProfiledPIDController& getThetaController() { + return m_thetaController; + } /** * Returns the X PIDController */ - PIDController& GetXController(); + constexpr PIDController& GetXController() { return m_xController; } /** * Returns the Y PIDController */ - PIDController& GetYController(); + constexpr PIDController& GetYController() { return m_yController; } /** * Returns the rotation ProfiledPIDController */ - ProfiledPIDController& GetThetaController(); + constexpr ProfiledPIDController& GetThetaController() { + return m_thetaController; + } private: Pose2d m_poseError; diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h index c202462c97..fde4081d08 100644 --- a/wpimath/src/main/native/include/frc/controller/PIDController.h +++ b/wpimath/src/main/native/include/frc/controller/PIDController.h @@ -4,14 +4,20 @@ #pragma once -#include +#include +#include #include +#include +#include #include #include #include +#include +#include "frc/MathUtil.h" #include "units/time.h" +#include "wpimath/MathShared.h" namespace frc { @@ -31,15 +37,55 @@ class WPILIB_DLLEXPORT PIDController * @param period The period between controller updates in seconds. The * default is 20 milliseconds. Must be positive. */ - PIDController(double Kp, double Ki, double Kd, - units::second_t period = 20_ms); + constexpr PIDController(double Kp, double Ki, double Kd, + units::second_t period = 20_ms) + : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { + bool invalidGains = false; + if (Kp < 0.0) { + wpi::math::MathSharedStore::ReportError( + "Kp must be a non-negative number, got {}!", Kp); + invalidGains = true; + } + if (Ki < 0.0) { + wpi::math::MathSharedStore::ReportError( + "Ki must be a non-negative number, got {}!", Ki); + invalidGains = true; + } + if (Kd < 0.0) { + wpi::math::MathSharedStore::ReportError( + "Kd must be a non-negative number, got {}!", Kd); + invalidGains = true; + } + if (invalidGains) { + m_Kp = 0.0; + m_Ki = 0.0; + m_Kd = 0.0; + wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0."); + } - ~PIDController() override = default; + if (period <= 0_s) { + wpi::math::MathSharedStore::ReportError( + "Controller period must be a positive number, got {}!", + period.value()); + m_period = 20_ms; + wpi::math::MathSharedStore::ReportWarning( + "Controller period defaulted to 20ms."); + } + if (!std::is_constant_evaluated()) { + ++instances; - PIDController(const PIDController&) = default; - PIDController& operator=(const PIDController&) = default; - PIDController(PIDController&&) = default; - PIDController& operator=(PIDController&&) = default; + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kController_PIDController2, instances); + wpi::SendableRegistry::Add(this, "PIDController", instances); + } + } + + constexpr ~PIDController() override = default; + + constexpr PIDController(const PIDController&) = default; + constexpr PIDController& operator=(const PIDController&) = default; + constexpr PIDController(PIDController&&) = default; + constexpr PIDController& operator=(PIDController&&) = default; /** * Sets the PID Controller gain parameters. @@ -50,28 +96,32 @@ class WPILIB_DLLEXPORT PIDController * @param Ki The integral coefficient. Must be >= 0. * @param Kd The differential coefficient. Must be >= 0. */ - void SetPID(double Kp, double Ki, double Kd); + constexpr void SetPID(double Kp, double Ki, double Kd) { + m_Kp = Kp; + m_Ki = Ki; + m_Kd = Kd; + } /** * Sets the proportional coefficient of the PID controller gain. * * @param Kp The proportional coefficient. Must be >= 0. */ - void SetP(double Kp); + constexpr void SetP(double Kp) { m_Kp = Kp; } /** * Sets the integral coefficient of the PID controller gain. * * @param Ki The integral coefficient. Must be >= 0. */ - void SetI(double Ki); + constexpr void SetI(double Ki) { m_Ki = Ki; } /** * Sets the differential coefficient of the PID controller gain. * * @param Kd The differential coefficient. Must be >= 0. */ - void SetD(double Kd); + constexpr void SetD(double Kd) { m_Kd = Kd; } /** * Sets the IZone range. When the absolute value of the position error is @@ -84,56 +134,64 @@ class WPILIB_DLLEXPORT PIDController * @param iZone Maximum magnitude of error to allow integral control. Must be * >= 0. */ - void SetIZone(double iZone); + constexpr void SetIZone(double iZone) { + if (std::is_constant_evaluated() && iZone < 0) { + wpi::math::MathSharedStore::ReportError( + "IZone must be a non-negative number, got {}!", iZone); + } + m_iZone = iZone; + } /** * Gets the proportional coefficient. * * @return proportional coefficient */ - double GetP() const; + constexpr double GetP() const { return m_Kp; } /** * Gets the integral coefficient. * * @return integral coefficient */ - double GetI() const; + constexpr double GetI() const { return m_Ki; } /** * Gets the differential coefficient. * * @return differential coefficient */ - double GetD() const; + constexpr double GetD() const { return m_Kd; } /** * Get the IZone range. * * @return Maximum magnitude of error to allow integral control. */ - double GetIZone() const; + constexpr double GetIZone() const { return m_iZone; } /** * Gets the period of this controller. * * @return The period of the controller. */ - units::second_t GetPeriod() const; + constexpr units::second_t GetPeriod() const { return m_period; } /** * Gets the error tolerance of this controller. * * @return The error tolerance of the controller. */ - double GetErrorTolerance() const; + constexpr double GetErrorTolerance() const { return m_errorTolerance; } /** * Gets the error derivative tolerance of this controller. * * @return The error derivative tolerance of the controller. */ - double GetErrorDerivativeTolerance() const; + constexpr double GetErrorDerivativeTolerance() const { + return m_errorDerivativeTolerance; + } /** * Gets the position tolerance of this controller. @@ -142,7 +200,9 @@ class WPILIB_DLLEXPORT PIDController * @deprecated Use GetErrorTolerance() instead. */ [[deprecated("Use the GetErrorTolerance method instead.")]] - double GetPositionTolerance() const; + constexpr double GetPositionTolerance() const { + return m_errorTolerance; + } /** * Gets the velocity tolerance of this controller. @@ -151,7 +211,9 @@ class WPILIB_DLLEXPORT PIDController * @deprecated Use GetErrorDerivativeTolerance() instead. */ [[deprecated("Use the GetErrorDerivativeTolerance method instead.")]] - double GetVelocityTolerance() const; + constexpr double GetVelocityTolerance() const { + return m_errorDerivativeTolerance; + } /** * Gets the accumulated error used in the integral calculation of this @@ -159,28 +221,45 @@ class WPILIB_DLLEXPORT PIDController * * @return The accumulated error of this controller. */ - double GetAccumulatedError() const; + constexpr double GetAccumulatedError() const { return m_totalError; } /** * Sets the setpoint for the PIDController. * * @param setpoint The desired setpoint. */ - void SetSetpoint(double setpoint); + constexpr void SetSetpoint(double setpoint) { + m_setpoint = setpoint; + m_haveSetpoint = true; + + if (m_continuous) { + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + m_error = + InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + } else { + m_error = m_setpoint - m_measurement; + } + + m_errorDerivative = (m_error - m_prevError) / m_period.value(); + } /** * Returns the current setpoint of the PIDController. * * @return The current setpoint. */ - double GetSetpoint() const; + constexpr double GetSetpoint() const { return m_setpoint; } /** * Returns true if the error is within the tolerance of the setpoint. * * This will return false until at least one input value has been computed. */ - bool AtSetpoint() const; + constexpr bool AtSetpoint() const { + return m_haveMeasurement && m_haveSetpoint && + gcem::abs(m_error) < m_errorTolerance && + gcem::abs(m_errorDerivative) < m_errorDerivativeTolerance; + } /** * Enables continuous input. @@ -192,17 +271,22 @@ class WPILIB_DLLEXPORT PIDController * @param minimumInput The minimum value expected from the input. * @param maximumInput The maximum value expected from the input. */ - void EnableContinuousInput(double minimumInput, double maximumInput); + constexpr void EnableContinuousInput(double minimumInput, + double maximumInput) { + m_continuous = true; + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + } /** * Disables continuous input. */ - void DisableContinuousInput(); + constexpr void DisableContinuousInput() { m_continuous = false; } /** * Returns true if continuous input is enabled. */ - bool IsContinuousInputEnabled() const; + constexpr bool IsContinuousInputEnabled() const { return m_continuous; } /** * Sets the minimum and maximum contributions of the integral term. @@ -214,7 +298,11 @@ class WPILIB_DLLEXPORT PIDController * @param minimumIntegral The minimum contribution of the integral term. * @param maximumIntegral The maximum contribution of the integral term. */ - void SetIntegratorRange(double minimumIntegral, double maximumIntegral); + constexpr void SetIntegratorRange(double minimumIntegral, + double maximumIntegral) { + m_minimumIntegral = minimumIntegral; + m_maximumIntegral = maximumIntegral; + } /** * Sets the error which is considered tolerable for use with AtSetpoint(). @@ -222,40 +310,73 @@ class WPILIB_DLLEXPORT PIDController * @param errorTolerance error which is tolerable. * @param errorDerivativeTolerance error derivative which is tolerable. */ - void SetTolerance(double errorTolerance, - double errorDerivativeTolerance = - std::numeric_limits::infinity()); + constexpr void SetTolerance(double errorTolerance, + double errorDerivativeTolerance = + std::numeric_limits::infinity()) { + m_errorTolerance = errorTolerance; + m_errorDerivativeTolerance = errorDerivativeTolerance; + } /** * Returns the difference between the setpoint and the measurement. */ - double GetError() const; + constexpr double GetError() const { return m_error; } /** * Returns the error derivative. */ - double GetErrorDerivative() const; + constexpr double GetErrorDerivative() const { return m_errorDerivative; } /** * Returns the difference between the setpoint and the measurement. * @deprecated Use GetError() instead. */ [[deprecated("Use GetError method instead.")]] - double GetPositionError() const; + constexpr double GetPositionError() const { + return m_error; + } /** * Returns the velocity error. * @deprecated Use GetErrorDerivative() instead. */ [[deprecated("Use GetErrorDerivative method instead.")]] - double GetVelocityError() const; + constexpr double GetVelocityError() const { + return m_errorDerivative; + } /** * Returns the next output of the PID controller. * * @param measurement The current measurement of the process variable. */ - double Calculate(double measurement); + constexpr double Calculate(double measurement) { + m_measurement = measurement; + m_prevError = m_error; + m_haveMeasurement = true; + + if (m_continuous) { + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + m_error = + InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + } else { + m_error = m_setpoint - m_measurement; + } + + m_errorDerivative = (m_error - m_prevError) / m_period.value(); + + // If the absolute value of the position error is outside of IZone, reset + // the total error + if (gcem::abs(m_error) > m_iZone) { + m_totalError = 0; + } else if (m_Ki != 0) { + m_totalError = + std::clamp(m_totalError + m_error * m_period.value(), + m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); + } + + return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative; + } /** * Returns the next output of the PID controller. @@ -263,12 +384,22 @@ class WPILIB_DLLEXPORT PIDController * @param measurement The current measurement of the process variable. * @param setpoint The new setpoint of the controller. */ - double Calculate(double measurement, double setpoint); + constexpr double Calculate(double measurement, double setpoint) { + m_setpoint = setpoint; + m_haveSetpoint = true; + return Calculate(measurement); + } /** * Reset the previous error, the integral term, and disable the controller. */ - void Reset(); + constexpr void Reset() { + m_error = 0; + m_prevError = 0; + m_totalError = 0; + m_errorDerivative = 0; + m_haveMeasurement = false; + } void InitSendable(wpi::SendableBuilder& builder) override; @@ -319,6 +450,9 @@ class WPILIB_DLLEXPORT PIDController bool m_haveSetpoint = false; bool m_haveMeasurement = false; + + // Usage reporting instances + inline static int instances = 0; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index a74991ea1a..a717382239 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -4,10 +4,9 @@ #pragma once -#include #include -#include #include +#include #include #include @@ -56,23 +55,27 @@ class ProfiledPIDController * @param period The period between controller updates in seconds. The * default is 20 milliseconds. Must be positive. */ - ProfiledPIDController(double Kp, double Ki, double Kd, - Constraints constraints, units::second_t period = 20_ms) + constexpr ProfiledPIDController(double Kp, double Ki, double Kd, + Constraints constraints, + units::second_t period = 20_ms) : m_controller{Kp, Ki, Kd, period}, m_constraints{constraints}, m_profile{m_constraints} { - int instances = detail::IncrementAndGetProfiledPIDControllerInstances(); - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kController_ProfiledPIDController, instances); - wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances); + if (!std::is_constant_evaluated()) { + int instances = detail::IncrementAndGetProfiledPIDControllerInstances(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kController_ProfiledPIDController, instances); + wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances); + } } - ~ProfiledPIDController() override = default; + constexpr ~ProfiledPIDController() override = default; - ProfiledPIDController(const ProfiledPIDController&) = default; - ProfiledPIDController& operator=(const ProfiledPIDController&) = default; - ProfiledPIDController(ProfiledPIDController&&) = default; - ProfiledPIDController& operator=(ProfiledPIDController&&) = default; + constexpr ProfiledPIDController(const ProfiledPIDController&) = default; + constexpr ProfiledPIDController& operator=(const ProfiledPIDController&) = + default; + constexpr ProfiledPIDController(ProfiledPIDController&&) = default; + constexpr ProfiledPIDController& operator=(ProfiledPIDController&&) = default; /** * Sets the PID Controller gain parameters. @@ -83,7 +86,7 @@ class ProfiledPIDController * @param Ki The integral coefficient. Must be >= 0. * @param Kd The differential coefficient. Must be >= 0. */ - void SetPID(double Kp, double Ki, double Kd) { + constexpr void SetPID(double Kp, double Ki, double Kd) { m_controller.SetPID(Kp, Ki, Kd); } @@ -92,21 +95,21 @@ class ProfiledPIDController * * @param Kp The proportional coefficient. Must be >= 0. */ - void SetP(double Kp) { m_controller.SetP(Kp); } + constexpr void SetP(double Kp) { m_controller.SetP(Kp); } /** * Sets the integral coefficient of the PID controller gain. * * @param Ki The integral coefficient. Must be >= 0. */ - void SetI(double Ki) { m_controller.SetI(Ki); } + constexpr void SetI(double Ki) { m_controller.SetI(Ki); } /** * Sets the differential coefficient of the PID controller gain. * * @param Kd The differential coefficient. Must be >= 0. */ - void SetD(double Kd) { m_controller.SetD(Kd); } + constexpr void SetD(double Kd) { m_controller.SetD(Kd); } /** * Sets the IZone range. When the absolute value of the position error is @@ -119,49 +122,51 @@ class ProfiledPIDController * @param iZone Maximum magnitude of error to allow integral control. Must be * >= 0. */ - void SetIZone(double iZone) { m_controller.SetIZone(iZone); } + constexpr void SetIZone(double iZone) { m_controller.SetIZone(iZone); } /** * Gets the proportional coefficient. * * @return proportional coefficient */ - double GetP() const { return m_controller.GetP(); } + constexpr double GetP() const { return m_controller.GetP(); } /** * Gets the integral coefficient. * * @return integral coefficient */ - double GetI() const { return m_controller.GetI(); } + constexpr double GetI() const { return m_controller.GetI(); } /** * Gets the differential coefficient. * * @return differential coefficient */ - double GetD() const { return m_controller.GetD(); } + constexpr double GetD() const { return m_controller.GetD(); } /** * Get the IZone range. * * @return Maximum magnitude of error to allow integral control. */ - double GetIZone() const { return m_controller.GetIZone(); } + constexpr double GetIZone() const { return m_controller.GetIZone(); } /** * Gets the period of this controller. * * @return The period of the controller. */ - units::second_t GetPeriod() const { return m_controller.GetPeriod(); } + constexpr units::second_t GetPeriod() const { + return m_controller.GetPeriod(); + } /** * Gets the position tolerance of this controller. * * @return The position tolerance of the controller. */ - double GetPositionTolerance() const { + constexpr double GetPositionTolerance() const { return m_controller.GetErrorTolerance(); } @@ -170,7 +175,7 @@ class ProfiledPIDController * * @return The velocity tolerance of the controller. */ - double GetVelocityTolerance() const { + constexpr double GetVelocityTolerance() const { return m_controller.GetErrorDerivativeTolerance(); } @@ -180,7 +185,7 @@ class ProfiledPIDController * * @return The accumulated error of this controller. */ - double GetAccumulatedError() const { + constexpr double GetAccumulatedError() const { return m_controller.GetAccumulatedError(); } @@ -189,33 +194,33 @@ class ProfiledPIDController * * @param goal The desired unprofiled setpoint. */ - void SetGoal(State goal) { m_goal = goal; } + constexpr void SetGoal(State goal) { m_goal = goal; } /** * Sets the goal for the ProfiledPIDController. * * @param goal The desired unprofiled setpoint. */ - void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; } + constexpr void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; } /** * Gets the goal for the ProfiledPIDController. */ - State GetGoal() const { return m_goal; } + constexpr State GetGoal() const { return m_goal; } /** * Returns true if the error is within the tolerance of the error. * * This will return false until at least one input value has been computed. */ - bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; } + constexpr bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; } /** * Set velocity and acceleration constraints for goal. * * @param constraints Velocity and acceleration constraints for goal. */ - void SetConstraints(Constraints constraints) { + constexpr void SetConstraints(Constraints constraints) { m_constraints = constraints; m_profile = TrapezoidProfile{m_constraints}; } @@ -224,14 +229,14 @@ class ProfiledPIDController * Get the velocity and acceleration constraints for this controller. * @return Velocity and acceleration constraints. */ - Constraints GetConstraints() { return m_constraints; } + constexpr Constraints GetConstraints() { return m_constraints; } /** * Returns the current setpoint of the ProfiledPIDController. * * @return The current setpoint. */ - State GetSetpoint() const { return m_setpoint; } + constexpr State GetSetpoint() const { return m_setpoint; } /** * Returns true if the error is within the tolerance of the error. @@ -242,7 +247,7 @@ class ProfiledPIDController * * This will return false until at least one input value has been computed. */ - bool AtSetpoint() const { return m_controller.AtSetpoint(); } + constexpr bool AtSetpoint() const { return m_controller.AtSetpoint(); } /** * Enables continuous input. @@ -254,7 +259,8 @@ class ProfiledPIDController * @param minimumInput The minimum value expected from the input. * @param maximumInput The maximum value expected from the input. */ - void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) { + constexpr void EnableContinuousInput(Distance_t minimumInput, + Distance_t maximumInput) { m_controller.EnableContinuousInput(minimumInput.value(), maximumInput.value()); m_minimumInput = minimumInput; @@ -264,7 +270,9 @@ class ProfiledPIDController /** * Disables continuous input. */ - void DisableContinuousInput() { m_controller.DisableContinuousInput(); } + constexpr void DisableContinuousInput() { + m_controller.DisableContinuousInput(); + } /** * Sets the minimum and maximum contributions of the integral term. @@ -276,7 +284,8 @@ class ProfiledPIDController * @param minimumIntegral The minimum contribution of the integral term. * @param maximumIntegral The maximum contribution of the integral term. */ - void SetIntegratorRange(double minimumIntegral, double maximumIntegral) { + constexpr void SetIntegratorRange(double minimumIntegral, + double maximumIntegral) { m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral); } @@ -287,9 +296,9 @@ class ProfiledPIDController * @param positionTolerance Position error which is tolerable. * @param velocityTolerance Velocity error which is tolerable. */ - void SetTolerance(Distance_t positionTolerance, - Velocity_t velocityTolerance = Velocity_t{ - std::numeric_limits::infinity()}) { + constexpr void SetTolerance(Distance_t positionTolerance, + Velocity_t velocityTolerance = Velocity_t{ + std::numeric_limits::infinity()}) { m_controller.SetTolerance(positionTolerance.value(), velocityTolerance.value()); } @@ -299,14 +308,14 @@ class ProfiledPIDController * * @return The error. */ - Distance_t GetPositionError() const { + constexpr Distance_t GetPositionError() const { return Distance_t{m_controller.GetError()}; } /** * Returns the change in error per second. */ - Velocity_t GetVelocityError() const { + constexpr Velocity_t GetVelocityError() const { return Velocity_t{m_controller.GetErrorDerivative()}; } @@ -315,7 +324,7 @@ class ProfiledPIDController * * @param measurement The current measurement of the process variable. */ - double Calculate(Distance_t measurement) { + constexpr double Calculate(Distance_t measurement) { if (m_controller.IsContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement auto errorBound = (m_maximumInput - m_minimumInput) / 2.0; @@ -345,7 +354,7 @@ class ProfiledPIDController * @param measurement The current measurement of the process variable. * @param goal The new goal of the controller. */ - double Calculate(Distance_t measurement, State goal) { + constexpr double Calculate(Distance_t measurement, State goal) { SetGoal(goal); return Calculate(measurement); } @@ -355,7 +364,7 @@ class ProfiledPIDController * @param measurement The current measurement of the process variable. * @param goal The new goal of the controller. */ - double Calculate(Distance_t measurement, Distance_t goal) { + constexpr double Calculate(Distance_t measurement, Distance_t goal) { SetGoal(goal); return Calculate(measurement); } @@ -367,7 +376,7 @@ class ProfiledPIDController * @param goal The new goal of the controller. * @param constraints Velocity and acceleration constraints for goal. */ - double Calculate( + constexpr double Calculate( Distance_t measurement, Distance_t goal, typename frc::TrapezoidProfile::Constraints constraints) { SetConstraints(constraints); @@ -379,7 +388,7 @@ class ProfiledPIDController * * @param measurement The current measured State of the system. */ - void Reset(const State& measurement) { + constexpr void Reset(const State& measurement) { m_controller.Reset(); m_setpoint = measurement; } @@ -390,7 +399,8 @@ class ProfiledPIDController * @param measuredPosition The current measured position of the system. * @param measuredVelocity The current measured velocity of the system. */ - void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) { + constexpr void Reset(Distance_t measuredPosition, + Velocity_t measuredVelocity) { Reset(State{measuredPosition, measuredVelocity}); } @@ -400,7 +410,7 @@ class ProfiledPIDController * @param measuredPosition The current measured position of the system. The * velocity is assumed to be zero. */ - void Reset(Distance_t measuredPosition) { + constexpr void Reset(Distance_t measuredPosition) { Reset(measuredPosition, Velocity_t{0}); } diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h index 8d8ec3fdae..1a31bc9ce1 100644 --- a/wpimath/src/main/native/include/frc/controller/RamseteController.h +++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h @@ -5,12 +5,15 @@ #pragma once #include +#include #include "frc/geometry/Pose2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/trajectory/Trajectory.h" +#include "units/angle.h" #include "units/angular_velocity.h" #include "units/length.h" +#include "units/math.h" #include "units/velocity.h" namespace frc { @@ -59,7 +62,11 @@ class WPILIB_DLLEXPORT RamseteController { * @deprecated Use LTVUnicycleController instead. */ [[deprecated("Use LTVUnicycleController instead.")]] - RamseteController(units::unit_t b, units::unit_t zeta); + constexpr RamseteController(units::unit_t b, + units::unit_t zeta) + : m_b{b}, m_zeta{zeta} {} + + WPI_IGNORE_DEPRECATED /** * Construct a Ramsete unicycle controller. The default arguments for @@ -69,12 +76,24 @@ class WPILIB_DLLEXPORT RamseteController { * @deprecated Use LTVUnicycleController instead. */ [[deprecated("Use LTVUnicycleController instead.")]] - RamseteController(); + constexpr RamseteController() + : RamseteController{units::unit_t{2.0}, + units::unit_t{0.7}} {} + + WPI_UNIGNORE_DEPRECATED /** * Returns true if the pose error is within tolerance of the reference. */ - bool AtReference() const; + constexpr bool AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_poseError.Rotation(); + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); + } /** * Sets the pose error which is considered tolerable for use with @@ -82,7 +101,9 @@ class WPILIB_DLLEXPORT RamseteController { * * @param poseTolerance Pose error which is tolerable. */ - void SetTolerance(const Pose2d& poseTolerance); + constexpr void SetTolerance(const Pose2d& poseTolerance) { + m_poseTolerance = poseTolerance; + } /** * Returns the next output of the Ramsete controller. @@ -95,9 +116,34 @@ class WPILIB_DLLEXPORT RamseteController { * @param linearVelocityRef The desired linear velocity. * @param angularVelocityRef The desired angular velocity. */ - ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef, - units::meters_per_second_t linearVelocityRef, - units::radians_per_second_t angularVelocityRef); + constexpr ChassisSpeeds Calculate( + const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef) { + if (!m_enabled) { + return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef}; + } + + m_poseError = poseRef.RelativeTo(currentPose); + + // Aliases for equation readability + const auto& eX = m_poseError.X(); + const auto& eY = m_poseError.Y(); + const auto& eTheta = m_poseError.Rotation().Radians(); + const auto& vRef = linearVelocityRef; + const auto& omegaRef = angularVelocityRef; + + // k = 2ζ√(ω_ref² + b v_ref²) + auto k = 2.0 * m_zeta * + units::math::sqrt(units::math::pow<2>(omegaRef) + + m_b * units::math::pow<2>(vRef)); + + // v_cmd = v_ref cos(e_θ) + k e_x + // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y + return ChassisSpeeds{ + vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps, + omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY}; + } /** * Returns the next output of the Ramsete controller. @@ -109,15 +155,18 @@ class WPILIB_DLLEXPORT RamseteController { * @param desiredState The desired pose, linear velocity, and angular velocity * from a trajectory. */ - ChassisSpeeds Calculate(const Pose2d& currentPose, - const Trajectory::State& desiredState); + constexpr ChassisSpeeds Calculate(const Pose2d& currentPose, + const Trajectory::State& desiredState) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + desiredState.velocity * desiredState.curvature); + } /** * Enables and disables the controller for troubleshooting purposes. * * @param enabled If the controller is enabled or not. */ - void SetEnabled(bool enabled); + constexpr void SetEnabled(bool enabled) { m_enabled = enabled; } private: units::unit_t m_b; @@ -126,6 +175,19 @@ class WPILIB_DLLEXPORT RamseteController { Pose2d m_poseError; Pose2d m_poseTolerance; bool m_enabled = true; + + /** + * Returns sin(x) / x. + * + * @param x Value of which to take sinc(x). + */ + static constexpr decltype(1 / 1_rad) Sinc(units::radian_t x) { + if (units::math::abs(x) < 1e-9_rad) { + return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()}; + } else { + return units::math::sin(x) / x; + } + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 94f9134fdb..69a4a05eff 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -199,28 +199,28 @@ class SimpleMotorFeedforward { * * @return The static gain. */ - units::volt_t GetKs() const { return kS; } + constexpr units::volt_t GetKs() const { return kS; } /** * Returns the velocity gain. * * @return The velocity gain. */ - units::unit_t GetKv() const { return kV; } + constexpr units::unit_t GetKv() const { return kV; } /** * Returns the acceleration gain. * * @return The acceleration gain. */ - units::unit_t GetKa() const { return kA; } + constexpr units::unit_t GetKa() const { return kA; } /** * Returns the period. * * @return The period. */ - units::second_t GetDt() const { return m_dt; } + constexpr units::second_t GetDt() const { return m_dt; } private: /** The static gain. */ diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 724845d8c9..47bcc46b14 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -157,7 +157,19 @@ class TimeInterpolatableBuffer { // Template specialization to ensure that Pose2d uses pose exponential template <> -WPILIB_DLLEXPORT TimeInterpolatableBuffer::TimeInterpolatableBuffer( - units::second_t historySize); +inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( + units::second_t historySize) + : m_historySize(historySize), + m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) { + if (t < 0) { + return start; + } else if (t >= 1) { + return end; + } else { + Twist2d twist = start.Log(end); + Twist2d scaledTwist = twist * t; + return start.Exp(scaledTwist); + } + }) {} } // namespace frc diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 2580b1be7e..2f109e4349 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -33,7 +33,39 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { CubicHermiteSpline(wpi::array xInitialControlVector, wpi::array xFinalControlVector, wpi::array yInitialControlVector, - wpi::array yFinalControlVector); + wpi::array yFinalControlVector) + : m_initialControlVector{xInitialControlVector, yInitialControlVector}, + m_finalControlVector{xFinalControlVector, yFinalControlVector} { + const auto hermite = MakeHermiteBasis(); + const auto x = + ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + const auto y = + ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + // Populate first two rows with coefficients. + m_coefficients.template block<1, 4>(0, 0) = hermite * x; + m_coefficients.template block<1, 4>(1, 0) = hermite * y; + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + for (int i = 0; i < 4; i++) { + // Here, we are multiplying by (3 - i) to manually take the derivative. + // The power of the term in index 0 is 3, index 1 is 2 and so on. To find + // the coefficient of the derivative, we can use the power rule and + // multiply the existing coefficient by its power. + m_coefficients.template block<2, 1>(2, i) = + m_coefficients.template block<2, 1>(0, i) * (3 - i); + } + + for (int i = 0; i < 3; i++) { + // Here, we are multiplying by (2 - i) to manually take the derivative. + // The power of the term in index 0 is 2, index 1 is 1 and so on. To find + // the coefficient of the derivative, we can use the power rule and + // multiply the existing coefficient by its power. + m_coefficients.template block<2, 1>(4, i) = + m_coefficients.template block<2, 1>(2, i) * (2 - i); + } + } /** * Returns the coefficients matrix. @@ -69,7 +101,7 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { * Returns the hermite basis matrix for cubic hermite spline interpolation. * @return The hermite basis matrix for cubic hermite spline interpolation. */ - static Eigen::Matrix4d MakeHermiteBasis() { + static constexpr Eigen::Matrix4d MakeHermiteBasis() { // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀. // @@ -90,12 +122,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { // [a₂] = [-3 -2 3 -1][P'(i) ] // [a₁] = [ 0 1 0 0][P(i+1) ] // [a₀] = [ 1 0 0 0][P'(i+1)] - - static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0}, - {-3.0, -2.0, +3.0, -1.0}, - {+0.0, +1.0, +0.0, +0.0}, - {+1.0, +0.0, +0.0, +0.0}}; - return basis; + return Eigen::Matrix4d{{+2.0, +1.0, -2.0, +1.0}, + {-3.0, -2.0, +3.0, -1.0}, + {+0.0, +1.0, +0.0, +0.0}, + {+1.0, +0.0, +0.0, +0.0}}; } /** @@ -107,10 +137,12 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { * * @return The control vector matrix for a dimension. */ - static Eigen::Vector4d ControlVectorFromArrays( + static constexpr Eigen::Vector4d ControlVectorFromArrays( wpi::array initialVector, wpi::array finalVector) { - return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0], - finalVector[1]}; + return Eigen::Vector4d{{initialVector[0]}, + {initialVector[1]}, + {finalVector[0]}, + {finalVector[1]}}; } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 1c592eb2fa..5ca47e3a15 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -33,7 +33,38 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { QuinticHermiteSpline(wpi::array xInitialControlVector, wpi::array xFinalControlVector, wpi::array yInitialControlVector, - wpi::array yFinalControlVector); + wpi::array yFinalControlVector) + : m_initialControlVector{xInitialControlVector, yInitialControlVector}, + m_finalControlVector{xFinalControlVector, yFinalControlVector} { + const auto hermite = MakeHermiteBasis(); + const auto x = + ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + const auto y = + ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + // Populate first two rows with coefficients. + m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose(); + m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose(); + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + for (int i = 0; i < 6; i++) { + // Here, we are multiplying by (5 - i) to manually take the derivative. + // The power of the term in index 0 is 5, index 1 is 4 and so on. To find + // the coefficient of the derivative, we can use the power rule and + // multiply the existing coefficient by its power. + m_coefficients.template block<2, 1>(2, i) = + m_coefficients.template block<2, 1>(0, i) * (5 - i); + } + for (int i = 0; i < 5; i++) { + // Here, we are multiplying by (4 - i) to manually take the derivative. + // The power of the term in index 0 is 4, index 1 is 3 and so on. To find + // the coefficient of the derivative, we can use the power rule and + // multiply the existing coefficient by its power. + m_coefficients.template block<2, 1>(4, i) = + m_coefficients.template block<2, 1>(2, i) * (4 - i); + } + } /** * Returns the coefficients matrix. @@ -69,7 +100,7 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { * Returns the hermite basis matrix for quintic hermite spline interpolation. * @return The hermite basis matrix for quintic hermite spline interpolation. */ - static Matrixd<6, 6> MakeHermiteBasis() { + static constexpr Matrixd<6, 6> MakeHermiteBasis() { // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors, // we want to find the coefficients of the spline // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀. @@ -97,15 +128,12 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ] // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)] // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)] - - static const Matrixd<6, 6> basis{ - {-06.0, -03.0, -00.5, +06.0, -03.0, +00.5}, - {+15.0, +08.0, +01.5, -15.0, +07.0, -01.0}, - {-10.0, -06.0, -01.5, +10.0, -04.0, +00.5}, - {+00.0, +00.0, +00.5, +00.0, +00.0, +00.0}, - {+00.0, +01.0, +00.0, +00.0, +00.0, +00.0}, - {+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}}; - return basis; + return Matrixd<6, 6>{{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5}, + {+15.0, +08.0, +01.5, -15.0, +07.0, -01.0}, + {-10.0, -06.0, -01.5, +10.0, -04.0, +00.5}, + {+00.0, +00.0, +00.5, +00.0, +00.0, +00.0}, + {+00.0, +01.0, +00.0, +00.0, +00.0, +00.0}, + {+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}}; } /** @@ -117,10 +145,11 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { * * @return The control vector matrix for a dimension. */ - static Vectord<6> ControlVectorFromArrays(wpi::array initialVector, - wpi::array finalVector) { - return Vectord<6>{initialVector[0], initialVector[1], initialVector[2], - finalVector[0], finalVector[1], finalVector[2]}; + static constexpr Vectord<6> ControlVectorFromArrays( + wpi::array initialVector, wpi::array finalVector) { + return Vectord<6>{{initialVector[0]}, {initialVector[1]}, + {initialVector[2]}, {finalVector[0]}, + {finalVector[1]}, {finalVector[2]}}; } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index 9b369e31b6..a10c42f00a 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "frc/EigenCore.h" @@ -15,6 +16,7 @@ #include "units/length.h" namespace frc { + /** * Represents a two-dimensional parametric spline that interpolates between two * points. @@ -26,15 +28,15 @@ class Spline { public: using PoseWithCurvature = std::pair; - Spline() = default; + constexpr Spline() = default; - Spline(const Spline&) = default; - Spline& operator=(const Spline&) = default; + constexpr Spline(const Spline&) = default; + constexpr Spline& operator=(const Spline&) = default; - Spline(Spline&&) = default; - Spline& operator=(Spline&&) = default; + constexpr Spline(Spline&&) = default; + constexpr Spline& operator=(Spline&&) = default; - virtual ~Spline() = default; + constexpr virtual ~Spline() = default; /** * Represents a control vector for a spline. @@ -62,7 +64,7 @@ class Spline { // Populate the polynomial bases for (int i = 0; i <= Degree; i++) { - polynomialBases(i) = std::pow(t, Degree - i); + polynomialBases(i) = gcem::pow(t, Degree - i); } // This simply multiplies by the coefficients. We need to divide out t some @@ -88,13 +90,13 @@ class Spline { ddy = combined(5) / t / t; } - if (std::hypot(dx, dy) < 1e-6) { + if (gcem::hypot(dx, dy) < 1e-6) { return std::nullopt; } // Find the curvature. const auto curvature = - (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy)); + (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * gcem::hypot(dx, dy)); return PoseWithCurvature{ {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}}, @@ -106,21 +108,21 @@ class Spline { * * @return The coefficients of the spline. */ - virtual Matrixd<6, Degree + 1> Coefficients() const = 0; + constexpr virtual Matrixd<6, Degree + 1> Coefficients() const = 0; /** * Returns the initial control vector that created this spline. * * @return The initial control vector that created this spline. */ - virtual const ControlVector& GetInitialControlVector() const = 0; + constexpr virtual const ControlVector& GetInitialControlVector() const = 0; /** * Returns the final control vector that created this spline. * * @return The final control vector that created this spline. */ - virtual const ControlVector& GetFinalControlVector() const = 0; + constexpr virtual const ControlVector& GetFinalControlVector() const = 0; protected: /** @@ -129,8 +131,9 @@ class Spline { * @param translation The Translation2d to convert. * @return The vector. */ - static Eigen::Vector2d ToVector(const Translation2d& translation) { - return Eigen::Vector2d{translation.X().value(), translation.Y().value()}; + static constexpr Eigen::Vector2d ToVector(const Translation2d& translation) { + return Eigen::Vector2d{{translation.X().value()}, + {translation.Y().value()}}; } /** @@ -139,8 +142,9 @@ class Spline { * @param vector The vector to convert. * @return The Translation2d. */ - static Translation2d FromVector(const Eigen::Vector2d& vector) { + static constexpr Translation2d FromVector(const Eigen::Vector2d& vector) { return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}}; } }; + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h index b023f12d73..4ad3694a31 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include #include @@ -32,7 +32,22 @@ class WPILIB_DLLEXPORT SplineHelper { static wpi::array::ControlVector, 2> CubicControlVectorsFromWaypoints( const Pose2d& start, const std::vector& interiorWaypoints, - const Pose2d& end); + const Pose2d& end) { + double scalar; + if (interiorWaypoints.empty()) { + scalar = 1.2 * start.Translation().Distance(end.Translation()).value(); + } else { + scalar = + 1.2 * start.Translation().Distance(interiorWaypoints.front()).value(); + } + const auto initialCV = CubicControlVector(scalar, start); + if (!interiorWaypoints.empty()) { + scalar = + 1.2 * end.Translation().Distance(interiorWaypoints.back()).value(); + } + const auto finalCV = CubicControlVector(scalar, end); + return {initialCV, finalCV}; + } /** * Returns quintic splines from a set of waypoints. @@ -41,7 +56,24 @@ class WPILIB_DLLEXPORT SplineHelper { * @return List of quintic splines. */ static std::vector QuinticSplinesFromWaypoints( - const std::vector& waypoints); + const std::vector& waypoints) { + std::vector splines; + splines.reserve(waypoints.size() - 1); + for (size_t i = 0; i < waypoints.size() - 1; ++i) { + auto& p0 = waypoints[i]; + auto& p1 = waypoints[i + 1]; + + // This just makes the splines look better. + const auto scalar = + 1.2 * p0.Translation().Distance(p1.Translation()).value(); + + auto controlVectorA = QuinticControlVector(scalar, p0); + auto controlVectorB = QuinticControlVector(scalar, p1); + splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y, + controlVectorB.y); + } + return splines; + } /** * Returns a set of cubic splines corresponding to the provided control @@ -63,7 +95,114 @@ class WPILIB_DLLEXPORT SplineHelper { static std::vector CubicSplinesFromControlVectors( const Spline<3>::ControlVector& start, std::vector waypoints, - const Spline<3>::ControlVector& end); + const Spline<3>::ControlVector& end) { + std::vector splines; + + wpi::array xInitial = start.x; + wpi::array yInitial = start.y; + wpi::array xFinal = end.x; + wpi::array yFinal = end.y; + + if (waypoints.size() > 1) { + waypoints.emplace(waypoints.begin(), + Translation2d{units::meter_t{xInitial[0]}, + units::meter_t{yInitial[0]}}); + waypoints.emplace_back( + Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}}); + + // Populate tridiagonal system for clamped cubic + /* See: + https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08 + /undervisningsmateriale/chap7alecture.pdf + */ + + // Above-diagonal of tridiagonal matrix, zero-padded + std::vector a; + // Diagonal of tridiagonal matrix + std::vector b(waypoints.size() - 2, 4.0); + // Below-diagonal of tridiagonal matrix, zero-padded + std::vector c; + // rhs vectors + std::vector dx, dy; + // solution vectors + std::vector fx(waypoints.size() - 2, 0.0), + fy(waypoints.size() - 2, 0.0); + + // populate above-diagonal and below-diagonal vectors + a.emplace_back(0); + for (size_t i = 0; i < waypoints.size() - 3; ++i) { + a.emplace_back(1); + c.emplace_back(1); + } + c.emplace_back(0); + + // populate rhs vectors + dx.emplace_back( + 3 * (waypoints[2].X().value() - waypoints[0].X().value()) - + xInitial[1]); + dy.emplace_back( + 3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) - + yInitial[1]); + if (waypoints.size() > 4) { + for (size_t i = 1; i <= waypoints.size() - 4; ++i) { + // dx and dy represent the derivatives of the internal waypoints. The + // derivative of the second internal waypoint should involve the third + // and first internal waypoint, which have indices of 1 and 3 in the + // waypoints list (which contains ALL waypoints). + dx.emplace_back( + 3 * (waypoints[i + 2].X().value() - waypoints[i].X().value())); + dy.emplace_back( + 3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value())); + } + } + dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() - + waypoints[waypoints.size() - 3].X().value()) - + xFinal[1]); + dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() - + waypoints[waypoints.size() - 3].Y().value()) - + yFinal[1]); + + // Compute solution to tridiagonal system + ThomasAlgorithm(a, b, c, dx, &fx); + ThomasAlgorithm(a, b, c, dy, &fy); + + fx.emplace(fx.begin(), xInitial[1]); + fx.emplace_back(xFinal[1]); + fy.emplace(fy.begin(), yInitial[1]); + fy.emplace_back(yFinal[1]); + + for (size_t i = 0; i < fx.size() - 1; ++i) { + // Create the spline. + const CubicHermiteSpline spline{ + {waypoints[i].X().value(), fx[i]}, + {waypoints[i + 1].X().value(), fx[i + 1]}, + {waypoints[i].Y().value(), fy[i]}, + {waypoints[i + 1].Y().value(), fy[i + 1]}}; + + splines.push_back(spline); + } + } else if (waypoints.size() == 1) { + const double xDeriv = + (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0; + const double yDeriv = + (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0; + + wpi::array midXControlVector{waypoints[0].X().value(), xDeriv}; + wpi::array midYControlVector{waypoints[0].Y().value(), yDeriv}; + + splines.emplace_back(xInitial, midXControlVector, yInitial, + midYControlVector); + splines.emplace_back(midXControlVector, xFinal, midYControlVector, + yFinal); + + } else { + // Create the spline. + const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal}; + splines.push_back(spline); + } + + return splines; + } /** * Returns a set of quintic splines corresponding to the provided control @@ -75,7 +214,17 @@ class WPILIB_DLLEXPORT SplineHelper { * provided waypoints. */ static std::vector QuinticSplinesFromControlVectors( - const std::vector::ControlVector>& controlVectors); + const std::vector::ControlVector>& controlVectors) { + std::vector splines; + for (size_t i = 0; i < controlVectors.size() - 1; ++i) { + auto& xInitial = controlVectors[i].x; + auto& yInitial = controlVectors[i].y; + auto& xFinal = controlVectors[i + 1].x; + auto& yFinal = controlVectors[i + 1].y; + splines.emplace_back(xInitial, xFinal, yInitial, yFinal); + } + return splines; + } /** * Optimizes the curvature of 2 or more quintic splines at knot points. @@ -86,7 +235,79 @@ class WPILIB_DLLEXPORT SplineHelper { * @return A vector of optimized quintic splines. */ static std::vector OptimizeCurvature( - const std::vector& splines); + const std::vector& splines) { + // If there's only one spline in the vector, we can't optimize anything so + // just return that. + if (splines.size() < 2) { + return splines; + } + + // Implements Section 4.1.2 of + // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf. + + // Cubic splines minimize the integral of the second derivative's absolute + // value. Therefore, we can create cubic splines with the same 0th and 1st + // derivatives and the provided quintic splines, find the second derivative + // of those cubic splines and then use a weighted average for the second + // derivatives of the quintic splines. + + std::vector optimizedSplines; + optimizedSplines.reserve(splines.size()); + optimizedSplines.push_back(splines[0]); + + for (size_t i = 0; i < splines.size() - 1; ++i) { + const auto& a = splines[i]; + const auto& b = splines[i + 1]; + + // Get the control vectors that created the quintic splines above. + const auto& aInitial = a.GetInitialControlVector(); + const auto& aFinal = a.GetFinalControlVector(); + const auto& bInitial = b.GetInitialControlVector(); + const auto& bFinal = b.GetFinalControlVector(); + + // Create cubic splines with the same control vectors. + auto Trim = [](const wpi::array& a) { + return wpi::array{a[0], a[1]}; + }; + CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y), + Trim(aFinal.y)}; + CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y), + Trim(bFinal.y)}; + + // Calculate the second derivatives at the knot points. + frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0}; + frc::Vectord<6> combinedA = ca.Coefficients() * bases; + + double ddxA = combinedA(4); + double ddyA = combinedA(5); + double ddxB = cb.Coefficients()(4, 1); + double ddyB = cb.Coefficients()(5, 1); + + // Calculate the parameters for weighted average. + double dAB = + std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]); + double dBC = + std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]); + double alpha = dBC / (dAB + dBC); + double beta = dAB / (dAB + dBC); + + // Calculate the weighted average. + double ddx = alpha * ddxA + beta * ddxB; + double ddy = alpha * ddyA + beta * ddyB; + + // Create new splines. + optimizedSplines[i] = {aInitial.x, + {aFinal.x[0], aFinal.x[1], ddx}, + aInitial.y, + {aFinal.y[0], aFinal.y[1], ddy}}; + optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx}, + bFinal.x, + {bInitial.y[0], bInitial.y[1], ddy}, + bFinal.y}); + } + + return optimizedSplines; + } private: static Spline<3>::ControlVector CubicControlVector(double scalar, @@ -114,6 +335,35 @@ class WPILIB_DLLEXPORT SplineHelper { const std::vector& b, const std::vector& c, const std::vector& d, - std::vector* solutionVector); + std::vector* solutionVector) { + auto& f = *solutionVector; + size_t N = d.size(); + + // Create the temporary vectors + // Note that this is inefficient as it is possible to call + // this function many times. A better implementation would + // pass these temporary matrices by non-const reference to + // save excess allocation and deallocation + std::vector c_star(N, 0.0); + std::vector d_star(N, 0.0); + + // This updates the coefficients in the first row + // Note that we should be checking for division by zero here + c_star[0] = c[0] / b[0]; + d_star[0] = d[0] / b[0]; + + // Create the c_star and d_star coefficients in the forward sweep + for (size_t i = 1; i < N; ++i) { + double m = 1.0 / (b[i] - a[i] * c_star[i - 1]); + c_star[i] = c[i] * m; + d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m; + } + + f[N - 1] = d_star[N - 1]; + // This is the reverse sweep, used to update the solution vector f + for (int i = N - 2; i >= 0; i--) { + f[i] = d_star[i] - c_star[i] * f[i + 1]; + } + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h index 21a3783173..0089d72a4f 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystem.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h @@ -6,14 +6,14 @@ #include #include -#include #include +#include +#include #include #include #include "frc/EigenCore.h" -#include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" #include "units/time.h" @@ -47,26 +47,41 @@ class LinearSystem { * @param D Feedthrough matrix. * @throws std::domain_error if any matrix element isn't finite. */ - LinearSystem(const Matrixd& A, - const Matrixd& B, - const Matrixd& C, - const Matrixd& D) { - if (!A.allFinite()) { + constexpr LinearSystem(const Matrixd& A, + const Matrixd& B, + const Matrixd& C, + const Matrixd& D) { + auto allFinite = [](const auto& mat) { + if (std::is_constant_evaluated()) { + for (int row = 0; row < mat.rows(); ++row) { + for (int col = 0; col < mat.cols(); ++col) { + if (!gcem::internal::is_finite(mat.coeff(row, col))) { + return false; + } + } + } + return true; + } else { + return mat.allFinite(); + } + }; + + if (!allFinite(A)) { throw std::domain_error( "Elements of A aren't finite. This is usually due to model " "implementation errors."); } - if (!B.allFinite()) { + if (!allFinite(B)) { throw std::domain_error( "Elements of B aren't finite. This is usually due to model " "implementation errors."); } - if (!C.allFinite()) { + if (!allFinite(C)) { throw std::domain_error( "Elements of C aren't finite. This is usually due to model " "implementation errors."); } - if (!D.allFinite()) { + if (!allFinite(D)) { throw std::domain_error( "Elements of D aren't finite. This is usually due to model " "implementation errors."); @@ -78,15 +93,15 @@ class LinearSystem { m_D = D; } - LinearSystem(const LinearSystem&) = default; - LinearSystem& operator=(const LinearSystem&) = default; - LinearSystem(LinearSystem&&) = default; - LinearSystem& operator=(LinearSystem&&) = default; + constexpr LinearSystem(const LinearSystem&) = default; + constexpr LinearSystem& operator=(const LinearSystem&) = default; + constexpr LinearSystem(LinearSystem&&) = default; + constexpr LinearSystem& operator=(LinearSystem&&) = default; /** * Returns the system matrix A. */ - const Matrixd& A() const { return m_A; } + constexpr const Matrixd& A() const { return m_A; } /** * Returns an element of the system matrix A. @@ -94,12 +109,12 @@ class LinearSystem { * @param i Row of A. * @param j Column of A. */ - double A(int i, int j) const { return m_A(i, j); } + constexpr double A(int i, int j) const { return m_A(i, j); } /** * Returns the input matrix B. */ - const Matrixd& B() const { return m_B; } + constexpr const Matrixd& B() const { return m_B; } /** * Returns an element of the input matrix B. @@ -107,12 +122,12 @@ class LinearSystem { * @param i Row of B. * @param j Column of B. */ - double B(int i, int j) const { return m_B(i, j); } + constexpr double B(int i, int j) const { return m_B(i, j); } /** * Returns the output matrix C. */ - const Matrixd& C() const { return m_C; } + constexpr const Matrixd& C() const { return m_C; } /** * Returns an element of the output matrix C. @@ -120,12 +135,12 @@ class LinearSystem { * @param i Row of C. * @param j Column of C. */ - double C(int i, int j) const { return m_C(i, j); } + constexpr double C(int i, int j) const { return m_C(i, j); } /** * Returns the feedthrough matrix D. */ - const Matrixd& D() const { return m_D; } + constexpr const Matrixd& D() const { return m_D; } /** * Returns an element of the feedthrough matrix D. @@ -133,7 +148,7 @@ class LinearSystem { * @param i Row of D. * @param j Column of D. */ - double D(int i, int j) const { return m_D(i, j); } + constexpr double D(int i, int j) const { return m_D(i, j); } /** * Computes the new x given the old x and the control input. diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index d149f8b65a..0af472b934 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "frc/system/LinearSystem.h" @@ -44,10 +45,32 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to carriage. * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0. */ - static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor, - units::kilogram_t mass, - units::meter_t radius, - double gearing); + static constexpr LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor, + units::kilogram_t mass, + units::meter_t radius, + double gearing) { + if (mass <= 0_kg) { + throw std::domain_error("mass must be greater than zero."); + } + if (radius <= 0_m) { + throw std::domain_error("radius must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-gcem::pow(gearing, 2) * motor.Kt / + (motor.R * units::math::pow<2>(radius) * mass * motor.Kv)) + .value()}}; + Matrixd<2, 1> B{{0.0}, + {(gearing * motor.Kt / (motor.R * radius * mass)).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{{0.0}, {0.0}}; + + return LinearSystem<2, 1, 2>(A, B, C, D); + } /** * Create a state-space model of a single-jointed arm system.The states of the @@ -59,8 +82,25 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to arm. * @throws std::domain_error if J <= 0 or gearing <= 0. */ - static LinearSystem<2, 1, 2> SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double gearing); + static constexpr LinearSystem<2, 1, 2> SingleJointedArmSystem( + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) + .value()}}; + Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{{0.0}, {0.0}}; + + return LinearSystem<2, 1, 2>(A, B, C, D); + } /** * Create a state-space model for a 1 DOF velocity system from its kV @@ -86,7 +126,7 @@ class WPILIB_DLLEXPORT LinearSystemId { template requires std::same_as || std::same_as - static LinearSystem<1, 1, 1> IdentifyVelocitySystem( + static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { if (kV < decltype(kV){0}) { @@ -96,10 +136,10 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("Ka must be greater than zero."); } - Matrixd<1, 1> A{-kV.value() / kA.value()}; - Matrixd<1, 1> B{1.0 / kA.value()}; - Matrixd<1, 1> C{1.0}; - Matrixd<1, 1> D{0.0}; + Matrixd<1, 1> A{{-kV.value() / kA.value()}}; + Matrixd<1, 1> B{{1.0 / kA.value()}}; + Matrixd<1, 1> C{{1.0}}; + Matrixd<1, 1> D{{0.0}}; return LinearSystem<1, 1, 1>(A, B, C, D); } @@ -129,7 +169,7 @@ class WPILIB_DLLEXPORT LinearSystemId { template requires std::same_as || std::same_as - static LinearSystem<2, 1, 1> IdentifyPositionSystem( + static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { if (kV < decltype(kV){0}) { @@ -169,9 +209,41 @@ class WPILIB_DLLEXPORT LinearSystemId { * @see https://github.com/wpilibsuite/sysid */ - static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( + static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular); + decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + + double A1 = -(kVLinear.value() / kALinear.value() + + kVAngular.value() / kAAngular.value()); + double A2 = -(kVLinear.value() / kALinear.value() - + kVAngular.value() / kAAngular.value()); + double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value(); + double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value(); + + A1 /= 2.0; + A2 /= 2.0; + B1 /= 2.0; + B2 /= 2.0; + + Matrixd<2, 2> A{{A1, A2}, {A2, A1}}; + Matrixd<2, 2> B{{B1, B2}, {B2, B1}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; + + return LinearSystem<2, 2, 2>(A, B, C, D); + } /** * Identify a differential drive drivetrain given the drivetrain's kV and kA @@ -198,10 +270,41 @@ class WPILIB_DLLEXPORT LinearSystemId { * @see https://github.com/wpilibsuite/sysid */ - static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( + static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, decltype(1_V / 1_rad_per_s) kVAngular, - decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth); + decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + if (trackwidth <= 0_m) { + throw std::domain_error("r must be greater than zero."); + } + + // We want to find a factor to include in Kv,angular that will convert + // `u = Kv,angular omega` to `u = Kv,angular v`. + // + // v = omega r + // omega = v/r + // omega = 1/r v + // omega = 1/(trackwidth/2) v + // omega = 2/trackwidth v + // + // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) + // to V/(m/s). + return IdentifyDrivetrainSystem(kVLinear, kALinear, + kVAngular * 2.0 / trackwidth * 1_rad, + kAAngular * 2.0 / trackwidth * 1_rad); + } /** * Create a state-space model of a flywheel system, the states of the system @@ -213,9 +316,24 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to flywheel. * @throws std::domain_error if J <= 0 or gearing <= 0. */ - static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor, - units::kilogram_square_meter_t J, - double gearing); + static constexpr LinearSystem<1, 1, 1> FlywheelSystem( + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + Matrixd<1, 1> A{ + {(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) + .value()}}; + Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}}; + Matrixd<1, 1> C{{1.0}}; + Matrixd<1, 1> D{{0.0}}; + + return LinearSystem<1, 1, 1>(A, B, C, D); + } /** * Create a state-space model of a DC motor system. The states of the system @@ -229,9 +347,25 @@ class WPILIB_DLLEXPORT LinearSystemId { * @see https://github.com/wpilibsuite/sysid */ - static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, - units::kilogram_square_meter_t J, - double gearing); + static constexpr LinearSystem<2, 1, 2> DCMotorSystem( + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) + .value()}}; + Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{{0.0}, {0.0}}; + + return LinearSystem<2, 1, 2>(A, B, C, D); + } /** * Create a state-space model of a DC motor system from its kV @@ -256,7 +390,7 @@ class WPILIB_DLLEXPORT LinearSystemId { template requires std::same_as || std::same_as - static LinearSystem<2, 1, 2> DCMotorSystem( + static constexpr LinearSystem<2, 1, 2> DCMotorSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { if (kV < decltype(kV){0}) { @@ -289,9 +423,42 @@ class WPILIB_DLLEXPORT LinearSystemId { * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or * gearing <= 0. */ - static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( + static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem( const DCMotor& motor, units::kilogram_t mass, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double gearing); + units::meter_t rb, units::kilogram_square_meter_t J, double gearing) { + if (mass <= 0_kg) { + throw std::domain_error("mass must be greater than zero."); + } + if (r <= 0_m) { + throw std::domain_error("r must be greater than zero."); + } + if (rb <= 0_m) { + throw std::domain_error("rb must be greater than zero."); + } + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + auto C1 = -gcem::pow(gearing, 2) * motor.Kt / + (motor.Kv * motor.R * units::math::pow<2>(r)); + auto C2 = gearing * motor.Kt / (motor.R * r); + + Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(), + ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()}, + {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(), + ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}}; + Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(), + ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()}, + {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(), + ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; + + return LinearSystem<2, 2, 2>(A, B, C, D); + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h index 49ebc649ca..469083f55b 100644 --- a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h @@ -72,7 +72,9 @@ class ExponentialProfile { * @param t The time since the beginning of the profile. * @return if the profile is finished at time t. */ - bool IsFinished(const units::second_t& t) const { return t >= totalTime; } + constexpr bool IsFinished(const units::second_t& t) const { + return t >= totalTime; + } }; /** @@ -87,7 +89,7 @@ class ExponentialProfile { * @param A The State-Space 1x1 system matrix. * @param B The State-Space 1x1 input matrix. */ - Constraints(Input_t maxInput, A_t A, B_t B) + constexpr Constraints(Input_t maxInput, A_t A, B_t B) : maxInput{maxInput}, A{A}, B{B} {} /** @@ -97,7 +99,7 @@ class ExponentialProfile { * @param kV The velocity gain. * @param kA The acceleration gain. */ - Constraints(Input_t maxInput, kV_t kV, kA_t kA) + constexpr Constraints(Input_t maxInput, kV_t kV, kA_t kA) : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {} /** @@ -105,7 +107,7 @@ class ExponentialProfile { * * @return The steady-state velocity achieved by this profile. */ - Velocity_t MaxVelocity() const { return -maxInput * B / A; } + constexpr Velocity_t MaxVelocity() const { return -maxInput * B / A; } /// Maximum unsigned input voltage. Input_t maxInput{0}; @@ -126,7 +128,7 @@ class ExponentialProfile { /// The velocity at this state. Velocity_t velocity{0}; - bool operator==(const State&) const = default; + constexpr bool operator==(const State&) const = default; }; /** @@ -134,13 +136,13 @@ class ExponentialProfile { * * @param constraints The constraints on the profile, like maximum input. */ - explicit ExponentialProfile(Constraints constraints) + constexpr explicit ExponentialProfile(Constraints constraints) : m_constraints(constraints) {} - ExponentialProfile(const ExponentialProfile&) = default; - ExponentialProfile& operator=(const ExponentialProfile&) = default; - ExponentialProfile(ExponentialProfile&&) = default; - ExponentialProfile& operator=(ExponentialProfile&&) = default; + constexpr ExponentialProfile(const ExponentialProfile&) = default; + constexpr ExponentialProfile& operator=(const ExponentialProfile&) = default; + constexpr ExponentialProfile(ExponentialProfile&&) = default; + constexpr ExponentialProfile& operator=(ExponentialProfile&&) = default; /** * Calculates the position and velocity for the profile at a time t where the @@ -152,8 +154,8 @@ class ExponentialProfile { * @param goal The desired state when the profile is complete. * @return The position and velocity of the profile at time t. */ - State Calculate(const units::second_t& t, const State& current, - const State& goal) const { + constexpr State Calculate(const units::second_t& t, const State& current, + const State& goal) const { auto direction = ShouldFlipInput(current, goal) ? -1 : 1; auto u = direction * m_constraints.maxInput; @@ -181,8 +183,8 @@ class ExponentialProfile { * @param goal The desired state when the profile is complete. * @return The position and velocity of the profile at the inflection point. */ - State CalculateInflectionPoint(const State& current, - const State& goal) const { + constexpr State CalculateInflectionPoint(const State& current, + const State& goal) const { auto direction = ShouldFlipInput(current, goal) ? -1 : 1; auto u = direction * m_constraints.maxInput; @@ -196,7 +198,8 @@ class ExponentialProfile { * @param goal The desired state when the profile is complete. * @return The total duration of this profile. */ - units::second_t TimeLeftUntil(const State& current, const State& goal) const { + constexpr units::second_t TimeLeftUntil(const State& current, + const State& goal) const { auto timing = CalculateProfileTiming(current, goal); return timing.totalTime; @@ -210,8 +213,8 @@ class ExponentialProfile { * @param goal The desired state when the profile is complete. * @return The timing information for this profile. */ - ProfileTiming CalculateProfileTiming(const State& current, - const State& goal) const { + constexpr ProfileTiming CalculateProfileTiming(const State& current, + const State& goal) const { auto direction = ShouldFlipInput(current, goal) ? -1 : 1; auto u = direction * m_constraints.maxInput; @@ -230,8 +233,9 @@ class ExponentialProfile { * state. * @return The position and velocity of the profile at the inflection point. */ - State CalculateInflectionPoint(const State& current, const State& goal, - const Input_t& input) const { + constexpr State CalculateInflectionPoint(const State& current, + const State& goal, + const Input_t& input) const { auto u = input; if (current == goal) { @@ -256,10 +260,10 @@ class ExponentialProfile { * state. * @return The timing information for this profile. */ - ProfileTiming CalculateProfileTiming(const State& current, - const State& inflectionPoint, - const State& goal, - const Input_t& input) const { + constexpr ProfileTiming CalculateProfileTiming(const State& current, + const State& inflectionPoint, + const State& goal, + const Input_t& input) const { auto u = input; auto u_dir = units::math::abs(u) / u; @@ -323,9 +327,9 @@ class ExponentialProfile { * @param initial The initial state. * @return The distance travelled by this profile. */ - Distance_t ComputeDistanceFromTime(const units::second_t& time, - const Input_t& input, - const State& initial) const { + constexpr Distance_t ComputeDistanceFromTime(const units::second_t& time, + const Input_t& input, + const State& initial) const { auto A = m_constraints.A; auto B = m_constraints.B; auto u = input; @@ -346,9 +350,9 @@ class ExponentialProfile { * @param initial The initial state. * @return The distance travelled by this profile. */ - Velocity_t ComputeVelocityFromTime(const units::second_t& time, - const Input_t& input, - const State& initial) const { + constexpr Velocity_t ComputeVelocityFromTime(const units::second_t& time, + const Input_t& input, + const State& initial) const { auto A = m_constraints.A; auto B = m_constraints.B; auto u = input; @@ -367,9 +371,9 @@ class ExponentialProfile { * @param initial The initial velocity. * @return The time required to reach the goal velocity. */ - units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity, - const Input_t& input, - const Velocity_t& initial) const { + constexpr units::second_t ComputeTimeFromVelocity( + const Velocity_t& velocity, const Input_t& input, + const Velocity_t& initial) const { auto A = m_constraints.A; auto B = m_constraints.B; auto u = input; @@ -387,9 +391,9 @@ class ExponentialProfile { * @param initial The initial state. * @return The distance reached when the given velocity is reached. */ - Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity, - const Input_t& input, - const State& initial) const { + constexpr Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity, + const Input_t& input, + const State& initial) const { auto A = m_constraints.A; auto B = m_constraints.B; auto u = input; @@ -410,9 +414,9 @@ class ExponentialProfile { * @param goal The goal state. * @return The inflection velocity. */ - Velocity_t SolveForInflectionVelocity(const Input_t& input, - const State& current, - const State& goal) const { + constexpr Velocity_t SolveForInflectionVelocity(const Input_t& input, + const State& current, + const State& goal) const { auto A = m_constraints.A; auto B = m_constraints.B; auto u = input; @@ -446,7 +450,8 @@ class ExponentialProfile { * @param current The initial state (usually the current state). * @param goal The desired state when the profile is complete. */ - bool ShouldFlipInput(const State& current, const State& goal) const { + constexpr bool ShouldFlipInput(const State& current, + const State& goal) const { auto u = m_constraints.maxInput; auto v0 = current.velocity; diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h index ba29617ee1..ef37e3249a 100644 --- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -4,8 +4,11 @@ #pragma once +#include +#include #include +#include #include #include @@ -13,6 +16,7 @@ #include "frc/geometry/Transform2d.h" #include "units/acceleration.h" #include "units/curvature.h" +#include "units/math.h" #include "units/time.h" #include "units/velocity.h" @@ -46,7 +50,7 @@ class WPILIB_DLLEXPORT Trajectory { /** * Checks equality between this State and another object. */ - bool operator==(const State&) const = default; + constexpr bool operator==(const State&) const = default; /** * Interpolates between two States. @@ -56,7 +60,46 @@ class WPILIB_DLLEXPORT Trajectory { * * @return The interpolated state. */ - State Interpolate(State endValue, double i) const; + constexpr State Interpolate(State endValue, double i) const { + // Find the new [t] value. + const auto newT = wpi::Lerp(t, endValue.t, i); + + // Find the delta time between the current state and the interpolated + // state. + const auto deltaT = newT - t; + + // If delta time is negative, flip the order of interpolation. + if (deltaT < 0_s) { + return endValue.Interpolate(*this, 1.0 - i); + } + + // Check whether the robot is reversing at this stage. + const auto reversing = + velocity < 0_mps || + (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq); + + // Calculate the new velocity. + // v = v_0 + at + const units::meters_per_second_t newV = + velocity + (acceleration * deltaT); + + // Calculate the change in position. + // delta_s = v_0 t + 0.5at² + const units::meter_t newS = + (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) * + (reversing ? -1.0 : 1.0); + + // Return the new state. To find the new position for the new state, we + // need to interpolate between the two endpoint poses. The fraction for + // interpolation is the change in position (delta s) divided by the total + // distance between the two endpoints. + const double interpolationFrac = + newS / endValue.pose.Translation().Distance(pose.Translation()); + + return {newT, newV, acceleration, + wpi::Lerp(pose, endValue.pose, interpolationFrac), + wpi::Lerp(curvature, endValue.curvature, interpolationFrac)}; + } }; Trajectory() = default; @@ -66,7 +109,14 @@ class WPILIB_DLLEXPORT Trajectory { * * @throws std::invalid_argument if the vector of states is empty. */ - explicit Trajectory(const std::vector& states); + explicit Trajectory(const std::vector& states) : m_states(states) { + if (m_states.empty()) { + throw std::invalid_argument( + "Trajectory manually initialized with no states."); + } + + m_totalTime = states.back().t; + } /** * Returns the overall duration of the trajectory. @@ -88,7 +138,41 @@ class WPILIB_DLLEXPORT Trajectory { * @return The state at that point in time. * @throws std::runtime_error if the trajectory has no states. */ - State Sample(units::second_t t) const; + State Sample(units::second_t t) const { + if (m_states.empty()) { + throw std::runtime_error( + "Trajectory cannot be sampled if it has no states."); + } + + if (t <= m_states.front().t) { + return m_states.front(); + } + if (t >= m_totalTime) { + return m_states.back(); + } + + // Use binary search to get the element with a timestamp no less than the + // requested timestamp. This starts at 1 because we use the previous state + // later on for interpolation. + auto sample = + std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t, + [](const auto& a, const auto& b) { return a.t < b; }); + + auto prevSample = sample - 1; + + // The sample's timestamp is now greater than or equal to the requested + // timestamp. If it is greater, we need to interpolate between the + // previous state and the current state to get the exact state that we + // want. + + // If the difference in states is negligible, then we are spot on! + if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) { + return *sample; + } + // Interpolate between the two states for the state that we want. + return prevSample->Interpolate( + *sample, (t - prevSample->t) / (sample->t - prevSample->t)); + } /** * Transforms all poses in the trajectory by the given transform. This is @@ -98,7 +182,24 @@ class WPILIB_DLLEXPORT Trajectory { * @param transform The transform to transform the trajectory by. * @return The transformed trajectory. */ - Trajectory TransformBy(const Transform2d& transform); + Trajectory TransformBy(const Transform2d& transform) { + auto& firstState = m_states[0]; + auto& firstPose = firstState.pose; + + // Calculate the transformed first pose. + auto newFirstPose = firstPose + transform; + auto newStates = m_states; + newStates[0].pose = newFirstPose; + + for (unsigned int i = 1; i < newStates.size(); i++) { + auto& state = newStates[i]; + // We are transforming relative to the coordinate frame of the new initial + // pose. + state.pose = newFirstPose + (state.pose - firstPose); + } + + return Trajectory(newStates); + } /** * Transforms all poses in the trajectory so that they are relative to the @@ -109,7 +210,13 @@ class WPILIB_DLLEXPORT Trajectory { * the current trajectory will be transformed into. * @return The transformed trajectory. */ - Trajectory RelativeTo(const Pose2d& pose); + Trajectory RelativeTo(const Pose2d& pose) { + auto newStates = m_states; + for (auto& state : newStates) { + state.pose = state.pose.RelativeTo(pose); + } + return Trajectory(newStates); + } /** * Concatenates another trajectory to the current trajectory. The user is @@ -119,7 +226,26 @@ class WPILIB_DLLEXPORT Trajectory { * @param other The trajectory to concatenate. * @return The concatenated trajectory. */ - Trajectory operator+(const Trajectory& other) const; + Trajectory operator+(const Trajectory& other) const { + // If this is a default constructed trajectory with no states, then we can + // simply return the rhs trajectory. + if (m_states.empty()) { + return other; + } + + auto states = m_states; + auto otherStates = other.States(); + for (auto& otherState : otherStates) { + otherState.t += m_totalTime; + } + + // Here we omit the first state of the other trajectory because we don't + // want two time points with different states. Sample() will automatically + // interpolate between the end of this trajectory and the second state of + // the other trajectory. + states.insert(states.end(), otherStates.begin() + 1, otherStates.end()); + return Trajectory(states); + } /** * Returns the initial pose of the trajectory. diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 20a88176a8..9dd82f4eee 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -4,6 +4,8 @@ #pragma once +#include + #include "units/math.h" #include "units/time.h" #include "wpimath/MathShared.h" @@ -65,9 +67,11 @@ class TrapezoidProfile { /** * Default constructor. */ - Constraints() { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); + constexpr Constraints() { + if (!std::is_constant_evaluated()) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); + } } /** @@ -76,10 +80,13 @@ class TrapezoidProfile { * @param maxVelocity Maximum velocity. * @param maxAcceleration Maximum acceleration. */ - Constraints(Velocity_t maxVelocity, Acceleration_t maxAcceleration) + constexpr Constraints(Velocity_t maxVelocity, + Acceleration_t maxAcceleration) : maxVelocity{maxVelocity}, maxAcceleration{maxAcceleration} { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); + if (!std::is_constant_evaluated()) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); + } } }; @@ -94,7 +101,7 @@ class TrapezoidProfile { /// The velocity at this state. Velocity_t velocity{0}; - bool operator==(const State&) const = default; + constexpr bool operator==(const State&) const = default; }; /** @@ -102,13 +109,13 @@ class TrapezoidProfile { * * @param constraints The constraints on the profile, like maximum velocity. */ - TrapezoidProfile(Constraints constraints) // NOLINT + constexpr TrapezoidProfile(Constraints constraints) // NOLINT : m_constraints(constraints) {} - TrapezoidProfile(const TrapezoidProfile&) = default; - TrapezoidProfile& operator=(const TrapezoidProfile&) = default; - TrapezoidProfile(TrapezoidProfile&&) = default; - TrapezoidProfile& operator=(TrapezoidProfile&&) = default; + constexpr TrapezoidProfile(const TrapezoidProfile&) = default; + constexpr TrapezoidProfile& operator=(const TrapezoidProfile&) = default; + constexpr TrapezoidProfile(TrapezoidProfile&&) = default; + constexpr TrapezoidProfile& operator=(TrapezoidProfile&&) = default; /** * Calculates the position and velocity for the profile at a time t where the @@ -120,7 +127,7 @@ class TrapezoidProfile { * @param goal The desired state when the profile is complete. * @return The position and velocity of the profile at time t. */ - State Calculate(units::second_t t, State current, State goal) { + constexpr State Calculate(units::second_t t, State current, State goal) { m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1; m_current = Direct(current); goal = Direct(goal); @@ -195,7 +202,7 @@ class TrapezoidProfile { * @param target The target distance. * @return The time left until a target distance in the profile is reached. */ - units::second_t TimeLeftUntil(Distance_t target) const { + constexpr units::second_t TimeLeftUntil(Distance_t target) const { Distance_t position = m_current.position * m_direction; Velocity_t velocity = m_current.velocity * m_direction; @@ -266,7 +273,7 @@ class TrapezoidProfile { * * @return The total time the profile takes to reach the goal. */ - units::second_t TotalTime() const { return m_endDecel; } + constexpr units::second_t TotalTime() const { return m_endDecel; } /** * Returns true if the profile has reached the goal. @@ -277,7 +284,9 @@ class TrapezoidProfile { * @param t The time since the beginning of the profile. * @return True if the profile has reached the goal. */ - bool IsFinished(units::second_t t) const { return t >= TotalTime(); } + constexpr bool IsFinished(units::second_t t) const { + return t >= TotalTime(); + } private: /** @@ -288,12 +297,13 @@ class TrapezoidProfile { * @param initial The initial state (usually the current state). * @param goal The desired state when the profile is complete. */ - static bool ShouldFlipAcceleration(const State& initial, const State& goal) { + static constexpr bool ShouldFlipAcceleration(const State& initial, + const State& goal) { return initial.position > goal.position; } // Flip the sign of the velocity and position if the profile is inverted - State Direct(const State& in) const { + constexpr State Direct(const State& in) const { State result = in; result.position *= m_direction; result.velocity *= m_direction; diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h index 5ef15a4428..be55464c88 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h @@ -9,6 +9,7 @@ #include "frc/trajectory/constraint/TrajectoryConstraint.h" #include "units/acceleration.h" #include "units/curvature.h" +#include "units/math.h" #include "units/velocity.h" namespace frc { @@ -25,15 +26,34 @@ namespace frc { class WPILIB_DLLEXPORT CentripetalAccelerationConstraint : public TrajectoryConstraint { public: - explicit CentripetalAccelerationConstraint( - units::meters_per_second_squared_t maxCentripetalAcceleration); + constexpr explicit CentripetalAccelerationConstraint( + units::meters_per_second_squared_t maxCentripetalAcceleration) + : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + // ac = v²/r + // k (curvature) = 1/r - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + // therefore, ac = v²k + // ac/k = v² + // v = √(ac/k) + + // We have to multiply by 1_rad here to get the units to cancel out nicely. + // The units library defines a unit for radians although it is technically + // unitless. + return units::math::sqrt(m_maxCentripetalAcceleration / + units::math::abs(curvature) * 1_rad); + } + + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { + // The acceleration of the robot has no impact on the centripetal + // acceleration of the robot. + return {}; + } private: units::meters_per_second_squared_t m_maxCentripetalAcceleration; diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h index 0d96893189..94b4d30148 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include "frc/kinematics/DifferentialDriveKinematics.h" @@ -20,15 +22,26 @@ namespace frc { class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint : public TrajectoryConstraint { public: - DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics, - units::meters_per_second_t maxSpeed); + constexpr DifferentialDriveKinematicsConstraint( + DifferentialDriveKinematics kinematics, + units::meters_per_second_t maxSpeed) + : m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature}); + wheelSpeeds.Desaturate(m_maxSpeed); - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx; + } + + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { + return {}; + } private: DifferentialDriveKinematics m_kinematics; diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h index 8f7ba2ce3f..3c49e905a2 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h @@ -4,12 +4,18 @@ #pragma once +#include +#include +#include + +#include #include #include "frc/controller/SimpleMotorFeedforward.h" #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" #include "units/length.h" +#include "units/math.h" #include "units/voltage.h" namespace frc { @@ -32,16 +38,89 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint * following the path. Should be somewhat less than the nominal battery * voltage (12V) to account for "voltage sag" due to current draw. */ - DifferentialDriveVoltageConstraint( + constexpr DifferentialDriveVoltageConstraint( const SimpleMotorFeedforward& feedforward, - DifferentialDriveKinematics kinematics, units::volt_t maxVoltage); + DifferentialDriveKinematics kinematics, units::volt_t maxVoltage) + : m_feedforward(feedforward), + m_kinematics(std::move(kinematics)), + m_maxVoltage(maxVoltage) {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + return units::meters_per_second_t{std::numeric_limits::max()}; + } - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature}); + + auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right); + auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right); + + // Calculate maximum/minimum possible accelerations from motor dynamics + // and max/min wheel speeds + auto maxWheelAcceleration = + m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed); + auto minWheelAcceleration = + m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed); + + // 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. + + // 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 + + // 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. + + if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) { + if (speed > 0_mps) { + minChassisAcceleration = -minChassisAcceleration; + } else if (speed < 0_mps) { + maxChassisAcceleration = -maxChassisAcceleration; + } + } + + return {minChassisAcceleration, maxChassisAcceleration}; + } private: SimpleMotorFeedforward m_feedforward; diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h index 62e2764bf0..58a366c67a 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h @@ -33,9 +33,11 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { * @deprecated Use constructor taking Ellipse2d instead. */ [[deprecated("Use constructor taking Ellipse2d instead.")]] - EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth, - units::meter_t yWidth, const Rotation2d& rotation, - const Constraint& constraint) + constexpr EllipticalRegionConstraint(const Translation2d& center, + units::meter_t xWidth, + units::meter_t yWidth, + const Rotation2d& rotation, + const Constraint& constraint) : m_ellipse{Pose2d{center, rotation}, xWidth / 2.0, yWidth / 2.0}, m_constraint(constraint) {} @@ -46,11 +48,11 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { * @param constraint The constraint to enforce when the robot is within the * region. */ - EllipticalRegionConstraint(const Ellipse2d& ellipse, - const Constraint& constraint) + constexpr EllipticalRegionConstraint(const Ellipse2d& ellipse, + const Constraint& constraint) : m_ellipse{ellipse}, m_constraint{constraint} {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override { if (m_ellipse.Contains(pose.Translation())) { @@ -61,8 +63,9 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { } } - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override { + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { if (m_ellipse.Contains(pose.Translation())) { return m_constraint.MinMaxAcceleration(pose, curvature, speed); } else { diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h index b7375d5d6d..79b0c0ac50 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h @@ -11,6 +11,7 @@ #include "units/velocity.h" namespace frc { + /** * Represents a constraint that enforces a max velocity. This can be composed * with the EllipticalRegionConstraint or RectangularRegionConstraint to enforce @@ -23,16 +24,24 @@ class WPILIB_DLLEXPORT MaxVelocityConstraint : public TrajectoryConstraint { * * @param maxVelocity The max velocity. */ - explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity); + constexpr explicit MaxVelocityConstraint( + units::meters_per_second_t maxVelocity) + : m_maxVelocity(units::math::abs(maxVelocity)) {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + return m_maxVelocity; + } - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { + return {}; + } private: units::meters_per_second_t m_maxVelocity; }; + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h index ac47dbfeb5..572d478877 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h @@ -10,6 +10,7 @@ #include "frc/kinematics/MecanumDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/math.h" #include "units/velocity.h" namespace frc { @@ -23,14 +24,27 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint : public TrajectoryConstraint { public: MecanumDriveKinematicsConstraint(const MecanumDriveKinematics& kinematics, - units::meters_per_second_t maxSpeed); + units::meters_per_second_t maxSpeed) + : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {} units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + auto xVelocity = velocity * pose.Rotation().Cos(); + auto yVelocity = velocity * pose.Rotation().Sin(); + auto wheelSpeeds = m_kinematics.ToWheelSpeeds( + {xVelocity, yVelocity, velocity * curvature}); + wheelSpeeds.Desaturate(m_maxSpeed); + + auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); + + return units::math::hypot(normSpeeds.vx, normSpeeds.vy); + } MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + units::meters_per_second_t speed) const override { + return {}; + } private: MecanumDriveKinematics m_kinematics; diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h index c0075b9041..471951c5cc 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h @@ -31,9 +31,9 @@ class RectangularRegionConstraint : public TrajectoryConstraint { * @deprecated Use constructor taking Rectangle2d instead. */ [[deprecated("Use constructor taking Rectangle2d instead.")]] - RectangularRegionConstraint(const Translation2d& bottomLeftPoint, - const Translation2d& topRightPoint, - const Constraint& constraint) + constexpr RectangularRegionConstraint(const Translation2d& bottomLeftPoint, + const Translation2d& topRightPoint, + const Constraint& constraint) : m_rectangle{bottomLeftPoint, topRightPoint}, m_constraint(constraint) {} /** @@ -43,11 +43,11 @@ class RectangularRegionConstraint : public TrajectoryConstraint { * @param constraint The constraint to enforce when the robot is within the * region. */ - RectangularRegionConstraint(const Rectangle2d& rectangle, - const Constraint& constraint) + constexpr RectangularRegionConstraint(const Rectangle2d& rectangle, + const Constraint& constraint) : m_rectangle{rectangle}, m_constraint{constraint} {} - units::meters_per_second_t MaxVelocity( + constexpr units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override { if (m_rectangle.Contains(pose.Translation())) { @@ -58,8 +58,9 @@ class RectangularRegionConstraint : public TrajectoryConstraint { } } - MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override { + constexpr MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const override { if (m_rectangle.Contains(pose.Translation())) { return m_constraint.MinMaxAcceleration(pose, curvature, speed); } else { diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h index 47ca820c52..502fc9d429 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h @@ -9,7 +9,6 @@ #include #include "frc/geometry/Pose2d.h" -#include "frc/spline/Spline.h" #include "units/acceleration.h" #include "units/curvature.h" #include "units/velocity.h" @@ -21,15 +20,16 @@ namespace frc { */ class WPILIB_DLLEXPORT TrajectoryConstraint { public: - TrajectoryConstraint() = default; + constexpr TrajectoryConstraint() = default; - TrajectoryConstraint(const TrajectoryConstraint&) = default; - TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default; + constexpr TrajectoryConstraint(const TrajectoryConstraint&) = default; + constexpr TrajectoryConstraint& operator=(const TrajectoryConstraint&) = + default; - TrajectoryConstraint(TrajectoryConstraint&&) = default; - TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default; + constexpr TrajectoryConstraint(TrajectoryConstraint&&) = default; + constexpr TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default; - virtual ~TrajectoryConstraint() = default; + constexpr virtual ~TrajectoryConstraint() = default; /** * Represents a minimum and maximum acceleration. @@ -58,7 +58,7 @@ class WPILIB_DLLEXPORT TrajectoryConstraint { * * @return The absolute maximum velocity. */ - virtual units::meters_per_second_t MaxVelocity( + constexpr virtual units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const = 0; @@ -72,8 +72,8 @@ class WPILIB_DLLEXPORT TrajectoryConstraint { * * @return The min and max acceleration bounds. */ - virtual MinMax MinMaxAcceleration(const Pose2d& pose, - units::curvature_t curvature, - units::meters_per_second_t speed) const = 0; + constexpr virtual MinMax MinMaxAcceleration( + const Pose2d& pose, units::curvature_t curvature, + units::meters_per_second_t speed) const = 0; }; } // namespace frc diff --git a/wpiutil/src/main/native/include/wpi/sendable/Sendable.h b/wpiutil/src/main/native/include/wpi/sendable/Sendable.h index d587c3c78d..fc012b2869 100644 --- a/wpiutil/src/main/native/include/wpi/sendable/Sendable.h +++ b/wpiutil/src/main/native/include/wpi/sendable/Sendable.h @@ -15,7 +15,7 @@ class SendableBuilder; */ class WPILIB_DLLEXPORT Sendable { public: - virtual ~Sendable() = default; + constexpr virtual ~Sendable() = default; /** * Initializes this Sendable object. diff --git a/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h b/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h index f8dd70388e..739a95ee08 100644 --- a/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h +++ b/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h @@ -4,6 +4,8 @@ #pragma once +#include + #include "wpi/sendable/SendableRegistry.h" namespace wpi { @@ -18,37 +20,43 @@ namespace wpi { template class SendableHelper { public: - SendableHelper(const SendableHelper& rhs) = default; - SendableHelper& operator=(const SendableHelper& rhs) = default; + constexpr SendableHelper(const SendableHelper& rhs) = default; + constexpr SendableHelper& operator=(const SendableHelper& rhs) = default; #if !defined(_MSC_VER) && (defined(__llvm__) || __GNUC__ > 7) // See https://bugzilla.mozilla.org/show_bug.cgi?id=1442819 __attribute__((no_sanitize("vptr"))) #endif - SendableHelper(SendableHelper&& rhs) { - // it is safe to call Move() multiple times with the same rhs - SendableRegistry::Move(static_cast(this), - static_cast(&rhs)); + constexpr SendableHelper(SendableHelper&& rhs) { + if (!std::is_constant_evaluated()) { + // it is safe to call Move() multiple times with the same rhs + SendableRegistry::Move(static_cast(this), + static_cast(&rhs)); + } } #if !defined(_MSC_VER) && (defined(__llvm__) || __GNUC__ > 7) // See https://bugzilla.mozilla.org/show_bug.cgi?id=1442819 __attribute__((no_sanitize("vptr"))) #endif - SendableHelper& + constexpr SendableHelper& operator=(SendableHelper&& rhs) { - // it is safe to call Move() multiple times with the same rhs - SendableRegistry::Move(static_cast(this), - static_cast(&rhs)); + if (!std::is_constant_evaluated()) { + // it is safe to call Move() multiple times with the same rhs + SendableRegistry::Move(static_cast(this), + static_cast(&rhs)); + } return *this; } protected: - SendableHelper() = default; + constexpr SendableHelper() = default; - ~SendableHelper() { - // it is safe to call Remove() multiple times with the same object - SendableRegistry::Remove(static_cast(this)); + constexpr ~SendableHelper() { + if (!std::is_constant_evaluated()) { + // it is safe to call Remove() multiple times with the same object + SendableRegistry::Remove(static_cast(this)); + } } };