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

[wpimath] Make various classes constexpr (#7237)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-22 06:58:06 -07:00
committed by GitHub
parent 0c824bd447
commit 05c7fd929b
21 changed files with 235 additions and 307 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpimath/src/main/native/include/frc/ComputerVisionUtil.h
View File

@@ -30,8 +30,12 @@ namespace frc {
* @return The robot's field-relative pose.
*/
WPILIB_DLLEXPORT
frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
const frc::Transform3d& cameraToObject,
const frc::Transform3d& robotToCamera);
constexpr frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
const frc::Transform3d& cameraToObject,
const frc::Transform3d& robotToCamera) {
const auto objectToCamera = cameraToObject.Inverse();
const auto cameraToRobot = robotToCamera.Inverse();
return objectInField + objectToCamera + cameraToRobot;
}
} // namespace frc

5
wpimath/src/main/native/include/frc/MathUtil.h
View File

@@ -7,6 +7,7 @@
#include <numbers>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include "units/angle.h"
@@ -28,10 +29,10 @@ namespace frc {
*/
template <typename T>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
T magnitude;
if constexpr (std::is_arithmetic_v<T>) {
magnitude = std::abs(value);
magnitude = gcem::abs(value);
} else {
magnitude = units::math::abs(value);
}

25
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -209,7 +209,11 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector3d{{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()}};
}
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -219,7 +223,11 @@ Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector4d{{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()}};
}
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -306,7 +314,10 @@ bool IsDetectable(const Matrixd<States, States>& A,
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector3d PoseToVector(const Pose2d& pose);
constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{
{pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
}
/**
* Clamps input vector between system's minimum and maximum allowable input.
@@ -318,12 +329,12 @@ Eigen::Vector3d PoseToVector(const Pose2d& pose);
* @return Clamped input vector.
*/
template <int Inputs>
Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result(i) = std::clamp(u(i), umin(i), umax(i));
result.coeffRef(i) = std::clamp(u.coeff(i), umin.coeff(i), umax.coeff(i));
}
return result;
}

