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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,9 +19,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
units::second_t nominalDt)
: m_observer(
&DifferentialDrivePoseEstimator::F,
[](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)};
[](const Vectord<5>& x, const Vectord<3>& u) {
return Vectord<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,11 +29,10 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
[](const Vectord<5>& x, const Vectord<3>&) {
return x.block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
@@ -73,7 +71,7 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -97,13 +95,13 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto angle = gyroAngle + m_gyroOffset;
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
auto u = Eigen::Vector<double, 3>{
(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
auto u = Vectord<3>{(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0,
omega.value()};
m_previousAngle = angle;
auto localY = Eigen::Vector<double, 3>{
leftDistance.value(), rightDistance.value(), angle.Radians().value()};
auto localY = Vectord<3>{leftDistance.value(), rightDistance.value(),
angle.Radians().value()};
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
m_observer.Predict(u, dt);
@@ -112,24 +110,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
return GetEstimatedPosition();
}
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x,
const Vectord<3>& u) {
// Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
// that for us.
auto& theta = x(2);
Eigen::Matrix<double, 5, 5> toFieldRotation{
Matrixd<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 *
Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
Vectord<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::Vector<double, Dim>& stdDevs) {
const Vectord<Dim>& stdDevs) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = stdDevs(i);
@@ -137,11 +135,11 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
return array;
}
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
Vectord<5> DifferentialDrivePoseEstimator::FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance) {
return Eigen::Vector<double, 5>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value(),
leftDistance.value(), rightDistance.value()};
return Vectord<5>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value(), leftDistance.value(),
rightDistance.value()};
}