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

[wpimath] Replace frc/EigenCore.h typedefs with Eigen's where possible (#5597)

Browse Source
This commit is contained in:
Tyler Veness
2023-08-31 11:03:37 -07:00
committed by GitHub
parent 383289bc4b
commit 99f66b1e24
25 changed files with 79 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -6,16 +6,16 @@
namespace frc {
Vectord<3> PoseTo3dVector(const Pose2d& pose) {
return Vectord<3>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
}
Vectord<4> PoseTo4dVector(const Pose2d& pose) {
return Vectord<4>{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()};
Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector4d{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()};
}
template <>
@@ -34,9 +34,9 @@ bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
}
Vectord<3> PoseToVector(const Pose2d& pose) {
return Vectord<3>{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
}
} // namespace frc

7
wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
View File

@@ -4,7 +4,8 @@
#include "frc/controller/DifferentialDriveFeedforward.h"
#include "frc/EigenCore.h"
#include <Eigen/Core>
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
@@ -30,8 +31,8 @@ DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
units::meters_per_second_t nextRightVelocity, units::second_t dt) {
frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};
Eigen::Vector2d nextR{nextLeftVelocity, nextRightVelocity};
auto u = feedforward.Calculate(r, nextR);
return {units::volt_t{u(0)}, units::volt_t{u(1)}};
}

3
wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
View File

@@ -8,6 +8,7 @@
#include <stdexcept>
#include <utility>
#include <Eigen/Core>
#include <Eigen/QR>
using namespace frc;
@@ -18,7 +19,7 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
// Construct a change of basis matrix from the source coordinate system to the
// NWU coordinate system. Each column vector in the change of basis matrix is
// one of the old basis vectors mapped to its representation in the new basis.
Matrixd<3, 3> R;
Eigen::Matrix3d R;
R.block<3, 1>(0, 0) = positiveX.m_axis;
R.block<3, 1>(0, 1) = positiveY.m_axis;
R.block<3, 1>(0, 2) = positiveZ.m_axis;

9
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -6,6 +6,7 @@
#include <cmath>
#include <Eigen/Core>
#include <wpi/json.h>
using namespace frc;
@@ -21,14 +22,14 @@ namespace {
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace

13
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -41,23 +41,23 @@ Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
: Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
const auto& R = rotationMatrix;
// Require that the rotation matrix is special orthogonal. This is true if the
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
std::string msg =
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
@@ -112,7 +112,8 @@ Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
m_q = Quaternion{w, x, y, z};
}
Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
const Eigen::Vector3d& final) {
double dot = initial.dot(final);
double normProduct = initial.norm() * final.norm();
double dotNorm = dot / normProduct;
@@ -230,7 +231,7 @@ units::radian_t Rotation3d::Z() const {
}
}
Vectord<3> Rotation3d::Axis() const {
Eigen::Vector3d Rotation3d::Axis() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return {0.0, 0.0, 0.0};

8
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -25,7 +25,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -37,7 +37,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Vectord<4> wheelSpeedsVector{
Eigen::Vector4d wheelSpeedsVector{
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
@@ -52,7 +52,7 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& start,
const MecanumDriveWheelPositions& end) const {
Vectord<4> wheelDeltasVector{
Eigen::Vector4d wheelDeltasVector{
end.frontLeft.value() - start.frontLeft.value(),
end.frontRight.value() - start.frontRight.value(),
end.rearLeft.value() - start.rearLeft.value(),
@@ -66,7 +66,7 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
Vectord<4> wheelDeltasVector{
Eigen::Vector4d wheelDeltasVector{
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};

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

@@ -226,7 +226,7 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
* @return The vector.
*/
WPILIB_DLLEXPORT
Vectord<3> PoseTo3dVector(const Pose2d& pose);
Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -236,7 +236,7 @@ Vectord<3> PoseTo3dVector(const Pose2d& pose);
* @return The vector.
*/
WPILIB_DLLEXPORT
Vectord<4> PoseTo4dVector(const Pose2d& pose);
Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -301,7 +301,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
* @return The vector.
*/
WPILIB_DLLEXPORT
Vectord<3> PoseToVector(const Pose2d& pose);
Eigen::Vector3d PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.

1
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -9,7 +9,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"

2
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -4,10 +4,10 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"

4
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -81,9 +81,9 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
// Step 3: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Vectord<3> k_times_twist =
Eigen::Vector3d k_times_twist =
m_visionK *
Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
// Step 4: Convert back to Twist2d
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},

4
wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
View File

@@ -4,9 +4,9 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
@@ -67,7 +67,7 @@ class WPILIB_DLLEXPORT CoordinateAxis {
private:
friend class CoordinateSystem;
Vectord<3> m_axis;
Eigen::Vector3d m_axis;
};
} // namespace frc

1
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -6,7 +6,6 @@
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
#include "frc/geometry/CoordinateAxis.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"

3
wpimath/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -4,10 +4,9 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
namespace wpi {
class json;
} // namespace wpi

8
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -4,9 +4,9 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Quaternion.h"
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
@@ -57,7 +57,7 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Vectord<3>& axis, units::radian_t angle);
Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle);
/**
* Constructs a Rotation3d with the given rotation vector representation. This
@@ -86,7 +86,7 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param initial The initial vector.
* @param final The final vector.
*/
Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final);
/**
* Adds two rotations together.
@@ -173,7 +173,7 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Vectord<3> Axis() const;
Eigen::Vector3d Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.

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

@@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Matrixd<4, 4> MakeHermiteBasis() {
static Eigen::Matrix4d MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
//
@@ -71,10 +71,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
// [a₁] = [ 0 1 0 0][P(i+1) ]
// [a₀] = [ 1 0 0 0][P'(i+1)]
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
return basis;
}

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

@@ -4,12 +4,9 @@
#pragma once
#include <frc/StateSpaceUtil.h>
#include <algorithm>
#include <array>
#include <Eigen/Core>
#include <cmath>
#include "units/time.h"