Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2026-02-15 00:51:21 -08:00
parent 36c489247c 77dfad97c6
commit af865f8020
98 changed files with 3031 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp
View File

@@ -135,6 +135,7 @@ class LinearQuadraticRegulator {
}
Reset();
wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", "");
}
/**
@@ -194,6 +195,7 @@ class LinearQuadraticRegulator {
}
Reset();
wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", "");
}
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;

2
wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp
View File

@@ -138,6 +138,7 @@ class ExtendedKalmanFilter {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", "");
}
/**
@@ -222,6 +223,7 @@ class ExtendedKalmanFilter {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", "");
}
/**

1
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp
View File

@@ -117,6 +117,7 @@ class KalmanFilter {
}
Reset();
wpi::math::MathSharedStore::ReportUsage("KalmanFilter", "");
}
/**

73
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -74,6 +74,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
wpi::math::MathSharedStore::ReportUsage("PoseEstimator", "");
}
/**
@@ -88,6 +89,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
void SetVisionMeasurementStdDevs(
const wpi::util::array<double, 3>& visionMeasurementStdDevs) {
// Diagonal of measurement covariance matrix R
wpi::util::array<double, 3> r{wpi::util::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -97,9 +99,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
m_vision_K.diagonal()[row] = 0.0;
} else {
m_visionK(row, row) =
m_vision_K.diagonal()[row] =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
@@ -141,24 +143,70 @@ class WPILIB_DLLEXPORT PoseEstimator {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
const VisionUpdate visionUpdate{
Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose2d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose translation
const VisionUpdate visionUpdate{
Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Gets the estimated robot pose.
@@ -274,9 +322,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
Eigen::Vector3d k_times_transform =
m_visionK * Eigen::Vector3d{transform.X().value(),
transform.Y().value(),
transform.Rotation().Radians().value()};
m_vision_K * Eigen::Vector3d{transform.X().value(),
transform.Y().value(),
transform.Rotation().Radians().value()};
// Step 6: Convert back to Transform2d.
Transform2d scaledTransform{
@@ -431,14 +479,21 @@ class WPILIB_DLLEXPORT PoseEstimator {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 3> m_q{wpi::util::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
// Kalman gain matrix K
Eigen::DiagonalMatrix<double, 3> m_vision_K =
Eigen::DiagonalMatrix<double, 3>::Zero();
// Maps timestamps to odometry-only pose estimates
TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
Pose2d m_poseEstimate;

85
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -79,6 +79,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
wpi::math::MathSharedStore::ReportUsage("PoseEstimator3d", "");
}
/**
@@ -93,6 +94,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*/
void SetVisionMeasurementStdDevs(
const wpi::util::array<double, 4>& visionMeasurementStdDevs) {
// Diagonal of measurement noise covariance matrix R
wpi::util::array<double, 4> r{wpi::util::empty_array};
for (size_t i = 0; i < 4; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -102,15 +104,15 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 4; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
m_vision_K.diagonal()[row] = 0.0;
} else {
m_visionK(row, row) =
m_vision_K.diagonal()[row] =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
double angle_gain = m_visionK(3, 3);
m_visionK(4, 4) = angle_gain;
m_visionK(5, 5) = angle_gain;
double angle_gain = m_vision_K.diagonal()[3];
m_vision_K.diagonal()[4] = angle_gain;
m_vision_K.diagonal()[5] = angle_gain;
}
/**
@@ -149,24 +151,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation3d& translation) {
m_odometry.ResetTranslation(translation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
const VisionUpdate visionUpdate{
Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose3d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation3d& rotation) {
m_odometry.ResetRotation(rotation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose translation
const VisionUpdate visionUpdate{
Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Gets the estimated robot pose.
@@ -282,12 +330,12 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
wpi::math::Vectord<6> k_times_transform =
m_visionK * wpi::math::Vectord<6>{transform.X().value(),
transform.Y().value(),
transform.Z().value(),
transform.Rotation().X().value(),
transform.Rotation().Y().value(),
transform.Rotation().Z().value()};
m_vision_K * wpi::math::Vectord<6>{transform.X().value(),
transform.Y().value(),
transform.Z().value(),
transform.Rotation().X().value(),
transform.Rotation().Y().value(),
transform.Rotation().Z().value()};
// Step 6: Convert back to Transform3d.
Transform3d scaledTransform{
@@ -445,14 +493,21 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 4> m_q{wpi::util::empty_array};
wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero();
// Kalman gain matrix K
Eigen::DiagonalMatrix<double, 6> m_vision_K =
Eigen::DiagonalMatrix<double, 6>::Zero();
// Maps timestamps to odometry-only pose estimates
TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
Pose3d m_poseEstimate;

1
wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp
View File

@@ -148,6 +148,7 @@ class SteadyStateKalmanFilter {
}
Reset();
wpi::math::MathSharedStore::ReportUsage("KalmanFilter", "");
}
SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;

3
wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
View File

@@ -15,6 +15,7 @@
#include "wpi/math/system/Discretization.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/array.hpp"
@@ -114,6 +115,7 @@ class UnscentedKalmanFilter {
m_dt = dt;
Reset();
wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", "");
}
/**
@@ -173,6 +175,7 @@ class UnscentedKalmanFilter {
m_dt = dt;
Reset();
wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", "");
}
/**

94
wpimath/src/main/native/include/wpi/math/filter/EdgeCounterFilter.hpp Normal file
View File

@@ -0,0 +1,94 @@
// 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 "wpi/units/time.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* A rising edge counter for boolean streams. Requires that the boolean change
* value to true for a specified number of times within a specified time window
* after the first rising edge before the filtered value changes.
*
* The filter activates when the input has risen (transitioned from false to
* true) the required number of times within the time window. Once activated,
* the output remains true as long as the input is true. The edge count resets
* when the time window expires or when the input goes false after activation.
*
* Input must be stable; consider using a Debouncer before this filter to avoid
* counting noise as multiple edges.
*/
class WPILIB_DLLEXPORT EdgeCounterFilter {
public:
/**
* Creates a new EdgeCounterFilter.
*
* @param requiredEdges The number of rising edges required before the output
* goes true.
* @param windowTime The maximum time window in which all required edges must
* occur after the first rising edge.
*/
explicit EdgeCounterFilter(int requiredEdges,
wpi::units::second_t windowTime);
/**
* Applies the edge counter filter to the input stream.
*
* @param input The current value of the input stream.
* @return True if the required number of edges have occurred within the time
* window and the input is currently true; false otherwise.
*/
bool Calculate(bool input);
/**
* Sets the time window duration.
*
* @param windowTime The maximum time window in which all required edges must
* occur after the first rising edge.
*/
constexpr void SetWindowTime(wpi::units::second_t windowTime) {
m_windowTime = windowTime;
}
/**
* Gets the time window duration.
*
* @return The maximum time window in which all required edges must occur
* after the first rising edge.
*/
constexpr wpi::units::second_t GetWindowTime() const { return m_windowTime; }
/**
* Sets the required number of edges.
*
* @param requiredEdges The number of rising edges required before the output
* goes true.
*/
constexpr void SetRequiredEdges(int requiredEdges) {
m_requiredEdges = requiredEdges;
}
/**
* Gets the required number of edges.
*
* @return The number of rising edges required before the output goes true.
*/
constexpr int GetRequiredEdges() const { return m_requiredEdges; }
private:
int m_requiredEdges;
wpi::units::second_t m_windowTime;
wpi::units::second_t m_firstEdgeTime;
int m_currentCount = 0;
bool m_lastInput = false;
void ResetTimer();
bool HasElapsed() const;
};
} // namespace wpi::math

