From d640c0f41f26543465708005950281f099376ec9 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:38:32 -0700 Subject: [PATCH 01/25] [wpimath] Fix pose estimator local measurement standard deviation docs (NFC) (#4113) --- .../first/math/estimator/MecanumDrivePoseEstimator.java | 6 +++--- .../first/math/estimator/SwerveDrivePoseEstimator.java | 6 +++--- .../include/frc/estimator/MecanumDrivePoseEstimator.h | 9 ++++----- .../include/frc/estimator/SwerveDrivePoseEstimator.h | 9 ++++----- 4 files changed, 14 insertions(+), 16 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index d6f10755f2c..b8fc4f6266a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -67,9 +67,9 @@ public class MecanumDrivePoseEstimator { * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in * meters and radians. - * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. - * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix - * is in the form [theta], with units in radians. + * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number + * to trust sensor readings from the gyro less. This matrix is in the form [theta], with units + * in radians. * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these * numbers to trust global measurements from vision less. This matrix is in the form [x, y, * theta]ᵀ, with units in meters and radians. diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 66b0075687e..c62fbe19400 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -67,9 +67,9 @@ public class SwerveDrivePoseEstimator { * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in * meters and radians. - * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. - * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix - * is in the form [theta], with units in radians. + * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number + * to trust sensor readings from the gyro less. This matrix is in the form [theta], with units + * in radians. * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these * numbers to trust global measurements from vision less. This matrix is in the form [x, y, * theta]ᵀ, with units in meters and radians. diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index e9817efb813..0b10ba6224c 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -59,11 +59,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { * model's state estimates less. This matrix * is in the form [x, y, theta]ᵀ, with units * in meters and radians. - * @param localMeasurementStdDevs Standard deviations of the encoder and gyro - * measurements. Increase these numbers to - * trust sensor readings from encoders - * and gyros less. This matrix is in the form - * [theta], with units in radians. + * @param localMeasurementStdDevs Standard deviation of the gyro measurement. + * Increase this number to trust sensor + * readings from the gyro less. This matrix is + * in the form [theta], with units in radians. * @param visionMeasurementStdDevs Standard deviations of the vision * measurements. Increase these numbers to * trust global measurements from vision diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 60cfcb14451..4536a2060cd 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -62,11 +62,10 @@ class SwerveDrivePoseEstimator { * model's state estimates less. This matrix * is in the form [x, y, theta]ᵀ, with units * in meters and radians. - * @param localMeasurementStdDevs Standard deviations of the encoder and gyro - * measurements. Increase these numbers to - * trust sensor readings from encoders - * and gyros less. This matrix is in the form - * [theta], with units in radians. + * @param localMeasurementStdDevs Standard deviation of the gyro measurement. + * Increase this number to trust sensor + * readings from the gyro less. This matrix is + * in the form [theta], with units in radians. * @param visionMeasurementStdDevs Standard deviations of the vision * measurements. Increase these numbers to * trust global measurements from vision From d0fef183789250c50d1892f1c45a186f6d3e2683 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:39:10 -0700 Subject: [PATCH 02/25] [wpimath] Remove redundant `this.` from ExtendedKalmanFilter.java (#4115) --- .../java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index d4f9e563e39..dc268f92c2c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -133,7 +133,7 @@ public ExtendedKalmanFilter( m_addFuncX = addFuncX; m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); - this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); + m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); m_dtSeconds = dtSeconds; reset(); From d5cb6fed6786ce1b1840558f3a82d0e2f419a090 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:40:26 -0700 Subject: [PATCH 03/25] [wpimath] Support zero cost entries in MakeCostMatrix() (#4100) The existing implementation will produce a cost of NaN if a tolerance of infinity is entered, but the limit approaches zero. Being able to specify that a state has no cost is useful, so this change adds support for that. --- .../edu/wpi/first/math/StateSpaceUtil.java | 29 +++++++++++-------- .../main/native/include/frc/StateSpaceUtil.h | 22 +++++++++----- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 69430eb0860..520eaa72fe3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -61,24 +61,29 @@ public static Matrix makeWhiteNoiseVector(Matrix s /** * Creates a cost matrix from the given vector for use with LQR. * - *

The cost matrix is constructed using Bryson's rule. The inverse square of each element in - * the input is taken and placed on the cost matrix diagonal. + *

The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is + * placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to + * zero. * - * @param Nat representing the states of the system. - * @param costs An array. For a Q matrix, its elements are the maximum allowed excursions of the - * states from the reference. For an R matrix, its elements are the maximum allowed excursions - * of the control inputs from no actuation. + * @param Nat representing the number of system states or inputs. + * @param tolerances An array. For a Q matrix, its elements are the maximum allowed excursions of + * the states from the reference. For an R matrix, its elements are the maximum allowed + * excursions of the control inputs from no actuation. * @return State excursion or control effort cost matrix. */ @SuppressWarnings("MethodTypeParameterName") - public static Matrix makeCostMatrix( - Matrix costs) { - Matrix result = - new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows())); + public static Matrix makeCostMatrix( + Matrix tolerances) { + Matrix result = + new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows())); result.fill(0.0); - for (int i = 0; i < costs.getNumRows(); i++) { - result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2))); + for (int i = 0; i < tolerances.getNumRows(); i++) { + if (tolerances.get(i, 0) == Double.POSITIVE_INFINITY) { + result.set(i, i, 0.0); + } else { + result.set(i, i, 1.0 / (Math.pow(tolerances.get(i, 0), 2))); + } } return result; diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 5fdd18cafd6..17ed68c42a7 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -32,7 +33,11 @@ void MatrixImpl(Matrix& result, T elem, Ts... elems) { template void CostMatrixImpl(Matrix& result, T elem, Ts... elems) { - result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2); + if (elem == std::numeric_limits::infinity()) { + result(result.rows() - (sizeof...(Ts) + 1)) = 0.0; + } else { + result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2); + } if constexpr (sizeof...(Ts) > 0) { CostMatrixImpl(result, elems...); } @@ -116,20 +121,21 @@ Eigen::Matrix MakeMatrix(Ts... elems) { * Creates a cost matrix from the given vector for use with LQR. * * The cost matrix is constructed using Bryson's rule. The inverse square of - * each element in the input is taken and placed on the cost matrix diagonal. + * each tolerance is placed on the cost matrix diagonal. If a tolerance is + * infinity, its cost matrix entry is set to zero. * - * @param costs An array. For a Q matrix, its elements are the maximum allowed - * excursions of the states from the reference. For an R matrix, - * its elements are the maximum allowed excursions of the control - * inputs from no actuation. + * @param tolerances An array. For a Q matrix, its elements are the maximum + * allowed excursions of the states from the reference. For an + * R matrix, its elements are the maximum allowed excursions + * of the control inputs from no actuation. * @return State excursion or control effort cost matrix. */ template ...>>> Eigen::Matrix MakeCostMatrix( - Ts... costs) { + Ts... tolerances) { Eigen::DiagonalMatrix result; - detail::CostMatrixImpl(result.diagonal(), costs...); + detail::CostMatrixImpl(result.diagonal(), tolerances...); return result; } From 95ae23b0e7bdbbe9419d80b581de5854c69d2fa6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:41:28 -0700 Subject: [PATCH 04/25] [wpimath] Improve EKF numerical stability (#4093) The Joseph form of the error covariance update equation is more numerically stable when the Kalman gain isn't optimal. Numerical instability and filter divergence can occur if the user goes long time periods between updates and the error covariance becomes ill-conditioned (the ratio between the largest and smallest eigenvalue gets too large). --- .../wpi/first/math/estimator/ExtendedKalmanFilter.java | 10 ++++++++-- .../include/frc/estimator/ExtendedKalmanFilter.h | 10 +++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index dc268f92c2c..10d6a548f33 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -366,7 +366,13 @@ public void correct( // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁)) m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u)))); - // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻ - m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P); + // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ + // Use Joseph form for numerical stability + m_P = + Matrix.eye(m_states) + .minus(K.times(C)) + .times(m_P) + .times(Matrix.eye(m_states).minus(K.times(C)).transpose()) + .plus(K.times(discR).times(K.transpose())); } } diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 3e5edb8d0b6..a7e0f3df946 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -327,11 +327,15 @@ class ExtendedKalmanFilter { Eigen::Matrix K = S.transpose().ldlt().solve(C * m_P.transpose()).transpose(); - // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁)) + // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁)) m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u))); - // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻ - m_P = (Eigen::Matrix::Identity() - K * C) * m_P; + // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ + // Use Joseph form for numerical stability + m_P = (Eigen::Matrix::Identity() - K * C) * m_P * + (Eigen::Matrix::Identity() - K * C) + .transpose() + + K * discR * K.transpose(); } private: From 89ffcbbe41d3b2a5bbb58a547e9500da839b61e0 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:41:53 -0700 Subject: [PATCH 05/25] [wpimath] Update TrapezoidProfile class name in comment (NFC) (#4107) --- .../main/native/include/frc/trajectory/TrapezoidProfile.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 0e623eb2b38..0e126fdb6de 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -19,14 +19,14 @@ namespace frc { * * Initialization: * @code{.cpp} - * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA}; + * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA}; * double previousProfiledReference = initialReference; * @endcode * * Run on update: * @code{.cpp} - * TrapezoidalMotionProfile profile{constraints, unprofiledReference, - * previousProfiledReference}; + * TrapezoidProfile profile{constraints, unprofiledReference, + * previousProfiledReference}; * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate); * @endcode * From 765efa325e07dc18a69bf6f6daa8d036da1b922d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:44:14 -0700 Subject: [PATCH 06/25] [wpimath] Remove redundant column index from vectors (#4116) --- .../frc/controller/ControlAffinePlantInversionFeedforward.h | 4 ++-- .../include/frc/controller/LinearPlantInversionFeedforward.h | 4 ++-- .../native/include/frc/controller/LinearQuadraticRegulator.h | 4 ++-- .../main/native/include/frc/estimator/ExtendedKalmanFilter.h | 4 ++-- .../main/native/include/frc/estimator/UnscentedKalmanFilter.h | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h index 656f767a6b7..75d85a7bc64 100644 --- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h @@ -104,7 +104,7 @@ class ControlAffinePlantInversionFeedforward { * * @return The row of the calculated feedforward. */ - double Uff(int i) const { return m_uff(i, 0); } + double Uff(int i) const { return m_uff(i); } /** * Returns the current reference vector r. @@ -120,7 +120,7 @@ class ControlAffinePlantInversionFeedforward { * * @return The row of the current reference vector. */ - double R(int i) const { return m_r(i, 0); } + double R(int i) const { return m_r(i); } /** * Resets the feedforward with a specified initial state vector. diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h index 519368da373..780d2e1d727 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h @@ -72,7 +72,7 @@ class LinearPlantInversionFeedforward { * * @return The row of the calculated feedforward. */ - double Uff(int i) const { return m_uff(i, 0); } + double Uff(int i) const { return m_uff(i); } /** * Returns the current reference vector r. @@ -88,7 +88,7 @@ class LinearPlantInversionFeedforward { * * @return The row of the current reference vector. */ - double R(int i) const { return m_r(i, 0); } + double R(int i) const { return m_r(i); } /** * Resets the feedforward with a specified initial state vector. diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index 44a850178ba..c0e5dfd5de0 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -173,7 +173,7 @@ class LinearQuadraticRegulatorImpl { * * @return The row of the reference vector. */ - double R(int i) const { return m_r(i, 0); } + double R(int i) const { return m_r(i); } /** * Returns the control input vector u. @@ -189,7 +189,7 @@ class LinearQuadraticRegulatorImpl { * * @return The row of the control input vector. */ - double U(int i) const { return m_u(i, 0); } + double U(int i) const { return m_u(i); } /** * Resets the controller. diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index a7e0f3df946..b42fbf07a74 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -197,7 +197,7 @@ class ExtendedKalmanFilter { * * @param i Row of x-hat. */ - double Xhat(int i) const { return m_xHat(i, 0); } + double Xhat(int i) const { return m_xHat(i); } /** * Set initial state estimate x-hat. @@ -212,7 +212,7 @@ class ExtendedKalmanFilter { * @param i Row of x-hat. * @param value Value for element of x-hat. */ - void SetXhat(int i, double value) { m_xHat(i, 0) = value; } + void SetXhat(int i, double value) { m_xHat(i) = value; } /** * Resets the observer. diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 3aa3e59b05c..1c87b5e7a8f 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -192,7 +192,7 @@ class UnscentedKalmanFilter { * * @param i Row of x-hat. */ - double Xhat(int i) const { return m_xHat(i, 0); } + double Xhat(int i) const { return m_xHat(i); } /** * Set initial state estimate x-hat. @@ -207,7 +207,7 @@ class UnscentedKalmanFilter { * @param i Row of x-hat. * @param value Value for element of x-hat. */ - void SetXhat(int i, double value) { m_xHat(i, 0) = value; } + void SetXhat(int i, double value) { m_xHat(i) = value; } /** * Resets the observer. From 0d70884dce4bd4b58fba8d399d175ad5f92d7ef9 Mon Sep 17 00:00:00 2001 From: "Ashray._.g" <43589795+Ashray-g@users.noreply.github.com> Date: Sat, 19 Mar 2022 20:46:42 -0700 Subject: [PATCH 07/25] [wpimath] Add InterpolatedTreeMap (#4073) - Add InterpolatedTreeMap for Java from team 254's 2016 MIT licensed code - Add InterpolatedMap for C++ from team 3512's code with @calcmogul (original author) permission Co-authored-by: Tyler Veness --- .../wpi/first/util/InterpolatingTreeMap.java | 96 +++++++++++++ .../native/include/wpi/interpolating_map.h | 87 ++++++++++++ .../first/util/InterpolatingTreeMapTest.java | 134 ++++++++++++++++++ .../test/native/cpp/InterpolatingMapTest.cpp | 52 +++++++ 4 files changed, 369 insertions(+) create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java create mode 100644 wpiutil/src/main/native/include/wpi/interpolating_map.h create mode 100644 wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java create mode 100644 wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp diff --git a/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java new file mode 100644 index 00000000000..ade2e5a42d7 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util; + +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined by making a guess + * from points that are defined. This uses linear interpolation. + */ +public class InterpolatingTreeMap { + private final TreeMap m_map = new TreeMap<>(); + + /** + * Inserts a key-value pair. + * + * @param key The key. + * @param value The value. + */ + public void put(K key, V value) { + m_map.put(key, value); + } + + /** + * Returns the value associated with a given key. + * + *

If there's no matching key, the value returned will be a linear interpolation between the + * keys before and after the provided one. + * + * @param key The key. + * @return The value associated with the given key. + */ + public Double get(K key) { + V val = m_map.get(key); + if (val == null) { + K ceilingKey = m_map.ceilingKey(key); + K floorKey = m_map.floorKey(key); + + if (ceilingKey == null && floorKey == null) { + return null; + } + if (ceilingKey == null) { + return m_map.get(floorKey).doubleValue(); + } + if (floorKey == null) { + return m_map.get(ceilingKey).doubleValue(); + } + V floor = m_map.get(floorKey); + V ceiling = m_map.get(ceilingKey); + + return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey)); + } else { + return val.doubleValue(); + } + } + + /** Clears the contents. */ + public void clear() { + m_map.clear(); + } + + /** + * Return the value interpolated between val1 and val2 by the interpolant d. + * + * @param val1 The lower part of the interpolation range. + * @param val2 The upper part of the interpolation range. + * @param d The interpolant in the range [0, 1]. + * @return The interpolated value. + */ + private double interpolate(V val1, V val2, double d) { + double dydx = val2.doubleValue() - val1.doubleValue(); + return dydx * d + val1.doubleValue(); + } + + /** + * Return where within interpolation range [0, 1] q is between down and up. + * + * @param up Upper part of interpolation range. + * @param q Query. + * @param down Lower part of interpolation range. + * @return Interpolant in range [0, 1]. + */ + private double inverseInterpolate(K up, K q, K down) { + double upperToLower = up.doubleValue() - down.doubleValue(); + if (upperToLower <= 0) { + return 0.0; + } + double queryToLower = q.doubleValue() - down.doubleValue(); + if (queryToLower <= 0) { + return 0.0; + } + return queryToLower / upperToLower; + } +} diff --git a/wpiutil/src/main/native/include/wpi/interpolating_map.h b/wpiutil/src/main/native/include/wpi/interpolating_map.h new file mode 100644 index 00000000000..5eff514c198 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/interpolating_map.h @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +namespace wpi { + +/** + * Implements a table of key-value pairs with linear interpolation between + * values. + * + * If there's no matching key, the value returned will be a linear interpolation + * between the keys before and after the provided one. + * + * @tparam Key The key type. + * @tparam Value The value type. + */ +template +class interpolating_map { + public: + /** + * Inserts a key-value pair. + * + * @param key The key. + * @param value The value. + */ + void insert(const Key& key, const Value& value) { + m_container.insert(std::make_pair(key, value)); + } + + /** + * Inserts a key-value pair. + * + * @param key The key. + * @param value The value. + */ + void insert(Key&& key, Value&& value) { + m_container.insert(std::make_pair(key, value)); + } + + /** + * Returns the value associated with a given key. + * + * If there's no matching key, the value returned will be a linear + * interpolation between the keys before and after the provided one. + * + * @param key The key. + */ + Value operator[](const Key& key) const { + using const_iterator = typename std::map::const_iterator; + + // Get iterator to upper bound key-value pair for the given key + const_iterator upper = m_container.upper_bound(key); + + // If key > largest key in table, return value for largest table key + if (upper == m_container.end()) { + return (--upper)->second; + } + + // If key <= smallest key in table, return value for smallest table key + if (upper == m_container.begin()) { + return upper->second; + } + + // Get iterator to lower bound key-value pair + const_iterator lower = upper; + --lower; + + // Perform linear interpolation between lower and upper bound + const double delta = (key - lower->first) / (upper->first - lower->first); + return delta * upper->second + (1.0 - delta) * lower->second; + } + + /** + * Clears the contents. + */ + void clear() { m_container.clear(); } + + private: + std::map m_container; +}; + +} // namespace wpi diff --git a/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java new file mode 100644 index 00000000000..42b87bcb571 --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java @@ -0,0 +1,134 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class InterpolatingTreeMapTest { + @Test + void testInterpolationDouble() { + InterpolatingTreeMap table = new InterpolatingTreeMap<>(); + + table.put(125.0, 450.0); + table.put(200.0, 510.0); + table.put(268.0, 525.0); + table.put(312.0, 550.0); + table.put(326.0, 650.0); + + // Key below minimum gives smallest value + assertEquals(450.0, table.get(100.0)); + + // Minimum key gives exact value + assertEquals(450.0, table.get(125.0)); + + // Key gives interpolated value + assertEquals(480.0, table.get(162.5)); + + // Key at right of interpolation range gives exact value + assertEquals(510.0, table.get(200.0)); + + // Maximum key gives exact value + assertEquals(650.0, table.get(326.0)); + + // Key above maximum gives largest value + assertEquals(650.0, table.get(400.0)); + } + + @Test + void testInterpolationClear() { + InterpolatingTreeMap table = new InterpolatingTreeMap<>(); + + table.put(125.0, 450.0); + table.put(200.0, 510.0); + table.put(268.0, 525.0); + table.put(312.0, 550.0); + table.put(326.0, 650.0); + + // Key below minimum gives smallest value + assertEquals(450.0, table.get(100.0)); + + // Minimum key gives exact value + assertEquals(450.0, table.get(125.0)); + + // Key gives interpolated value + assertEquals(480.0, table.get(162.5)); + + // Key at right of interpolation range gives exact value + assertEquals(510.0, table.get(200.0)); + + // Maximum key gives exact value + assertEquals(650.0, table.get(326.0)); + + // Key above maximum gives largest value + assertEquals(650.0, table.get(400.0)); + + table.clear(); + + table.put(100.0, 250.0); + table.put(200.0, 500.0); + + assertEquals(375.0, table.get(150.0)); + } + + @Test + void testInterpolationInteger() { + InterpolatingTreeMap table = new InterpolatingTreeMap<>(); + + table.put(125, 450); + table.put(200, 510); + table.put(268, 525); + table.put(312, 550); + table.put(326, 650); + + // Key below minimum gives smallest value + assertEquals(450.0, table.get(100)); + + // Minimum key gives exact value + assertEquals(450.0, table.get(125)); + + // Key gives interpolated value + assertEquals(479.6, table.get(162)); + + // Key at right of interpolation range gives exact value + assertEquals(510.0, table.get(200)); + + // Maximum key gives exact value + assertEquals(650.0, table.get(326)); + + // Key above maximum gives largest value + assertEquals(650.0, table.get(400)); + } + + @Test + void testInterpolationLong() { + InterpolatingTreeMap table = new InterpolatingTreeMap<>(); + + table.put(125L, 450L); + table.put(200L, 510L); + table.put(268L, 525L); + table.put(312L, 550L); + table.put(326L, 650L); + + // Key below minimum gives smallest value + assertEquals(450.0, table.get(100L)); + + // Minimum key gives exact value + assertEquals(450.0, table.get(125L)); + + // Key gives interpolated value + assertEquals(479.6, table.get(162L)); + + // Key at right of interpolation range gives exact value + assertEquals(510.0, table.get(200L)); + + // Maximum key gives exact value + assertEquals(650.0, table.get(326L)); + + // Key above maximum gives largest value + assertEquals(650.0, table.get(400L)); + } +} diff --git a/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp b/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp new file mode 100644 index 00000000000..4479da59146 --- /dev/null +++ b/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/interpolating_map.h" // NOLINT(build/include_order) + +#include + +TEST(InterpolatingMapTest, Insert) { + wpi::interpolating_map table; + + table.insert(125, 450); + table.insert(200, 510); + table.insert(268, 525); + table.insert(312, 550); + table.insert(326, 650); + + // Key below minimum gives smallest value + EXPECT_EQ(450, table[100]); + + // Minimum key gives exact value + EXPECT_EQ(450, table[125]); + + // Key gives interpolated value + EXPECT_EQ(480, table[162.5]); + + // Key at right of interpolation range gives exact value + EXPECT_EQ(510, table[200]); + + // Maximum key gives exact value + EXPECT_EQ(650, table[326]); + + // Key above maximum gives largest value + EXPECT_EQ(650, table[400]); +} + +TEST(InterpolatingMapTest, Clear) { + wpi::interpolating_map table; + + table.insert(125, 450); + table.insert(200, 510); + table.insert(268, 525); + table.insert(312, 550); + table.insert(326, 650); + + table.clear(); + + table.insert(100, 250); + table.insert(200, 500); + + EXPECT_EQ(375, table[150]); +} From cdafc723fb688c133d5fd3880646b9706474814e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 20:47:09 -0700 Subject: [PATCH 08/25] [examples] Remove unused LinearPlantInversionFeedforward includes (#4069) --- .../src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp | 1 - .../src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 0f03d37b4e3..89e98984b56 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 69d1953d4f1..ed588249ac6 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include From 78108c2aba1c2f9b4318e5e13291631a608eae21 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 19 Mar 2022 23:59:00 -0700 Subject: [PATCH 09/25] [wpimath] Fix PIDController having incorrect error after calling SetSetpoint() (#4070) --- .../first/math/controller/PIDController.java | 28 +++++++++---------- .../native/cpp/controller/PIDController.cpp | 28 +++++++++---------- 2 files changed, 26 insertions(+), 30 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java index 12c4175570d..f0420b74262 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java @@ -181,6 +181,15 @@ public double getPeriod() { */ public void setSetpoint(double setpoint) { m_setpoint = setpoint; + + if (m_continuous) { + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + } else { + m_positionError = m_setpoint - m_measurement; + } + + m_velocityError = (m_positionError - m_prevError) / m_period; } /** @@ -200,18 +209,8 @@ public double getSetpoint() { * @return Whether the error is within the acceptable bounds. */ public boolean atSetpoint() { - double positionError; - if (m_continuous) { - double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); - } else { - positionError = m_setpoint - m_measurement; - } - - double velocityError = (positionError - m_prevError) / m_period; - - return Math.abs(positionError) < m_positionTolerance - && Math.abs(velocityError) < m_velocityTolerance; + return Math.abs(m_positionError) < m_positionTolerance + && Math.abs(m_velocityError) < m_velocityTolerance; } /** @@ -303,8 +302,7 @@ public double getVelocityError() { * @return The next controller output. */ public double calculate(double measurement, double setpoint) { - // Set setpoint to provided value - setSetpoint(setpoint); + m_setpoint = setpoint; return calculate(measurement); } @@ -322,7 +320,7 @@ public double calculate(double measurement) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - measurement; + m_positionError = m_setpoint - m_measurement; } m_velocityError = (m_positionError - m_prevError) / m_period; diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index 34af2aa97a6..f5d4801fe68 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -70,26 +70,25 @@ units::second_t PIDController::GetPeriod() const { void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; -} - -double PIDController::GetSetpoint() const { - return m_setpoint; -} -bool PIDController::AtSetpoint() const { - double positionError; if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - positionError = + m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - positionError = m_setpoint - m_measurement; + m_positionError = m_setpoint - m_measurement; } - double velocityError = (positionError - m_prevError) / m_period.value(); + m_velocityError = (m_positionError - m_prevError) / m_period.value(); +} - return std::abs(positionError) < m_positionTolerance && - std::abs(velocityError) < m_velocityTolerance; +double PIDController::GetSetpoint() const { + return m_setpoint; +} + +bool PIDController::AtSetpoint() const { + return std::abs(m_positionError) < m_positionTolerance && + std::abs(m_velocityError) < m_velocityTolerance; } void PIDController::EnableContinuousInput(double minimumInput, @@ -136,7 +135,7 @@ double PIDController::Calculate(double measurement) { m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - measurement; + m_positionError = m_setpoint - m_measurement; } m_velocityError = (m_positionError - m_prevError) / m_period.value(); @@ -151,8 +150,7 @@ double PIDController::Calculate(double measurement) { } double PIDController::Calculate(double measurement, double setpoint) { - // Set setpoint to provided value - SetSetpoint(setpoint); + m_setpoint = setpoint; return Calculate(measurement); } From 8d79dc873801ab4e2c90ae66d499402fbfceda47 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 20 Mar 2022 00:36:12 -0700 Subject: [PATCH 10/25] [wpimath] Add ImplicitModelFollower (#4056) --- .../controller/ImplicitModelFollower.java | 138 ++++++++++++++++++ .../frc/controller/ImplicitModelFollower.h | 137 +++++++++++++++++ .../controller/ImplicitModelFollowerTest.java | 109 ++++++++++++++ .../controller/ImplicitModelFollowerTest.cpp | 109 ++++++++++++++ 4 files changed, 493 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java create mode 100644 wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java create mode 100644 wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java new file mode 100644 index 00000000000..e8343030a36 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java @@ -0,0 +1,138 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; +import org.ejml.simple.SimpleMatrix; + +/** + * Contains the controller coefficients and logic for an implicit model follower. + * + *

Implicit model following lets us design a feedback controller that erases the dynamics of our + * system and makes it behave like some other system. This can be used to make a drivetrain more + * controllable during teleop driving by making it behave like a slower or more benign drivetrain. + * + *

For more on the underlying math, read appendix B.3 in + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +@SuppressWarnings("ClassTypeParameterName") +public class ImplicitModelFollower { + // Computed controller output + @SuppressWarnings("MemberName") + private Matrix m_u; + + // State space conversion gain + @SuppressWarnings("MemberName") + private Matrix m_A; + + // Input space conversion gain + @SuppressWarnings("MemberName") + private Matrix m_B; + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param plantRef The plant whose dynamics should be followed. + * @param dtSeconds Discretization timestep. + */ + public ImplicitModelFollower( + LinearSystem plant, + LinearSystem plantRef, + double dtSeconds) { + this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB(), dtSeconds); + } + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings("ParameterName") + public ImplicitModelFollower( + Matrix A, + Matrix B, + Matrix Aref, + Matrix Bref, + double dtSeconds) { + m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); + + // Discretize real dynamics + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + // Discretize desired dynamics + var discABrefPair = Discretization.discretizeAB(Aref, Bref, dtSeconds); + var discAref = discABrefPair.getFirst(); + var discBref = discABrefPair.getSecond(); + + // Find u_imf that makes real model match reference model. + // + // x_k+1 = Ax_k + Bu_imf + // z_k+1 = Aref z_k + Bref u_k + // + // Let x_k = z_k. + // + // x_k+1 = z_k+1 + // Ax_k + Bu_imf = Aref x_k + Bref u_k + // Bu_imf = Aref x_k - Ax_k + Bref u_k + // Bu_imf = (Aref - A)x_k + Bref u_k + // u_imf = B^+ ((Aref - A)x_k + Bref u_k) + // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + + // The first term makes the open-loop poles that of the reference + // system, and the second term makes the input behave like that of the + // reference system. + m_A = discB.solve(discA.minus(discAref)).times(-1.0); + m_B = discB.solve(discBref); + + reset(); + } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + public Matrix getU() { + return m_u; + } + + /** + * Returns an element of the control input vector u. + * + * @param i Row of u. + * @return The row of the control input vector. + */ + public double getU(int i) { + return m_u.get(i, 0); + } + + /** Resets the controller. */ + public void reset() { + m_u.fill(0.0); + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + * @param u The current input for the original model. + * @return The next controller output. + */ + public Matrix calculate(Matrix x, Matrix u) { + m_u = m_A.times(x).plus(m_B.times(u)); + return m_u; + } +} diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h new file mode 100644 index 00000000000..e8ebcf620be --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h @@ -0,0 +1,137 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "Eigen/Core" +#include "Eigen/QR" +#include "units/time.h" + +namespace frc { + +/** + * Contains the controller coefficients and logic for an implicit model + * follower. + * + * Implicit model following lets us design a feedback controller that erases the + * dynamics of our system and makes it behave like some other system. This can + * be used to make a drivetrain more controllable during teleop driving by + * making it behave like a slower or more benign drivetrain. + * + * For more on the underlying math, read appendix B.3 in + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +template +class ImplicitModelFollower { + public: + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param plantRef The plant whose dynamics should be followed. + * @param dt Discretization timestep. + */ + template + ImplicitModelFollower(const LinearSystem& plant, + const LinearSystem& plantRef, + units::second_t dt) + : ImplicitModelFollower(plant.A(), plant.B(), + plantRef.A(), plantRef.B(), dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. + * @param dt Discretization timestep. + */ + ImplicitModelFollower(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Aref, + const Eigen::Matrix& Bref, + units::second_t dt) { + // Discretize real dynamics + Eigen::Matrix discA; + Eigen::Matrix discB; + frc::DiscretizeAB(A, B, dt, &discA, &discB); + + // Discretize desired dynamics + Eigen::Matrix discAref; + Eigen::Matrix discBref; + frc::DiscretizeAB(Aref, Bref, dt, &discAref, &discBref); + + // Find u_imf that makes real model match reference model. + // + // x_k+1 = Ax_k + Bu_imf + // z_k+1 = Aref z_k + Bref u_k + // + // Let x_k = z_k. + // + // x_k+1 = z_k+1 + // Ax_k + Bu_imf = Aref x_k + Bref u_k + // Bu_imf = Aref x_k - Ax_k + Bref u_k + // Bu_imf = (Aref - A)x_k + Bref u_k + // u_imf = B^+ ((Aref - A)x_k + Bref u_k) + // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + + // The first term makes the open-loop poles that of the reference + // system, and the second term makes the input behave like that of the + // reference system. + m_A = -discB.householderQr().solve(discA - discAref); + m_B = discB.householderQr().solve(discBref); + + Reset(); + } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + const Eigen::Vector& U() const { return m_u; } + + /** + * Returns an element of the control input vector u. + * + * @param i Row of u. + * + * @return The row of the control input vector. + */ + double U(int i) const { return m_u(i); } + + /** + * Resets the controller. + */ + void Reset() { m_u.setZero(); } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + * @param u The current input for the original model. + */ + Eigen::Vector Calculate( + const Eigen::Vector& x, + const Eigen::Vector& u) { + m_u = m_A * x + m_B * u; + return m_u; + } + + private: + // Computed controller output + Eigen::Vector m_u; + + // State space conversion gain + Eigen::Matrix m_A; + + // Input space conversion gain + Eigen::Matrix m_B; +}; + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java new file mode 100644 index 00000000000..e16dcb629d1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.plant.LinearSystemId; +import org.junit.jupiter.api.Test; + +class ImplicitModelFollowerTest { + @Test + @SuppressWarnings("LocalVariableName") + void testSameModel() { + final double dt = 0.005; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + var imf = new ImplicitModelFollower(plant, plant, dt); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + + // Backward + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + + // Rotate CCW + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + } + + @Test + @SuppressWarnings("LocalVariableName") + void testSlowerRefModel() { + final double dt = 0.005; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + // Linear acceleration is slower, but angular acceleration is the same + var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0); + + var imf = new ImplicitModelFollower(plant, plantRef, dt); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertTrue(x.get(0, 0) >= xImf.get(0, 0)); + assertTrue(x.get(1, 0) >= xImf.get(1, 0)); + } + + // Backward + x.fill(0.0); + xImf.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertTrue(x.get(0, 0) <= xImf.get(0, 0)); + assertTrue(x.get(1, 0) <= xImf.get(1, 0)); + } + + // Rotate CCW + x.fill(0.0); + xImf.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5); + assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5); + } + } +} diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp new file mode 100644 index 00000000000..05a6b7100c2 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/controller/ImplicitModelFollower.h" +#include "frc/system/plant/LinearSystemId.h" + +namespace frc { + +TEST(ImplicitModelFollowerTest, SameModel) { + constexpr auto dt = 5_ms; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + ImplicitModelFollower<2, 2> imf{plant, plant, dt}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xImf{0.0, 0.0}; + + // Forward + Eigen::Vector u{12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } + + // Backward + u = Eigen::Vector{-12.0, -12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } + + // Rotate CCW + u = Eigen::Vector{-12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } +} + +TEST(ImplicitModelFollowerTest, SlowerRefModel) { + constexpr auto dt = 5_ms; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + // Linear acceleration is slower, but angular acceleration is the same + auto plantRef = LinearSystemId::IdentifyDrivetrainSystem( + Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0}); + + ImplicitModelFollower<2, 2> imf{plant, plantRef, dt}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xImf{0.0, 0.0}; + + // Forward + Eigen::Vector u{12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_GE(x(0), xImf(0)); + EXPECT_GE(x(1), xImf(1)); + } + + // Backward + x.setZero(); + xImf.setZero(); + u = Eigen::Vector{-12.0, -12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_LE(x(0), xImf(0)); + EXPECT_LE(x(1), xImf(1)); + } + + // Rotate CCW + x.setZero(); + xImf.setZero(); + u = Eigen::Vector{-12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); + + EXPECT_NEAR(x(0), xImf(0), 1e-5); + EXPECT_NEAR(x(1), xImf(1), 1e-5); + } +} + +} // namespace frc From e1b6e5f21207d32ef6a1edaf49aacbc3209caf67 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Sun, 20 Mar 2022 21:54:43 -0700 Subject: [PATCH 11/25] [wpilib] Improve MotorSafety documentation (NFC) (#4120) Remove OBE RobotDrive porting guide from MecanumDrive --- wpilibc/src/main/native/include/frc/MotorSafety.h | 7 +++++-- .../main/native/include/frc/drive/DifferentialDrive.h | 4 ++++ .../src/main/native/include/frc/drive/KilloughDrive.h | 3 +++ .../src/main/native/include/frc/drive/MecanumDrive.h | 11 ++--------- .../main/native/include/frc/drive/RobotDriveBase.h | 2 ++ .../main/java/edu/wpi/first/wpilibj/MotorSafety.java | 6 ++++-- .../wpi/first/wpilibj/drive/DifferentialDrive.java | 3 +++ .../edu/wpi/first/wpilibj/drive/KilloughDrive.java | 3 +++ .../edu/wpi/first/wpilibj/drive/MecanumDrive.java | 9 ++------- .../edu/wpi/first/wpilibj/drive/RobotDriveBase.java | 6 +++++- 10 files changed, 33 insertions(+), 21 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h index 1bfd34a81a4..a17cdc0cabd 100644 --- a/wpilibc/src/main/native/include/frc/MotorSafety.h +++ b/wpilibc/src/main/native/include/frc/MotorSafety.h @@ -14,8 +14,11 @@ namespace frc { /** - * This base class runs a watchdog timer and calls the subclass's StopMotor() - * function if the timeout expires. + * The Motor Safety feature acts as a watchdog timer for an individual motor. It + * operates by maintaining a timer that tracks how long it has been since the + * feed() method has been called for that actuator. Code in the Driver Station + * class initiates a comparison of these timers to the timeout values for any + * actuator with safety enabled every 5 received packets (100ms nominal). * * The subclass should call Feed() whenever the motor value is updated. */ diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h index 2b5831b49ca..0b793a75428 100644 --- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h @@ -95,6 +95,10 @@ class SpeedController; * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled * so that the full range is still used. This deadband value can be changed * with SetDeadband(). + * + * MotorSafety is enabled by default. The tankDrive, arcadeDrive, + * or curvatureDrive methods should be called periodically to avoid Motor + * Safety timeouts. */ class DifferentialDrive : public RobotDriveBase, public wpi::Sendable, diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h index d4c5c5c2e45..c09f218574d 100644 --- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h @@ -53,6 +53,9 @@ class SpeedController; * The positive X axis points ahead, the positive Y axis points right, and the * and the positive Z axis points down. Rotations follow the right-hand rule, so * clockwise rotation around the Z axis is positive. + * + * MotorSafety is enabled by default. The DriveCartesian or DrivePolar + * methods should be called periodically to avoid Motor Safety timeouts. */ class KilloughDrive : public RobotDriveBase, public wpi::Sendable, diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h index 757ae675702..7e447ef298a 100644 --- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h @@ -57,15 +57,8 @@ class SpeedController; * so that the full range is still used. This deadband value can be changed * with SetDeadband(). * - * RobotDrive porting guide: - *
DriveCartesian(double, double, double, double) is equivalent to - * RobotDrive's MecanumDrive_Cartesian(double, double, double, double) - * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted - * compared to RobotDrive (eg DriveCartesian(xSpeed, -ySpeed, zRotation, - * -gyroAngle). - *
DrivePolar(double, double, double) is equivalent to - * RobotDrive's MecanumDrive_Polar(double, double, double) if a - * deadband of 0 is used. + * MotorSafety is enabled by default. The DriveCartesian or DrivePolar + * methods should be called periodically to avoid Motor Safety timeouts. */ class MecanumDrive : public RobotDriveBase, public wpi::Sendable, diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h index 8ce5636709f..662bdaebcf3 100644 --- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h @@ -16,6 +16,8 @@ namespace frc { /** * Common base class for drive platforms. + * + * MotorSafety is enabled by default. */ class RobotDriveBase : public MotorSafety { public: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java index 4e7693649db..5fb10b4ff82 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java @@ -8,8 +8,10 @@ import java.util.Set; /** - * This base class runs a watchdog timer and calls the subclass's StopMotor() function if the - * timeout expires. + * The Motor Safety feature acts as a watchdog timer for an individual motor. It operates by + * maintaining a timer that tracks how long it has been since the feed() method has been called for + * that actuator. Code in the Driver Station class initiates a comparison of these timers to the + * timeout values for any actuator with safety enabled every 5 received packets (100ms nominal). * *

The subclass should call feed() whenever the motor value is updated. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index cb8729200e5..b6f38ffa4ae 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -83,6 +83,9 @@ *

Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will * be set to 0, and larger values will be scaled so that the full range is still used. This deadband * value can be changed with {@link #setDeadband}. + * + *

{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The tankDrive, arcadeDrive, + * or curvatureDrive methods should be called periodically to avoid Motor Safety timeouts. */ @SuppressWarnings("removal") public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java index 9915c87da6e..6eb934ddef1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -39,6 +39,9 @@ *

The positive X axis points ahead, the positive Y axis points right, and the positive Z axis * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is * positive. + * + *

{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or + * drivePolar methods should be called periodically to avoid Motor Safety timeouts. */ @SuppressWarnings("removal") public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index d22593f0a80..af93f3997a0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -47,13 +47,8 @@ * be set to 0, and larger values will be scaled so that the full range is still used. This deadband * value can be changed with {@link #setDeadband}. * - *

RobotDrive porting guide:
- * {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's - * mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed - * and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed, - * zRotation, -gyroAngle).
- * {@link #drivePolar(double, double, double)} is equivalent to RobotDrive's - * mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used. + *

{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or + * drivePolar methods should be called periodically to avoid Motor Safety timeouts. */ @SuppressWarnings("removal") public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java index 569f94c8f5f..82673ff0557 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -7,7 +7,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.MotorSafety; -/** Common base class for drive platforms. */ +/** + * Common base class for drive platforms. + * + *

{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. + */ public abstract class RobotDriveBase extends MotorSafety { public static final double kDefaultDeadband = 0.02; public static final double kDefaultMaxOutput = 1.0; From ba0dccaae46878187f6df77d44e1cae15c2d0c3f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 20 Mar 2022 21:57:03 -0700 Subject: [PATCH 12/25] [wpimath] Fix reference to Rotation2d.fromRadians() (#4118) Rotation2d.fromRadians() doesn't exist. The constructor should be used instead. --- .../src/main/java/edu/wpi/first/math/geometry/Rotation2d.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 8eed76cd180..838a8310c7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -71,7 +71,7 @@ public static Rotation2d fromDegrees(double degrees) { * Adds two rotations together, with the result being bounded between -pi and pi. * *

For example, Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60)) equals - * Rotation2d.fromRadians(Math.PI/2.0) + * Rotation2d(Math.PI/2.0) * * @param other The rotation to add. * @return The sum of the two rotations. @@ -84,7 +84,7 @@ public Rotation2d plus(Rotation2d other) { * Subtracts the new rotation from the current rotation and returns the new rotation. * *

For example, Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100)) - * equals Rotation2d.fromRadians(-Math.PI/2.0) + * equals Rotation2d(-Math.PI/2.0) * * @param other The rotation to subtract. * @return The difference between the two rotations. From 126e3de91a88ed6fd5b3b415b1fbb2bcdf876aa7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 24 Mar 2022 07:24:12 -0700 Subject: [PATCH 13/25] [wpilibc] Remove unused SetPriority() call from Ultrasonic (#4123) --- wpilibc/src/main/native/cpp/Ultrasonic.cpp | 5 ----- wpilibc/src/main/native/include/frc/Ultrasonic.h | 3 --- 2 files changed, 8 deletions(-) diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp index 02035dd14a8..c441c7164d9 100644 --- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp +++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp @@ -120,11 +120,6 @@ void Ultrasonic::SetAutomaticMode(bool enabling) { } m_thread = std::thread(&Ultrasonic::UltrasonicChecker); - - // TODO: Currently, lvuser does not have permissions to set task priorities. - // Until that is the case, uncommenting this will break user code that calls - // Ultrasonic::SetAutomicMode(). - // m_task.SetPriority(kPriority); } else { // Wait for background task to stop running if (m_thread.joinable()) { diff --git a/wpilibc/src/main/native/include/frc/Ultrasonic.h b/wpilibc/src/main/native/include/frc/Ultrasonic.h index 253192f1c71..850f7b3337e 100644 --- a/wpilibc/src/main/native/include/frc/Ultrasonic.h +++ b/wpilibc/src/main/native/include/frc/Ultrasonic.h @@ -167,9 +167,6 @@ class Ultrasonic : public wpi::Sendable, // Time (sec) for the ping trigger pulse. static constexpr double kPingTime = 10 * 1e-6; - // Priority that the ultrasonic round robin task runs. - static constexpr int kPriority = 64; - // Max time (ms) between readings. static constexpr auto kMaxUltrasonicTime = 0.1_s; static constexpr auto kSpeedOfSound = 1130_fps; From 069f932e59a7667d596da29589902bb4f9000aec Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Mon, 28 Mar 2022 22:31:51 -0700 Subject: [PATCH 14/25] [build] Fix gl3w cmake build (#4139) --- imgui/CMakeLists.txt.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imgui/CMakeLists.txt.in b/imgui/CMakeLists.txt.in index ee8e2a22a7d..9e911a77c18 100644 --- a/imgui/CMakeLists.txt.in +++ b/imgui/CMakeLists.txt.in @@ -15,7 +15,7 @@ ExternalProject_Add(glfw3 ) ExternalProject_Add(gl3w GIT_REPOSITORY https://github.com/skaslev/gl3w - GIT_TAG 3755745085ac2e865fd22270cfe9169c26640f70 + GIT_TAG 5f8d7fd191ba22ff2b60c1106d7135bb9a335533 SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/gl3w-src" BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/gl3w-build" INSTALL_COMMAND "" From 2e462a19d39d07c3c5df30ecfca386803dd5e4b6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 29 Mar 2022 08:42:08 -0700 Subject: [PATCH 15/25] [wpimath] Constexprify units unary operators (#4138) Fixes #4137. --- wpimath/src/main/native/include/units/base.h | 4 ++-- wpimath/src/test/native/cpp/UnitsTest.cpp | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h index bc9fb446f73..eba34998313 100644 --- a/wpimath/src/main/native/include/units/base.h +++ b/wpimath/src/main/native/include/units/base.h @@ -2328,7 +2328,7 @@ namespace units // unary addition: +T template class NonLinearScale> - inline unit_t operator+(const unit_t& u) noexcept + constexpr inline unit_t operator+(const unit_t& u) noexcept { return u; } @@ -2352,7 +2352,7 @@ namespace units // unary addition: -T template class NonLinearScale> - inline unit_t operator-(const unit_t& u) noexcept + constexpr inline unit_t operator-(const unit_t& u) noexcept { return unit_t(-u()); } diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp index d5ba717e878..6f70d83a7eb 100644 --- a/wpimath/src/test/native/cpp/UnitsTest.cpp +++ b/wpimath/src/test/native/cpp/UnitsTest.cpp @@ -3063,14 +3063,17 @@ TEST_F(Constexpr, construction) { constexpr meter_t result0(0); constexpr auto result1 = make_unit(1); constexpr auto result2 = meter_t(2); + constexpr auto result3 = -3_m; EXPECT_EQ(meter_t(0), result0); EXPECT_EQ(meter_t(1), result1); EXPECT_EQ(meter_t(2), result2); + EXPECT_EQ(meter_t(-3), result3); EXPECT_TRUE(noexcept(result0)); EXPECT_TRUE(noexcept(result1)); EXPECT_TRUE(noexcept(result2)); + EXPECT_TRUE(noexcept(result3)); } TEST_F(Constexpr, constants) { From b4620f01f92ad766c2a83702384df8519e4eb20c Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 29 Mar 2022 08:42:43 -0700 Subject: [PATCH 16/25] [wpimath] Fix Rotation2d interpolation in Java (#4125) Fixes #4112. --- .../edu/wpi/first/math/geometry/Rotation2d.java | 2 +- .../wpi/first/math/geometry/Rotation2dTest.java | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 838a8310c7f..25f7b93985c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -206,6 +206,6 @@ public int hashCode() { @Override @SuppressWarnings("ParameterName") public Rotation2d interpolate(Rotation2d endValue, double t) { - return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t)); + return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index cb3f0f3dfdb..87055ef2108 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -76,4 +76,19 @@ void testInequality() { var rot2 = Rotation2d.fromDegrees(43.5); assertNotEquals(rot1, rot2); } + + @Test + void testInterpolate() { + // 50 + (70 - 50) * 0.5 = 60 + var rot1 = Rotation2d.fromDegrees(50); + var rot2 = Rotation2d.fromDegrees(70); + var interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(60.0, interpolated.getDegrees(), 1e-2); + + // -160 minus half distance between 170 and -160 (15) = -175 + var rot3 = Rotation2d.fromDegrees(170); + var rot4 = Rotation2d.fromDegrees(-160); + interpolated = rot3.interpolate(rot4, 0.5); + assertEquals(-175.0, interpolated.getDegrees()); + } } From c8905ec29aa3299c941138b015281bef0bf9d815 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 29 Mar 2022 11:29:06 -0700 Subject: [PATCH 17/25] [wpimath] Remove ImplicitModelFollower dt argument (#4119) The math works just fine without model discretization. --- .../controller/ImplicitModelFollower.java | 44 +++++----------- .../frc/controller/ImplicitModelFollower.h | 51 +++++++------------ .../controller/ImplicitModelFollowerTest.java | 4 +- .../controller/ImplicitModelFollowerTest.cpp | 4 +- 4 files changed, 36 insertions(+), 67 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java index e8343030a36..6d620014a5b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Num; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.system.Discretization; import edu.wpi.first.math.system.LinearSystem; import org.ejml.simple.SimpleMatrix; @@ -40,13 +39,10 @@ public class ImplicitModelFollower plant, - LinearSystem plantRef, - double dtSeconds) { - this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB(), dtSeconds); + LinearSystem plant, LinearSystem plantRef) { + this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB()); } /** @@ -56,46 +52,34 @@ public ImplicitModelFollower( * @param B Continuous input matrix of the plant being controlled. * @param Aref Continuous system matrix whose dynamics should be followed. * @param Bref Continuous input matrix whose dynamics should be followed. - * @param dtSeconds Discretization timestep. */ @SuppressWarnings("ParameterName") public ImplicitModelFollower( Matrix A, Matrix B, Matrix Aref, - Matrix Bref, - double dtSeconds) { + Matrix Bref) { m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); - // Discretize real dynamics - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); - var discA = discABPair.getFirst(); - var discB = discABPair.getSecond(); - - // Discretize desired dynamics - var discABrefPair = Discretization.discretizeAB(Aref, Bref, dtSeconds); - var discAref = discABrefPair.getFirst(); - var discBref = discABrefPair.getSecond(); - // Find u_imf that makes real model match reference model. // - // x_k+1 = Ax_k + Bu_imf - // z_k+1 = Aref z_k + Bref u_k + // dx/dt = Ax + Bu_imf + // dz/dt = A_ref z + B_ref u // - // Let x_k = z_k. + // Let x = z. // - // x_k+1 = z_k+1 - // Ax_k + Bu_imf = Aref x_k + Bref u_k - // Bu_imf = Aref x_k - Ax_k + Bref u_k - // Bu_imf = (Aref - A)x_k + Bref u_k - // u_imf = B^+ ((Aref - A)x_k + Bref u_k) - // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + // dx/dt = dz/dt + // Ax + Bu_imf = Aref x + B_ref u + // Bu_imf = A_ref x - Ax + B_ref u + // Bu_imf = (A_ref - A)x + B_ref u + // u_imf = B⁻¹((A_ref - A)x + Bref u) + // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u // The first term makes the open-loop poles that of the reference // system, and the second term makes the input behave like that of the // reference system. - m_A = discB.solve(discA.minus(discAref)).times(-1.0); - m_B = discB.solve(discBref); + m_A = B.solve(A.minus(Aref)).times(-1.0); + m_B = B.solve(Bref); reset(); } diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h index e8ebcf620be..e093582bfd1 100644 --- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h +++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include "Eigen/Core" @@ -33,58 +32,44 @@ class ImplicitModelFollower { * * @param plant The plant being controlled. * @param plantRef The plant whose dynamics should be followed. - * @param dt Discretization timestep. */ template ImplicitModelFollower(const LinearSystem& plant, - const LinearSystem& plantRef, - units::second_t dt) + const LinearSystem& plantRef) : ImplicitModelFollower(plant.A(), plant.B(), - plantRef.A(), plantRef.B(), dt) {} + plantRef.A(), plantRef.B()) {} /** * Constructs a controller with the given coefficients and plant. * - * @param A Continuous system matrix of the plant being controlled. - * @param B Continuous input matrix of the plant being controlled. - * @param Aref Continuous system matrix whose dynamics should be followed. - * @param Bref Continuous input matrix whose dynamics should be followed. - * @param dt Discretization timestep. + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. */ ImplicitModelFollower(const Eigen::Matrix& A, const Eigen::Matrix& B, const Eigen::Matrix& Aref, - const Eigen::Matrix& Bref, - units::second_t dt) { - // Discretize real dynamics - Eigen::Matrix discA; - Eigen::Matrix discB; - frc::DiscretizeAB(A, B, dt, &discA, &discB); - - // Discretize desired dynamics - Eigen::Matrix discAref; - Eigen::Matrix discBref; - frc::DiscretizeAB(Aref, Bref, dt, &discAref, &discBref); - + const Eigen::Matrix& Bref) { // Find u_imf that makes real model match reference model. // - // x_k+1 = Ax_k + Bu_imf - // z_k+1 = Aref z_k + Bref u_k + // dx/dt = Ax + Bu_imf + // dz/dt = A_ref z + B_ref u // - // Let x_k = z_k. + // Let x = z. // - // x_k+1 = z_k+1 - // Ax_k + Bu_imf = Aref x_k + Bref u_k - // Bu_imf = Aref x_k - Ax_k + Bref u_k - // Bu_imf = (Aref - A)x_k + Bref u_k - // u_imf = B^+ ((Aref - A)x_k + Bref u_k) - // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + // dx/dt = dz/dt + // Ax + Bu_imf = Aref x + B_ref u + // Bu_imf = A_ref x - Ax + B_ref u + // Bu_imf = (A_ref - A)x + B_ref u + // u_imf = B⁻¹((A_ref - A)x + Bref u) + // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u // The first term makes the open-loop poles that of the reference // system, and the second term makes the input behave like that of the // reference system. - m_A = -discB.householderQr().solve(discA - discAref); - m_B = discB.householderQr().solve(discBref); + m_A = -B.householderQr().solve(A - Aref); + m_B = B.householderQr().solve(Bref); Reset(); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java index e16dcb629d1..e3d8aeec595 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -21,7 +21,7 @@ void testSameModel() { var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); - var imf = new ImplicitModelFollower(plant, plant, dt); + var imf = new ImplicitModelFollower(plant, plant); var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); @@ -67,7 +67,7 @@ void testSlowerRefModel() { // Linear acceleration is slower, but angular acceleration is the same var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0); - var imf = new ImplicitModelFollower(plant, plantRef, dt); + var imf = new ImplicitModelFollower(plant, plantRef); var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp index 05a6b7100c2..9916370ef9f 100644 --- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -17,7 +17,7 @@ TEST(ImplicitModelFollowerTest, SameModel) { auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, Kv_t{1.0}, Ka_t{1.0}); - ImplicitModelFollower<2, 2> imf{plant, plant, dt}; + ImplicitModelFollower<2, 2> imf{plant, plant}; Eigen::Vector x{0.0, 0.0}; Eigen::Vector xImf{0.0, 0.0}; @@ -66,7 +66,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { auto plantRef = LinearSystemId::IdentifyDrivetrainSystem( Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0}); - ImplicitModelFollower<2, 2> imf{plant, plantRef, dt}; + ImplicitModelFollower<2, 2> imf{plant, plantRef}; Eigen::Vector x{0.0, 0.0}; Eigen::Vector xImf{0.0, 0.0}; From 9650e6733e955e1c1cc3e90aca41d5a8ccb907bc Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 29 Mar 2022 12:28:59 -0700 Subject: [PATCH 18/25] [wpiutil] DataLog: Document finish and thread safety (NFC) (#4140) --- .../java/edu/wpi/first/util/datalog/DataLog.java | 11 +++++++++++ wpiutil/src/main/native/include/wpi/DataLog.h | 16 ++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java index 246a3b1db9f..af2027c274c 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java @@ -10,6 +10,17 @@ * *

The data log is periodically flushed to disk. It can also be explicitly flushed to disk by * using the flush() function. + * + *

The finish() function is needed only to indicate in the log that a particular entry is no + * longer being used (it releases the name to ID mapping). The finish() function is not required to + * be called for data to be flushed to disk; entries in the log are written as append() calls are + * being made. In fact, finish() does not need to be called at all. + * + *

DataLog calls are thread safe. DataLog uses a typical multiple-supplier, single-consumer + * setup. Writes to the log are atomic, but there is no guaranteed order in the log when multiple + * threads are writing to it; whichever thread grabs the write mutex first will get written first. + * For this reason (as well as the fact that timestamps can be set to arbitrary values), records in + * the log are not guaranteed to be sorted by timestamp. */ @SuppressWarnings({"PMD.TooManyMethods", "PMD.ExcessivePublicCount"}) public final class DataLog implements AutoCloseable { diff --git a/wpiutil/src/main/native/include/wpi/DataLog.h b/wpiutil/src/main/native/include/wpi/DataLog.h index 46382b18c1b..c66cd10cc64 100644 --- a/wpiutil/src/main/native/include/wpi/DataLog.h +++ b/wpiutil/src/main/native/include/wpi/DataLog.h @@ -46,6 +46,22 @@ enum ControlRecordType { * * The data log is periodically flushed to disk. It can also be explicitly * flushed to disk by using the Flush() function. + * + * Finish() is needed only to indicate in the log that a particular entry is + * no longer being used (it releases the name to ID mapping). Finish() is not + * required to be called for data to be flushed to disk; entries in the log + * are written as Append() calls are being made. In fact, Finish() does not + * need to be called at all; this is helpful to avoid shutdown races where the + * DataLog object might be destroyed before other objects. It's often not a + * good idea to call Finish() from destructors for this reason. + * + * DataLog calls are thread safe. DataLog uses a typical multiple-supplier, + * single-consumer setup. Writes to the log are atomic, but there is no + * guaranteed order in the log when multiple threads are writing to it; + * whichever thread grabs the write mutex first will get written first. + * For this reason (as well as the fact that timestamps can be set to + * arbitrary values), records in the log are not guaranteed to be sorted by + * timestamp. */ class DataLog final { public: From 81c5b41ce1ef1dacda3d0b3b81fb38ad0c67138c Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 31 Mar 2022 00:29:44 -0700 Subject: [PATCH 19/25] [wpilibj] Document MechanismLigament2d angle unit (NFC) (#4142) --- .../first/wpilibj/smartdashboard/MechanismLigament2d.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java index 88236debe5d..8f0386f78c4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java @@ -30,7 +30,7 @@ public class MechanismLigament2d extends MechanismObject2d { * * @param name The ligament name. * @param length The ligament length. - * @param angle The ligament angle. + * @param angle The ligament angle in degrees. * @param lineWidth The ligament's line width. * @param color The ligament's color. */ @@ -48,7 +48,7 @@ public MechanismLigament2d( * * @param name The ligament's name. * @param length The ligament's length. - * @param angle The ligament's angle relative to its parent. + * @param angle The ligament's angle relative to its parent in degrees. */ public MechanismLigament2d(String name, double length, double angle) { this(name, length, angle, 10, new Color8Bit(235, 137, 52)); @@ -57,7 +57,7 @@ public MechanismLigament2d(String name, double length, double angle) { /** * Set the ligament's angle relative to its parent. * - * @param degrees the angle, in degrees + * @param degrees the angle in degrees */ public synchronized void setAngle(double degrees) { m_angle = degrees; @@ -76,7 +76,7 @@ public synchronized void setAngle(Rotation2d angle) { /** * Get the ligament's angle relative to its parent. * - * @return the angle, in degrees + * @return the angle in degrees */ public synchronized double getAngle() { if (m_angleEntry != null) { From 88222daa3d1035ab693677e0ea59b4abb1a894c7 Mon Sep 17 00:00:00 2001 From: apple <19359968+apppppppple@users.noreply.github.com> Date: Fri, 8 Apr 2022 00:57:01 -0400 Subject: [PATCH 20/25] [hal] Fix misspelling in AnalogInput/Output docs (NFC) (#4153) value -> valid (NFC) --- hal/src/main/native/include/hal/AnalogInput.h | 2 +- hal/src/main/native/include/hal/AnalogOutput.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hal/src/main/native/include/hal/AnalogInput.h b/hal/src/main/native/include/hal/AnalogInput.h index 956cd217b88..fd591fed410 100644 --- a/hal/src/main/native/include/hal/AnalogInput.h +++ b/hal/src/main/native/include/hal/AnalogInput.h @@ -46,7 +46,7 @@ void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle); HAL_Bool HAL_CheckAnalogModule(int32_t module); /** - * Checks that the analog output channel number is value. + * Checks that the analog output channel number is valid. * Verifies that the analog channel number is one of the legal channel numbers. * Channel numbers are 0-based. * diff --git a/hal/src/main/native/include/hal/AnalogOutput.h b/hal/src/main/native/include/hal/AnalogOutput.h index 26e52318f54..e6a5efc9042 100644 --- a/hal/src/main/native/include/hal/AnalogOutput.h +++ b/hal/src/main/native/include/hal/AnalogOutput.h @@ -58,7 +58,7 @@ double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle, int32_t* status); /** - * Checks that the analog output channel number is value. + * Checks that the analog output channel number is valid. * * Verifies that the analog channel number is one of the legal channel numbers. * Channel numbers are 0-based. From 1b26e2d5da818b744e0951a21ca8fab69e061610 Mon Sep 17 00:00:00 2001 From: Excalibur FRC | 6738 <46348717+ExcaliburFRC6738@users.noreply.github.com> Date: Fri, 8 Apr 2022 08:02:08 +0300 Subject: [PATCH 21/25] [commands] Add RepeatCommand (#4009) Co-authored-by: Starlight220 <53231611+Starlight220@users.noreply.github.com> --- .../wpi/first/wpilibj2/command/Command.java | 16 ++++ .../first/wpilibj2/command/RepeatCommand.java | 70 ++++++++++++++++ .../main/native/cpp/frc2/command/Command.cpp | 5 ++ .../native/cpp/frc2/command/RepeatCommand.cpp | 45 +++++++++++ .../native/include/frc2/command/Command.h | 9 +++ .../include/frc2/command/RepeatCommand.h | 81 +++++++++++++++++++ .../wpilibj2/command/RepeatCommandTest.java | 71 ++++++++++++++++ .../cpp/frc2/command/RepeatCommandTest.cpp | 62 ++++++++++++++ 8 files changed, 359 insertions(+) create mode 100644 wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java create mode 100644 wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp create mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h create mode 100644 wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java create mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java index 82c6bc49959..0420301eade 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java @@ -257,6 +257,22 @@ default PerpetualCommand perpetually() { return new PerpetualCommand(this); } + /** + * Decorates this command to run repeatedly, restarting it when it ends, until this command is + * interrupted. The decorated command can still be canceled. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with {@link + * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further + * decorated without issue. + * + * @return the decorated command + */ + default RepeatCommand repeat() { + return new RepeatCommand(this); + } + /** * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}. This * is useful for "forking off" from command groups when the user does not wish to extend the diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java new file mode 100644 index 00000000000..f228fa7fa13 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj2.command; + +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands; +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped; + +/** + * A command that runs another command repeatedly, restarting it when it ends, until this command is + * interrupted. While this class does not extend {@link CommandGroupBase}, it is still considered a + * CommandGroup, as it allows one to compose another command within it; the command instances that + * are passed to it cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + * + *

This class is provided by the NewCommands VendorDep + */ +public class RepeatCommand extends CommandBase { + protected final Command m_command; + + /** + * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it + * ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + public RepeatCommand(Command command) { + requireUngrouped(command); + registerGroupedCommands(command); + m_command = command; + m_requirements.addAll(command.getRequirements()); + } + + @Override + public void initialize() { + m_command.initialize(); + } + + @Override + public void execute() { + m_command.execute(); + if (m_command.isFinished()) { + // restart command + m_command.end(false); + m_command.initialize(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + m_command.end(interrupted); + } + + @Override + public boolean runsWhenDisabled() { + return m_command.runsWhenDisabled(); + } + + @Override + public RepeatCommand repeat() { + return this; + } +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index ea6276b50bf..93510513d00 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -11,6 +11,7 @@ #include "frc2/command/ParallelRaceGroup.h" #include "frc2/command/PerpetualCommand.h" #include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/RepeatCommand.h" #include "frc2/command/SequentialCommandGroup.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" @@ -87,6 +88,10 @@ PerpetualCommand Command::Perpetually() && { return PerpetualCommand(std::move(*this).TransferOwnership()); } +RepeatCommand Command::Repeat() && { + return RepeatCommand(std::move(*this).TransferOwnership()); +} + ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp new file mode 100644 index 00000000000..023ccb157f8 --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc2/command/RepeatCommand.h" + +using namespace frc2; + +RepeatCommand::RepeatCommand(std::unique_ptr&& command) { + if (!CommandGroupBase::RequireUngrouped(*command)) { + return; + } + m_command = std::move(command); + m_command->SetGrouped(true); + AddRequirements(m_command->GetRequirements()); +} + +void RepeatCommand::Initialize() { + m_command->Initialize(); +} + +void RepeatCommand::Execute() { + m_command->Execute(); + if (m_command->IsFinished()) { + // restart command + m_command->End(false); + m_command->Initialize(); + } +} + +bool RepeatCommand::IsFinished() { + return false; +} + +void RepeatCommand::End(bool interrupted) { + m_command->End(interrupted); +} + +bool RepeatCommand::RunsWhenDisabled() const { + return m_command->RunsWhenDisabled(); +} + +RepeatCommand RepeatCommand::Repeat() && { + return std::move(*this); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index f8da72815bd..bfbe5161818 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -29,6 +29,7 @@ class ParallelDeadlineGroup; class SequentialCommandGroup; class PerpetualCommand; class ProxyScheduleCommand; +class RepeatCommand; /** * A state machine representing a complete action to be performed by the robot. @@ -185,6 +186,14 @@ class Command { */ virtual PerpetualCommand Perpetually() &&; + /** + * Decorates this command to run repeatedly, restarting it when it ends, until + * this command is interrupted. The decorated command can still be canceled. + * + * @return the decorated command + */ + virtual RepeatCommand Repeat() &&; + /** * Decorates this command to run "by proxy" by wrapping it in a * ProxyScheduleCommand. This is useful for "forking off" from command groups diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h new file mode 100644 index 00000000000..1d9a1f55ccb --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h @@ -0,0 +1,81 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4521) +#endif + +#include +#include + +#include "frc2/command/CommandBase.h" +#include "frc2/command/CommandGroupBase.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that runs another command repeatedly, restarting it when it ends, + * until this command is interrupted. While this class does not extend {@link + * CommandGroupBase}, it is still considered a CommandGroup, as it allows one to + * compose another command within it; the command instances that are passed to + * it cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + * + *

This class is provided by the NewCommands VendorDep + */ +class RepeatCommand : public CommandHelper { + public: + /** + * Creates a new RepeatCommand. Will run another command repeatedly, + * restarting it whenever it ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + explicit RepeatCommand(std::unique_ptr&& command); + + /** + * Creates a new RepeatCommand. Will run another command repeatedly, + * restarting it whenever it ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + template >>> + explicit RepeatCommand(T&& command) + : RepeatCommand(std::make_unique>( + std::forward(command))) {} + + RepeatCommand(RepeatCommand&& other) = default; + + // No copy constructors for command groups + RepeatCommand(const RepeatCommand& other) = delete; + + // Prevent template expansion from emulating copy ctor + RepeatCommand(RepeatCommand&) = delete; + + void Initialize() override; + + void Execute() override; + + bool IsFinished() override; + + void End(bool interrupted) override; + + bool RunsWhenDisabled() const override; + + RepeatCommand Repeat() && override; + + private: + std::unique_ptr m_command; +}; +} // namespace frc2 + +#ifdef _WIN32 +#pragma warning(pop) +#endif diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java new file mode 100644 index 00000000000..7c4d4164302 --- /dev/null +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj2.command; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class RepeatCommandTest { + @Test + void callsMethodsCorrectly() { + HAL.initialize(500, 0); + // enable so that we don't need to mess with `runsWhenDisabled` for each command + DriverStationSim.setEnabled(true); + + var initCounter = new AtomicInteger(0); + var exeCounter = new AtomicInteger(0); + var isFinishedCounter = new AtomicInteger(0); + var endCounter = new AtomicInteger(0); + var isFinishedHook = new AtomicBoolean(false); + + final var command = + new RepeatCommand( + new FunctionalCommand( + initCounter::incrementAndGet, + exeCounter::incrementAndGet, + interrupted -> endCounter.incrementAndGet(), + () -> { + isFinishedCounter.incrementAndGet(); + return isFinishedHook.get(); + })); + + assertEquals(0, initCounter.get()); + assertEquals(0, exeCounter.get()); + assertEquals(0, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + CommandScheduler.getInstance().schedule(command); + assertEquals(1, initCounter.get()); + assertEquals(0, exeCounter.get()); + assertEquals(0, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + isFinishedHook.set(false); + CommandScheduler.getInstance().run(); + assertEquals(1, initCounter.get()); + assertEquals(1, exeCounter.get()); + assertEquals(1, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + isFinishedHook.set(true); + CommandScheduler.getInstance().run(); + assertEquals(2, initCounter.get()); + assertEquals(2, exeCounter.get()); + assertEquals(2, isFinishedCounter.get()); + assertEquals(1, endCounter.get()); + + isFinishedHook.set(false); + CommandScheduler.getInstance().run(); + assertEquals(2, initCounter.get()); + assertEquals(3, exeCounter.get()); + assertEquals(3, isFinishedCounter.get()); + assertEquals(1, endCounter.get()); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp new file mode 100644 index 00000000000..8d1f4845bd5 --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" +#include "frc2/command/RepeatCommand.h" + +using namespace frc2; +class RepeatCommandTest : public CommandTestBase {}; + +TEST_F(RepeatCommandTest, CallsMethodsCorrectly) { + CommandScheduler scheduler = GetScheduler(); + + int initCounter = 0; + int exeCounter = 0; + int isFinishedCounter = 0; + int endCounter = 0; + bool isFinishedHook = false; + + auto command = + FunctionalCommand([&initCounter] { initCounter++; }, + [&exeCounter] { exeCounter++; }, + [&endCounter](bool interrupted) { endCounter++; }, + [&isFinishedCounter, &isFinishedHook] { + isFinishedCounter++; + return isFinishedHook; + }) + .Repeat(); + + EXPECT_EQ(0, initCounter); + EXPECT_EQ(0, exeCounter); + EXPECT_EQ(0, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + scheduler.Schedule(&command); + EXPECT_EQ(1, initCounter); + EXPECT_EQ(0, exeCounter); + EXPECT_EQ(0, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + isFinishedHook = false; + scheduler.Run(); + EXPECT_EQ(1, initCounter); + EXPECT_EQ(1, exeCounter); + EXPECT_EQ(1, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + isFinishedHook = true; + scheduler.Run(); + EXPECT_EQ(2, initCounter); + EXPECT_EQ(2, exeCounter); + EXPECT_EQ(2, isFinishedCounter); + EXPECT_EQ(1, endCounter); + + isFinishedHook = false; + scheduler.Run(); + EXPECT_EQ(2, initCounter); + EXPECT_EQ(3, exeCounter); + EXPECT_EQ(3, isFinishedCounter); + EXPECT_EQ(1, endCounter); +} From f27a1f9bfb9d28ac629a50138cb49faddfad3fe4 Mon Sep 17 00:00:00 2001 From: ohowe <42757516+carelesshippo@users.noreply.github.com> Date: Fri, 8 Apr 2022 22:20:23 -0600 Subject: [PATCH 22/25] [commands] Fix JoystickButton.getAsBoolean (#4131) This previously always returned false; the get method it inherited was not used in the getAsBoolean defined in the Trigger class. The fix is to swap get() and getAsBoolean() implementations in the Trigger class. --- .../edu/wpi/first/wpilibj2/command/button/Trigger.java | 6 +++--- .../wpi/first/wpilibj2/command/button/ButtonTest.java | 9 +++++++++ 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index f36aeb2fcdc..d692f96f04a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -56,7 +56,7 @@ public Trigger() { * @return whether or not the trigger condition is active. */ public boolean get() { - return this.getAsBoolean(); + return m_isActive.getAsBoolean(); } /** @@ -69,8 +69,8 @@ public boolean get() { * @return whether or not the trigger condition is active. */ @Override - public boolean getAsBoolean() { - return m_isActive.getAsBoolean(); + public final boolean getAsBoolean() { + return this.get(); } /** diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java index 1a4f05d4501..81b12ef865f 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java @@ -195,4 +195,13 @@ void debounceTest() { scheduler.run(); verify(command).schedule(true); } + + @Test + void booleanSupplierTest() { + InternalButton button = new InternalButton(); + + assertFalse(button.getAsBoolean()); + button.setPressed(true); + assertTrue(button.getAsBoolean()); + } } From 5bf46a9093a1d14097b6d85261739ba5a5483fad Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 8 Apr 2022 21:20:53 -0700 Subject: [PATCH 23/25] [wpimath] Add ComputerVisionUtil (#4124) Closes #4108. --- .../wpi/first/math/ComputerVisionUtil.java | 138 ++++++++++++++++++ .../main/native/cpp/ComputerVisionUtil.cpp | 56 +++++++ .../native/include/frc/ComputerVisionUtil.h | 121 +++++++++++++++ .../first/math/ComputerVisionUtilTest.java | 96 ++++++++++++ .../native/cpp/ComputerVisionUtilTest.cpp | 81 ++++++++++ 5 files changed, 492 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java create mode 100644 wpimath/src/main/native/cpp/ComputerVisionUtil.cpp create mode 100644 wpimath/src/main/native/include/frc/ComputerVisionUtil.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java create mode 100644 wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java new file mode 100644 index 00000000000..6829f20a0bc --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java @@ -0,0 +1,138 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; + +public final class ComputerVisionUtil { + private ComputerVisionUtil() { + throw new AssertionError("utility class"); + } + + /** + * Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates + * range to a target using the target's elevation. This method can produce more stable results + * than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method + * requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and + * for there to exist a height differential between goal and camera. The larger this differential, + * the more accurate the distance estimate will be. + * + *

Units can be converted using the {@link edu.wpi.first.math.util.Units} class. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param targetHeightMeters The physical height of the target off the floor in meters. This + * should be the height of whatever is being targeted (i.e. if the targeting region is set to + * top, this should be the height of the top of the target). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @return The estimated distance to the target in meters. + */ + public static double calculateDistanceToTarget( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians) { + return (targetHeightMeters - cameraHeightMeters) + / Math.tan(cameraPitchRadians + targetPitchRadians); + } + + /** + * Estimate the position of the robot in the field. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param targetHeightMeters The physical height of the target off the floor in meters. This + * should be the height of whatever is being targeted (i.e. if the targeting region is set to + * top, this should be the height of the top of the target). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and + * Photon returns CW-positive. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param cameraToRobot The position of the robot relative to the camera. If the camera was + * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be + * Transform2d(3 inches, 0 inches, 0 degrees). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians, + Rotation2d targetYaw, + Rotation2d gyroAngle, + Pose2d fieldToTarget, + Transform2d cameraToRobot) { + return estimateFieldToRobot( + estimateCameraToTarget( + new Translation2d( + calculateDistanceToTarget( + cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + } + + /** + * Estimates the pose of the robot in the field coordinate system, given the position of the + * target relative to the camera, the target relative to the field, and the robot relative to the + * camera. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param cameraToRobot The position of the robot relative to the camera. If the camera was + * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be + * Transform2d(3 inches, 0 inches, 0 degrees). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) { + return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot); + } + + /** + * Estimates a {@link Transform2d} that maps the camera position to the target position, using the + * robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system + * -- that is, it should read zero degrees when pointed towards the opposing alliance station, and + * increase as the robot rotates CCW. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target + * relative to the camera. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @return A Transform2d that takes us from the camera to the target. + */ + public static Transform2d estimateCameraToTarget( + Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) { + // Map our camera at the origin out to our target, in the robot reference + // frame. Gyro angle is needed because there's a circle of possible camera + // poses for which the camera has the same yaw from camera to target. + return new Transform2d( + cameraToTargetTranslation, gyroAngle.unaryMinus().minus(fieldToTarget.getRotation())); + } + + /** + * Estimates the pose of the camera in the field coordinate system, given the position of the + * target relative to the camera, and the target relative to the field. This *only* tracks the + * position of the camera, not the position of the robot itself. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ + public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) { + var targetToCamera = cameraToTarget.inverse(); + return fieldToTarget.transformBy(targetToCamera); + } +} diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp new file mode 100644 index 00000000000..7279e199705 --- /dev/null +++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/ComputerVisionUtil.h" + +#include "units/math.h" + +namespace frc { + +units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, + units::meter_t targetHeight, + units::radian_t cameraPitch, + units::radian_t targetPitch) { + return (targetHeight - cameraHeight) / + units::math::tan(cameraPitch + targetPitch); +} + +frc::Pose2d EstimateFieldToRobot( + units::meter_t cameraHeight, units::meter_t targetHeight, + units::radian_t cameraPitch, units::radian_t targetPitch, + const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, + const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) { + return EstimateFieldToRobot( + EstimateCameraToTarget(frc::Translation2d{CalculateDistanceToTarget( + cameraHeight, targetHeight, + cameraPitch, targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); +} + +frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget, + const frc::Transform2d& cameraToRobot) { + return EstimateFieldToCamera(cameraToTarget, fieldToTarget) + .TransformBy(cameraToRobot); +} + +frc::Transform2d EstimateCameraToTarget( + const frc::Translation2d& cameraToTargetTranslation, + const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) { + // Map our camera at the origin out to our target, in the robot reference + // frame. Gyro angle is needed because there's a circle of possible camera + // poses for which the camera has the same yaw from camera to target. + return frc::Transform2d{cameraToTargetTranslation, + -gyroAngle - fieldToTarget.Rotation()}; +} + +frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget) { + auto targetToCamera = cameraToTarget.Inverse(); + return fieldToTarget.TransformBy(targetToCamera); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h new file mode 100644 index 00000000000..da73c06d479 --- /dev/null +++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "frc/geometry/Translation2d.h" +#include "units/angle.h" +#include "units/length.h" + +namespace frc { + +/** + * Algorithm from + * https://docs.limelightvision.io/en/latest/cs_estimating_distance.html + * Estimates range to a target using the target's elevation. This method can + * produce more stable results than SolvePNP when well tuned, if the full 6d + * robot pose is not required. + * + * @param cameraHeight The height of the camera off the floor. + * @param targetHeight The height of the target off the floor. + * @param cameraPitch The pitch of the camera from the horizontal plane. + * Positive values up. + * @param targetPitch The pitch of the target in the camera's lens. Positive + * values up. + * @return The estimated distance to the target. + */ +WPILIB_DLLEXPORT +units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, + units::meter_t targetHeight, + units::radian_t cameraPitch, + units::radian_t targetPitch); + +/** + * Estimate the position of the robot in the field. + * + * @param cameraHeight The physical height of the camera off the floor. + * @param targetHeight The physical height of the target off the floor. This + * should be the height of whatever is being targeted (i.e. + * if the targeting region is set to top, this should be the + * height of the top of the target). + * @param cameraPitch The pitch of the camera from the horizontal plane. + * Positive values up. + * @param targetPitch The pitch of the target in the camera's lens. Positive + * values up. + * @param targetYaw The observed yaw of the target. Note that this *must* be + * CCW-positive, and Photon returns CW-positive. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @param fieldToTarget A Pose2d representing the target position in the field + * coordinate system. + * @param cameraToRobot The position of the robot relative to the camera. If the + * camera was mounted 3 inches behind the "origin" (usually + * physical center) of the robot, this would be + * frc::Transform2d(3_in, 0_in, 0_deg). + * @return The position of the robot in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToRobot( + units::meter_t cameraHeight, units::meter_t targetHeight, + units::radian_t cameraPitch, units::radian_t targetPitch, + const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, + const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot); + +/** + * Estimates the pose of the robot in the field coordinate system, given the + * position of the target relative to the camera, the target relative to the + * field, and the robot relative to the camera. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param cameraToRobot The position of the robot relative to the camera. If + * the camera was mounted 3 inches behind the "origin" + * (usually physical center) of the robot, this would be + * frc::Transform2d(3_in, 0_in, 0_deg). + * @return The position of the robot in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget, + const frc::Transform2d& cameraToRobot); + +/** + * Estimates a Transform2d that maps the camera position to the target position, + * using the robot's gyro. Note that the gyro angle provided *must* line up with + * the field coordinate system -- that is, it should read zero degrees when + * pointed towards the opposing alliance station, and increase as the robot + * rotates CCW. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y + * position of the target relative to the + * camera. + * @param fieldToTarget A Pose2d representing the target position in the field + * coordinate system. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @return A Transform2d that takes us from the camera to the target. + */ +WPILIB_DLLEXPORT +frc::Transform2d EstimateCameraToTarget( + const frc::Translation2d& cameraToTargetTranslation, + const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle); + +/** + * Estimates the pose of the camera in the field coordinate system, given the + * position of the target relative to the camera, and the target relative to + * the field. This *only* tracks the position of the camera, not the position + * of the robot itself. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget); + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java new file mode 100644 index 00000000000..2e3c92a398f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +class ComputerVisionUtilTest { + @Test + void testCalculateDistanceToTarget() { + var camHeight = 1; + var targetHeight = 3; + var camPitch = Units.degreesToRadians(0); + var targetPitch = Units.degreesToRadians(30); + + var dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + + Assertions.assertEquals(3.464, dist, 0.01); + + camHeight = 1; + targetHeight = 2; + camPitch = Units.degreesToRadians(20); + targetPitch = Units.degreesToRadians(-10); + + dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + Assertions.assertEquals(5.671, dist, 0.01); + + camHeight = 3; + targetHeight = 1; + camPitch = Units.degreesToRadians(0); + targetPitch = Units.degreesToRadians(-30); + + dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + + Assertions.assertEquals(3.464, dist, 0.01); + } + + @Test + void testEstimateFieldToRobot() { + var camHeight = 1; + var targetHeight = 3; + var camPitch = 0; + var targetPitch = Units.degreesToRadians(30); + var targetYaw = new Rotation2d(); + var gyroAngle = new Rotation2d(); + var fieldToTarget = new Pose2d(); + var cameraToRobot = new Transform2d(); + + var fieldToRobot = + ComputerVisionUtil.estimateFieldToRobot( + ComputerVisionUtil.estimateCameraToTarget( + new Translation2d( + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + + Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getY(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1); + + gyroAngle = Rotation2d.fromDegrees(-30); + + fieldToRobot = + ComputerVisionUtil.estimateFieldToRobot( + ComputerVisionUtil.estimateCameraToTarget( + new Translation2d( + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + + Assertions.assertEquals(-3.0, fieldToRobot.getX(), 0.1); + Assertions.assertEquals(1.732, fieldToRobot.getY(), 0.1); + Assertions.assertEquals(-30.0, fieldToRobot.getRotation().getDegrees(), 0.1); + } +} diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp new file mode 100644 index 00000000000..6b84ce438bf --- /dev/null +++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp @@ -0,0 +1,81 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/ComputerVisionUtil.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "gtest/gtest.h" +#include "units/angle.h" +#include "units/length.h" + +TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) { + auto camHeight = 1_m; + auto targetHeight = 3_m; + auto camPitch = 0_deg; + auto targetPitch = 30_deg; + + auto dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + + EXPECT_NEAR(3.464, dist.value(), 0.01); + + camHeight = 1_m; + targetHeight = 2_m; + camPitch = 20_deg; + targetPitch = -10_deg; + + dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + EXPECT_NEAR(5.671, dist.value(), 0.01); + + camHeight = 3_m; + targetHeight = 1_m; + camPitch = 0_deg; + targetPitch = -30_deg; + + dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + + EXPECT_NEAR(3.464, dist.value(), 0.01); +} + +TEST(ComputerVisionUtilTest, EstimateFieldToRobot) { + auto camHeight = 1_m; + auto targetHeight = 3_m; + auto camPitch = 0_deg; + auto targetPitch = 30_deg; + frc::Rotation2d targetYaw; + frc::Rotation2d gyroAngle; + frc::Pose2d fieldToTarget; + frc::Transform2d cameraToRobot; + + auto fieldToRobot = frc::EstimateFieldToRobot( + frc::EstimateCameraToTarget( + frc::Translation2d{ + frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); + + EXPECT_NEAR(-3.464, fieldToRobot.X().value(), 0.1); + EXPECT_NEAR(0, fieldToRobot.Y().value(), 0.1); + EXPECT_NEAR(0, fieldToRobot.Rotation().Degrees().value(), 0.1); + + gyroAngle = -30_deg; + + fieldToRobot = frc::EstimateFieldToRobot( + frc::EstimateCameraToTarget( + frc::Translation2d{ + frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); + + EXPECT_NEAR(-3.0, fieldToRobot.X().value(), 0.1); + EXPECT_NEAR(1.732, fieldToRobot.Y().value(), 0.1); + EXPECT_NEAR(-30.0, fieldToRobot.Rotation().Degrees().value(), 0.1); +} From 975171609e1f800c2ca98dd7880ca5a88ae0e7ed Mon Sep 17 00:00:00 2001 From: Spud Date: Fri, 8 Apr 2022 21:31:08 -0700 Subject: [PATCH 24/25] [wpilib] Compressor: Rename enabled to isEnabled (#4147) This is a less confusing name, as enabled() can imply it enables the compressor. --- wpilibc/src/main/native/cpp/Compressor.cpp | 6 +++++- wpilibc/src/main/native/include/frc/Compressor.h | 15 ++++++++++++++- .../java/edu/wpi/first/wpilibj/Compressor.java | 16 ++++++++++++++-- 3 files changed, 33 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp index 3f7044759d9..24460863d3c 100644 --- a/wpilibc/src/main/native/cpp/Compressor.cpp +++ b/wpilibc/src/main/native/cpp/Compressor.cpp @@ -43,6 +43,10 @@ void Compressor::Stop() { } bool Compressor::Enabled() const { + return IsEnabled(); +} + +bool Compressor::IsEnabled() const { return m_module->GetCompressor(); } @@ -87,7 +91,7 @@ CompressorConfigType Compressor::GetConfigType() const { void Compressor::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Compressor"); builder.AddBooleanProperty( - "Enabled", [=] { return Enabled(); }, nullptr); + "Enabled", [=] { return IsEnabled(); }, nullptr); builder.AddBooleanProperty( "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr); } diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h index d8125ede448..a686b1c7975 100644 --- a/wpilibc/src/main/native/include/frc/Compressor.h +++ b/wpilibc/src/main/native/include/frc/Compressor.h @@ -78,11 +78,24 @@ class Compressor : public wpi::Sendable, /** * Check if compressor output is active. + * To (re)enable the compressor use EnableDigital() or EnableAnalog(...). * - * @return true if the compressor is on + * @return true if the compressor is on. + * @deprecated To avoid confusion in thinking this (re)enables the compressor + * use IsEnabled(). */ + WPI_DEPRECATED( + "To avoid confusion in thinking this (re)enables the compressor use " + "IsEnabled()") bool Enabled() const; + /** + * Returns whether the compressor is active or not. + * + * @return true if the compressor is on - otherwise false. + */ + bool IsEnabled() const; + /** * Check if the pressure switch is triggered. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java index a7d079b1466..374e0fea0e5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java @@ -91,11 +91,23 @@ public void stop() { } /** - * Get the status of the compressor. + * Get the status of the compressor. To (re)enable the compressor use enableDigital() or + * enableAnalog(...). * * @return true if the compressor is on + * @deprecated To avoid confusion in thinking this (re)enables the compressor use IsEnabled(). */ + @Deprecated(since = "2023", forRemoval = true) public boolean enabled() { + return isEnabled(); + } + + /** + * Returns whether the compressor is active or not. + * + * @return true if the compressor is on - otherwise false. + */ + public boolean isEnabled() { return m_module.getCompressor(); } @@ -184,7 +196,7 @@ public CompressorConfigType getConfigType() { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Compressor"); - builder.addBooleanProperty("Enabled", this::enabled, null); + builder.addBooleanProperty("Enabled", this::isEnabled, null); builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null); } } From aef4b16d4cc671cee3a1fe77d83f9a7f9313f4e6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 8 Apr 2022 21:31:42 -0700 Subject: [PATCH 25/25] [wpimath] Remove unnecessary NOLINT in LinearPlantInversionFeedforward (NFC) (#4155) --- .../include/frc/controller/LinearPlantInversionFeedforward.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h index 780d2e1d727..eccc4833270 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h @@ -123,7 +123,7 @@ class LinearPlantInversionFeedforward { */ Eigen::Vector Calculate( const Eigen::Vector& nextR) { - return Calculate(m_r, nextR); // NOLINT + return Calculate(m_r, nextR); } /**