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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-08-03 11:51:25 -07:00
parent 23f5725853 2d11946d98
commit ed7982563b
50 changed files with 1390 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -128,7 +128,11 @@ Eigen::Matrix<double, States, States> DARE(
//
// V₂ᵀ = W.solve(Gₖᵀ)
// V₂ = W.solve(Gₖᵀ)ᵀ
StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
//
// Since W, Gₖ, and Hₖ are symmetric, drop the transposes on Gₖ and V₂.
//
// V₂ = W.solve(Gₖ)
StateMatrix V_2 = W_solver.solve(G_k);
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ

7
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -397,8 +397,11 @@ class ExtendedKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Rows> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));

7
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -235,8 +235,11 @@ class KalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Outputs> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));

6
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -114,7 +114,11 @@ class SteadyStateKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.value().transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
m_K = S.ldlt().solve(C * P.value()).transpose();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =

55
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -13,6 +13,7 @@
#include <wpi/json_fwd.h>
#include "frc/geometry/Rotation2d.h"
#include "units/area.h"
#include "units/length.h"
#include "units/math.h"
@@ -75,6 +76,23 @@ class WPILIB_DLLEXPORT Translation2d {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Calculates the square of the distance between two translations in 2D space.
* This is equivalent to squaring the result of Distance(Translation2d), but
* avoids computing a square root.
*
* The square of the distance between translations is defined as
* (x₂−x₁)²+(y₂−y₁)².
*
* @param other The translation to compute the squared distance to.
* @return The square of the distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
const Translation2d& other) const {
return units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y);
}
/**
* Returns the X component of the translation.
*
@@ -105,6 +123,17 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); }
/**
* Returns the squared norm, or squared distance from the origin to the
* translation. This is equivalent to squaring the result of Norm(), but
* avoids computing a square root.
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
return units::math::pow<2>(m_x) + units::math::pow<2>(m_y);
}
/**
* Returns the angle this translation forms with the positive X axis.
*
@@ -157,6 +186,32 @@ class WPILIB_DLLEXPORT Translation2d {
other.Y()};
}
/**
* Computes the dot product between this translation and another translation
* in 2D space.
*
* The dot product between two translations is defined as x₁x₂+y₁y₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation2d& other) const {
return m_x * other.X() + m_y * other.Y();
}
/**
* Computes the cross product between this translation and another translation
* in 2D space.
*
* The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr units::square_meter_t Cross(const Translation2d& other) const {
return m_x * other.Y() - m_y * other.X();
}
/**
* Returns the sum of two translations in 2D space.
*

62
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -14,6 +14,7 @@
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation2d.h"
#include "units/area.h"
#include "units/length.h"
#include "units/math.h"
@@ -96,6 +97,24 @@ class WPILIB_DLLEXPORT Translation3d {
units::math::pow<2>(other.m_z - m_z));
}
/**
* Calculates the squared distance between two translations in 3D space. This
* is equivalent to squaring the result of Distance(Translation3d), but avoids
* computing a square root.
*
* The squared distance between translations is defined as
* (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
*
* @param other The translation to compute the squared distance to.
* @return The squared distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
const Translation3d& other) const {
return units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z);
}
/**
* Returns the X component of the translation.
*
@@ -135,6 +154,17 @@ class WPILIB_DLLEXPORT Translation3d {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Returns the squared norm, or squared distance from the origin to the
* translation. This is equivalent to squaring the result of Norm(), but
* avoids computing a square root.
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
return m_x * m_x + m_y * m_y + m_z * m_z;
}
/**
* Applies a rotation to the translation in 3D space.
*
@@ -164,6 +194,38 @@ class WPILIB_DLLEXPORT Translation3d {
return (*this - other).RotateBy(rot) + other;
}
/**
* Computes the dot product between this translation and another translation
* in 3D space.
*
* The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation3d& other) const {
return m_x * other.X() + m_y * other.Y() + m_z * other.Z();
}
/**
* Computes the cross product between this translation and another
* translation in 3D space. The resulting translation will be perpendicular to
* both translations.
*
* The 3D cross product between two translations is defined as <y₁z₂-y₂z₁,
* z₁x₂-z₂x₁, x₁y₂-x₂y₁>.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
const Translation3d& other) const {
return Eigen::Vector<units::square_meter_t, 3>{
{m_y * other.Z() - other.Y() * m_z},
{m_z * other.X() - other.Z() * m_x},
{m_x * other.Y() - other.X() * m_y}};
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.

13
wpimath/src/main/native/include/units/math.h
View File

@@ -482,11 +482,10 @@ constexpr dimensionless::scalar_t log2(const ScalarUnit x) noexcept {
template <
class UnitType,
std::enable_if_t<units::traits::has_linear_scale<UnitType>::value, int> = 0>
inline constexpr auto sqrt(const UnitType& value) noexcept
-> unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
linear_scale> {
inline constexpr auto sqrt(const UnitType& value) noexcept -> unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
linear_scale> {
return unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
@@ -742,8 +741,8 @@ template <class UnitTypeLhs, class UnitMultiply, class UnitAdd,
class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
traits::is_unit_t<UnitMultiply>::value &&
traits::is_unit_t<UnitAdd>::value>>
auto fma(const UnitTypeLhs x, const UnitMultiply y,
const UnitAdd z) noexcept -> decltype(x * y) {
auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
-> decltype(x * y) {
using resultType = decltype(x * y);
static_assert(
traits::is_convertible_unit_t<

7
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Dense vendored Normal file
View File

@@ -0,0 +1,7 @@
#include "Core"
#include "LU"
#include "Cholesky"
#include "QR"
#include "SVD"
#include "Geometry"
#include "Eigenvalues"

30
wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector vendored Normal file
View File

@@ -0,0 +1,30 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// 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_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#include "Core"
#include <vector>
#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && \
(EIGEN_MAX_STATIC_ALIGN_BYTES <= 16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
#else
// IWYU pragma: begin_exports
#include "src/StlSupport/StdVector.h"
// IWYU pragma: end_exports
#endif
#endif // EIGEN_STDVECTOR_MODULE_H

51
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h vendored Normal file
View File

@@ -0,0 +1,51 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// 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_STDVECTOR_H
#define EIGEN_STDVECTOR_H
#ifndef EIGEN_STDVECTOR_MODULE_H
#error "Please include Eigen/StdVector instead of including this file directly."
#endif
#include "details.h"
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::vector such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) \
namespace std { \
template <> \
class vector<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > { \
typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > vector_base; \
\
public: \
typedef __VA_ARGS__ value_type; \
typedef vector_base::allocator_type allocator_type; \
typedef vector_base::size_type size_type; \
typedef vector_base::iterator iterator; \
explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
template <typename InputIterator> \
vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
: vector_base(first, last, a) {} \
vector(const vector& c) : vector_base(c) {} \
explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
vector(iterator start_, iterator end_) : vector_base(start_, end_) {} \
vector& operator=(const vector& x) { \
vector_base::operator=(x); \
return *this; \
} \
}; \
}
#endif // EIGEN_STDVECTOR_H

82
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h vendored Normal file
View File

@@ -0,0 +1,82 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// 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_STL_DETAILS_H
#define EIGEN_STL_DETAILS_H
#ifndef EIGEN_ALIGNED_ALLOCATOR
#define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
#endif
namespace Eigen {
// This one is needed to prevent reimplementing the whole std::vector.
template <class T>
class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR<T> {
public:
typedef std::size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
template <class U>
struct rebind {
typedef aligned_allocator_indirection<U> other;
};
aligned_allocator_indirection() {}
aligned_allocator_indirection(const aligned_allocator_indirection&) : EIGEN_ALIGNED_ALLOCATOR<T>() {}
aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<T>&) {}
template <class U>
aligned_allocator_indirection(const aligned_allocator_indirection<U>&) {}
template <class U>
aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<U>&) {}
~aligned_allocator_indirection() {}
};
#if EIGEN_COMP_MSVC
// sometimes, MSVC detects, at compile time, that the argument x
// in std::vector::resize(size_t s,T x) won't be aligned and generate an error
// even if this function is never called. Whence this little wrapper.
#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \
std::conditional_t<Eigen::internal::is_arithmetic<T>::value, T, Eigen::internal::workaround_msvc_stl_support<T> >
namespace internal {
template <typename T>
struct workaround_msvc_stl_support : public T {
inline workaround_msvc_stl_support() : T() {}
inline workaround_msvc_stl_support(const T& other) : T(other) {}
inline operator T&() { return *static_cast<T*>(this); }
inline operator const T&() const { return *static_cast<const T*>(this); }
template <typename OtherT>
inline T& operator=(const OtherT& other) {
T::operator=(other);
return *this;
}
inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other) {
T::operator=(other);
return *this;
}
};
} // namespace internal
#else
#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T
#endif
} // namespace Eigen
#endif // EIGEN_STL_DETAILS_H