Merge branch 'wpilibsuite:main' into main

wpilibsuite · Oct 22, 2024 · 5092322 · 5092322
2 parents b8b0bcf + 0c824bd
commit 5092322
Show file tree

Hide file tree

Showing 27 changed files with 5,728 additions and 11 deletions.
diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py
@@ -54,6 +54,7 @@ def eigen_inclusions(dp, f):
         "Cholesky",
         "Core",
         "Eigenvalues",
+        "Geometry",
         "Householder",
         "IterativeLinearSolvers",
         "Jacobi",

diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -24,4 +24,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     : PoseEstimator<DifferentialDriveWheelSpeeds,
                     DifferentialDriveWheelPositions>(
           kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
-      m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
+      m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
+  ResetPose(m_odometryImpl.GetPose());
+}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const wpi::array<double, 3>& visionMeasurementStdDevs)
     : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
           kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
-      m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
+      m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
+  ResetPose(m_odometryImpl.GetPose());
+}
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -45,6 +45,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
   /**
    * Constructs a PoseEstimator.
    *
+   * @warning The initial pose estimate will always be the default pose,
+   * regardless of the odometry's current pose.
+   *
    * @param kinematics A correctly-configured kinematics object for your
    *     drivetrain.
    * @param odometry A correctly-configured odometry object for your drivetrain.
@@ -60,7 +63,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
                 Odometry<WheelSpeeds, WheelPositions>& odometry,
                 const wpi::array<double, 3>& stateStdDevs,
                 const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
+      : m_odometry(odometry) {
     for (size_t i = 0; i < 3; ++i) {
       m_q[i] = stateStdDevs[i] * stateStdDevs[i];
     }

diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -85,7 +85,9 @@ class SwerveDrivePoseEstimator
       : PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
                       wpi::array<SwerveModulePosition, NumModules>>(
             kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
-        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
+    this->ResetPose(initialPose);
+  }
 
  private:
   SwerveDriveOdometry<NumModules> m_odometryImpl;

diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -187,7 +187,7 @@ class LinearFilter {
    */
   template <int Derivative, int Samples>
   static LinearFilter<T> FiniteDifference(
-      const wpi::array<int, Samples> stencil, units::second_t period) {
+      const wpi::array<int, Samples>& stencil, units::second_t period) {
     // See
     // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
     //

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
@@ -14,7 +14,7 @@
 #include "Jacobi"
 #include "Householder"
 #include "LU"
-// #include "Geometry"
+#include "Geometry"
 
 #include "src/Core/util/DisableStupidWarnings.h"
 

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry
@@ -0,0 +1,61 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_GEOMETRY_MODULE_H
+#define EIGEN_GEOMETRY_MODULE_H
+
+#include "Core"
+
+#include "SVD"
+#include "LU"
+#include <limits>
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup Geometry_Module Geometry module
+ *
+ * This module provides support for:
+ *  - fixed-size homogeneous transformations
+ *  - translation, scaling, 2D and 3D rotations
+ *  - \link Quaternion quaternions \endlink
+ *  - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3)
+ *  - orthogonal vector generation (\ref MatrixBase::unitOrthogonal)
+ *  - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes
+ * \endlink
+ *  - \link AlignedBox axis aligned bounding boxes \endlink
+ *  - \link umeyama least-square transformation fitting \endlink
+ *
+ * \code
+ * #include <Eigen/Geometry>
+ * \endcode
+ */
+
+// IWYU pragma: begin_exports
+#include "src/Geometry/OrthoMethods.h"
+#include "src/Geometry/EulerAngles.h"
+#include "src/Geometry/Homogeneous.h"
+#include "src/Geometry/RotationBase.h"
+#include "src/Geometry/Rotation2D.h"
+#include "src/Geometry/Quaternion.h"
+#include "src/Geometry/AngleAxis.h"
+#include "src/Geometry/Transform.h"
+#include "src/Geometry/Translation.h"
+#include "src/Geometry/Scaling.h"
+#include "src/Geometry/Hyperplane.h"
+#include "src/Geometry/ParametrizedLine.h"
+#include "src/Geometry/AlignedBox.h"
+#include "src/Geometry/Umeyama.h"
+
+// Use the SSE optimized version whenever possible.
+#if (defined EIGEN_VECTORIZE_SSE) || (defined EIGEN_VECTORIZE_NEON)
+#include "src/Geometry/arch/Geometry_SIMD.h"
+#endif
+// IWYU pragma: end_exports
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif  // EIGEN_GEOMETRY_MODULE_H