Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make controllers and some trajectory classes constexpr (#7343)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-07 13:02:11 -08:00
committed by GitHub
parent 44a45d44e2
commit a66fa339dc
71 changed files with 1512 additions and 1900 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -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> angle,
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
constexpr units::volt_t Calculate(
units::unit_t<Angle> angle, units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> 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<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> 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<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const;
constexpr units::volt_t Calculate(
units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> 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<Velocity> MaxAchievableVelocity(
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> 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<Velocity> MinAchievableVelocity(
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> 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<Acceleration> MaxAchievableAcceleration(
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Velocity> 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<Acceleration> MinAchievableAcceleration(
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Velocity> 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<kv_unit> GetKv() const { return kV; }
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
private:
/// The static gain, in volts.

33
wpimath/src/main/native/include/frc/controller/BangBangController.h
View File

@@ -6,6 +6,7 @@
#include <limits>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
@@ -37,57 +38,60 @@ class WPILIB_DLLEXPORT BangBangController
*
* @param tolerance Tolerance for atSetpoint.
*/
explicit BangBangController(
double tolerance = std::numeric_limits<double>::infinity());
constexpr explicit BangBangController(
double tolerance = std::numeric_limits<double>::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;

19
wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <utility>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
@@ -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.

30
wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
View File

@@ -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

17
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -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> velocity,
units::unit_t<Acceleration> acceleration) const {
constexpr units::volt_t Calculate(
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
@@ -99,8 +100,8 @@ class ElevatorFeedforward {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(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<kv_unit> GetKv() const { return kV; }
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
private:
/// The static gain.

105
wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#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<units::radian> thetaController);
ProfiledPIDController<units::radian> 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<units::radian>& getThetaController();
constexpr ProfiledPIDController<units::radian>& 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<units::radian>& GetThetaController();
constexpr ProfiledPIDController<units::radian>& GetThetaController() {
return m_thetaController;
}
private:
Pose2d m_poseError;

214
wpimath/src/main/native/include/frc/controller/PIDController.h
View File

@@ -4,14 +4,20 @@
#pragma once
#include <functional>
#include <algorithm>
#include <cmath>
#include <limits>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include <wpi/sendable/SendableRegistry.h>
#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<double>::infinity());
constexpr void SetTolerance(double errorTolerance,
double errorDerivativeTolerance =
std::numeric_limits<double>::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

108
wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -4,10 +4,9 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <functional>
#include <limits>
#include <type_traits>
#include <wpi/SymbolExports.h>
#include <wpi/sendable/Sendable.h>
@@ -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<Distance>{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<double>::infinity()}) {
constexpr void SetTolerance(Distance_t positionTolerance,
Velocity_t velocityTolerance = Velocity_t{
std::numeric_limits<double>::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<Distance>::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});
}

82
wpimath/src/main/native/include/frc/controller/RamseteController.h
View File

@@ -5,12 +5,15 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#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_unit> b, units::unit_t<zeta_unit> zeta);
constexpr RamseteController(units::unit_t<b_unit> b,
units::unit_t<zeta_unit> 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<b_unit>{2.0},
units::unit_t<zeta_unit>{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<b_unit> 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

8
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -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<kv_unit> GetKv() const { return kV; }
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr units::unit_t<ka_unit> 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. */

16
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -157,7 +157,19 @@ class TimeInterpolatableBuffer {
// Template specialization to ensure that Pose2d uses pose exponential
template <>
WPILIB_DLLEXPORT TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
units::second_t historySize);
inline TimeInterpolatableBuffer<Pose2d>::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

54
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -33,7 +33,39 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector);
wpi::array<double, 2> 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<double, 2> initialVector, wpi::array<double, 2> 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

59
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -33,7 +33,38 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
QuinticHermiteSpline(wpi::array<double, 3> xInitialControlVector,
wpi::array<double, 3> xFinalControlVector,
wpi::array<double, 3> yInitialControlVector,
wpi::array<double, 3> yFinalControlVector);
wpi::array<double, 3> 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<double, 3> initialVector,
wpi::array<double, 3> finalVector) {
return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]};
static constexpr Vectord<6> ControlVectorFromArrays(
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return Vectord<6>{{initialVector[0]}, {initialVector[1]},
{initialVector[2]}, {finalVector[0]},
{finalVector[1]}, {finalVector[2]}};
}
};
} // namespace frc

34
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -7,6 +7,7 @@
#include <optional>
#include <utility>
#include <gcem.hpp>
#include <wpi/array.h>
#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<Pose2d, units::curvature_t>;
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

264
wpimath/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <utility>
#include <cstddef>
#include <vector>
#include <wpi/SymbolExports.h>
@@ -32,7 +32,22 @@ class WPILIB_DLLEXPORT SplineHelper {
static wpi::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& 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<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints);
const std::vector<Pose2d>& waypoints) {
std::vector<QuinticHermiteSpline> 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<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end);
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
wpi::array<double, 2> xInitial = start.x;
wpi::array<double, 2> yInitial = start.y;
wpi::array<double, 2> xFinal = end.x;
wpi::array<double, 2> 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<double> a;
// Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
// Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
// rhs vectors
std::vector<double> dx, dy;
// solution vectors
std::vector<double> 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<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
wpi::array<double, 2> 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<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors);
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> 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<QuinticHermiteSpline> OptimizeCurvature(
const std::vector<QuinticHermiteSpline>& splines);
const std::vector<QuinticHermiteSpline>& 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<QuinticHermiteSpline> 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<double, 3>& a) {
return wpi::array<double, 2>{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<double>& b,
const std::vector<double>& c,
const std::vector<double>& d,
std::vector<double>* solutionVector);
std::vector<double>* 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<double> c_star(N, 0.0);
std::vector<double> 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

59
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -6,14 +6,14 @@
#include <algorithm>
#include <concepts>
#include <functional>
#include <stdexcept>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/Algorithm.h>
#include <wpi/SmallVector.h>
#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<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
constexpr LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& 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<States, States>& A() const { return m_A; }
constexpr const Matrixd<States, States>& 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<States, Inputs>& B() const { return m_B; }
constexpr const Matrixd<States, Inputs>& 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<Outputs, States>& C() const { return m_C; }
constexpr const Matrixd<Outputs, States>& 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<Outputs, Inputs>& D() const { return m_D; }
constexpr const Matrixd<Outputs, Inputs>& 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.

217
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -7,6 +7,7 @@
#include <concepts>
#include <stdexcept>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#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 <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(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 <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<2, 1, 1> IdentifyPositionSystem(
static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -169,9 +209,41 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @see <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
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 <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
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 <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
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 <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<2, 1, 2> DCMotorSystem(
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(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

83
wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
View File

@@ -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;

140
wpimath/src/main/native/include/frc/trajectory/Trajectory.h
View File

@@ -4,8 +4,11 @@
#pragma once
#include <algorithm>
#include <stdexcept>
#include <vector>
#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -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<State>& states);
explicit Trajectory(const std::vector<State>& 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.

46
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <type_traits>
#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;

32
wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
View File

@@ -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;

25
wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#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;

91
wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
View File

@@ -4,12 +4,18 @@
#pragma once
#include <algorithm>
#include <limits>
#include <utility>
#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#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<units::meter>& 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<double>::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<units::meter> m_feedforward;

19
wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
View File

@@ -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 {

19
wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
View File

@@ -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

20
wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
View File

@@ -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;

17
wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
View File

@@ -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 {

22
wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
View File

@@ -9,7 +9,6 @@
#include <wpi/SymbolExports.h>
#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