46
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -5,13 +5,14 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <initializer_list>
#include <span>
#include <stdexcept>
#include <type_traits>
#include <vector>
#include <Eigen/QR>
#include <gcem.hpp>
#include <wpi/array.h>
#include <wpi/circular_buffer.h>
@@ -80,7 +81,8 @@ class LinearFilter {
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
LinearFilter(std::span<const double> ffGains, std::span<const double> fbGains)
constexpr LinearFilter(std::span<const double> ffGains,
std::span<const double> fbGains)
: m_inputs(ffGains.size()),
m_outputs(fbGains.size()),
m_inputGains(ffGains.begin(), ffGains.end()),
@@ -92,10 +94,11 @@ class LinearFilter {
m_outputs.emplace_front(0.0);
}
static int instances = 0;
instances++;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kFilter_Linear, instances);
if (!std::is_constant_evaluated()) {
++instances;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kFilter_Linear, instances);
}
}
/**
@@ -104,8 +107,8 @@ class LinearFilter {
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
LinearFilter(std::initializer_list<double> ffGains,
std::initializer_list<double> fbGains)
constexpr LinearFilter(std::initializer_list<double> ffGains,
std::initializer_list<double> fbGains)
: LinearFilter({ffGains.begin(), ffGains.end()},
{fbGains.begin(), fbGains.end()}) {}
@@ -124,9 +127,9 @@ class LinearFilter {
* @param period The period in seconds between samples taken by the
* user.
*/
static LinearFilter<T> SinglePoleIIR(double timeConstant,
units::second_t period) {
double gain = std::exp(-period.value() / timeConstant);
static constexpr LinearFilter<T> SinglePoleIIR(double timeConstant,
units::second_t period) {
double gain = gcem::exp(-period.value() / timeConstant);
return LinearFilter({1.0 - gain}, {-gain});
}
@@ -144,8 +147,9 @@ class LinearFilter {
* @param period The period in seconds between samples taken by the
* user.
*/
static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
double gain = std::exp(-period.value() / timeConstant);
static constexpr LinearFilter<T> HighPass(double timeConstant,
units::second_t period) {
double gain = gcem::exp(-period.value() / timeConstant);
return LinearFilter({gain, -gain}, {-gain});
}
@@ -213,7 +217,7 @@ class LinearFilter {
Matrixd<Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
S(row, col) = std::pow(stencil[col], row);
S(row, col) = gcem::pow(stencil[col], row);
}
}
@@ -224,7 +228,7 @@ class LinearFilter {
}
Vectord<Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
S.householderQr().solve(d) / gcem::pow(period.value(), Derivative);
// Reverse gains list
std::vector<double> ffGains;
@@ -266,7 +270,7 @@ class LinearFilter {
/**
* Reset the filter state.
*/
void Reset() {
constexpr void Reset() {
std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
}
@@ -321,7 +325,8 @@ class LinearFilter {
* @throws std::runtime_error if size of inputBuffer or outputBuffer does not
* match the size of ffGains and fbGains provided in the constructor.
*/
void Reset(std::span<const T> inputBuffer, std::span<const T> outputBuffer) {
constexpr void Reset(std::span<const T> inputBuffer,
std::span<const T> outputBuffer) {
// Clear buffers
Reset();
@@ -346,7 +351,7 @@ class LinearFilter {
*
* @return The filtered value at this step
*/
T Calculate(T input) {
constexpr T Calculate(T input) {
T retVal{0.0};
// Rotate the inputs
@@ -376,7 +381,7 @@ class LinearFilter {
*
* @return The last value.
*/
T LastValue() const { return m_lastOutput; }
constexpr T LastValue() const { return m_lastOutput; }
private:
wpi::circular_buffer<T> m_inputs;
@@ -385,6 +390,9 @@ class LinearFilter {
std::vector<double> m_outputGains;
T m_lastOutput{0.0};
// Usage reporting instances
inline static int instances = 0;
/**
* Factorial of n.
*

9
wpimath/src/main/native/include/frc/filter/MedianFilter.h
View File

@@ -25,7 +25,8 @@ class MedianFilter {
*
* @param size The number of samples in the moving window.
*/
explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
constexpr explicit MedianFilter(size_t size)
: m_valueBuffer(size), m_size{size} {}
/**
* Calculates the moving-window median for the next value of the input stream.
@@ -33,7 +34,7 @@ class MedianFilter {
* @param next The next input value.
* @return The median of the moving window, updated to include the next value.
*/
T Calculate(T next) {
constexpr T Calculate(T next) {
// Insert next value at proper point in sorted array
wpi::insert_sorted(m_orderedValues, next);
@@ -66,12 +67,12 @@ class MedianFilter {
*
* @return The last value.
*/
T LastValue() const { return m_valueBuffer.front(); }
constexpr T LastValue() const { return m_valueBuffer.front(); }
/**
* Resets the filter, clearing the window of all elements.
*/
void Reset() {
constexpr void Reset() {
m_orderedValues.clear();
m_valueBuffer.reset();
}

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

@@ -5,9 +5,7 @@
#pragma once
#include <algorithm>
#include <array>
#include <functional>
#include <map>
#include <optional>
#include <utility>
#include <vector>

22
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -45,7 +45,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Twist2d.
*/
Twist2d ToTwist2d(units::second_t dt) const {
constexpr Twist2d ToTwist2d(units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
@@ -68,10 +68,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Discretized ChassisSpeeds.
*/
static ChassisSpeeds Discretize(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
@@ -101,8 +101,8 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Discretized ChassisSpeeds.
*/
static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds,
units::second_t dt) {
static constexpr ChassisSpeeds Discretize(
const ChassisSpeeds& continuousSpeeds, units::second_t dt) {
return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
continuousSpeeds.omega, dt);
}
@@ -123,7 +123,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
// CW rotation into chassis frame
@@ -148,7 +148,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
fieldRelativeSpeeds.vy,
@@ -171,7 +171,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
// CCW rotation out of chassis frame
@@ -196,7 +196,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
robotRelativeSpeeds.vy,

17
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <type_traits>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Twist2d.h"
@@ -36,10 +38,12 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
explicit DifferentialDriveKinematics(units::meter_t trackWidth)
constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
: trackWidth(trackWidth) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
}
}
/**
@@ -83,12 +87,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
(rightDistance - leftDistance) / trackWidth * 1_rad};
}
Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end) const override {
constexpr Twist2d ToTwist2d(
const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end) const override {
return ToTwist2d(end.left - start.left, end.right - start.right);
}
DifferentialDriveWheelPositions Interpolate(
constexpr DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end, double t) const override {
return start.Interpolate(end, t);

14
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
View File

@@ -31,18 +31,10 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const DifferentialDriveWheelPositions& other) const = default;
constexpr bool operator==(
const DifferentialDriveWheelPositions& other) const = default;
/**
* Checks inequality between this DifferentialDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
DifferentialDriveWheelPositions Interpolate(
constexpr DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(left, endValue.left, t),
wpi::Lerp(right, endValue.right, t)};

11
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
View File

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
@@ -35,7 +36,15 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
/**
* Adds two DifferentialDriveWheelSpeeds and returns the sum.

14
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
View File

@@ -40,18 +40,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const MecanumDriveWheelPositions& other) const = default;
constexpr bool operator==(const MecanumDriveWheelPositions& other) const =
default;
/**
* Checks inequality between this MecanumDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const MecanumDriveWheelPositions& other) const = default;
MecanumDriveWheelPositions Interpolate(
constexpr MecanumDriveWheelPositions Interpolate(
const MecanumDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
wpi::Lerp(frontRight, endValue.frontRight, t),

25
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
View File

@@ -4,8 +4,13 @@
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <wpi/SymbolExports.h>
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
@@ -45,7 +50,25 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = units::math::abs(
*std::max_element(wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
}));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
frontLeft = wheelSpeeds[0];
frontRight = wheelSpeeds[1];
rearLeft = wheelSpeeds[2];
rearRight = wheelSpeeds[3];
}
}
/**
* Adds two MecanumDriveWheelSpeeds and returns the sum.

10
wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
View File

@@ -8,7 +8,6 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/math.h"
@@ -33,10 +32,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModulePosition& other) const;
constexpr bool operator==(const SwerveModulePosition& other) const {
return units::math::abs(distance - other.distance) < 1E-9_m &&
angle == other.angle;
}
SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
double t) const {
constexpr SwerveModulePosition Interpolate(
const SwerveModulePosition& endValue, double t) const {
return {wpi::Lerp(distance, endValue.distance, t),
wpi::Lerp(angle, endValue.angle, t)};
}

20
wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
View File

@@ -32,7 +32,10 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModuleState& other) const;
constexpr bool operator==(const SwerveModuleState& other) const {
return units::math::abs(speed - other.speed) < 1E-9_mps &&
angle == other.angle;
}
/**
* Minimize the change in the heading this swerve module state would
@@ -42,7 +45,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*
* @param currentAngle The current module angle.
*/
void Optimize(const Rotation2d& currentAngle) {
constexpr void Optimize(const Rotation2d& currentAngle) {
auto delta = angle - currentAngle;
if (units::math::abs(delta.Degrees()) > 90_deg) {
speed *= -1;
@@ -60,8 +63,15 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param currentAngle The current module angle.
*/
[[deprecated("Use instance method instead.")]]
static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
const Rotation2d& currentAngle);
constexpr static SwerveModuleState Optimize(
const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
auto delta = desiredState.angle - currentAngle;
if (units::math::abs(delta.Degrees()) > 90_deg) {
return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
} else {
return {desiredState.speed, desiredState.angle};
}
}
/**
* Scales speed by cosine of angle error. This scales down movement
@@ -70,7 +80,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*
* @param currentAngle The current module angle.
*/
void CosineScale(const Rotation2d& currentAngle) {
constexpr void CosineScale(const Rotation2d& currentAngle) {
speed *= (angle - currentAngle).Cos();
}
};