Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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()};