26
wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View File

@@ -85,7 +85,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
constexpr static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return translation.RotateBy(from.m_rotation - to.m_rotation);
// Convert to NWU, then convert to the new coordinate system
return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
}
/**
@@ -99,7 +100,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
constexpr static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return rotation.RotateBy(from.m_rotation - to.m_rotation);
// Convert to NWU, then convert to the new coordinate system
return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
}
/**
@@ -128,14 +130,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
constexpr static Transform3d Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
const auto coordRot = from.m_rotation - to.m_rotation;
// coordRot is the rotation that converts between the coordinate systems
// when applied extrinsically. It first converts to NWU, then converts to
// the new coordinate system.
const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation);
// The new rotation is the extrinsic rotation from convert(zero) to
// convert(transformRot). That is, applying convertedRot extrinsically to
// convert(zero) produces convert(transformRot). In the below snippet, we
// use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
//
// convertedRot convert(zero) = convert(transformRot)
// convertedRot = convert(transformRot) convert(zero)⁻¹
// = (coordRot transformRot) (coordRot zero)⁻¹
// = (coordRot transformRot) coordRot⁻¹
//
// In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the
// change in order), and the equivalent for rot⁻¹ is -rot.
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
}
private:
// Rotation from this coordinate system to NWU coordinate system
// Rotation from this coordinate system to NWU coordinate system when applied
// extrinsically
Rotation3d m_rotation;
};

4
wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp
View File

@@ -286,8 +286,8 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
constexpr Pose2d Pose2d::TransformBy(
const wpi::math::Transform2d& other) const {
return {m_translation + other.Translation().RotateBy(m_rotation),
other.Rotation() + m_rotation};
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation().RotateBy(m_rotation)};
}
constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {

7
wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View File

@@ -312,8 +312,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
}
constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
return {m_translation + other.Translation().RotateBy(m_rotation),
other.Rotation() + m_rotation};
// Rotating the transform's rotation by the pose's rotation extrinsically is
// equivalent to rotating the pose's rotation by the transform's rotation
// intrinsically. (We define transforms as being applied intrinsically.)
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation().RotateBy(m_rotation)};
}
constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {

13
wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View File

@@ -190,6 +190,19 @@ class WPILIB_DLLEXPORT Rotation2d {
Cos() * other.Sin() + Sin() * other.Cos()};
}
/**
* Returns the current rotation relative to the given rotation.
*
* @param other The rotation describing the orientation of the new coordinate
* frame that the current rotation will be converted into.
*
* @return The current rotation relative to the new orientation of the
* coordinate frame.
*/
constexpr Rotation2d RelativeTo(const Rotation2d& other) const {
return RotateBy(-other);
}
/**
* Returns matrix representation of this rotation.
*/

