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

[wpimath] Clean up Eigen usage

Browse Source 
* Replace Matrix<> with Vector<> where vectors are explicitly intended.
  I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
  "\(Eigen"` on main (the initializer list constructors make more false
  positives).
* Replace MakeMatrix() and operator<< usage with initializer list
  constructors. I found these via `rg MakeMatrix` and `rg "<<"`
  respectively.
* Deprecate MakeMatrix()
This commit is contained in:
Tyler Veness
2021-08-19 00:23:48 -07:00
committed by Peter Johnson
parent 72716f51ce
commit 9359431bad
63 changed files with 821 additions and 955 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 {
Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose) {
return frc::MakeMatrix<3, 1>(pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>());
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>()};
}
Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose) {
return frc::MakeMatrix<4, 1>(pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Cos(), pose.Rotation().Sin());
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector<double, 4>{pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Cos(), pose.Rotation().Sin()};
}
template <>
@@ -30,9 +30,9 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
return detail::IsStabilizableImpl<2, 1>(A, B);
}
Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
pose.Rotation().Radians().to<double>());
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.X().to<double>(), pose.Y().to<double>(),
pose.Rotation().Radians().to<double>()};
}
} // namespace frc

55
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -19,9 +19,9 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
units::second_t nominalDt)
: m_observer(
&DifferentialDrivePoseEstimator::F,
[](const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) {
return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
[](const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 3>& u) {
return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
@@ -30,12 +30,13 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y) {
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 3, 1>&) { return x.block<3, 1>(0, 0); },
[](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
return x.block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
};
@@ -94,15 +95,15 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto angle = gyroAngle + m_gyroOffset;
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
auto u = frc::MakeMatrix<3, 1>(
auto u = Eigen::Vector<double, 3>{
(wheelSpeeds.left + wheelSpeeds.right).to<double>() / 2.0, 0.0,
omega.to<double>());
omega.to<double>()};
m_previousAngle = angle;
auto localY = frc::MakeMatrix<3, 1>(leftDistance.to<double>(),
rightDistance.to<double>(),
angle.Radians().to<double>());
auto localY = Eigen::Vector<double, 3>{leftDistance.to<double>(),
rightDistance.to<double>(),
angle.Radians().to<double>()};
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
m_observer.Predict(u, dt);
@@ -111,26 +112,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
return GetEstimatedPosition();
}
Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) {
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
// Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
// that for us.
auto& theta = x(2, 0);
Eigen::Matrix<double, 5, 5> toFieldRotation = frc::MakeMatrix<5, 5>(
// clang-format off
std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0,
std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0); // clang-format on
auto& theta = x(2);
Eigen::Matrix<double, 5, 5> toFieldRotation{
{std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
{std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 1.0}};
return toFieldRotation *
frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0));
Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
}
template <int Dim>
wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& stdDevs) {
const Eigen::Vector<double, Dim>& stdDevs) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = stdDevs(i);
@@ -138,11 +137,11 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
return array;
}
Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::FillStateVector(
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance) {
return frc::MakeMatrix<5, 1>(
return Eigen::Vector<double, 5>{
pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
rightDistance.to<double>());
rightDistance.to<double>()};
}

34
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -18,26 +18,26 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer([](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) { return u; },
[](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
: m_observer(
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return u; },
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create vision correction mechanism.
m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y) {
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>&) { return x; },
[](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
return x;
},
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
@@ -100,11 +100,11 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
auto u = frc::MakeMatrix<3, 1>(fieldRelativeVelocities.X().to<double>(),
fieldRelativeVelocities.Y().to<double>(),
omega.to<double>());
Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().to<double>(),
fieldRelativeVelocities.Y().to<double>(),
omega.to<double>()};
auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
Eigen::Vector<double, 1> localY{angle.Radians().template to<double>()};
m_previousAngle = angle;
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);

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

@@ -21,31 +21,29 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(),
chassisSpeeds.omega.to<double>()};
Eigen::Matrix<double, 4, 1> wheelsMatrix =
Eigen::Vector<double, 4> wheelsVector =
m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
return wheelSpeeds;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
// clang-format off
wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
// clang-format on
Eigen::Vector<double, 4> wheelSpeedsVector{
wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>()};
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsMatrix);
m_forwardKinematics.solve(wheelSpeedsVector);
return {units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
units::meters_per_second_t{chassisSpeedsVector(1)},
@@ -56,10 +54,9 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
// clang-format off
m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
1, 1, (fr.X() - fr.Y()).template to<double>(),
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics = Eigen::Matrix<double, 4, 3>{
{1, -1, (-(fl.X() + fl.Y())).template to<double>()},
{1, 1, (fr.X() - fr.Y()).template to<double>()},
{1, 1, (rl.X() - rl.Y()).template to<double>()},
{1, -1, (-(rr.X() + rr.Y())).template to<double>()}};
}