100
wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp
View File

@@ -15,13 +15,56 @@
#include "wpi/math/linalg/ct_matrix.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/math.hpp"
#include "wpi/util/MathExtras.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace wpi::math {
/**
* A rotation in a 3D coordinate frame represented by a quaternion.
* A rotation in a 3D coordinate frame, represented by a quaternion. Note that
* unlike 2D rotations, 3D rotations are not always commutative, so changing the
* order of rotations changes the result.
*
* As an example of the order of rotations mattering, suppose we have a card
* that looks like this:
*
* <pre>
*   ┌───┐ ┌───┐
* │ X │ │ x │
* Front: │ | │ Back: │ · │
* │ | │ │ · │
* └───┘ └───┘
* </pre>
*
* If we rotate 90º clockwise around the axis into the page, then flip around
* the left/right axis, we get one result:
*
* <pre>
*  ┌───┐
* │ X │ ┌───────┐ ┌───────┐
* │ | │ → │------X│ → │······x│
* │ | │ └───────┘ └───────┘
* └───┘
* </pre>
*
* If we flip around the left/right axis, then rotate 90º clockwise around the
* axis into the page, we get a different result:
*
* <pre>
*   ┌───┐ ┌───┐
* │ X │ │ · │ ┌───────┐
* │ | │ → │ · │ → │x······│
* │ | │ │ x │ └───────┘
* └───┘ └───┘
* </pre>
*
* Because order matters for 3D rotations, we need to distinguish between
* <em>extrinsic</em> rotations and <em>intrinsic</em> rotations. Rotating
* extrinsically means rotating around the global axes, while rotating
* intrinsically means rotating from the perspective of the other rotation. A
* neat property is that applying a series of rotations extrinsically is the
* same as applying the same series in the opposite order intrinsically.
*/
class WPILIB_DLLEXPORT Rotation3d {
public:
@@ -240,9 +283,18 @@ class WPILIB_DLLEXPORT Rotation3d {
: Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
/**
* Adds two rotations together.
* Adds two rotations together. The other rotation is applied extrinsically to
* this rotation, which is equivalent to this rotation being applied
* intrinsically to the other rotation. See the class comment for definitions
* of extrinsic and intrinsic rotations.
*
* @param other The rotation to add.
* Note that `a - b + b` always equals `a`, but `b + (a - b)`
* sometimes doesn't. To apply a rotation offset, use either `offset =
* -measurement + actual; newAngle = angle + offset;` or `offset = actual -
* measurement; newAngle = offset + angle;`, depending on how the corrected
* angle needs to change as the input angle changes.
*
* @param other The rotation to add (applied extrinsically).
*
* @return The sum of the two rotations.
*/
@@ -252,11 +304,20 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
* rotation. The new rotation is from the perspective of the other rotation
* (like Pose3d::operator-), so it needs to be applied intrinsically. See the
* class comment for definitions of extrinsic and intrinsic rotations.
*
* Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes
* doesn't. To apply a rotation offset, use either `offset = -measurement +
* actual; newAngle = angle + offset;` or `offset = actual - measurement;
* newAngle = offset + angle;`, depending on how the corrected angle needs to
* change as the input angle changes.
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
* @return The difference between the two rotations, from the perspective of
* the other rotation.
*/
constexpr Rotation3d operator-(const Rotation3d& other) const {
return *this + -other;
@@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d {
return Rotation3d{other.m_q * m_q};
}
/**
* Returns the current rotation relative to the given rotation. Because the
* result is in the perspective of the given rotation, it must be applied
* intrinsically. See the class comment for definitions of extrinsic and
* intrinsic rotations.
*
* @param other The rotation describing the orientation of the new coordinate
* frame that the current rotation will be converted into.
*
* @return The current rotation relative to the new orientation of the
* coordinate frame.
*/
constexpr Rotation3d RelativeTo(const Rotation3d& other) const {
// To apply a quaternion intrinsically, we must right-multiply by that
// quaternion. Therefore, "this_q relative to other_q" is the q such that
// other_q q = this_q:
//
// other_q q = this_q
// q = other_q⁻¹ this_q
return Rotation3d{other.m_q.Inverse() * m_q};
}
/**
* Returns the quaternion representation of the Rotation3d.
*/
@@ -450,5 +533,12 @@ void from_json(const wpi::util::json& json, Rotation3d& rotation);
} // namespace wpi::math
template <>
constexpr wpi::math::Rotation3d wpi::util::Lerp(
const wpi::math::Rotation3d& startValue,
const wpi::math::Rotation3d& endValue, double t) {
return (endValue - startValue) * t + startValue;
}
#include "wpi/math/geometry/proto/Rotation3dProto.hpp"
#include "wpi/math/geometry/struct/Rotation3dStruct.hpp"

7
wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp
View File

@@ -177,13 +177,12 @@ class WPILIB_DLLEXPORT Transform2d {
namespace wpi::math {
constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
// To transform the global translation delta to be relative to the initial
// pose, rotate by the inverse of the initial pose's orientation.
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
m_rotation = final.Rotation().RelativeTo(initial.Rotation());
}
constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {

11
wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp
View File

@@ -16,7 +16,9 @@ class Pose3d;
struct Twist3d;
/**
* Represents a transformation for a Pose3d in the pose's frame.
* Represents a transformation for a Pose3d in the pose's frame. Translation is
* applied before rotation. (The translation is applied in the pose's original
* frame, not the transformed frame.)
*/
class WPILIB_DLLEXPORT Transform3d {
public:
@@ -202,13 +204,12 @@ class WPILIB_DLLEXPORT Transform3d {
namespace wpi::math {
constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
// To transform the global translation delta to be relative to the initial
// pose, rotate by the inverse of the initial pose's orientation.
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
m_rotation = final.Rotation().RelativeTo(initial.Rotation());
}
constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {

21
wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp
View File

@@ -11,6 +11,8 @@
#include <vector>
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/MathExtras.hpp"
@@ -155,7 +157,8 @@ class TimeInterpolatableBuffer {
std::function<T(const T&, const T&, double)> m_interpolatingFunc;
};
// Template specialization to ensure that Pose2d uses pose exponential
// Template specializations to ensure that Pose2d and Pose3d use pose
// exponential
template <>
inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
wpi::units::second_t historySize)
@@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
}
}) {}
template <>
inline TimeInterpolatableBuffer<Pose3d>::TimeInterpolatableBuffer(
wpi::units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) {
if (t < 0) {
return start;
} else if (t >= 1) {
return end;
} else {
Twist3d twist = (end - start).Log();
Twist3d scaledTwist = twist * t;
return start + scaledTwist.Exp();
}
}) {}
} // namespace wpi::math

14
wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp
View File

@@ -47,7 +47,7 @@ class WPILIB_DLLEXPORT Odometry {
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
}
/**
@@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT Odometry {
const WheelPositions& wheelPositions, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_previousWheelPositions = wheelPositions;
}
@@ -74,7 +74,8 @@ class WPILIB_DLLEXPORT Odometry {
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose) {
m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
m_gyroOffset =
m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}
@@ -94,7 +95,7 @@ class WPILIB_DLLEXPORT Odometry {
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation) {
m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
m_pose = Pose2d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}
@@ -118,7 +119,7 @@ class WPILIB_DLLEXPORT Odometry {
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto angle = gyroAngle.RotateBy(m_gyroOffset);
auto twist =
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
@@ -139,7 +140,10 @@ class WPILIB_DLLEXPORT Odometry {
Pose2d m_pose;
WheelPositions m_previousWheelPositions;
// Always equal to m_pose.Rotation()
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};

24
wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp
View File

@@ -47,7 +47,9 @@ class WPILIB_DLLEXPORT Odometry3d {
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
}
/**
@@ -64,7 +66,9 @@ class WPILIB_DLLEXPORT Odometry3d {
const WheelPositions& wheelPositions, const Pose3d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_previousWheelPositions = wheelPositions;
}
@@ -74,7 +78,9 @@ class WPILIB_DLLEXPORT Odometry3d {
* @param pose The pose to reset to.
*/
void ResetPose(const Pose3d& pose) {
m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset =
m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}
@@ -94,7 +100,8 @@ class WPILIB_DLLEXPORT Odometry3d {
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation3d& rotation) {
m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
m_pose = Pose3d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}
@@ -118,7 +125,7 @@ class WPILIB_DLLEXPORT Odometry3d {
*/
const Pose3d& Update(const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto angle = gyroAngle.RotateBy(m_gyroOffset);
auto angle_difference = (angle - m_previousAngle).ToVector();
auto twist2d =
@@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d {
Pose3d m_pose;
WheelPositions m_previousWheelPositions;
// Always equal to m_pose.Rotation()
Rotation3d m_previousAngle;
// Applying a rotation intrinsically to the measured gyro angle should cause
// the corrected angle to be rotated intrinsically in the same way, so the
// measured gyro angle must be applied intrinsically. This is equivalent to
// applying the offset extrinsically to the measured gyro angle.
Rotation3d m_gyroOffset;
};

1
wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp
View File

@@ -131,6 +131,7 @@ class LinearSystemLoop {
m_clampFunc(clampFunction) {
m_nextR.setZero();
Reset(m_nextR);
wpi::math::MathSharedStore::ReportUsage("LinearSystemLoop", "");
}
LinearSystemLoop(LinearSystemLoop&&) = default;